Fix mass-properties update after collider change

8 files changed, 118 insertions, 34 deletions

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index acf6dfb..b3277e9 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -76,6 +76,12 @@ bitflags::bitflags! {
     }
 }
 
+impl Default for JointAxesMask {
+    fn default() -> Self {
+        Self::empty()
+    }
+}
+
 /// Identifiers of degrees of freedoms of a joint.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -230,7 +236,8 @@ impl GenericJoint {
         self.limit_axes.is_empty() && self.motor_axes.is_empty()
     }
 
-    fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
+    #[doc(hidden)]
+    pub fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
         let basis = axis.orthonormal_basis();
 
         #[cfg(feature = "dim2")]
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 44a6bc3..d4746ba 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -383,7 +383,7 @@ impl RigidBody {
     ///
     /// Returns zero if this rigid body has an infinite mass.
     pub fn mass(&self) -> Real {
-        utils::inv(self.rb_mprops.local_mprops.inv_mass)
+        self.rb_mprops.local_mprops.mass()
     }
 
     /// The predicted position of this rigid-body.
@@ -889,7 +889,7 @@ pub struct RigidBodyBuilder {
     rb_type: RigidBodyType,
     mprops_flags: LockedAxes,
     /// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
-    pub mass_properties: MassProperties,
+    pub additional_mass_properties: MassProperties,
     /// 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.
@@ -916,7 +916,7 @@ impl RigidBodyBuilder {
             angular_damping: 0.0,
             rb_type,
             mprops_flags: LockedAxes::empty(),
-            mass_properties: MassProperties::zero(),
+            additional_mass_properties: MassProperties::zero(),
             can_sleep: true,
             sleeping: false,
             ccd_enabled: false,
@@ -1008,7 +1008,7 @@ impl RigidBodyBuilder {
     /// 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.mass_properties = props;
+        self.additional_mass_properties = props;
         self
     }
 
@@ -1072,7 +1072,7 @@ impl RigidBodyBuilder {
     /// 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.mass_properties.set_mass(mass, false);
+        self.additional_mass_properties.set_mass(mass, false);
         self
     }
 
@@ -1093,7 +1093,7 @@ impl RigidBodyBuilder {
     /// 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.mass_properties.inv_principal_inertia_sqrt =
+        self.additional_mass_properties.inv_principal_inertia_sqrt =
             utils::inv(ComplexField::sqrt(inertia.max(0.0)));
         self
     }
@@ -1119,7 +1119,7 @@ impl RigidBodyBuilder {
     /// 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.mass_properties.inv_principal_inertia_sqrt =
+        self.additional_mass_properties.inv_principal_inertia_sqrt =
             inertia.map(|e| utils::inv(ComplexField::sqrt(e.max(0.0))));
         self
     }
@@ -1197,7 +1197,12 @@ impl RigidBodyBuilder {
         rb.rb_vels.angvel = self.angvel;
         rb.rb_type = self.rb_type;
         rb.user_data = self.user_data;
-        rb.rb_mprops.local_mprops = self.mass_properties;
+
+        if self.additional_mass_properties != MassProperties::default() {
+            rb.rb_mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties));
+            rb.rb_mprops.local_mprops = self.additional_mass_properties;
+        }
+
         rb.rb_mprops.flags = self.mprops_flags;
         rb.rb_damping.linear_damping = self.linear_damping;
         rb.rb_damping.angular_damping = self.angular_damping;
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index cac0600..b8f88ef 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -1,4 +1,4 @@
-use crate::data::{ComponentSetMut, ComponentSetOption};
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::MassProperties;
 use crate::geometry::{
     ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
@@ -225,13 +225,14 @@ bitflags::bitflags! {
 
 // TODO: split this into "LocalMassProps" and `WorldMassProps"?
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Clone, Debug, Copy, PartialEq)]
+#[derive(Clone, Debug, PartialEq)]
 /// The mass properties of this rigid-bodies.
 pub struct RigidBodyMassProps {
     /// Flags for locking rotation and translation.
     pub flags: LockedAxes,
     /// The local mass properties of the rigid-body.
     pub local_mprops: MassProperties,
+    pub additional_local_mprops: Option<Box<MassProperties>>,
     /// The world-space center of mass of the rigid-body.
     pub world_com: Point<Real>,
     /// The inverse mass taking into account translation locking.
@@ -246,6 +247,7 @@ impl Default for RigidBodyMassProps {
         Self {
             flags: LockedAxes::empty(),
             local_mprops: MassProperties::zero(),
+            additional_local_mprops: None,
             world_com: Point::origin(),
             effective_inv_mass: Vector::zero(),
             effective_world_inv_inertia_sqrt: AngularInertia::zero(),
@@ -292,6 +294,38 @@ impl RigidBodyMassProps {
         self.effective_world_inv_inertia_sqrt.squared().inverse()
     }
 
+    /// Recompute the mass-properties of this rigid-bodies based on its currentely attached colliders.
+    pub fn recompute_mass_properties_from_colliders<Colliders>(
+        &mut self,
+        colliders: &Colliders,
+        attached_colliders: &RigidBodyColliders,
+        position: &Isometry<Real>,
+    ) where
+        Colliders: ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderMassProps>
+            + ComponentSet<ColliderShape>,
+    {
+        self.local_mprops = self
+            .additional_local_mprops
+            .as_ref()
+            .map(|mprops| **mprops)
+            .unwrap_or_else(MassProperties::default);
+
+        for handle in &attached_colliders.0 {
+            let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+            if let Some(co_parent) = co_parent {
+                let (co_mprops, co_shape): (&ColliderMassProps, &ColliderShape) =
+                    colliders.index_bundle(handle.0);
+                let to_add = co_mprops
+                    .mass_properties(&**co_shape)
+                    .transform_by(&co_parent.pos_wrt_parent);
+                self.local_mprops += to_add;
+            }
+        }
+
+        self.update_world_mass_properties(position);
+    }
+
     /// Update the world-space mass properties of `self`, taking into account the new position.
     pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
         self.world_com = self.local_mprops.world_com(&position);
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index d806251..2bcbacc 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -431,14 +431,14 @@ impl InteractionGroups {
                     let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
                     (*data.0, data.1.active_set_offset)
                 } else {
-                    (RigidBodyType::Fixed, 0)
+                    (RigidBodyType::Fixed, usize::MAX)
                 };
                 let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
                 {
                     let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
                     (*data.0, data.1.active_set_offset)
                 } else {
-                    (RigidBodyType::Fixed, 0)
+                    (RigidBodyType::Fixed, usize::MAX)
                 };
 
                 let is_fixed1 = !status1.is_dynamic();
@@ -451,7 +451,9 @@ impl InteractionGroups {
 
                 let i1 = active_set_offset1;
                 let i2 = active_set_offset2;
-                let conflicts = self.body_masks[i1] | self.body_masks[i2];
+                let mask1 = if !is_fixed1 { self.body_masks[i1] } else { 0 };
+                let mask2 = if !is_fixed2 { self.body_masks[i2] } else { 0 };
+                let conflicts = mask1 | mask2;
                 let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
                 let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
 
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index eaca5a6..3113e5e 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -411,21 +411,18 @@ impl ColliderBuilder {
 
     /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
     pub fn capsule_x(half_height: Real, radius: Real) -> Self {
-        let p = Point::from(Vector::x() * half_height);
-        Self::new(SharedShape::capsule(-p, p, radius))
+        Self::new(SharedShape::capsule_x(half_height, radius))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
     pub fn capsule_y(half_height: Real, radius: Real) -> Self {
-        let p = Point::from(Vector::y() * half_height);
-        Self::new(SharedShape::capsule(-p, p, radius))
+        Self::new(SharedShape::capsule_y(half_height, radius))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
     #[cfg(feature = "dim3")]
     pub fn capsule_z(half_height: Real, radius: Real) -> Self {
-        let p = Point::from(Vector::z() * half_height);
-        Self::new(SharedShape::capsule(-p, p, radius))
+        Self::new(SharedShape::capsule_z(half_height, radius))
     }
 
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 2d77646..009dfa9 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -3,12 +3,12 @@
 use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
     RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
-    RigidBodyIds, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
-    ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
-    ColliderShape, ColliderType, NarrowPhase,
+    ColliderHandle, ColliderMassProps, ColliderMaterial, ColliderPair, ColliderParent,
+    ColliderPosition, ColliderShape, ColliderType, NarrowPhase,
 };
 use crate::math::Real;
 use crate::pipeline::{EventHandler, PhysicsHooks};
@@ -169,6 +169,7 @@ impl CollisionPipeline {
             + ComponentSetMut<RigidBodyIds>
             + ComponentSetMut<RigidBodyActivation>
             + ComponentSetMut<RigidBodyChanges>
+            + ComponentSetMut<RigidBodyMassProps>
             + ComponentSet<RigidBodyColliders>
             + ComponentSet<RigidBodyDominance>
             + ComponentSet<RigidBodyType>,
@@ -179,7 +180,8 @@ impl CollisionPipeline {
             + ComponentSetOption<ColliderParent>
             + ComponentSet<ColliderType>
             + ComponentSet<ColliderMaterial>
-            + ComponentSet<ColliderFlags>,
+            + ComponentSet<ColliderFlags>
+            + ComponentSet<ColliderMassProps>,
     {
         super::user_changes::handle_user_changes_to_colliders(
             bodies,
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 6156854..2d90c4b 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -15,7 +15,7 @@ use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
     ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
-    ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase,
+    ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase, ColliderMassProps
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{EventHandler, PhysicsHooks};
@@ -504,7 +504,8 @@ impl PhysicsPipeline {
             + ComponentSetOption<ColliderParent>
             + ComponentSet<ColliderType>
             + ComponentSet<ColliderMaterial>
-            + ComponentSet<ColliderFlags>,
+            + ComponentSet<ColliderFlags>
+            + ComponentSet<ColliderMassProps>,
     {
         self.counters.reset();
         self.counters.step_started();
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index bd69bb2..2c03f1c 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -1,39 +1,75 @@
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
     IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle,
-    RigidBodyIds, RigidBodyPosition, RigidBodyType,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
 };
-use crate::geometry::{ColliderChanges, ColliderHandle, ColliderParent, ColliderPosition};
+use crate::geometry::{
+    ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
+    ColliderShape,
+};
+use parry::utils::hashmap::HashMap;
 
-pub(crate) fn handle_user_changes_to_colliders<Colliders>(
-    bodies: &mut impl ComponentSet<RigidBodyPosition>,
+pub(crate) fn handle_user_changes_to_colliders<Bodies, Colliders>(
+    bodies: &mut Bodies,
     colliders: &mut Colliders,
     modified_colliders: &[ColliderHandle],
 ) where
+    Bodies: ComponentSet<RigidBodyPosition>
+        + ComponentSet<RigidBodyColliders>
+        + ComponentSetMut<RigidBodyMassProps>,
     Colliders: ComponentSetMut<ColliderChanges>
         + ComponentSetMut<ColliderPosition>
-        + ComponentSetOption<ColliderParent>,
+        + ComponentSetOption<ColliderParent>
+        + ComponentSet<ColliderShape>
+        + ComponentSet<ColliderMassProps>,
 {
+    // TODO: avoid this hashmap? We could perhaps add a new flag to RigidBodyChanges to
+    //       indicated that the mass properties need to be recomputed?
+    let mut mprops_to_update = HashMap::default();
+
     for handle in modified_colliders {
         // NOTE: we use `get` because the collider may no longer
         //       exist if it has been removed.
-        let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
+        let co_changes: Option<ColliderChanges> = colliders.get(handle.0).copied();
 
         if let Some(co_changes) = co_changes {
             if co_changes.contains(ColliderChanges::PARENT) {
                 let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
 
                 if let Some(co_parent) = co_parent {
-                    let parent_pos = bodies.index(co_parent.handle.0);
+                    let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
 
                     let new_pos = parent_pos.position * co_parent.pos_wrt_parent;
-                    let new_changes = *co_changes | ColliderChanges::POSITION;
+                    let new_changes = co_changes | ColliderChanges::POSITION;
                     colliders.set_internal(handle.0, ColliderPosition(new_pos));
                     colliders.set_internal(handle.0, new_changes);
                 }
             }
+
+            if co_changes.contains(ColliderChanges::SHAPE) {
+                let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+                if let Some(co_parent) = co_parent {
+                    mprops_to_update.insert(co_parent.handle, ());
+                }
+            }
         }
     }
+
+    for (to_update, _) in mprops_to_update {
+        let (rb_pos, rb_colliders): (&RigidBodyPosition, &RigidBodyColliders) =
+            bodies.index_bundle(to_update.0);
+        let position = rb_pos.position;
+        // FIXME: remove the clone once we remove the ComponentSets.
+        let attached_colliders = rb_colliders.clone();
+
+        bodies.map_mut_internal(to_update.0, |rb_mprops| {
+            rb_mprops.recompute_mass_properties_from_colliders(
+                colliders,
+                &attached_colliders,
+                &position,
+            )
+        });
+    }
 }
 
 pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(