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>

Author: ThierryBerger

src/data/mod.rs

@@ -2,8 +2,10 @@
 
 pub use self::arena::{Arena, Index};
 pub use self::coarena::Coarena;
+pub(crate) use self::modified_objects::{HasModifiedFlag, ModifiedObjects};
 
 pub mod arena;
 mod coarena;
 pub(crate) mod graph;
+mod modified_objects;
 pub mod pubsub;

src/data/modified_objects.rs

@@ -0,0 +1,65 @@
+use std::marker::PhantomData;
+use std::ops::Deref;
+
+/// Contains handles of modified objects.
+///
+/// This is a wrapper over a `Vec` to ensure we don’t forget to set the object’s
+/// MODIFIED flag when adding it to this set.
+/// It is possible to bypass the wrapper with `.as_mut_internal`. But this should only
+/// be done for internal engine usage (like the physics pipeline).
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug)]
+pub(crate) struct ModifiedObjects<Handle, Object>(Vec<Handle>, PhantomData<Object>);
+
+impl<Handle, Object> Default for ModifiedObjects<Handle, Object> {
+    fn default() -> Self {
+        Self(Vec::new(), PhantomData)
+    }
+}
+
+pub(crate) trait HasModifiedFlag {
+    fn has_modified_flag(&self) -> bool;
+    fn set_modified_flag(&mut self);
+}
+
+impl<Handle, Object> Deref for ModifiedObjects<Handle, Object> {
+    type Target = Vec<Handle>;
+    fn deref(&self) -> &Self::Target {
+        &self.0
+    }
+}
+
+impl<Handle, Object: HasModifiedFlag> ModifiedObjects<Handle, Object> {
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self(Vec::with_capacity(capacity), PhantomData)
+    }
+
+    /// Remove every handle from this set.
+    ///
+    /// Note that the corresponding object MODIFIED flags won’t be reset automatically by this function.
+    pub fn clear(&mut self) {
+        self.0.clear()
+    }
+
+    /// Pushes a object handle to this set after checking that it doesn’t have the MODIFIED
+    /// flag set.
+    ///
+    /// This will also set the object’s MODIFIED flag.
+    pub fn push_once(&mut self, handle: Handle, object: &mut Object) {
+        if !object.has_modified_flag() {
+            self.push_unchecked(handle, object);
+        }
+    }
+
+    /// Pushes an object handle to this set without checking if the object already has the MODIFIED
+    /// flags.
+    ///
+    /// Only use in situation where you are certain (due to other contextual information) that
+    /// the object isn’t already in the set.
+    ///
+    /// This will also set the object’s MODIFIED flag.
+    pub fn push_unchecked(&mut self, handle: Handle, object: &mut Object) {
+        object.set_modified_flag();
+        self.0.push(handle);
+    }
+}

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,23 +1020,21 @@ 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 {
             // NOTE: the ColliderParent component must exist if we enter this method.
             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,27 +45,27 @@ 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 {
     /// Create a new empty set of rigid bodies.
     pub fn new() -> Self {
         RigidBodySet {
             bodies: Arena::new(),
-            modified_bodies: Vec::new(),
+            modified_bodies: ModifiedObjects::default(),
         }
     }
 
     /// Create a new set of rigid bodies, with an initial capacity.
     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
     }
 }