Improve the API for initializing/setting mass-properties

2 files changed, 147 insertions, 114 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index eaf3967..494dc6c 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,14 +1,14 @@
 use crate::dynamics::{
-    LockedAxes, MassProperties, RigidBodyActivation, RigidBodyCcd, RigidBodyChanges,
-    RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    LockedAxes, MassProperties, RigidBodyActivation, RigidBodyAdditionalMassProps, RigidBodyCcd,
+    RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{
-    Collider, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderShape,
+    Collider, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet,
+    ColliderShape,
 };
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::{self, WCross};
-use na::ComplexField;
+use crate::utils::WCross;
 use num::Zero;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -123,7 +123,7 @@ impl RigidBody {
         }
     }
 
-    /// The mass properties of this rigid-body.
+    /// The mass-properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &MassProperties {
         &self.mprops.local_mprops
@@ -310,26 +310,82 @@ impl RigidBody {
         self.ccd.ccd_active
     }
 
-    /// Sets the rigid-body's additional mass properties.
+    /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.
+    pub fn recompute_mass_properties_from_colliders(&mut self, colliders: &ColliderSet) {
+        self.mprops.recompute_mass_properties_from_colliders(
+            colliders,
+            &self.colliders,
+            &self.pos.position,
+        );
+    }
+
+    /// Sets the rigid-body's additional mass.
+    ///
+    /// The total angular inertia of the rigid-body will be scaled automatically based on this
+    /// additional mass. If this scaling effect isn’t desired, use [`Self::additional_mass_properties`]
+    /// instead of this method.
+    ///
+    /// This is only the "additional" mass because the total mass of the  rigid-body is
+    /// equal to the sum of this additional mass and the mass computed from the colliders
+    /// (with non-zero densities) attached to this rigid-body.
+    ///
+    /// That total mass (which includes the attached colliders’ contributions)
+    /// will be updated at the name physics step, or can be updated manually with
+    /// [`Self::recompute_mass_properties_from_colliders`].
+    ///
+    /// This will override any previous mass-properties set by [`Self::set_additional_mass`],
+    /// [`Self::set_additional_mass_properties`], [`RigidBodyBuilder::additional_mass`], or
+    /// [`RigidBodyBuilder::additional_mass_properties`] for this rigid-body.
+    ///
+    /// If `wake_up` is `true` then the rigid-body will be woken up if it was
+    /// put to sleep because it did not move for a while.
+    #[inline]
+    pub fn set_additional_mass(&mut self, additional_mass: Real, wake_up: bool) {
+        self.do_set_additional_mass_properties(
+            RigidBodyAdditionalMassProps::Mass(additional_mass),
+            wake_up,
+        )
+    }
+
+    /// Sets the rigid-body's additional mass-properties.
+    ///
+    /// This is only the "additional" mass-properties because the total mass-properties of the
+    /// rigid-body is equal to the sum of this additional mass-properties and the mass computed from
+    /// the colliders (with non-zero densities) attached to this rigid-body.
+    ///
+    /// That total mass-properties (which include the attached colliders’ contributions)
+    /// will be updated at the name physics step, or can be updated manually with
+    /// [`Self::recompute_mass_properties_from_colliders`].
+    ///
+    /// This will override any previous mass-properties set by [`Self::set_additional_mass`],
+    /// [`Self::set_additional_mass_properties`], [`RigidBodyBuilder::additional_mass`], or
+    /// [`RigidBodyBuilder::additional_mass_properties`] for this rigid-body.
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
     #[inline]
     pub fn set_additional_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
-        if let Some(add_mprops) = &mut self.mprops.additional_local_mprops {
-            self.mprops.local_mprops += props;
-            self.mprops.local_mprops -= **add_mprops;
-            **add_mprops = props;
-        } else {
-            self.mprops.additional_local_mprops = Some(Box::new(props));
-            self.mprops.local_mprops += props;
-        }
+        self.do_set_additional_mass_properties(
+            RigidBodyAdditionalMassProps::MassProps(props),
+            wake_up,
+        )
+    }
 
-        if self.is_dynamic() && wake_up {
-            self.wake_up(true);
-        }
+    fn do_set_additional_mass_properties(
+        &mut self,
+        props: RigidBodyAdditionalMassProps,
+        wake_up: bool,
+    ) {
+        let new_mprops = Some(Box::new(props));
 
-        self.update_world_mass_properties();
+        if self.mprops.additional_local_mprops != new_mprops {
+            self.changes.insert(RigidBodyChanges::LOCAL_MASS_PROPERTIES);
+            self.mprops.additional_local_mprops = new_mprops;
+
+            if self.is_dynamic() && wake_up {
+                self.wake_up(true);
+            }
+        }
     }
 
     /// The handles of colliders attached to this rigid body.
@@ -432,12 +488,6 @@ impl RigidBody {
         if let Some(i) = self.colliders.0.iter().position(|e| *e == handle) {
             self.changes.set(RigidBodyChanges::COLLIDERS, true);
             self.colliders.0.swap_remove(i);
-
-            let mass_properties = coll
-                .mass_properties()
-                .transform_by(coll.position_wrt_parent().unwrap());
-            self.mprops.local_mprops -= mass_properties;
-            self.update_world_mass_properties();
         }
     }
 
@@ -859,8 +909,8 @@ pub struct RigidBodyBuilder {
     pub angular_damping: Real,
     body_type: RigidBodyType,
     mprops_flags: LockedAxes,
-    /// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
-    pub additional_mass_properties: MassProperties,
+    /// The additional mass-properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
+    additional_mass_properties: RigidBodyAdditionalMassProps,
     /// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
     pub can_sleep: bool,
     /// Whether or not the rigid-body is to be created asleep.
@@ -887,7 +937,7 @@ impl RigidBodyBuilder {
             angular_damping: 0.0,
             body_type,
             mprops_flags: LockedAxes::empty(),
-            additional_mass_properties: MassProperties::zero(),
+            additional_mass_properties: RigidBodyAdditionalMassProps::default(),
             can_sleep: true,
             sleeping: false,
             ccd_enabled: false,
@@ -968,18 +1018,41 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets the additional mass properties of the rigid-body being built.
+    /// Sets the additional mass-properties of the rigid-body being built.
     ///
-    /// Note that "additional" means that the final mass properties of the rigid-bodies depends
+    /// This will be overridden by a call to [`Self::additional_mass`] so it only makes sense to call
+    /// either [`Self::additional_mass`] or [`Self::additional_mass_properties`].    
+    ///
+    /// Note that "additional" means that the final mass-properties of the rigid-bodies depends
     /// on the initial mass-properties of the rigid-body (set by this method)
     /// to which is added the contributions of all the colliders with non-zero density
     /// attached to this rigid-body.
     ///
-    /// Therefore, if you want your provided mass properties to be the final
-    /// mass properties of your rigid-body, don't attach colliders to it, or
+    /// Therefore, if you want your provided mass-properties to be the final
+    /// mass-properties of your rigid-body, don't attach colliders to it, or
     /// only attach colliders with densities equal to zero.
-    pub fn additional_mass_properties(mut self, props: MassProperties) -> Self {
-        self.additional_mass_properties = props;
+    pub fn additional_mass_properties(mut self, mprops: MassProperties) -> Self {
+        self.additional_mass_properties = RigidBodyAdditionalMassProps::MassProps(mprops);
+        self
+    }
+
+    /// Sets the additional mass of the rigid-body being built.
+    ///
+    /// This will be overridden by a call to [`Self::additional_mass_properties`] so it only makes
+    /// sense to call either [`Self::additional_mass`] or [`Self::additional_mass_properties`].    
+    ///
+    /// This is only the "additional" mass because the total mass of the  rigid-body is
+    /// equal to the sum of this additional mass and the mass computed from the colliders
+    /// (with non-zero densities) attached to this rigid-body.
+    ///
+    /// The total angular inertia of the rigid-body will be scaled automatically based on this
+    /// additional mass. If this scaling effect isn’t desired, use [`Self::additional_mass_properties`]
+    /// instead of this method.
+    ///
+    /// # Parameters
+    /// * `mass`- The mass that will be added to the created rigid-body.
+    pub fn additional_mass(mut self, mass: Real) -> Self {
+        self.additional_mass_properties = RigidBodyAdditionalMassProps::Mass(mass);
         self
     }
 
@@ -1037,78 +1110,6 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets the additional mass of the rigid-body being built.
-    ///
-    /// This is only the "additional" mass because the total mass of the  rigid-body is
-    /// equal to the sum of this additional mass and the mass computed from the colliders
-    /// (with non-zero densities) attached to this rigid-body.
-    pub fn additional_mass(mut self, mass: Real) -> Self {
-        self.additional_mass_properties.set_mass(mass, false);
-        self
-    }
-
-    /// Sets the additional mass of the rigid-body being built.
-    ///
-    /// This is only the "additional" mass because the total mass of the  rigid-body is
-    /// equal to the sum of this additional mass and the mass computed from the colliders
-    /// (with non-zero densities) attached to this rigid-body.
-    #[deprecated(note = "renamed to `additional_mass`.")]
-    pub fn mass(self, mass: Real) -> Self {
-        self.additional_mass(mass)
-    }
-
-    /// Sets the additional angular inertia of this rigid-body.
-    ///
-    /// This is only the "additional" angular inertia because the total angular inertia of
-    /// the rigid-body is equal to the sum of this additional value and the angular inertia
-    /// computed from the colliders (with non-zero densities) attached to this rigid-body.
-    #[cfg(feature = "dim2")]
-    pub fn additional_principal_angular_inertia(mut self, inertia: Real) -> Self {
-        self.additional_mass_properties.inv_principal_inertia_sqrt =
-            utils::inv(ComplexField::sqrt(inertia.max(0.0)));
-        self
-    }
-
-    /// Sets the angular inertia of this rigid-body.
-    #[cfg(feature = "dim2")]
-    #[deprecated(note = "renamed to `additional_principal_angular_inertia`.")]
-    pub fn principal_angular_inertia(self, inertia: Real) -> Self {
-        self.additional_principal_angular_inertia(inertia)
-    }
-
-    /// Use `self.principal_angular_inertia` instead.
-    #[cfg(feature = "dim2")]
-    #[deprecated(note = "renamed to `additional_principal_angular_inertia`.")]
-    pub fn principal_inertia(self, inertia: Real) -> Self {
-        self.additional_principal_angular_inertia(inertia)
-    }
-
-    /// Sets the additional principal angular inertia of this rigid-body.
-    ///
-    /// This is only the "additional" angular inertia because the total angular inertia of
-    /// the rigid-body is equal to the sum of this additional value and the angular inertia
-    /// computed from the colliders (with non-zero densities) attached to this rigid-body.
-    #[cfg(feature = "dim3")]
-    pub fn additional_principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
-        self.additional_mass_properties.inv_principal_inertia_sqrt =
-            inertia.map(|e| utils::inv(ComplexField::sqrt(e.max(0.0))));
-        self
-    }
-
-    /// Sets the principal angular inertia of this rigid-body.
-    #[cfg(feature = "dim3")]
-    #[deprecated(note = "renamed to `additional_principal_angular_inertia`.")]
-    pub fn principal_angular_inertia(self, inertia: AngVector<Real>) -> Self {
-        self.additional_principal_angular_inertia(inertia)
-    }
-
-    /// Use `self.principal_angular_inertia` instead.
-    #[cfg(feature = "dim3")]
-    #[deprecated(note = "renamed to `additional_principal_angular_inertia`.")]
-    pub fn principal_inertia(self, inertia: AngVector<Real>) -> Self {
-        self.additional_principal_angular_inertia(inertia)
-    }
-
     /// Sets the damping factor for the linear part of the rigid-body motion.
     ///
     /// The higher the linear damping factor is, the more quickly the rigid-body
@@ -1169,9 +1170,11 @@ impl RigidBodyBuilder {
         rb.body_type = self.body_type;
         rb.user_data = self.user_data;
 
-        if self.additional_mass_properties != MassProperties::default() {
+        if self.additional_mass_properties
+            != RigidBodyAdditionalMassProps::MassProps(MassProperties::zero())
+            && self.additional_mass_properties != RigidBodyAdditionalMassProps::Mass(0.0)
+        {
             rb.mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties));
-            rb.mprops.local_mprops = self.additional_mass_properties;
         }
 
         rb.mprops.flags = self.mprops_flags;
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index 3d35d17..cabf6ca 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -110,6 +110,8 @@ bitflags::bitflags! {
         const TYPE        = 1 << 4;
         /// Flag indicating that the `RigidBodyDominance` component of this rigid-body has been modified.
         const DOMINANCE   = 1 << 5;
+        /// Flag indicating that the local mass-properties of this rigid-body must be recomputed.
+        const LOCAL_MASS_PROPERTIES = 1 << 6;
     }
 }
 
@@ -222,7 +224,23 @@ bitflags::bitflags! {
     }
 }
 
-// TODO: split this into "LocalMassProps" and `WorldMassProps"?
+/// Mass and angular inertia added to a rigid-body on top of its attached colliders’ contributions.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum RigidBodyAdditionalMassProps {
+    /// Mass properties to be added as-is.
+    MassProps(MassProperties),
+    /// Mass to be added to the rigid-body. This will also automatically scale
+    /// the attached colliders total angular inertia to account for the added mass.
+    Mass(Real),
+}
+
+impl Default for RigidBodyAdditionalMassProps {
+    fn default() -> Self {
+        RigidBodyAdditionalMassProps::MassProps(MassProperties::default())
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Debug, PartialEq)]
 /// The mass properties of this rigid-bodies.
@@ -232,7 +250,7 @@ pub struct RigidBodyMassProps {
     /// The local mass properties of the rigid-body.
     pub local_mprops: MassProperties,
     /// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders.
-    pub additional_local_mprops: Option<Box<MassProperties>>,
+    pub additional_local_mprops: Option<Box<RigidBodyAdditionalMassProps>>,
     /// The world-space center of mass of the rigid-body.
     pub world_com: Point<Real>,
     /// The inverse mass taking into account translation locking.
@@ -294,18 +312,20 @@ impl RigidBodyMassProps {
         self.effective_world_inv_inertia_sqrt.squared().inverse()
     }
 
-    /// Recompute the mass-properties of this rigid-bodies based on its currentely attached colliders.
+    /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.
     pub fn recompute_mass_properties_from_colliders(
         &mut self,
         colliders: &ColliderSet,
         attached_colliders: &RigidBodyColliders,
         position: &Isometry<Real>,
     ) {
-        self.local_mprops = self
+        let added_mprops = self
             .additional_local_mprops
             .as_ref()
             .map(|mprops| **mprops)
-            .unwrap_or_else(MassProperties::default);
+            .unwrap_or_else(|| RigidBodyAdditionalMassProps::MassProps(MassProperties::default()));
+
+        self.local_mprops = MassProperties::default();
 
         for handle in &attached_colliders.0 {
             if let Some(co) = colliders.get(*handle) {
@@ -319,6 +339,16 @@ impl RigidBodyMassProps {
             }
         }
 
+        match added_mprops {
+            RigidBodyAdditionalMassProps::MassProps(mprops) => {
+                self.local_mprops += mprops;
+            }
+            RigidBodyAdditionalMassProps::Mass(mass) => {
+                let new_mass = self.local_mprops.mass() + mass;
+                self.local_mprops.set_mass(new_mass, true);
+            }
+        }
+
         self.update_world_mass_properties(position);
     }