Experiment with incremental island having only one awake island.

14 files changed, 350 insertions, 99 deletions

diff --git a/src/data/coarena.rs b/src/data/coarena.rs
index 78cbfa7..6cdfbde 100644
--- a/src/data/coarena.rs
+++ b/src/data/coarena.rs
@@ -29,6 +29,27 @@ impl<T> Coarena<T> {
             .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
     }
 
+    pub fn ensure_element_exists(&mut self, index: Index, default: T) -> &mut T
+    where
+        T: Clone,
+    {
+        let (i1, g1) = index.into_raw_parts();
+
+        let elt1 = {
+            if self.data.len() <= i1 {
+                self.data.resize(i1 + 1, (u32::MAX as u64, default.clone()));
+            }
+
+            &mut self.data[i1]
+        };
+
+        if elt1.0 != g1 {
+            *elt1 = (g1, default);
+        }
+
+        &mut elt1.1
+    }
+
     /// Ensure that elements at the two given indices exist in this coarena, and return their reference.
     ///
     /// Missing elements are created automatically and initialized with the `default` value.
@@ -69,3 +90,16 @@ impl<T> Coarena<T> {
         (&mut elt1.1, &mut elt2.1)
     }
 }
+
+impl<T> std::ops::Index<Index> for Coarena<T> {
+    type Output = T;
+    fn index(&self, id: Index) -> &T {
+        self.get(id).expect("Index out of bounds.")
+    }
+}
+
+impl<T> std::ops::IndexMut<Index> for Coarena<T> {
+    fn index_mut(&mut self, id: Index) -> &mut T {
+        self.get_mut(id).expect("Index out of bounds.")
+    }
+}
diff --git a/src/dynamics/island_set2.rs b/src/dynamics/island_set2.rs
index 1c38b7f..098171a 100644
--- a/src/dynamics/island_set2.rs
+++ b/src/dynamics/island_set2.rs
@@ -1,7 +1,8 @@
+use crate::data::Coarena;
 use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
-use crate::geometry::NarrowPhase;
+use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::utils;
-use downcast_rs::__std::collections::VecDeque;
+use std::collections::VecDeque;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
@@ -19,22 +20,38 @@ impl Island {
     }
 
     pub fn is_sleeping(&self) -> bool {
-        self.wake_up_count == 0
+        false // FIXME: should be true
+    }
+
+    pub fn bodies(&self) -> &[RigidBodyHandle] {
+        &self.bodies
     }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct IslandSet2 {
-    inactive_islands: Vec<Island>,
-    active_island: Island,
+pub struct IslandSet {
+    // We can't store this in the rigid-bodies because that would
+    // cause borrowing issues during the traversal.
+    traversal_body_timestamps: Coarena<u64>,
+    islands: Vec<Island>,
+    active_island: usize,
+    islands_to_extract: VecDeque<RigidBodyHandle>,
+    islands_extraction_queue: Vec<RigidBodyHandle>,
+    // NOTE: this value must never be 0, otherwise some
+    // invariants used during the traversal can break.
+    traversal_timestamp: u64,
 }
 
-impl IslandSet2 {
+impl IslandSet {
     pub fn new() -> Self {
-        IslandSet2 {
-            inactive_islands: vec![],
-            active_island: Island::new(),
+        IslandSet {
+            traversal_body_timestamps: Coarena::new(),
+            islands: vec![Island::new()],
+            active_island: 0,
+            islands_to_extract: VecDeque::new(),
+            islands_extraction_queue: vec![],
+            traversal_timestamp: 1,
         }
     }
 
@@ -42,28 +59,16 @@ impl IslandSet2 {
         1
     }
 
-    pub fn active_island(&self, i: usize) -> &Island {
-        &self.active_island
-    }
-
-    pub fn active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
-        self.active_island.bodies.iter()
+    pub fn is_island_sleeping(&self, island_id: usize) -> bool {
+        island_id != self.active_island
     }
 
-    pub fn contact_stopped(
-        &mut self,
-        bodies: &RigidBodySet,
-        h1: RigidBodyHandle,
-        _h2: RigidBodyHandle,
-    ) {
+    pub fn active_bodies(&self) -> &[RigidBodyHandle] {
+        &self.islands[self.active_island].bodies
     }
 
     pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {}
 
-    pub fn is_island_sleeping(&self, island_id: usize) -> bool {
-        island_id != crate::INVALID_USIZE
-    }
-
     pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) {
         assert_eq!(body.island_id, crate::INVALID_USIZE);
 
@@ -71,63 +76,257 @@ impl IslandSet2 {
             return;
         }
 
+        self.traversal_body_timestamps
+            .ensure_element_exists(handle.0, 0);
+
         if body.can_sleep() {
-            let new_island_id = handle.into_raw_parts().0;
+            body.island_id = self.islands.len();
+            body.island_offset = 0;
+            self.islands.push(Island {
+                bodies: vec![handle],
+            })
+        } else {
+            let mut active_island = &mut self.islands[self.active_island];
+            body.island_id = self.active_island;
+            body.island_offset = active_island.bodies.len();
+            active_island.bodies.push(handle);
+        }
+    }
 
-            if self.islands.len() <= new_island_id {
-                self.islands.resize(new_island_id + 1, Island::new());
-            }
+    pub fn contact_started(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        mut h1: RigidBodyHandle,
+        mut h2: RigidBodyHandle,
+    ) {
+        let island1 = bodies[h1].island_id;
+        let island2 = bodies[h2].island_id;
 
-            body.island_id = new_island_id;
-            body.island_offset = self.islands[new_island_id].bodies.len();
-            self.islands[new_island_id].bodies.push(handle);
-        } else {
-            body.island_offset = self.active_island.len();
-            self.active_island.push(handle);
+        self.wake_up_island(bodies, island1);
+        self.wake_up_island(bodies, island2);
+    }
+
+    pub fn body_sleep_state_changed(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle) {
+        let body = &mut bodies[handle];
+        let island_id = body.island_id;
+
+        if island_id == crate::INVALID_USIZE {
+            self.add_rigid_body(handle, body);
+        } else if island_id == self.active_island && body.can_sleep() {
+            // The body is sleeping now.
+            self.islands_to_extract.push_back(handle);
+        } else if island_id != self.active_island && !body.can_sleep() {
+            // The body is awake now.
+            self.wake_up_island(bodies, island_id);
         }
     }
 
-    pub fn body_sleep_state_changed(&mut self, body: &RigidBody) {
-        // Non-dynamic bodies never take part in the island computation
-        // since they don't transmit any forces.
-        if !body.is_dynamic() {
-            return;
+    #[inline(always)]
+    fn push_contacting_bodies(
+        handle: RigidBodyHandle,
+        bodies: &mut RigidBodySet,
+        colliders: &ColliderSet,
+        narrow_phase: &NarrowPhase,
+        queue: &mut Vec<RigidBodyHandle>,
+        traversal_body_timestamps: &mut Coarena<u64>,
+        first_traversal_timestamp: u64,
+        traversal_timestamp: u64,
+    ) -> bool {
+        for collider_handle in &bodies[handle].colliders {
+            if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
+                for inter in contacts {
+                    let other = crate::utils::select_other((inter.0, inter.1), *collider_handle);
+                    let other_body_handle = colliders[other].parent;
+                    let other_body = &bodies[other_body_handle];
+
+                    if other_body.is_dynamic() {
+                        if !other_body.can_sleep() {
+                            return false;
+                        }
+
+                        let other_body_timestamp =
+                            &mut traversal_body_timestamps[other_body_handle.0];
+
+                        if *other_body_timestamp >= first_traversal_timestamp
+                            && *other_body_timestamp < traversal_timestamp
+                        {
+                            // We already saw this body during another traversal this frame.
+                            // So we don't need to continue any further.
+                            return false;
+                        }
+
+                        if *other_body_timestamp != traversal_timestamp {
+                            *other_body_timestamp = traversal_timestamp;
+                            queue.push(other_body_handle);
+                        }
+                    }
+                }
+            }
         }
 
-        let island = &mut self.islands[body.island_id];
+        true
+    }
 
-        if body.can_sleep() {
-            island.wake_up_count -= 1;
+    pub fn update_sleeping_islands(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        narrow_phase: &NarrowPhase,
+    ) {
+        let first_traversal_timestamp = self.traversal_timestamp + 1;
+        let mut island_found = false;
 
-            if island.wake_up_count == 0 {
-                // The island is sleeping now.
-                // Remove it from the active set.
-                let active_island_id_to_remove = island.active_island_id;
-                island.active_island_id = crate::INVALID_USIZE;
-                self.active_islands.swap_remove(active_island_id_to_remove);
+        while let Some(handle) = self.islands_to_extract.pop_front() {
+            self.traversal_timestamp += 1;
+            island_found = self.extract_sleeping_island(
+                bodies,
+                colliders,
+                narrow_phase,
+                handle,
+                first_traversal_timestamp,
+            ) || island_found;
 
-                if let Some(moved_island) = self.active_islands.get(active_island_id_to_remove) {
-                    self.islands[*moved_island].active_island_id = active_island_id_to_remove;
+            // Make sure our `traversal_timestamp` remains correct
+            // even if it overflows (can happen in very long-running
+            // simulations).
+            if self.traversal_timestamp == u64::MAX - 1 {
+                // Stop the work now because the overflowing timestamp
+                // will cause issues with the traversal. Forcibly reset all
+                // the timestamps and continue next frame.
+                for body in bodies.iter_mut_internal() {
+                    body.1.traversal_timestamp = 0;
                 }
+                self.traversal_timestamp = 1;
+                break;
             }
-        } else {
-            if island.wake_up_count == 0 {
-                // The islands is waking up.
-                // Add it to the active set.
-                assert_eq!(island.active_island_id, crate::INVALID_USIZE);
-                island.active_island_id = self.active_islands.len();
-                self.active_islands.push(body.island_id);
+        }
+
+        if island_found {
+            // Now we need to remove from the active set all the bodies
+            // we moved to a sleeping island.
+            let mut i = 0;
+            let mut new_len = 0;
+            let active_island = &mut self.islands[self.active_island];
+
+            while i < active_island.bodies.len() {
+                let handle = active_island.bodies[i];
+                let body = &mut bodies[handle];
+
+                if body.island_id == self.active_island {
+                    // This body still belongs to this island.
+                    // Update its offset.
+                    body.island_offset = new_len;
+                    active_island.bodies[new_len] = handle;
+                    new_len += 1;
+                }
+
+                i += 1;
             }
 
-            island.wake_up_count += 1;
+            active_island.bodies.truncate(new_len);
         }
     }
 
-    pub fn contact_started(
+    fn extract_sleeping_island(
         &mut self,
         bodies: &mut RigidBodySet,
-        mut h1: RigidBodyHandle,
-        mut h2: RigidBodyHandle,
-    ) {
+        colliders: &mut ColliderSet,
+        narrow_phase: &NarrowPhase,
+        handle: RigidBodyHandle,
+        first_traversal_timestamp: u64,
+    ) -> bool {
+        // Attempt to extract a sleeping island by traversing the
+        // contact graph starting with the given rigid-body.
+        if let Some(body) = bodies.get(handle) {
+            if body.is_dynamic() && body.can_sleep() {
+                // Perform a breadth-first search of the contact graph starting
+                // with this body. We stop the search when an active body is
+                // found (meaning that the island cannot sleep), or if we
+                // extracted the whole island.
+                // We do a breadth-first search in order to increase our chances
+                // to find a non-sleeping body quickly: if this body just started
+                // sleeping, chances are that a non-sleeping body is close.
+                self.islands_extraction_queue.clear();
+                self.islands_extraction_queue.push(handle);
+                self.traversal_body_timestamps[handle.0] = self.traversal_timestamp;
+                let mut i = 0;
+
+                while i < self.islands_extraction_queue.len() {
+                    let handle = self.islands_extraction_queue[i];
+
+                    let continue_traversal = Self::push_contacting_bodies(
+                        handle,
+                        bodies,
+                        colliders,
+                        narrow_phase,
+                        &mut self.islands_extraction_queue,
+                        &mut self.traversal_body_timestamps,
+                        first_traversal_timestamp,
+                        self.traversal_timestamp,
+                    );
+
+                    if !continue_traversal {
+                        // This island cannot sleep yet.
+                        return false;
+                    }
+
+                    i += 1;
+                }
+
+                // If we reached this point, then we successfully found a sleeping island.
+                for (k, handle) in self.islands_extraction_queue.iter().enumerate() {
+                    let body = &mut bodies[*handle];
+                    body.island_id = self.islands.len();
+                    body.island_offset = k;
+                }
+
+                // FIXME: recycle old islands.
+                self.islands.push(Island {
+                    bodies: std::mem::replace(&mut self.islands_extraction_queue, Vec::new()),
+                });
+
+                return true;
+            }
+        }
+
+        false
+    }
+
+    pub fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) {
+        if island_id == crate::INVALID_USIZE {
+            return;
+        }
+
+        let mut island_id1 = self.active_island;
+        let mut island_id2 = island_id;
+
+        if island_id1 != island_id2 {
+            let (mut island1, mut island2) =
+                utils::get2_mut(&mut self.islands, island_id1, island_id2);
+
+            // Make sure island1 points to the biggest island.
+            // The typical scenario is that only a few object are awaking
+            // one big island. So in this typical scenario, we actually want
+            // to merge the active island into the sleeping island, and then
+            // mark the sleeping island as active.
+            if island2.len() > island1.len() {
+                std::mem::swap(&mut island1, &mut island2);
+                std::mem::swap(&mut island_id1, &mut island_id2);
+            }
+
+            // Now merge island2 (the smallest island) into island1 (the bigger one).
+            island1.bodies.reserve(island2.len());
+
+            for handle in island2.bodies.drain(..) {
+                let body = &mut bodies[handle];
+                body.island_id = island_id1;
+                body.island_offset = island1.len();
+                island1.bodies.push(handle);
+            }
+
+            // Islands can get big so let's save some memory.
+            island2.bodies = vec![];
+            self.active_island = island_id1;
+        }
     }
 }
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
index cad9869..85e8ea8 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/joint_set.rs
@@ -197,9 +197,9 @@ impl JointSet {
                 && (!rb2.is_dynamic() || !rb2.is_sleeping())
             {
                 let island_index = if !rb1.is_dynamic() {
-                    islands.islands()[rb2.island_id].active_island_id()
+                    0 // islands.islands()[rb2.island_id].active_island_id()
                 } else {
-                    islands.islands()[rb1.island_id].active_island_id()
+                    0 // islands.islands()[rb1.island_id].active_island_id()
                 };
 
                 out[island_index].push(i);
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 2ff6f5c..691e504 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -12,7 +12,8 @@ pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
 pub use parry::mass_properties::MassProperties;
 // #[cfg(not(feature = "parallel"))]
 pub use self::coefficient_combine_rule::CoefficientCombineRule;
-pub use self::island_set::{Island, IslandSet};
+// pub use self::island_set::{Island, IslandSet};
+pub use self::island_set2::{Island, IslandSet};
 
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::rigid_body::RigidBodyChanges;
@@ -23,8 +24,8 @@ pub(crate) use self::solver::ParallelIslandSolver;
 
 mod coefficient_combine_rule;
 mod integration_parameters;
-mod island_set;
-// mod island_set2;
+// mod island_set;
+mod island_set2;
 mod joint;
 mod rigid_body;
 mod rigid_body_set;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index f2c3807..74e00b6 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -88,6 +88,7 @@ pub struct RigidBody {
     pub(crate) active_set_id: usize,
     pub(crate) active_set_offset: usize,
     pub(crate) active_set_timestamp: u32,
+    pub(crate) traversal_timestamp: u64,
     flags: RigidBodyFlags,
     pub(crate) changes: RigidBodyChanges,
     /// The status of the body, governing how it is affected by external forces.
@@ -121,6 +122,7 @@ impl RigidBody {
             active_set_id: 0,
             active_set_offset: 0,
             active_set_timestamp: 0,
+            traversal_timestamp: 0,
             flags: RigidBodyFlags::empty(),
             changes: RigidBodyChanges::all(),
             body_status: BodyStatus::Dynamic,
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 01ef0ac..10bc655 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -2,8 +2,7 @@
 use rayon::prelude::*;
 
 use crate::data::arena::Arena;
-use crate::dynamics::island_set::IslandSet;
-use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
+use crate::dynamics::{IslandSet, Joint, JointSet, RigidBody, RigidBodyChanges};
 use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
 use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
@@ -264,6 +263,12 @@ impl RigidBodySet {
         self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
     }
 
+    pub(crate) fn iter_mut_internal(
+        &mut self,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
+        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,
@@ -374,6 +379,7 @@ impl RigidBodySet {
         modified_inactive_set: &mut Vec<RigidBodyHandle>,
         active_kinematic_set: &mut Vec<RigidBodyHandle>,
         active_dynamic_set: &mut Vec<RigidBodyHandle>,
+        out_changed_sleep: &mut Vec<RigidBodyHandle>,
     ) {
         // Update the positions of the colliders.
         if rb.changes.contains(RigidBodyChanges::POSITION)
@@ -403,17 +409,18 @@ impl RigidBodySet {
         }
 
         if rb.changes.contains(RigidBodyChanges::SLEEP) {
-            if rb.island_id == crate::INVALID_USIZE {
-                islands.add_rigid_body(handle, rb);
-            }
-
-            islands.body_sleep_state_changed(rb);
+            out_changed_sleep.push(handle);
         }
 
         rb.changes = RigidBodyChanges::empty();
     }
 
-    pub(crate) fn maintain(&mut self, islands: &mut IslandSet, colliders: &mut ColliderSet) {
+    pub(crate) fn maintain(
+        &mut self,
+        islands: &mut IslandSet,
+        colliders: &mut ColliderSet,
+        out_changed_sleep: &mut Vec<RigidBodyHandle>,
+    ) {
         if self.modified_all_bodies {
             for (handle, rb) in self.bodies.iter_mut() {
                 Self::maintain_one(
@@ -424,6 +431,7 @@ impl RigidBodySet {
                     &mut self.modified_inactive_set,
                     &mut self.active_kinematic_set,
                     &mut self.active_dynamic_set,
+                    out_changed_sleep,
                 )
             }
 
@@ -440,6 +448,7 @@ impl RigidBodySet {
                         &mut self.modified_inactive_set,
                         &mut self.active_kinematic_set,
                         &mut self.active_dynamic_set,
+                        out_changed_sleep,
                     )
                 }
             }
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index fef7565..f0d88dd 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -196,8 +196,7 @@ impl InteractionGroups {
         // FIXME: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped constraints.
-        self.body_masks
-            .resize(islands.active_island(island_id).len(), 0u128);
+        self.body_masks.resize(islands.active_bodies().len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -331,8 +330,7 @@ impl InteractionGroups {
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped contacts.
         // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
-        self.body_masks
-            .resize(islands.active_island(island_id).len(), 0u128);
+        self.body_masks.resize(islands.active_bodies().len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 81fc5d0..6ad9935 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -65,7 +65,7 @@ impl IslandSolver {
         }
 
         counters.solver.velocity_update_time.resume();
-        for handle in islands.active_island(island_id).bodies() {
+        for handle in islands.active_bodies() {
             if let Some(rb) = bodies.get_mut_internal(*handle) {
                 rb.integrate(params.dt)
             }
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index d979233..bf44fbe 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -27,8 +27,7 @@ impl PositionSolver {
         self.positions.clear();
         self.positions.extend(
             islands
-                .active_island(island_id)
-                .bodies()
+                .active_bodies()
                 .iter()
                 .filter_map(|h| bodies.get(*h))
                 .map(|b| b.position),
@@ -44,7 +43,7 @@ impl PositionSolver {
             }
         }
 
-        for handle in islands.active_island(island_id).bodies() {
+        for handle in islands.active_bodies() {
             if let Some(rb) = bodies.get_mut(*handle) {
                 rb.set_position_internal(self.positions[rb.island_offset])
             }
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 68669e9..7b18ea9 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -31,7 +31,7 @@ impl VelocitySolver {
     ) {
         self.mj_lambdas.clear();
         self.mj_lambdas
-            .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+            .resize(islands.active_bodies().len(), DeltaVel::zero());
 
         /*
          * Warmstart constraints.
@@ -58,7 +58,7 @@ impl VelocitySolver {
         }
 
         // Update velocities.
-        for handle in islands.active_island(island_id).bodies() {
+        for handle in islands.active_bodies() {
             if let Some(rb) = bodies.get_mut(*handle) {
                 let dvel = self.mj_lambdas[rb.island_offset];
                 rb.linvel += dvel.linear;
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index c27e5aa..d5146b3 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -634,11 +634,10 @@ impl BroadPhase {
         for body_handle in bodies
             .modified_inactive_set
             .iter()
-            .copied()
-            .chain(islands.active_bodies())
-            .chain(bodies.active_kinematic_set.iter().copied())
+            .chain(islands.active_bodies().iter())
+            .chain(bodies.active_kinematic_set.iter())
         {
-            for handle in &bodies[body_handle].colliders {
+            for handle in &bodies[*body_handle].colliders {
                 let collider = &mut colliders[*handle];
                 let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
 
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 9c6b892..9c9f784 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -626,9 +626,9 @@ impl NarrowPhase {
                     && (!rb2.is_dynamic() || !islands.is_island_sleeping(rb2.island_id))
                 {
                     let island_index = if !rb1.is_dynamic() {
-                        islands.islands()[rb2.island_id].active_island_id()
+                        0 // islands.islands()[rb2.island_id].active_island_id()
                     } else {
-                        islands.islands()[rb1.island_id].active_island_id()
+                        0 // islands.islands()[rb1.island_id].active_island_id()
                     };
 
                     out[island_index].push(out_manifolds.len());
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 70474a7..cd83baf 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -77,10 +77,18 @@ impl PhysicsPipeline {
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
-        bodies.maintain(islands, colliders);
+        bodies.maintain(
+            islands,
+            colliders,
+            &mut self.bodies_with_changed_sleep_state,
+        );
         broad_phase.maintain(colliders);
         narrow_phase.maintain(colliders, bodies);
 
+        for handle in self.bodies_with_changed_sleep_state.drain(..) {
+            islands.body_sleep_state_changed(bodies, handle);
+        }
+
         // Update kinematic bodies velocities.
         // TODO: what is the best place for this? It should at least be
         // located before the island computation because we test the velocity
@@ -165,7 +173,7 @@ impl PhysicsPipeline {
 
         self.counters.stages.update_time.start();
         for handle in islands.active_bodies() {
-            if let Some(rb) = bodies.get_mut(handle) {
+            if let Some(rb) = bodies.get_mut(*handle) {
                 rb.update_world_mass_properties();
                 rb.integrate_accelerations(integration_parameters.dt, *gravity)
             }
@@ -243,7 +251,7 @@ impl PhysicsPipeline {
         // Update colliders positions and kinematic bodies positions.
         // FIXME: do this in the solver?
         for handle in islands.active_bodies() {
-            if let Some(rb) = bodies.get_mut(handle) {
+            if let Some(rb) = bodies.get_mut(*handle) {
                 rb.update_predicted_position(integration_parameters.dt);
                 rb.update_colliders_positions(colliders);
 
@@ -252,15 +260,17 @@ impl PhysicsPipeline {
                 rb.update_energy();
 
                 if prev_sleep_state != rb.can_sleep() {
-                    self.bodies_with_changed_sleep_state.push(handle);
+                    self.bodies_with_changed_sleep_state.push(*handle);
                 }
             }
         }
 
         for handle in self.bodies_with_changed_sleep_state.drain(..) {
-            islands.body_sleep_state_changed(&bodies[handle]);
+            islands.body_sleep_state_changed(bodies, handle);
         }
 
+        islands.update_sleeping_islands(bodies, colliders, narrow_phase);
+
         bodies.foreach_active_kinematic_body_mut_internal(|_, rb| {
             rb.position = rb.predicted_position;
             rb.linvel = na::zero();
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 6b33e49..cc2a8cf 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -119,7 +119,7 @@ impl QueryPipeline {
         }
 
         for handle in islands.active_bodies() {
-            if let Some(body) = bodies.get(handle) {
+            if let Some(body) = bodies.get(*handle) {
                 for handle in &body.colliders {
                     self.quadtree.pre_update(*handle)
                 }