Merge pull request #69 from dimforge/rigid_body_modifications

Track some user-initiated rigid-body modifications

17 files changed, 447 insertions, 150 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 88f3f8f..82e72e3 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -13,9 +13,12 @@ use std::cmp::Ordering;
 mod collision_groups3;
 mod compound3;
 mod damping3;
+mod debug_add_remove_collider3;
 mod debug_boxes3;
 mod debug_cylinder3;
+mod debug_dynamic_collider_add3;
 mod debug_infinite_fall3;
+mod debug_rollback3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
@@ -81,11 +84,20 @@ pub fn main() {
         ("Sensor", sensor3::init_world),
         ("Trimesh", trimesh3::init_world),
         ("Keva tower", keva3::init_world),
+        (
+            "(Debug) add/rm collider",
+            debug_add_remove_collider3::init_world,
+        ),
         ("(Debug) boxes", debug_boxes3::init_world),
+        (
+            "(Debug) dyn. coll. add",
+            debug_dynamic_collider_add3::init_world,
+        ),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
         ("(Debug) cylinder", debug_cylinder3::init_world),
         ("(Debug) infinite fall", debug_infinite_fall3::init_world),
+        ("(Debug) rollback", debug_rollback3::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.
diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs
new file mode 100644
index 0000000..c8d72fc
--- /dev/null
+++ b/examples3d/debug_add_remove_collider3.rs
@@ -0,0 +1,59 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    /*
+     * Ground.
+     */
+    let ground_size = 3.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4).build();
+    let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Rolling ball
+     */
+    let ball_rad = 0.1;
+    let rb = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 0.2, 0.0)
+        .build();
+    let ball_handle = bodies.insert(rb);
+    let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
+    colliders.insert(collider, ball_handle, &mut bodies);
+
+    testbed.add_callback(move |window, physics, _, graphics, _| {
+        // Remove then re-add the ground collider.
+        let coll = physics
+            .colliders
+            .remove(ground_collider_handle, &mut physics.bodies, true)
+            .unwrap();
+        ground_collider_handle = physics
+            .colliders
+            .insert(coll, ground_handle, &mut physics.bodies);
+        graphics.add_collider(window, ground_collider_handle, &physics.colliders);
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs
new file mode 100644
index 0000000..4ea6836
--- /dev/null
+++ b/examples3d/debug_dynamic_collider_add3.rs
@@ -0,0 +1,112 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    /*
+     * Ground.
+     */
+    let ground_size = 20.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
+        .friction(0.15)
+        // .restitution(0.5)
+        .build();
+    let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Rolling ball
+     */
+    let ball_rad = 0.1;
+    let rb = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 0.2, 0.0)
+        .linvel(10.0, 0.0, 0.0)
+        .build();
+    let ball_handle = bodies.insert(rb);
+    let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
+    colliders.insert(collider, ball_handle, &mut bodies);
+
+    let mut linvel = Vector3::zeros();
+    let mut angvel = Vector3::zeros();
+    let mut pos = Isometry3::identity();
+    let mut step = 0;
+    let mut extra_colliders = Vec::new();
+    let snapped_frame = 51;
+
+    testbed.add_callback(move |window, physics, _, graphics, _| {
+        step += 1;
+
+        // Add a bigger ball collider
+        let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32))
+            .density(100.0)
+            .build();
+        let new_ball_collider_handle =
+            physics
+                .colliders
+                .insert(collider, ball_handle, &mut physics.bodies);
+        graphics.add_collider(window, new_ball_collider_handle, &physics.colliders);
+        extra_colliders.push(new_ball_collider_handle);
+
+        // Snap the ball velocity or restore it.
+        let mut ball = physics.bodies.get_mut(ball_handle).unwrap();
+
+        if step == snapped_frame {
+            linvel = *ball.linvel();
+            angvel = *ball.angvel();
+            pos = *ball.position();
+        }
+
+        if step == 100 {
+            ball.set_linvel(linvel, true);
+            ball.set_angvel(angvel, true);
+            ball.set_position(pos, true);
+            step = snapped_frame;
+
+            for handle in &extra_colliders {
+                physics.colliders.remove(*handle, &mut physics.bodies, true);
+            }
+
+            extra_colliders.clear();
+        }
+
+        // Remove then re-add the ground collider.
+        // let ground = physics.bodies.get_mut(ground_handle).unwrap();
+        // ground.set_position(Isometry3::translation(0.0, step as f32 * 0.001, 0.0), false);
+        // let coll = physics
+        //     .colliders
+        //     .remove(ground_collider_handle, &mut physics.bodies, true)
+        //     .unwrap();
+        let coll = ColliderBuilder::cuboid(ground_size, ground_height + step as f32 * 0.01, 0.4)
+            .friction(0.15)
+            .build();
+        let new_ground_collider_handle =
+            physics
+                .colliders
+                .insert(coll, ground_handle, &mut physics.bodies);
+        graphics.add_collider(window, new_ground_collider_handle, &physics.colliders);
+        extra_colliders.push(new_ground_collider_handle);
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/debug_rollback3.rs b/examples3d/debug_rollback3.rs
new file mode 100644
index 0000000..6479e1f
--- /dev/null
+++ b/examples3d/debug_rollback3.rs
@@ -0,0 +1,77 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    /*
+     * Ground.
+     */
+    let ground_size = 20.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
+        .friction(0.15)
+        // .restitution(0.5)
+        .build();
+    let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Rolling ball
+     */
+    let ball_rad = 0.1;
+    let rb = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 0.2, 0.0)
+        .linvel(10.0, 0.0, 0.0)
+        .build();
+    let ball_handle = bodies.insert(rb);
+    let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
+    colliders.insert(collider, ball_handle, &mut bodies);
+
+    let mut linvel = Vector3::zeros();
+    let mut angvel = Vector3::zeros();
+    let mut pos = Isometry3::identity();
+    let mut step = 0;
+    let snapped_frame = 51;
+
+    testbed.add_callback(move |window, physics, _, graphics, _| {
+        step += 1;
+
+        // Snap the ball velocity or restore it.
+        let mut ball = physics.bodies.get_mut(ball_handle).unwrap();
+
+        if step == snapped_frame {
+            linvel = *ball.linvel();
+            angvel = *ball.angvel();
+            pos = *ball.position();
+        }
+
+        if step == 100 {
+            ball.set_linvel(linvel, true);
+            ball.set_angvel(angvel, true);
+            ball.set_position(pos, true);
+            step = snapped_frame;
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs
index 0843300..0975f39 100644
--- a/examples3d/platform3.rs
+++ b/examples3d/platform3.rs
@@ -71,7 +71,7 @@ pub fn init_world(testbed: &mut Testbed) {
             return;
         }
 
-        if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
+        if let Some(platform) = physics.bodies.get_mut(platform_handle) {
             let mut next_pos = *platform.position();
 
             let dt = 0.016;
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..0d340cc 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  = 1 << 0;
+        const POSITION  = 1 << 1;
+        const SLEEP     = 1 << 2;
+        const COLLIDERS = 1 << 3;
+    }
+}
+
 #[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,12 @@ 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) {
+        self.changes.set(
+            RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
+            true,
+        );
+
         let mass_properties = coll
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
@@ -160,9 +180,18 @@ 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) {
+            self.changes.set(RigidBodyChanges::COLLIDERS, true);
             self.colliders.swap_remove(i);
             let mass_properties = coll
                 .mass_properties()
@@ -189,7 +218,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 +333,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 +648,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])
         });
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 4cea113..863990d 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -586,9 +586,6 @@ impl BroadPhase {
 
         let proxy = &mut self.proxies[proxy_index];
 
-        // Push the proxy to infinity, but not beyond the sentinels.
-        proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
-        proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
         // Discretize the AABB to find the regions that need to be invalidated.
         let start = point_key(proxy.aabb.mins);
         let end = point_key(proxy.aabb.maxs);
@@ -615,6 +612,9 @@ impl BroadPhase {
             }
         }
 
+        // Push the proxy to infinity, but not beyond the sentinels.
+        proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
+        proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
         self.proxies.remove(proxy_index);
     }
 
@@ -631,8 +631,9 @@ impl BroadPhase {
         self.complete_removals();
 
         for body_handle in bodies
-            .active_dynamic_set
+            .modified_inactive_set
             .iter()
+            .chain(bodies.active_dynamic_set.iter())
             .chain(bodies.active_kinematic_set.iter())
         {
             for handle in &bodies[*body_handle].colliders {
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 3789cca..c04be35 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
-    InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
+    Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle,
+    Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, RoundCylinder};
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_se