Fix mass property update when adding a collider.

2 files changed, 73 insertions, 56 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()
     }