Rename island_set2 -> island_setperf_experiments

1 files changed, 231 insertions, 152 deletions

diff --git a/src/dynamics/island_set.rs b/src/dynamics/island_set.rs
index a0da207..71dc9eb 100644
--- a/src/dynamics/island_set.rs
+++ b/src/dynamics/island_set.rs
@@ -1,114 +1,69 @@
+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)]
 pub struct Island {
     bodies: Vec<RigidBodyHandle>,
-
-    // Number of bodies that are awake on this island.
-    // If this is equal to zero, ten the whole island is asleep.
-    wake_up_count: usize,
-    // Index in the island_set.active_islands vector.
-    active_island_id: usize,
-    dirty: bool,
 }
 
 impl Island {
     pub fn new() -> Self {
-        Self {
-            bodies: vec![],
-            wake_up_count: 0,
-            active_island_id: crate::INVALID_USIZE,
-            dirty: false,
-        }
+        Self { bodies: vec![] }
     }
 
     pub fn len(&self) -> usize {
         self.bodies.len()
     }
 
-    pub fn is_sleeping(&self) -> bool {
-        self.wake_up_count == 0
-    }
-
-    pub fn active_island_id(&self) -> usize {
-        self.active_island_id
-    }
-
     pub fn bodies(&self) -> &[RigidBodyHandle] {
-        &self.bodies[..]
+        &self.bodies
     }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub struct IslandSet {
-    active_islands: Vec<usize>,
+    // 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>,
-    to_update: VecDeque<usize>,
+    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 IslandSet {
     pub fn new() -> Self {
         IslandSet {
-            active_islands: vec![],
-            islands: vec![],
-            to_update: VecDeque::new(),
+            traversal_body_timestamps: Coarena::new(),
+            islands: vec![Island::new()],
+            active_island: 0,
+            islands_to_extract: VecDeque::new(),
+            islands_extraction_queue: vec![],
+            traversal_timestamp: 1,
         }
     }
 
-    pub fn islands(&self) -> &[Island] {
-        &self.islands[..]
-    }
-
     pub fn num_active_islands(&self) -> usize {
-        self.active_islands.len()
-    }
-
-    pub fn active_island(&self, i: usize) -> &Island {
-        &self.islands[self.active_islands[i]]
-    }
-
-    pub fn active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
-        let islands = &self.islands;
-        self.active_islands
-            .iter()
-            .copied()
-            .flat_map(move |i| islands[i].bodies.iter())
-            .copied()
+        1
     }
 
-    pub fn contact_stopped(
-        &mut self,
-        bodies: &RigidBodySet,
-        h1: RigidBodyHandle,
-        _h2: RigidBodyHandle,
-    ) {
-        // // NOTE: we don't actually need h2 because they are both on the same island anyway.
-        // // Yet we keep the `h2` argument to show that this function properly take into account
-        // // both bodies.
-        // if let Some(island_id) = bodies.get(h1).map(|b| b.island_id) {
-        //     if let Some(island) = self.islands.get_mut(island_id) {
-        //         if !island.dirty {
-        //             island.dirty = true;
-        //             self.to_update.push_back(island_id);
-        //         }
-        //     }
-        // }
+    pub fn is_island_sleeping(&self, island_id: usize) -> bool {
+        island_id != self.active_island
     }
 
-    pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {
-        // if let Some(island) = self.to_update.pop_front() {
-        //     // Next island to update.
-        // }
+    pub fn active_bodies(&self) -> &[RigidBodyHandle] {
+        &self.islands[self.active_island].bodies
     }
 
-    pub fn is_island_sleeping(&self, island_id: usize) -> bool {
-        self.islands[island_id].is_sleeping()
-    }
+    pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {}
 
     pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) {
         assert_eq!(body.island_id, crate::INVALID_USIZE);
@@ -117,85 +72,239 @@ impl IslandSet {
             return;
         }
 
-        let new_island_id = handle.into_raw_parts().0;
+        self.traversal_body_timestamps
+            .ensure_element_exists(handle.0, 0);
 
-        if self.islands.len() <= new_island_id {
-            self.islands.resize(new_island_id + 1, Island::new());
+        if body.can_sleep() {
+            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);
         }
+    }
 
-        body.island_id = new_island_id;
-        body.island_offset = self.islands[new_island_id].bodies.len();
-        self.islands[new_island_id].bodies.push(handle);
+    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;
 
-        // NOTE: `body_sleep_state_changed` must be called afterwards.
+        self.wake_up_island(bodies, island1);
+        self.wake_up_island(bodies, island2);
     }
 
-    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;
+    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);
         }
+    }
 
-        let island = &mut self.islands[body.island_id];
-
-        if body.can_sleep() {
-            island.wake_up_count -= 1;
+    #[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);
+                        }
+                    }
+                }
+            }
+        }
 
-            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);
+        true
+    }
 
-                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;
+    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;
+
+        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;
+
+            // 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);
-            }
-
-            island.wake_up_count += 1;
         }
 
-        if self.active_islands.len() == 0 {
-            dbg!("Hurray!");
+        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;
+            }
+
+            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,
-    ) {
-        let body1 = &bodies[h1];
-        let body2 = &bodies[h2];
+        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 !body1.is_dynamic() || !body2.is_dynamic() {
-            // Non-dynamic bodies don't transmit forces.
-            // So we can ignore their contact for island computation.
-            return;
+                // 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;
+            }
         }
 
-        assert_ne!(body1.island_id, crate::INVALID_USIZE);
-        assert_ne!(body2.island_id, crate::INVALID_USIZE);
+        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 = body1.island_id;
-        let mut island_id2 = body2.island_id;
+        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);
@@ -213,37 +322,7 @@ impl IslandSet {
 
             // Islands can get big so let's save some memory.
             island2.bodies = vec![];
-
-            // Update the wake-up count and determine if the island is awake.
-            // Note that if both `wake_up_count` are zero, then the island
-            // won't end up in the active island set (which is what we want).
-            let island2_was_sleeping = island2.is_sleeping();
-
-            if island1.is_sleeping() && !island2_was_sleeping {
-                // Add the first island to the active island set.
-                assert_eq!(island1.active_island_id, crate::INVALID_USIZE);
-                island1.active_island_id = self.active_islands.len();
-                self.active_islands.push(island_id1);
-            }
-
-            island1.wake_up_count += island2.wake_up_count;
-            island2.wake_up_count = 0;
-
-            assert!(island1.wake_up_count != 0 || island1.active_island_id == crate::INVALID_USIZE);
-
-            if !island2_was_sleeping {
-                // The second island will be emptied, so we
-                // can remove it from the set of active islands.
-                let active_island_id_to_remove = island2.active_island_id;
-                island2.active_island_id = crate::INVALID_USIZE;
-                self.active_islands.swap_remove(active_island_id_to_remove);
-
-                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;
-                }
-            } else {
-                assert_eq!(island2.active_island_id, crate::INVALID_USIZE);
-            }
+            self.active_island = island_id1;
         }
     }
 }