Properly track some user-initiatied rigid-body modifications.

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")]

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;

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::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
-use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph, NarrowPhase};
+use crate::geometry::{ColliderHandle, ColliderSet, InteractionGraph, NarrowPhase};
-use crossbeam::channel::{Receiver, Sender};
+use std::ops::{Index, IndexMut};
-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();
-        }
-    }
-}
 
 /// 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() {
+        let rb = &mut self.bodies[handle];
-            rb.active_set_id = self.active_dynamic_set.len();
-            self.active_dynamic_set.push(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)> {
+    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
-        let sender = &self.activation_channel.0;
+        let result = self.bodies.get_unknown_gen_mut(i)?;
-        self.bodies
+        if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
-            .get_unknown_gen_mut(i)
+            result.0.changes = RigidBodyChanges::MODIFIED;
-            .map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
+            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> {
+    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
-        let sender = &self.activation_channel.0;
+        let result = self.bodies.get_mut(handle)?;
-        self.bodies
+        if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
-            .get_mut(handle)
+            result.changes = RigidBodyChanges::MODIFIED;
-            .map(|rb| RigidBodyMut::new(handle, rb, sender))
+            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)> {
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
-        let sender = &self.activation_channel.0;
+        self.modified_bodies.clear();
-        self.bodies
+        self.modified_all_bodies = true;
-            .iter_mut()
+        self.bodies.iter_mut()
-            .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
     }
 
     /// 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) {
+    // Utility function to avoid some borrowing issue in the `maintain` method.
-        for handle in self.activation_channel.1.try_iter() {
+    fn maintain_one(
-            if let Some(rb) = self.bodies.get_mut(handle) {
+        colliders: &mut ColliderSet,
-                // Push the body to the active set if it is not
+        handle: RigidBodyHandle,
-                // sleeping and if it is not already inside of the active set.
+        rb: &mut RigidBody,
-                if !rb.is_sleeping() // May happen if the body was put to sleep manually.
+        modified_inactive_set: &mut Vec<RigidBodyHandle>,
-                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+        active_kinematic_set: &mut Vec<RigidBodyHandle>,
-                    && self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
+        active_dynamic_set: &mut Vec<RigidBodyHandle>,
-                {
+    ) {
-                    rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+        // Update the positions of the colliders.
-                    self.active_dynamic_set.push(handle);
+        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,
+                    )
                 }
             }
         }

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]);
                     }
                 }
             })

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])
         });
     }
 }

src/geometry/broad_phase_multi_sap.rs

@@ -634,6 +634,7 @@ impl BroadPhase {
             .active_dynamic_set
             .iter()
             .chain(bodies.active_kinematic_set.iter())
+            .chain(bodies.modified_inactive_set.iter())
         {
             for handle in &bodies[*body_handle].colliders {
                 let collider = &mut colliders[*handle];

src/geometry/collider.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
+    Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle,
-    InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
+    Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, RoundCylinder};

src/geometry/collider_set.rs

@@ -1,7 +1,7 @@
 use crate::data::arena::Arena;
 use crate::data::pubsub::PubSub;
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::{Collider, ColliderGraphIndex};
+use crate::geometry::Collider;
 use std::ops::{Index, IndexMut};
 
 /// The unique identifier of a collider added to a collider set.
@@ -70,8 +70,7 @@ impl ColliderSet {
         coll.predicted_position = parent.predicted_position * coll.delta;
         let handle = self.colliders.insert(coll);
         let coll = self.colliders.get(handle).unwrap();
-        parent.add_collider_internal(handle, &coll);
+        parent.add_collider(handle, &coll);
-        bodies.activate(parent_handle);
         handle
     }
 
@@ -147,13 +146,13 @@ impl ColliderSet {
         self.colliders.get_mut(handle)
     }
 
-    pub(crate) fn get2_mut_internal(
+    // pub(crate) fn get2_mut_internal(
-        &mut self,
+    //     &mut self,
-        h1: ColliderHandle,
+    //     h1: ColliderHandle,
-        h2: ColliderHandle,
+    //     h2: ColliderHandle,
-    ) -> (Option<&mut Collider>, Option<&mut Collider>) {
+    // ) -> (Option<&mut Collider>, Option<&mut Collider>) {
-        self.colliders.get2_mut(h1, h2)
+    //     self.colliders.get2_mut(h1, h2)
-    }
+    // }
 
     // pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, ColliderMut)> {
     //     //        let sender = &self.activation_channel_sender;

src/pipeline/collision_pipeline.rs

@@ -47,7 +47,7 @@ impl CollisionPipeline {
         proximity_pair_filter: Option<&dyn ProximityPairFilter>,
         events: &dyn EventHandler,
     ) {
-        bodies.maintain_active_set();
+        bodies.maintain(colliders);
         self.broadphase_collider_pairs.clear();
 
         broad_phase.update_aabbs(prediction_distance, bodies, colliders);

src/pipeline/physics_pipeline.rs

@@ -74,9 +74,9 @@ impl PhysicsPipeline {
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
+        bodies.maintain(colliders);
         broad_phase.maintain(colliders);
         narrow_phase.maintain(colliders, bodies);
-        bodies.maintain_active_set();
 
         // Update kinematic bodies velocities.
         // TODO: what is the best place for this? It should at least be
@@ -242,11 +242,7 @@ impl PhysicsPipeline {
                 rb.update_predicted_position(integration_parameters.dt());
             }
 
-            for handle in &rb.colliders {
+            rb.update_colliders_positions(colliders);
-                let collider = &mut colliders[*handle];
-                collider.position = rb.position * collider.delta;
-                collider.predicted_position = rb.predicted_position * collider.delta;
-            }
         });
 
         self.counters.stages.solver_time.pause();

src_testbed/physx_backend.rs

@@ -402,7 +402,7 @@ impl PhysxWorld {
 
     pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
         for (rapier_handle, physx_handle) in self.rapier2physx.iter() {
-            let mut rb = bodies.get_mut(*rapier_handle).unwrap();
+            let rb = bodies.get_mut(*rapier_handle).unwrap();
             let ra = self.scene.get_rigid_actor(*physx_handle).unwrap();
             let pos = ra.get_global_pose().into_na();
             let iso = na::convert_unchecked(pos);