Properly track some user-initiatied rigid-body modifications.

5 files changed, 143 insertions, 121 deletions

diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 10cdd29..6967904 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -9,9 +9,10 @@ pub use self::joint::{
 };
 pub use self::mass_properties::MassProperties;
 pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
-pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet};
+pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
 // #[cfg(not(feature = "parallel"))]
 pub(crate) use self::joint::JointGraphEdge;
+pub(crate) use self::rigid_body::RigidBodyChanges;
 #[cfg(not(feature = "parallel"))]
 pub(crate) use self::solver::IslandSolver;
 #[cfg(feature = "parallel")]
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 4783cfc..537e073 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,5 +1,7 @@
 use crate::dynamics::MassProperties;
-use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::geometry::{
+    Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
+};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
 use crate::utils::{WCross, WDot};
 use num::Zero;
@@ -23,6 +25,17 @@ pub enum BodyStatus {
     // Disabled,
 }
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub(crate) struct RigidBodyChanges: u32 {
+        const MODIFIED = 0b001;
+        const POSITION = 0b010;
+        const SLEEP = 0b100;
+        const COLLIDERS = 0b1000;
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A rigid body.
 ///
@@ -56,6 +69,7 @@ pub struct RigidBody {
     pub(crate) active_set_id: usize,
     pub(crate) active_set_offset: usize,
     pub(crate) active_set_timestamp: u32,
+    pub(crate) changes: RigidBodyChanges,
     /// The status of the body, governing how it is affected by external forces.
     pub body_status: BodyStatus,
     /// User-defined data associated to this rigid-body.
@@ -83,6 +97,7 @@ impl RigidBody {
             active_set_id: 0,
             active_set_offset: 0,
             active_set_timestamp: 0,
+            changes: RigidBodyChanges::all(),
             body_status: BodyStatus::Dynamic,
             user_data: 0,
         }
@@ -151,7 +166,14 @@ impl RigidBody {
     }
 
     /// Adds a collider to this rigid-body.
-    pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
+    pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
+        if !self.changes.contains(RigidBodyChanges::MODIFIED) {
+            self.changes.set(
+                RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
+                true,
+            );
+        }
+
         let mass_properties = coll
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
@@ -160,6 +182,14 @@ impl RigidBody {
         self.update_world_mass_properties();
     }
 
+    pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
+        for handle in &self.colliders {
+            let collider = &mut colliders[*handle];
+            collider.position = self.position * collider.delta;
+            collider.predicted_position = self.predicted_position * collider.delta;
+        }
+    }
+
     /// Removes a collider from this rigid-body.
     pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
         if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
@@ -189,7 +219,10 @@ impl RigidBody {
     /// If `strong` is `true` then it is assured that the rigid-body will
     /// remain awake during multiple subsequent timesteps.
     pub fn wake_up(&mut self, strong: bool) {
-        self.activation.sleeping = false;
+        if self.activation.sleeping {
+            self.changes.insert(RigidBodyChanges::SLEEP);
+            self.activation.sleeping = false;
+        }
 
         if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
             self.activation.energy = self.activation.threshold.abs() * 2.0;
@@ -301,14 +334,21 @@ impl RigidBody {
     /// 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.
     pub fn set_position(&mut self, pos: Isometry<f32>, wake_up: bool) {
+        self.changes.insert(RigidBodyChanges::POSITION);
+        self.set_position_internal(pos);
+
+        // TODO: Do we really need to check that the body isn't dynamic?
+        if wake_up && self.is_dynamic() {
+            self.wake_up(true)
+        }
+    }
+
+    pub(crate) fn set_position_internal(&mut self, pos: Isometry<f32>) {
         self.position = pos;
 
         // TODO: update the predicted position for dynamic bodies too?
         if self.is_static() || self.is_kinematic() {
             self.predicted_position = pos;
-        } else if wake_up {
-            // wake_up is true and the rigid-body is dynamic.
-            self.wake_up(true);
         }
     }
 
@@ -609,7 +649,7 @@ impl RigidBodyBuilder {
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
         rb.predicted_position = self.position; // FIXME: compute the correct value?
-        rb.set_position(self.position, false);
+        rb.set_position_internal(self.position);
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 48d558f..b8667bc 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -2,54 +2,9 @@
 use rayon::prelude::*;
 
 use crate::data::arena::Arena;
-use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
-use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph, NarrowPhase};
-use crossbeam::channel::{Receiver, Sender};
-use std::ops::{Deref, DerefMut, Index, IndexMut};
-
-/// A mutable reference to a rigid-body.
-pub struct RigidBodyMut<'a> {
-    rb: &'a mut RigidBody,
-    was_sleeping: bool,
-    handle: RigidBodyHandle,
-    sender: &'a Sender<RigidBodyHandle>,
-}
-
-impl<'a> RigidBodyMut<'a> {
-    fn new(
-        handle: RigidBodyHandle,
-        rb: &'a mut RigidBody,
-        sender: &'a Sender<RigidBodyHandle>,
-    ) -> Self {
-        Self {
-            was_sleeping: rb.is_sleeping(),
-            handle,
-            sender,
-            rb,
-        }
-    }
-}
-
-impl<'a> Deref for RigidBodyMut<'a> {
-    type Target = RigidBody;
-    fn deref(&self) -> &RigidBody {
-        &*self.rb
-    }
-}
-
-impl<'a> DerefMut for RigidBodyMut<'a> {
-    fn deref_mut(&mut self) -> &mut RigidBody {
-        self.rb
-    }
-}
-
-impl<'a> Drop for RigidBodyMut<'a> {
-    fn drop(&mut self) {
-        if self.was_sleeping && !self.rb.is_sleeping() {
-            self.sender.send(self.handle).unwrap();
-        }
-    }
-}
+use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
+use crate::geometry::{ColliderHandle, ColliderSet, InteractionGraph, NarrowPhase};
+use std::ops::{Index, IndexMut};
 
 /// The unique handle of a rigid body added to a `RigidBodySet`.
 pub type RigidBodyHandle = crate::data::arena::Index;
@@ -90,15 +45,12 @@ pub struct RigidBodySet {
     pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
     pub(crate) active_islands: Vec<usize>,
     active_set_timestamp: u32,
+    pub(crate) modified_bodies: Vec<RigidBodyHandle>,
+    pub(crate) modified_all_bodies: bool,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     can_sleep: Vec<RigidBodyHandle>, // Workspace.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     stack: Vec<RigidBodyHandle>, // Workspace.
-    #[cfg_attr(
-        feature = "serde-serialize",
-        serde(skip, default = "crossbeam::channel::unbounded")
-    )]
-    activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
 }
 
 impl RigidBodySet {
@@ -111,9 +63,10 @@ impl RigidBodySet {
             modified_inactive_set: Vec::new(),
             active_islands: Vec::new(),
             active_set_timestamp: 0,
+            modified_bodies: Vec::new(),
+            modified_all_bodies: false,
             can_sleep: Vec::new(),
             stack: Vec::new(),
-            activation_channel: crossbeam::channel::unbounded(),
         }
     }
 
@@ -127,28 +80,6 @@ impl RigidBodySet {
         self.bodies.len()
     }
 
-    pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
-        let mut rb = &mut self.bodies[handle];
-        match rb.body_status {
-            // XXX: this case should only concern the dynamic bodies.
-            // For static bodies we should use the modified_inactive_set, or something
-            // similar. Right now we do this for static bodies as well so the broad-phase
-            // takes them into account the first time they are inserted.
-            BodyStatus::Dynamic | BodyStatus::Static => {
-                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
-                    rb.active_set_id = self.active_dynamic_set.len();
-                    self.active_dynamic_set.push(handle);
-                }
-            }
-            BodyStatus::Kinematic => {
-                if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
-                    rb.active_set_id = self.active_kinematic_set.len();
-                    self.active_kinematic_set.push(handle);
-                }
-            }
-        }
-    }
-
     /// Is the given body handle valid?
     pub fn contains(&self, handle: RigidBodyHandle) -> bool {
         self.bodies.contains(handle)
@@ -159,24 +90,18 @@ impl RigidBodySet {
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         rb.reset_internal_references();
+        rb.changes.set(RigidBodyChanges::all(), true);
 
         let handle = self.bodies.insert(rb);
-        let rb = &mut self.bodies[handle];
+        self.modified_bodies.push(handle);
 
-        if !rb.is_sleeping() && rb.is_dynamic() {
-            rb.active_set_id = self.active_dynamic_set.len();
-            self.active_dynamic_set.push(handle);
-        }
+        let rb = &mut self.bodies[handle];
 
         if rb.is_kinematic() {
             rb.active_set_id = self.active_kinematic_set.len();
             self.active_kinematic_set.push(handle);
         }
 
-        if !rb.is_dynamic() {
-            self.modified_inactive_set.push(handle);
-        }
-
         handle
     }
 
@@ -262,11 +187,13 @@ impl RigidBodySet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
-    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
-        let sender = &self.activation_channel.0;
-        self.bodies
-            .get_unknown_gen_mut(i)
-            .map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
+    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
+        let result = self.bodies.get_unknown_gen_mut(i)?;
+        if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
+            result.0.changes = RigidBodyChanges::MODIFIED;
+            self.modified_bodies.push(result.1);
+        }
+        Some(result)
     }
 
     /// Gets the rigid-body with the given handle.
@@ -275,11 +202,13 @@ impl RigidBodySet {
     }
 
     /// Gets a mutable reference to the rigid-body with the given handle.
-    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
-        let sender = &self.activation_channel.0;
-        self.bodies
-            .get_mut(handle)
-            .map(|rb| RigidBodyMut::new(handle, rb, sender))
+    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
+        let result = self.bodies.get_mut(handle)?;
+        if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
+            result.changes = RigidBodyChanges::MODIFIED;
+            self.modified_bodies.push(handle);
+        }
+        Some(result)
     }
 
     pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
@@ -300,11 +229,10 @@ impl RigidBodySet {
     }
 
     /// Iterates mutably through all the rigid-bodies on this set.
-    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
-        let sender = &self.activation_channel.0;
-        self.bodies
-            .iter_mut()
-            .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
+        self.modified_bodies.clear();
+        self.modified_all_bodies = true;
+        self.bodies.iter_mut()
     }
 
     /// Iter through all the active kinematic rigid-bodies on this set.
@@ -433,17 +361,70 @@ impl RigidBodySet {
         &self.active_dynamic_set[self.active_island_range(island_id)]
     }
 
-    pub(crate) fn maintain_active_set(&mut self) {
-        for handle in self.activation_channel.1.try_iter() {
-            if let Some(rb) = self.bodies.get_mut(handle) {
-                // Push the body to the active set if it is not
-                // sleeping and if it is not already inside of the active set.
-                if !rb.is_sleeping() // May happen if the body was put to sleep manually.
-                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
-                    && self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
-                {
-                    rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
-                    self.active_dynamic_set.push(handle);
+    // Utility function to avoid some borrowing issue in the `maintain` method.
+    fn maintain_one(
+        colliders: &mut ColliderSet,
+        handle: RigidBodyHandle,
+        rb: &mut RigidBody,
+        modified_inactive_set: &mut Vec<RigidBodyHandle>,
+        active_kinematic_set: &mut Vec<RigidBodyHandle>,
+        active_dynamic_set: &mut Vec<RigidBodyHandle>,
+    ) {
+        // Update the positions of the colliders.
+        if rb.changes.contains(RigidBodyChanges::POSITION)
+            || rb.changes.contains(RigidBodyChanges::COLLIDERS)
+        {
+            rb.update_colliders_positions(colliders);
+
+            if rb.is_static() {
+                modified_inactive_set.push(handle);
+            }
+
+            if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
+                rb.active_set_id = active_kinematic_set.len();
+                active_kinematic_set.push(handle);
+            }
+        }
+
+        // Push the body to the active set if it is not
+        // sleeping and if it is not already inside of the active set.
+        if rb.changes.contains(RigidBodyChanges::SLEEP)
+            && !rb.is_sleeping() // May happen if the body was put to sleep manually.
+            && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+            && active_dynamic_set.get(rb.active_set_id) != Some(&handle)
+        {
+            rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+            active_dynamic_set.push(handle);
+        }
+
+        rb.changes = RigidBodyChanges::empty();
+    }
+
+    pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) {
+        if self.modified_all_bodies {
+            for (handle, rb) in self.bodies.iter_mut() {
+                Self::maintain_one(
+                    colliders,
+                    handle,
+                    rb,
+                    &mut self.modified_inactive_set,
+                    &mut self.active_kinematic_set,
+                    &mut self.active_dynamic_set,
+                )
+            }
+
+            self.modified_bodies.clear();
+        } else {
+            for handle in self.modified_bodies.drain(..) {
+                if let Some(rb) = self.bodies.get_mut(handle) {
+                    Self::maintain_one(
+                        colliders,
+                        handle,
+                        rb,
+                        &mut self.modified_inactive_set,
+                        &mut self.active_kinematic_set,
+                        &mut self.active_dynamic_set,
+                    )
                 }
             }
         }
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index dd5e535..3b7ab9f 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -250,7 +250,7 @@ impl ParallelIslandSolver {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.position_writeback_index] {
                         let rb = &mut bodies[*handle];
-                        rb.set_position(positions[rb.active_set_offset], false);
+                        rb.set_position_internal(positions[rb.active_set_offset]);
                     }
                 }
             })
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index dac1d9e..be70d74 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -120,7 +120,7 @@ impl PositionSolver {
         }
 
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_position(self.positions[rb.active_set_offset], false)
+            rb.set_position_internal(self.positions[rb.active_set_offset])
         });
     }
 }