Fix user changes handling (#803)

* add failing test from @Johannes0021

* apply fix on update_positions

* apply fix on ColliderSet::iter_mut

* fix clippy..

* more complete test

* feat: refactor modified sets into a wrapper to avoid future mistakes

* chore: fix typos

---------

Co-authored-by: Sébastien Crozet <sebcrozet@dimforge.com>

src/dynamics/mod.rs

@@ -8,6 +8,7 @@ pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::joint::JointIndex;
 pub use self::joint::*;
 pub use self::rigid_body_components::*;
+pub(crate) use self::rigid_body_set::ModifiedRigidBodies;
 // #[cfg(not(feature = "parallel"))]
 pub(crate) use self::solver::IslandSolver;
 // #[cfg(feature = "parallel")]

src/dynamics/rigid_body_components.rs

@@ -4,7 +4,7 @@ use crate::control::PdErrors;
 use crate::dynamics::MassProperties;
 use crate::geometry::{
     ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
-    ColliderSet, ColliderShape,
+    ColliderSet, ColliderShape, ModifiedColliders,
 };
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
@@ -1020,10 +1020,10 @@ impl RigidBodyColliders {
     }
 
     /// Update the positions of all the colliders attached to this rigid-body.
-    pub fn update_positions(
+    pub(crate) fn update_positions(
         &self,
         colliders: &mut ColliderSet,
-        modified_colliders: &mut Vec<ColliderHandle>,
+        modified_colliders: &mut ModifiedColliders,
         parent_pos: &Isometry<Real>,
     ) {
         for handle in &self.0 {
@@ -1031,12 +1031,10 @@ impl RigidBodyColliders {
             let co = colliders.index_mut_internal(*handle);
             let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent;
 
-            if !co.changes.contains(ColliderChanges::MODIFIED) {
-                modified_colliders.push(*handle);
-            }
-
             // Set the modification flag so we can benefit from the modification-tracking
             // when updating the narrow-phase/broad-phase afterwards.
+            modified_colliders.push_once(*handle, co);
+
             co.changes |= ColliderChanges::POSITION;
             co.pos = ColliderPosition(new_pos);
         }

src/dynamics/rigid_body_set.rs

@@ -1,4 +1,4 @@
-use crate::data::Arena;
+use crate::data::{Arena, HasModifiedFlag, ModifiedObjects};
 use crate::dynamics::{
     ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle,
 };
@@ -22,6 +22,20 @@ impl BodyPair {
     }
 }
 
+pub(crate) type ModifiedRigidBodies = ModifiedObjects<RigidBodyHandle, RigidBody>;
+
+impl HasModifiedFlag for RigidBody {
+    #[inline]
+    fn has_modified_flag(&self) -> bool {
+        self.changes.contains(RigidBodyChanges::MODIFIED)
+    }
+
+    #[inline]
+    fn set_modified_flag(&mut self) {
+        self.changes |= RigidBodyChanges::MODIFIED;
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Default, Debug)]
 /// A set of rigid bodies that can be handled by a physics pipeline.
@@ -31,7 +45,7 @@ pub struct RigidBodySet {
     // parallelism because the `Receiver` breaks the Sync impl.
     // Could we avoid this?
     pub(crate) bodies: Arena<RigidBody>,
-    pub(crate) modified_bodies: Vec<RigidBodyHandle>,
+    pub(crate) modified_bodies: ModifiedRigidBodies,
 }
 
 impl RigidBodySet {
@@ -39,7 +53,7 @@ impl RigidBodySet {
     pub fn new() -> Self {
         RigidBodySet {
             bodies: Arena::new(),
-            modified_bodies: Vec::new(),
+            modified_bodies: ModifiedObjects::default(),
         }
     }
 
@@ -47,11 +61,11 @@ impl RigidBodySet {
     pub fn with_capacity(capacity: usize) -> Self {
         RigidBodySet {
             bodies: Arena::with_capacity(capacity),
-            modified_bodies: Vec::with_capacity(capacity),
+            modified_bodies: ModifiedRigidBodies::with_capacity(capacity),
         }
     }
 
-    pub(crate) fn take_modified(&mut self) -> Vec<RigidBodyHandle> {
+    pub(crate) fn take_modified(&mut self) -> ModifiedRigidBodies {
         std::mem::take(&mut self.modified_bodies)
     }
 
@@ -79,7 +93,10 @@ impl RigidBodySet {
         rb.changes.set(RigidBodyChanges::all(), true);
 
         let handle = RigidBodyHandle(self.bodies.insert(rb));
-        self.modified_bodies.push(handle);
+        // Using push_unchecked because this is a brand new rigid-body with the MODIFIED
+        // flags set but isn’t in the modified_bodies yet.
+        self.modified_bodies
+            .push_unchecked(handle, &mut self.bodies[handle.0]);
         handle
     }
 
@@ -152,7 +169,7 @@ impl RigidBodySet {
     pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut RigidBody, RigidBodyHandle)> {
         let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
         let handle = RigidBodyHandle(handle);
-        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
+        self.modified_bodies.push_once(handle, rb);
         Some((rb, handle))
     }
 
@@ -161,22 +178,11 @@ impl RigidBodySet {
         self.bodies.get(handle.0)
     }
 
-    pub(crate) fn mark_as_modified(
-        handle: RigidBodyHandle,
-        rb: &mut RigidBody,
-        modified_bodies: &mut Vec<RigidBodyHandle>,
-    ) {
-        if !rb.changes.contains(RigidBodyChanges::MODIFIED) {
-            rb.changes = RigidBodyChanges::MODIFIED;
-            modified_bodies.push(handle);
-        }
-    }
-
     /// Gets a mutable reference to the rigid-body with the given handle.
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
-        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
+        self.modified_bodies.push_once(handle, result);
         Some(result)
     }
 
@@ -195,7 +201,7 @@ impl RigidBodySet {
         handle: RigidBodyHandle,
     ) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
-        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
+        self.modified_bodies.push_once(handle, result);
         Some(result)
     }
 
@@ -210,7 +216,9 @@ impl RigidBodySet {
         self.modified_bodies.clear();
         let modified_bodies = &mut self.modified_bodies;
         self.bodies.iter_mut().map(move |(h, b)| {
-            modified_bodies.push(RigidBodyHandle(h));
+            // NOTE: using `push_unchecked` because we just cleared `modified_bodies`
+            //       before iterating.
+            modified_bodies.push_unchecked(RigidBodyHandle(h), b);
             (RigidBodyHandle(h), b)
         })
     }
@@ -256,7 +264,7 @@ impl Index<crate::data::Index> for RigidBodySet {
 impl IndexMut<RigidBodyHandle> for RigidBodySet {
     fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
         let rb = &mut self.bodies[handle.0];
-        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
+        self.modified_bodies.push_once(handle, rb);
         rb
     }
 }