Fix mass property update when adding a collider.

7 files changed, 127 insertions, 64 deletions

diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index 8affe0a..f2fce4b 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -24,59 +24,6 @@ pub struct MassProperties {
     pub principal_inertia_local_frame: Rotation<f32>,
 }
 
-impl approx::AbsDiffEq for MassProperties {
-    type Epsilon = f32;
-    fn default_epsilon() -> Self::Epsilon {
-        f32::default_epsilon()
-    }
-
-    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
-        self.local_com.abs_diff_eq(&other.local_com, epsilon)
-            && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
-            && self
-                .inv_principal_inertia_sqrt
-                .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
-        // && self
-        //     .principal_inertia_local_frame
-        //     .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
-    }
-}
-
-impl approx::RelativeEq for MassProperties {
-    fn default_max_relative() -> Self::Epsilon {
-        f32::default_max_relative()
-    }
-
-    fn relative_eq(
-        &self,
-        other: &Self,
-        epsilon: Self::Epsilon,
-        max_relative: Self::Epsilon,
-    ) -> bool {
-        #[cfg(feature = "dim2")]
-        let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
-            &other.inv_principal_inertia_sqrt,
-            epsilon,
-            max_relative,
-        );
-
-        #[cfg(feature = "dim3")]
-        let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
-            &other.reconstruct_inverse_inertia_matrix(),
-            epsilon,
-            max_relative,
-        );
-
-        inertia_is_ok
-            && self
-                .local_com
-                .relative_eq(&other.local_com, epsilon, max_relative)
-            && self
-                .inv_mass
-                .relative_eq(&other.inv_mass, epsilon, max_relative)
-    }
-}
-
 impl MassProperties {
     #[cfg(feature = "dim2")]
     pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
@@ -190,6 +137,19 @@ impl MassProperties {
             Matrix3::zeros()
         }
     }
+
+    /// Transform each element of the mass properties.
+    pub fn transform_by(&self, m: &Isometry<f32>) -> Self {
+        // NOTE: we don't apply the parallel axis theorem here
+        // because the center of mass is also transformed.
+        Self {
+            local_com: m * self.local_com,
+            inv_mass: self.inv_mass,
+            inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt,
+            #[cfg(feature = "dim3")]
+            principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
+        }
+    }
 }
 
 impl Zero for MassProperties {
@@ -331,6 +291,59 @@ impl AddAssign<MassProperties> for MassProperties {
     }
 }
 
+impl approx::AbsDiffEq for MassProperties {
+    type Epsilon = f32;
+    fn default_epsilon() -> Self::Epsilon {
+        f32::default_epsilon()
+    }
+
+    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
+        self.local_com.abs_diff_eq(&other.local_com, epsilon)
+            && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
+            && self
+                .inv_principal_inertia_sqrt
+                .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
+        // && self
+        //     .principal_inertia_local_frame
+        //     .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
+    }
+}
+
+impl approx::RelativeEq for MassProperties {
+    fn default_max_relative() -> Self::Epsilon {
+        f32::default_max_relative()
+    }
+
+    fn relative_eq(
+        &self,
+        other: &Self,
+        epsilon: Self::Epsilon,
+        max_relative: Self::Epsilon,
+    ) -> bool {
+        #[cfg(feature = "dim2")]
+        let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
+            &other.inv_principal_inertia_sqrt,
+            epsilon,
+            max_relative,
+        );
+
+        #[cfg(feature = "dim3")]
+        let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
+            &other.reconstruct_inverse_inertia_matrix(),
+            epsilon,
+            max_relative,
+        );
+
+        inertia_is_ok
+            && self
+                .local_com
+                .relative_eq(&other.local_com, epsilon, max_relative)
+            && self
+                .inv_mass
+                .relative_eq(&other.inv_mass, epsilon, max_relative)
+    }
+}
+
 #[cfg(test)]
 mod test {
     use super::MassProperties;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index d9cddd1..a2fcacc 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -139,7 +139,9 @@ impl RigidBody {
 
     /// Adds a collider to this rigid-body.
     pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
-        let mass_properties = coll.mass_properties();
+        let mass_properties = coll
+            .mass_properties()
+            .transform_by(coll.position_wrt_parent());
         self.colliders.push(handle);
         self.mass_properties += mass_properties;
         self.update_world_mass_properties();
@@ -149,7 +151,9 @@ impl RigidBody {
     pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
         if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
             self.colliders.swap_remove(i);
-            let mass_properties = coll.mass_properties();
+            let mass_properties = coll
+                .mass_properties()
+                .transform_by(coll.position_wrt_parent());
             self.mass_properties -= mass_properties;
             self.update_world_mass_properties();
         }
@@ -189,7 +193,7 @@ impl RigidBody {
     }
 
     fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
-        let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
+        let com = &self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 01c78ac..aed76c8 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -150,6 +150,7 @@ impl Collider {
     }
 
     /// The position of this collider expressed in the local-space of the rigid-body it is attached to.
+    #[deprecated(note = "use `.position_wrt_parent()` instead.")]
     pub fn delta(&self) -> &Isometry<f32> {
         &self.delta
     }
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
index 2cda3e0..7e235c2 100644
--- a/src/geometry/contact.rs
+++ b/src/geometry/contact.rs
@@ -360,8 +360,8 @@ impl ContactManifold {
             pair,
             (subshape1, subshape2),
             BodyPair::new(coll1.parent, coll2.parent),
-            *coll1.delta(),
-            *coll2.delta(),
+            *coll1.position_wrt_parent(),
+            *coll2.position_wrt_parent(),
             (coll1.friction + coll2.friction) * 0.5,
             (coll1.restitution + coll2.restitution) * 0.5,
         )
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index f66d987..c9ff2d1 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -165,7 +165,7 @@ impl NPhysicsWorld {
 
             for coll_handle in rb.colliders() {
                 let collider = &mut colliders[*coll_handle];
-                collider.set_position_debug(pos * collider.delta());
+                collider.set_position_debug(pos * collider.position_wrt_parent());
             }
         }
     }
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 045e697..66036c8 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -225,6 +225,28 @@ impl PhysxWorld {
             }
         }
 
+        // Update mass properties.
+        for (rapier_handle, physx_handle) in rapier2physx.iter() {
+            let rb = &bodies[*rapier_handle];
+            if let Some(mut ra) = scene.get_dynamic_mut(*physx_handle) {
+                let densities: Vec<_> = rb
+                    .colliders()
+                    .iter()
+                    .map(|h| colliders[*h].density())
+                    .collect();
+
+                unsafe {
+                    physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
+                        ra.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
+                        densities.as_ptr(),
+                        densities.len() as u32,
+                        std::ptr::null(),
+                        false,
+                    );
+                }
+            }
+        }
+
         let mut res = Self {
             physics,
             cooking,
@@ -390,7 +412,7 @@ impl PhysxWorld {
 
             for coll_handle in rb.colliders() {
                 let collider = &mut colliders[*coll_handle];
-                collider.set_position_debug(iso * collider.delta());
+                collider.set_position_debug(iso * collider.position_wrt_parent());
             }
         }
     }
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 868bd91..450170e 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -791,6 +791,29 @@ impl Testbed {
                 .state
                 .action_flags
                 .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
+            WindowEvent::Key(Key::C, Action::Release, _) => {
+                // Delete 1 collider of 10% of the remaining dynamic bodies.
+                let mut colliders: Vec<_> = self
+                    .physics
+                    .bodies
+                    .iter()
+                    .filter(|e| e.1.is_dynamic())
+                    .filter(|e| !e.1.colliders().is_empty())
+                    .map(|e| e.1.colliders().to_vec())
+                    .collect();
+                colliders.sort_by_key(|co| -(co.len() as isize));
+
+                let num_to_delete = (colliders.len() / 10).max(1);
+                for to_delete in &colliders[..num_to_delete] {
+                    self.physics.pipeline.remove_collider(
+                        to_delete[0],
+                        &mut self.physics.broad_phase,
+                        &mut self.physics.narrow_phase,
+                        &mut self.physics.bodies,
+                        &mut self.physics.colliders,
+                    );
+                }
+            }
             WindowEvent::Key(Key::D, Action::Release, _) => {
                 // Delete 10% of the remaining dynamic bodies.
                 let dynamic_bodies: Vec<_> = self
@@ -1539,7 +1562,7 @@ impl State for Testbed {
         }
 
         if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) {
-            draw_contacts(window, &self.physics.narrow_phase, &self.physics.bodies);
+            draw_contacts(window, &self.physics.narrow_phase, &self.physics.colliders);
         }
 
         if self.state.running == RunMode::Step {
@@ -1634,7 +1657,7 @@ Hashes at frame: {}
     }
 }
 
-fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
+fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
     for (_, _, pair) in nf.contact_graph().interaction_pairs() {
         for manifold in &pair.manifolds {
             for pt in manifold.all_contacts() {
@@ -1643,8 +1666,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
                 } else {
                     Point3::new(1.0, 0.0, 0.0)
                 };
-                let pos1 = bodies[manifold.body_pair.body1].position;
-                let pos2 = bodies[manifold.body_pair.body2].position;
+                let pos1 = colliders[manifold.pair.collider1].position();
+                let pos2 = colliders[manifold.pair.collider2].position();
                 let start = pos1 * pt.local_p1;
                 let end = pos2 * pt.local_p2;
                 let n = pos1 * manifold.local_n1;