Merge pull request #127 from Jondolf/mass-prop-fixes

Fix center of mass and inertia computations and add tests

src/components/mass_properties.rs

@@ -80,6 +80,33 @@ impl Inertia {
     pub fn inverse(&self) -> InverseInertia {
         InverseInertia(self.0.inverse())
     }
+
+    /// Computes the inertia of a body with the given mass, shifted by the given offset.
+    #[cfg(feature = "2d")]
+    pub fn shifted(&self, mass: Scalar, offset: Vector) -> Scalar {
+        if mass > 0.0 && mass.is_finite() {
+            self.0 + offset.length_squared() * mass
+        } else {
+            self.0
+        }
+    }
+
+    /// Computes the inertia of a body with the given mass, shifted by the given offset.
+    #[cfg(feature = "3d")]
+    pub fn shifted(&self, mass: Scalar, offset: Vector) -> Matrix3 {
+        type NaMatrix3 = parry::na::Matrix3<math::Scalar>;
+        use parry::na::*;
+
+        if mass > 0.0 && mass.is_finite() {
+            let matrix = NaMatrix3::from(self.0);
+            let offset = Vector::from(offset);
+            let diagonal_el = offset.norm_squared();
+            let diagonal_mat = NaMatrix3::from_diagonal_element(diagonal_el);
+            math::Matrix3::from(matrix + (diagonal_mat + offset * offset.transpose()) * mass)
+        } else {
+            self.0
+        }
+    }
 }
 
 /// The inverse moment of inertia of the body. This represents the inverse of the torque needed for a desired angular acceleration.

src/components/world_queries.rs

@@ -93,20 +93,191 @@ pub(crate) struct ColliderQuery {
 
 impl<'w> AddAssign<ColliderMassProperties> for MassPropertiesQueryItem<'w> {
     fn add_assign(&mut self, rhs: ColliderMassProperties) {
-        self.mass.0 += rhs.mass.0;
+        let new_mass = self.mass.0 + rhs.mass.0;
+
+        if new_mass <= 0.0 {
+            return;
+        }
+
+        let com1 = self.center_of_mass.0;
+        let com2 = rhs.center_of_mass.0;
+
+        // Compute the combined center of mass and combined inertia tensor
+        let new_com = (com1 * self.mass.0 + com2 * rhs.mass.0) / new_mass;
+        let i1 = self.inertia.shifted(self.mass.0, new_com - com1);
+        let i2 = rhs.inertia.shifted(rhs.mass.0, new_com - com2);
+        let new_inertia = i1 + i2;
+
+        // Update mass properties
+        self.mass.0 = new_mass;
         self.inverse_mass.0 = 1.0 / self.mass.0;
-        self.inertia.0 += rhs.inertia.0;
+        self.inertia.0 = new_inertia;
         self.inverse_inertia.0 = self.inertia.inverse().0;
-        self.center_of_mass.0 += rhs.center_of_mass.0;
+        self.center_of_mass.0 = new_com;
     }
 }
 
 impl<'w> SubAssign<ColliderMassProperties> for MassPropertiesQueryItem<'w> {
     fn sub_assign(&mut self, rhs: ColliderMassProperties) {
-        self.mass.0 -= rhs.mass.0;
+        if self.mass.0 + rhs.mass.0 <= 0.0 {
+            return;
+        }
+
+        let new_mass = (self.mass.0 - rhs.mass.0).max(0.0);
+        let com1 = self.center_of_mass.0;
+        let com2 = rhs.center_of_mass.0;
+
+        // Compute the combined center of mass and combined inertia tensor
+        let new_com = if new_mass > Scalar::EPSILON {
+            (com1 * self.mass.0 - com2 * rhs.mass.0) / new_mass
+        } else {
+            com1
+        };
+        let i1 = self.inertia.shifted(self.mass.0, new_com - com1);
+        let i2 = rhs.inertia.shifted(rhs.mass.0, new_com - com2);
+        let new_inertia = i1 - i2;
+
+        // Update mass properties
+        self.mass.0 = new_mass;
         self.inverse_mass.0 = 1.0 / self.mass.0;
-        self.inertia.0 -= rhs.inertia.0;
+        self.inertia.0 = new_inertia;
         self.inverse_inertia.0 = self.inertia.inverse().0;
-        self.center_of_mass.0 -= rhs.center_of_mass.0;
+        self.center_of_mass.0 = new_com;
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::prelude::*;
+    use approx::assert_relative_eq;
+    use bevy::prelude::*;
+
+    // Todo: Test if inertia values are correct
+    #[test]
+    fn mass_properties_add_assign_works() {
+        // Create app
+        let mut app = App::new();
+        app.add_plugins(MinimalPlugins);
+
+        // Spawn an entity with mass properties
+        app.world.spawn(MassPropertiesBundle {
+            mass: Mass(1.6),
+            inverse_mass: InverseMass(1.0 / 1.6),
+            center_of_mass: CenterOfMass(Vector::NEG_X * 3.8),
+            ..default()
+        });
+
+        // Create collider mass properties that will be added to the existing mass properties
+        let collider_mass_props = ColliderMassProperties {
+            mass: Mass(8.1),
+            inverse_mass: InverseMass(1.0 / 8.1),
+            center_of_mass: CenterOfMass(Vector::X * 1.2 + Vector::Y),
+            ..default()
+        };
+
+        // Get the mass properties and add the collider mass properties
+        let mut query = app.world.query::<MassPropertiesQuery>();
+        let mut mass_props = query.single_mut(&mut app.world);
+        mass_props += collider_mass_props;
+
+        // Test if values are correct
+        // (reference values were calculated by hand)
+        assert_eq!(mass_props.mass.0, 9.7);
+        assert_eq!(mass_props.inverse_mass.0, 1.0 / 9.7);
+        assert_relative_eq!(
+            mass_props.center_of_mass.0,
+            Vector::X * 0.375_257 + Vector::Y * 0.835_051,
+            epsilon = 0.000_001
+        );
+    }
+
+    // Todo: Test if inertia values are correct
+    #[test]
+    fn mass_properties_sub_assign_works() {
+        // Create app
+        let mut app = App::new();
+        app.add_plugins(MinimalPlugins);
+
+        // Spawn an entity with mass properties
+        app.world.spawn(MassPropertiesBundle {
+            mass: Mass(8.1),
+            inverse_mass: InverseMass(1.0 / 8.1),
+            center_of_mass: CenterOfMass(Vector::NEG_X * 3.8),
+            ..default()
+        });
+
+        // Create collider mass properties that will be subtracted from the existing mass properties
+        let collider_mass_props = ColliderMassProperties {
+            mass: Mass(1.6),
+            inverse_mass: InverseMass(1.0 / 1.6),
+            center_of_mass: CenterOfMass(Vector::X * 1.2 + Vector::Y),
+            ..default()
+        };
+
+        // Get the mass properties and subtract the collider mass properties
+        let mut query = app.world.query::<MassPropertiesQuery>();
+        let mut mass_props = query.single_mut(&mut app.world);
+        mass_props -= collider_mass_props;
+
+        // Test if values are correct.
+        // The reference values were calculated by hand.
+        // The center of mass is computed as: (com1 * mass1 - com2 * mass2) / (mass1 - mass2).max(0.0)
+        assert_eq!(mass_props.mass.0, 6.5);
+        assert_eq!(mass_props.inverse_mass.0, 1.0 / 6.5);
+        assert_relative_eq!(
+            mass_props.center_of_mass.0,
+            Vector::NEG_X * 5.030_769 + Vector::NEG_Y * 0.246_153,
+            epsilon = 0.000_001
+        );
+    }
+
+    #[test]
+    fn mass_properties_add_sub_works() {
+        // Create app
+        let mut app = App::new();
+        app.add_plugins(MinimalPlugins);
+
+        let original_mass_props =
+            MassPropertiesBundle::new_computed(&Collider::capsule(2.4, 0.6), 3.9);
+
+        // Spawn an entity with mass properties
+        app.world.spawn(original_mass_props.clone());
+
+        // Create collider mass properties
+        let collider_mass_props =
+            ColliderMassProperties::new_computed(&Collider::capsule(7.4, 2.1), 14.3);
+
+        // Get the mass properties and then add and subtract the collider mass properties
+        let mut query = app.world.query::<MassPropertiesQuery>();
+        let mut mass_props = query.single_mut(&mut app.world);
+        mass_props += collider_mass_props;
+        mass_props -= collider_mass_props;
+
+        // Test if values are correct. They should be equal to the original values.
+        assert_relative_eq!(
+            mass_props.mass.0,
+            original_mass_props.mass.0,
+            epsilon = 0.000_001
+        );
+        assert_relative_eq!(
+            mass_props.inverse_mass.0,
+            original_mass_props.inverse_mass.0,
+            epsilon = 0.000_001
+        );
+        assert_relative_eq!(
+            mass_props.inertia.0,
+            original_mass_props.inertia.0,
+            epsilon = 0.000_001
+        );
+        assert_relative_eq!(
+            mass_props.inverse_inertia.0,
+            original_mass_props.inverse_inertia.0,
+            epsilon = 0.000_001
+        );
+        assert_relative_eq!(
+            mass_props.center_of_mass.0,
+            original_mass_props.center_of_mass.0,
+            epsilon = 0.000_001
+        );
     }
 }