Split rigid-bodies and colliders into multiple components

1 files changed, 93 insertions, 565 deletions

diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 5a85ada..10e7bf8 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -1,48 +1,19 @@
 #[cfg(feature = "parallel")]
 use rayon::prelude::*;
 
-use crate::data::arena::Arena;
-use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges};
-use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
+use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{
+    IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
+    RigidBodyType,
+};
+use crate::dynamics::{
+    JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+};
+use crate::geometry::ColliderSet;
 use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
 
-/// The unique handle of a rigid body added to a `RigidBodySet`.
-#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[repr(transparent)]
-pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
-
-impl RigidBodyHandle {
-    /// Converts this handle into its (index, generation) components.
-    pub fn into_raw_parts(self) -> (usize, u64) {
-        self.0.into_raw_parts()
-    }
-
-    /// Reconstructs an handle from its (index, generation) components.
-    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(id, generation))
-    }
-
-    /// An always-invalid rigid-body handle.
-    pub fn invalid() -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_USIZE,
-            crate::INVALID_U64,
-        ))
-    }
-}
-
-impl IndexedData for RigidBodyHandle {
-    fn default() -> Self {
-        Self(IndexedData::default())
-    }
-
-    fn index(&self) -> usize {
-        self.0.index()
-    }
-}
-
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A pair of rigid body handles.
@@ -69,38 +40,75 @@ pub struct RigidBodySet {
     // parallelism because the `Receiver` breaks the Sync impl.
     // Could we avoid this?
     pub(crate) bodies: Arena<RigidBody>,
-    pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
-    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
-    // Set of inactive bodies which have been modified.
-    // This typically include static bodies which have been modified.
-    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.
 }
 
+macro_rules! impl_field_component_set(
+    ($T: ty, $field: ident) => {
+        impl ComponentSetOption<$T> for RigidBodySet {
+            fn get(&self, handle: crate::data::Index) -> Option<&$T> {
+                self.get(RigidBodyHandle(handle)).map(|b| &b.$field)
+            }
+        }
+
+        impl ComponentSet<$T> for RigidBodySet {
+            fn size_hint(&self) -> usize {
+                self.len()
+            }
+
+            #[inline(always)]
+            fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
+                for (handle, body) in self.bodies.iter() {
+                    f(handle, &body.$field)
+                }
+            }
+        }
+
+        impl ComponentSetMut<$T> for RigidBodySet {
+            fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
+                if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) {
+                    rb.$field = val;
+                }
+            }
+
+            #[inline(always)]
+            fn map_mut_internal<Result>(
+                &mut self,
+                handle: crate::data::Index,
+                f: impl FnOnce(&mut $T) -> Result,
+            ) -> Option<Result> {
+                self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field))
+            }
+        }
+    }
+);
+
+impl_field_component_set!(RigidBodyPosition, rb_pos);
+impl_field_component_set!(RigidBodyMassProps, rb_mprops);
+impl_field_component_set!(RigidBodyVelocity, rb_vels);
+impl_field_component_set!(RigidBodyDamping, rb_damping);
+impl_field_component_set!(RigidBodyForces, rb_forces);
+impl_field_component_set!(RigidBodyCcd, rb_ccd);
+impl_field_component_set!(RigidBodyIds, rb_ids);
+impl_field_component_set!(RigidBodyType, rb_type);
+impl_field_component_set!(RigidBodyActivation, rb_activation);
+impl_field_component_set!(RigidBodyColliders, rb_colliders);
+impl_field_component_set!(RigidBodyDominance, rb_dominance);
+impl_field_component_set!(RigidBodyChanges, changes);
+
 impl RigidBodySet {
     /// Create a new empty set of rigid bodies.
     pub fn new() -> Self {
         RigidBodySet {
             bodies: Arena::new(),
-            active_dynamic_set: Vec::new(),
-            active_kinematic_set: Vec::new(),
-            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(),
         }
     }
 
+    pub(crate) fn take_modified(&mut self) -> Vec<RigidBodyHandle> {
+        std::mem::replace(&mut self.modified_bodies, vec![])
+    }
+
     /// The number of rigid bodies on this set.
     pub fn len(&self) -> usize {
         self.bodies.len()
@@ -121,18 +129,10 @@ 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);
+        rb.changes_mut_internal().set(RigidBodyChanges::all(), true);
 
         let handle = RigidBodyHandle(self.bodies.insert(rb));
         self.modified_bodies.push(handle);
-
-        let rb = &mut self.bodies[handle.0];
-
-        if rb.is_kinematic() {
-            rb.active_set_id = self.active_kinematic_set.len();
-            self.active_kinematic_set.push(handle);
-        }
-
         handle
     }
 
@@ -140,6 +140,7 @@ impl RigidBodySet {
     pub fn remove(
         &mut self,
         handle: RigidBodyHandle,
+        islands: &mut IslandManager,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
     ) -> Option<RigidBody> {
@@ -147,55 +148,23 @@ impl RigidBodySet {
         /*
          * Update active sets.
          */
-        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
-
-        for active_set in &mut active_sets {
-            if active_set.get(rb.active_set_id) == Some(&handle) {
-                active_set.swap_remove(rb.active_set_id);
-
-                if let Some(replacement) = active_set.get(rb.active_set_id) {
-                    self.bodies[replacement.0].active_set_id = rb.active_set_id;
-                }
-            }
-        }
+        islands.rigid_body_removed(handle, &rb.rb_ids, self);
 
         /*
          * Remove colliders attached to this rigid-body.
          */
-        for collider in &rb.colliders {
-            colliders.remove(*collider, self, false);
+        for collider in rb.colliders() {
+            colliders.remove(*collider, islands, self, false);
         }
 
         /*
          * Remove joints attached to this rigid-body.
          */
-        joints.remove_rigid_body(rb.joint_graph_index, self);
+        joints.remove_rigid_body(rb.rb_ids.joint_graph_index, islands, self);
 
         Some(rb)
     }
 
-    pub(crate) fn num_islands(&self) -> usize {
-        self.active_islands.len() - 1
-    }
-
-    /// Forces the specified rigid-body to wake up if it is dynamic.
-    ///
-    /// 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, handle: RigidBodyHandle, strong: bool) {
-        if let Some(rb) = self.bodies.get_mut(handle.0) {
-            // TODO: what about kinematic bodies?
-            if rb.is_dynamic() {
-                rb.wake_up(strong);
-
-                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);
-                }
-            }
-        }
-    }
-
     /// Gets the rigid-body with the given handle without a known generation.
     ///
     /// This is useful when you know you want the rigid-body at position `i` but
@@ -224,12 +193,7 @@ impl RigidBodySet {
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> 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_all_bodies,
-        );
+        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
         Some((rb, handle))
     }
 
@@ -238,14 +202,13 @@ impl RigidBodySet {
         self.bodies.get(handle.0)
     }
 
-    fn mark_as_modified(
+    pub(crate) fn mark_as_modified(
         handle: RigidBodyHandle,
         rb: &mut RigidBody,
         modified_bodies: &mut Vec<RigidBodyHandle>,
-        modified_all_bodies: bool,
     ) {
-        if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
-            rb.changes = RigidBodyChanges::MODIFIED;
+        if !rb.changes().contains(RigidBodyChanges::MODIFIED) {
+            *rb.changes_mut_internal() = RigidBodyChanges::MODIFIED;
             modified_bodies.push(handle);
         }
     }
@@ -254,12 +217,7 @@ impl RigidBodySet {
     #[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_all_bodies,
-        );
+        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
         Some(result)
     }
 
@@ -274,23 +232,10 @@ 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_all_bodies,
-        );
+        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
         Some(result)
     }
 
-    pub(crate) fn get2_mut_internal(
-        &mut self,
-        h1: RigidBodyHandle,
-        h2: RigidBodyHandle,
-    ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
-        self.bodies.get2_mut(h1.0, h2.0)
-    }
-
     /// Iterates through all the rigid-bodies on this set.
     pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
         self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
@@ -300,431 +245,11 @@ impl RigidBodySet {
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     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().map(|(h, b)| (RigidBodyHandle(h), b))
-    }
-
-    /// Iter through all the active kinematic rigid-bodies on this set.
-    pub fn iter_active_kinematic<'a>(
-        &'a self,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let bodies: &'a _ = &self.bodies;
-        self.active_kinematic_set
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    /// Iter through all the active dynamic rigid-bodies on this set.
-    pub fn iter_active_dynamic<'a>(
-        &'a self,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let bodies: &'a _ = &self.bodies;
-        self.active_dynamic_set
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    #[cfg(not(feature = "parallel"))]
-    pub(crate) fn iter_active_island<'a>(
-        &'a self,
-        island_id: usize,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        let bodies: &'a _ = &self.bodies;
-        self.active_dynamic_set[island_range]
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    /// Applies the given function on all the active dynamic rigid-bodies
-    /// contained by this set.
-    #[inline(always)]
-    #[cfg(not(feature = "dev-remove-slow-accessors"))]
-    pub fn foreach_active_dynamic_body_mut(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                Self::mark_as_modified(
-                    *handle,
-                    rb,
-                    &mut self.modified_bodies,
-                    self.modified_all_bodies,
-                );
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-
-        for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_dynamic_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_kinematic_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    #[cfg(not(feature = "parallel"))]
-    pub(crate) fn foreach_active_island_body_mut_internal(
-        &mut self,
-        island_id: usize,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        for handle in &self.active_dynamic_set[island_range] {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[cfg(feature = "parallel")]
-    #[inline(always)]
-    #[allow(dead_code)]
-    pub(crate) fn foreach_active_island_body_mut_internal_parallel(
-        &mut self,
-        island_id: usize,
-        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
-    ) {
-        use std::sync::atomic::Ordering;
-
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
-        self.active_dynamic_set[island_range]
-            .par_iter()
-            .for_each_init(
-                || bodies.load(Ordering::Relaxed),
-                |bodies, handle| {
-                    let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
-                    if let Some(rb) = bodies.get_mut(handle.0) {
-                        f(*handle, rb)
-                    }
-                },
-            );
-    }
-
-    // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
-    //     &self.active_dynamic_set
-    // }
-
-    pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
-        self.active_islands[island_id]..self.active_islands[island_id + 1]
-    }
-
-    pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
-        &self.active_dynamic_set[self.active_island_range(island_id)]
-    }
-
-    // Utility function to avoid some borrowing issue in the `maintain` method.
-    fn maintain_one(
-        bodies: &mut Arena<RigidBody>,
-        colliders: &mut ColliderSet,
-        handle: RigidBodyHandle,
-        modified_inactive_set: &mut Vec<RigidBodyHandle>,
-        active_kinematic_set: &mut Vec<RigidBodyHandle>,
-        active_dynamic_set: &mut Vec<RigidBodyHandle>,
-    ) {
-        enum FinalAction {
-            UpdateActiveKinematicSetId,
-            UpdateActiveDynamicSetId,
-        }
-
-        if let Some(rb) = bodies.get_mut(handle.0) {
-            let mut final_action = None;
-
-            // The body's status changed. We need to make sure
-            // it is on the correct active set.
-            if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
-                match rb.body_status() {
-                    BodyStatus::Dynamic => {
-                        // Remove from the active kinematic set if it was there.
-                        if active_kinematic_set.get(rb.active_set_id) == Some(&handle) {
-                            active_kinematic_set.swap_remove(rb.active_set_id);
-                            final_action =
-                                Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id));
-                        }
-
-                        // Add to the active dynamic set.
-                        rb.wake_up(true);
-                        // Make sure the sleep change flag is set (even if for some
-                        // reasons the rigid-body was already awake) to make
-                        // sure the code handling sleeping change adds the body to
-                        // the active_dynamic_set.
-                        rb.changes.set(RigidBodyChanges::SLEEP, true);
-                    }
-                    BodyStatus::Kinematic => {
-                        // Remove from the active dynamic set if it was there.
-                        if active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
-                            active_dynamic_set.swap_remove(rb.active_set_id);
-                            final_action =
-                                Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id));
-                        }
-
-                        // Add to the active kinematic set.
-                        if active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
-                            rb.active_set_id = active_kinematic_set.len();
-                            active_kinematic_set.push(handle);
-                        }
-                    }
-                    BodyStatus::Static => {}
-                }
-            }
-
-            // 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();
-
-            // Adjust some ids, if needed.
-            if let Some((action, id)) = final_action {
-                let active_set = match action {
-                    FinalAction::UpdateActiveKinematicSetId => active_kinematic_set,
-                    FinalAction::UpdateActiveDynamicSetId => active_dynamic_set,
-                };
-
-                if id < active_set.len() {
-                    if let Some(rb2) = bodies.get_mut(active_set[id].0) {
-                        rb2.active_set_id = id;
-                    }
-                }
-            }
-        }
-    }
-
-    pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
-        if self.modified_all_bodies {
-            // Unfortunately, we have to push all the bodies to `modified_bodies`
-            // instead of just calling `maintain_one` on each element i
-            // `self.bodies.iter_mut()` because otherwise it would be difficult to
-            // handle the final  change of active_set_id in Self::maintain_one
-            // (because it has  to modify another rigid-body because of the swap-remove.
-            // So this causes borrowing problems if we do this while iterating
-            // through self.bodies.iter_mut()).
-            for (handle, _) in self.bodies.iter_mut() {
-                self.modified_bodies.push(RigidBodyHandle(handle));
-            }
-        }
-
-        for handle in self.modified_bodies.drain(..) {
-            Self::maintain_one(
-                &mut self.bodies,
-                colliders,
-                handle,
-                &mut self.modified_inactive_set,
-                &mut self.active_kinematic_set,
-                &mut self.active_dynamic_set,
-            )
-        }
-
-        if self.modified_all_bodies {
-            self.modified_bodies.shrink_to_fit(); // save some memory.
-            self.modified_all_bodies = false;
-        }
-    }
-
-    pub(crate) fn update_active_set_with_contacts(
-        &mut self,
-        colliders: &ColliderSet,
-        narrow_phase: &NarrowPhase,
-        joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
-        min_island_size: usize,
-    ) {
-        assert!(
-            min_island_size > 0,
-            "The minimum island size must be at least 1."
-        );
-
-        // Update the energy of every rigid body and
-        // keep only those that may not sleep.
-        //        let t = instant::now();
-        self.active_set_timestamp += 1;
-        self.stack.clear();
-        self.can_sleep.clear();
-
-        // NOTE: the `.rev()` is here so that two successive timesteps preserve
-        // the order of the bodies in the `active_dynamic_set` vec. This reversal
-        // does not seem to affect performances nor stability. However it makes
-        // debugging slightly nicer so we keep this rev.
-        for h in self.active_dynamic_set.drain(..).rev() {
-            let rb = &mut self.bodies[h.0];
-            rb.update_energy();
-            if rb.activation.energy <= rb.activation.threshold {
-                // Mark them as sleeping for now. This will
-                // be set to false during the graph traversal
-                // if it should not be put to sleep.
-                rb.activation.sleeping = true;
-                self.can_sleep.push(h);
-            } else {
-                self.stack.push(h);
-            }
-        }
-
-        // Read all the contacts and push objects touching touching this rigid-body.
-        #[inline(always)]
-        fn push_contacting_bodies(
-            rb: &RigidBody,
-            colliders: &ColliderSet,
-            narrow_phase: &NarrowPhase,
-            stack: &mut Vec<RigidBodyHandle>,
-        ) {
-            for collider_handle in &rb.colliders {
-                if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
-                    for inter in contacts {
-                        for manifold in &inter.2.manifolds {
-                            if !manifold.data.solver_contacts.is_empty() {
-                                let other = crate::utils::select_other(
-                                    (inter.0, inter.1),
-                                    *collider_handle,
-                                );
-                                let other_body = colliders[other].parent;
-                                stack.push(other_body);
-                                break;
-                            }
-                        }
-                    }
-                }
-            }
-        }
-
-        // Now iterate on all active kinematic bodies and push all the bodies
-        // touching them to the stack so they can be woken up.
-        for h in self.active_kinematic_set.iter() {
-            let rb = &self.bodies[h.0];
-
-            if !rb.is_moving() {
-                // If the kinematic body does not move, it does not have
-                // to wake up any dynamic body.
-                continue;
-            }
-
-            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
-        }
-
-        //        println!("Selection: {}", instant::now() - t);
-
-        //        let t = instant::now();
-        // Propagation of awake state and awake island computation through the
-        // traversal of the interaction graph.
-        self.active_islands.clear();
-        self.active_islands.push(0);
-
-        // The max avoid underflow when the stack is empty.
-        let mut island_marker = self.stack.len().max(1) - 1;
-
-        while let Some(handle) = self.stack.pop() {
-            let rb = &mut self.bodies[handle.0];
-
-            if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
-                // We already visited this body and its neighbors.
-                // Also, we don't propagate awake state through static bodies.
-                continue;
-            }
-
-            if self.stack.len() < island_marker {
-                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
-                    >= min_island_size
-                {
-                    // We are starting a new island.
-                    self.active_islands.push(self.active_dynamic_set.len());
-                }
-
-                island_marker = self.stack.len();
-            }
-
-            rb.wake_up(false);
-            rb.active_island_id = self.active_islands.len() - 1;
-            rb.active_set_id = self.active_dynamic_set.len();
-            rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
-            rb.active_set_timestamp = self.active_set_timestamp;
-            self.active_dynamic_set.push(handle);
-
-            // Transmit the active state to all the rigid-bodies with colliders
-            // in contact or joined with this collider.
-            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
-
-            for inter in joint_graph.interactions_with(rb.joint_graph_index) {
-                let other = crate::utils::select_other((inter.0, inter.1), handle);
-                self.stack.push(other);
-            }
-        }
-
-        self.active_islands.push(self.active_dynamic_set.len());
-        //        println!(
-        //            "Extraction: {}, num islands: {}",
-        //            instant::now() - t,
-        //            self.active_islands.len() - 1
-        //        );
-
-        // Actually put to sleep bodies which have not been detected as awake.
-        //        let t = instant::now();
-        for h in &self.can_sleep {
-            let b = &mut self.bodies[h.0];
-            if b.activation.sleeping {
-                b.sleep();
-            }
-        }
-        //        println!("Activation: {}", instant::now() - t);
+        let modified_bodies = &mut self.modified_bodies;
+        self.bodies.iter_mut().map(move |(h, b)| {
+            modified_bodies.push(RigidBodyHandle(h));
+            (RigidBodyHandle(h), b)
+        })
     }
 }
 
@@ -736,16 +261,19 @@ impl Index<RigidBodyHandle> for RigidBodySet {
     }
 }
 
+impl Index<crate::data::Index> for RigidBodySet {
+    type Output = RigidBody;
+
+    fn index(&self, index: crate::data::Index) -> &RigidBody {
+        &self.bodies[index]
+    }
+}
+
 #[cfg(not(feature = "dev-remove-slow-accessors"))]
 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_all_bodies,
-        );
+        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
         rb
     }
 }