Fix wrong sign in MassProperties::construct_shifted_inertia_matrix (#334)

Author: Jondolf

CHANGELOG.md

@@ -23,6 +23,7 @@
 ### Fix
 
 - Fix trimesh inertia tensor computation [#331](https://github.com/dimforge/parry/pull/331).
+- Fix shifted inertia tensor computation [#334](https://github.com/dimforge/parry/pull/334).
 
 ## v0.18.0
 

src/mass_properties/mass_properties.rs

@@ -202,7 +202,7 @@ impl MassProperties {
             let mass = 1.0 / self.inv_mass;
             let diag = shift.norm_squared();
             let diagm = Matrix3::from_diagonal_element(diag);
-            matrix + (diagm + shift * shift.transpose()) * mass
+            matrix + (diagm - shift * shift.transpose()) * mass
         } else {
             matrix
         }
@@ -590,10 +590,68 @@ mod test {
         assert_relative_eq!((((m1m2m3 - m1) - m2) - m3).mass(), 0.0, epsilon = 1.0e-6);
     }
 
+    #[test]
+    #[cfg(feature = "alloc")]
+    fn mass_properties_compound() {
+        use na::Isometry;
+
+        use crate::{
+            math::Vector,
+            shape::{Compound, Cuboid, SharedShape},
+        };
+
+        // Compute the mass properties of a compound shape made of three 1x1x1 cuboids.
+        let shape = Cuboid::new(Vector::repeat(0.5));
+        let mp = shape.mass_properties(1.0);
+        let iso2 = Isometry::from_parts(Vector::y().into(), Default::default());
+        let iso3 = Isometry::from_parts((-Vector::y()).into(), Default::default());
+
+        // Test sum shifted result through `MassProperties::add`
+        let sum = [mp, mp.transform_by(&iso2), mp.transform_by(&iso3)]
+            .into_iter()
+            .sum::<MassProperties>();
+
+        // Test compound through `MassProperties::from_compound`
+        let compound_shape = Compound::new(vec![
+            (
+                Isometry::from_parts(Vector::default().into(), Default::default()),
+                SharedShape::new(shape),
+            ),
+            (iso2, SharedShape::new(shape)),
+            (iso3, SharedShape::new(shape)),
+        ]);
+        let mp_compound = compound_shape.mass_properties(1.0);
+
+        // Check that the mass properties of the compound shape match the mass properties
+        // of a single 1x3x1 cuboid.
+        #[cfg(feature = "dim2")]
+        let expected = Cuboid::new(Vector::new(0.5, 1.5)).mass_properties(1.0);
+        #[cfg(feature = "dim3")]
+        let expected = Cuboid::new(Vector::new(0.5, 1.5, 0.5)).mass_properties(1.0);
+
+        // Sum shifted
+        assert_relative_eq!(sum.local_com, expected.local_com, epsilon = 1.0e-6);
+        assert_relative_eq!(sum.inv_mass, expected.inv_mass, epsilon = 1.0e-6);
+        assert_relative_eq!(
+            sum.inv_principal_inertia_sqrt,
+            expected.inv_principal_inertia_sqrt,
+            epsilon = 1.0e-6
+        );
+
+        // Compound
+        assert_relative_eq!(mp_compound.local_com, expected.local_com, epsilon = 1.0e-6);
+        assert_relative_eq!(mp_compound.inv_mass, expected.inv_mass, epsilon = 1.0e-6);
+        assert_relative_eq!(
+            mp_compound.inv_principal_inertia_sqrt,
+            expected.inv_principal_inertia_sqrt,
+            epsilon = 1.0e-6
+        );
+    }
+
     #[test]
     #[cfg(feature = "alloc")]
     fn mass_properties_sum_no_nan() {
-        let mp: MassProperties = [MassProperties::zero()].iter().map(|v| *v).sum();
+        let mp: MassProperties = [MassProperties::zero()].iter().copied().sum();
         assert!(!mp.local_com.x.is_nan() && !mp.local_com.y.is_nan());
         #[cfg(feature = "dim3")]
         assert!(!mp.local_com.z.is_nan());