feat: persistent islands + manifold reduction (#895)

* feat: initial implementation of contact manifold reduction

* feat: try bepu-like manifold reduction

* feat: simplification of the constraints counting and indexing logic

* feat: add concept of incremental islands with a single awake island

More islands manager fixes

* feat: start adding support for multiple awake islands

* feat: add more timings

* feat: implement incremental island split & merge

* chore: refactor islands manager into multiple files

* chore: refactor manifold reduction to its own file + add naive reduction method

* feat: add islands manager validation checks

* fix various bugs in the new islands system

* chore: remove redundant active_set_offset field

src/dynamics/island_manager/island.rs

@@ -0,0 +1,163 @@
+use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
+
+use super::IslandManager;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Default)]
+pub(crate) struct Island {
+    /// The rigid-bodies part of this island.
+    pub(super) bodies: Vec<RigidBodyHandle>,
+    /// The additional solver iterations needed by this island.
+    pub(super) additional_solver_iterations: usize,
+    /// Index of this island in `IslandManager::awake_islands`.
+    ///
+    /// If `None`, the island is sleeping.
+    pub(super) id_in_awake_list: Option<usize>,
+}
+
+impl Island {
+    pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self {
+        Self {
+            bodies: vec![handle],
+            additional_solver_iterations: rb.additional_solver_iterations,
+            id_in_awake_list: None,
+        }
+    }
+
+    pub fn bodies(&self) -> &[RigidBodyHandle] {
+        &self.bodies
+    }
+
+    pub fn additional_solver_iterations(&self) -> usize {
+        self.additional_solver_iterations
+    }
+
+    pub fn is_sleeping(&self) -> bool {
+        self.id_in_awake_list.is_none()
+    }
+
+    pub fn len(&self) -> usize {
+        self.bodies.len()
+    }
+
+    pub(crate) fn id_in_awake_list(&self) -> Option<usize> {
+        self.id_in_awake_list
+    }
+}
+
+impl IslandManager {
+    /// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and
+    /// insert `new_island` into the islands set.
+    ///
+    /// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`.
+    pub(super) fn extract_sub_island(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        source_id: usize,
+        mut new_island: Island,
+        sleep: bool,
+    ) {
+        let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len());
+        let source_island = &mut self.islands[source_id];
+
+        for (id, handle) in new_island.bodies.iter().enumerate() {
+            let rb = bodies.index_mut_internal(*handle);
+
+            // If the new island is sleeping, ensure all its bodies are sleeping.
+            if sleep {
+                rb.sleep();
+            }
+
+            let id_to_remove = rb.ids.active_set_id;
+
+            assert_eq!(
+                rb.ids.active_island_id, source_id,
+                "note, new id: {}",
+                new_island_id
+            );
+            rb.ids.active_island_id = new_island_id;
+            rb.ids.active_set_id = id;
+
+            new_island.additional_solver_iterations = new_island
+                .additional_solver_iterations
+                .max(rb.additional_solver_iterations);
+            source_island.bodies.swap_remove(id_to_remove);
+
+            if let Some(moved_handle) = source_island.bodies.get(id_to_remove).copied() {
+                let moved_rb = bodies.index_mut_internal(moved_handle);
+                moved_rb.ids.active_set_id = id_to_remove;
+            }
+        }
+
+        // If the new island is awake, add it to the awake list.
+        if !sleep {
+            new_island.id_in_awake_list = Some(self.awake_islands.len());
+            self.awake_islands.push(new_island_id);
+        } else {
+            new_island.id_in_awake_list = None;
+        }
+
+        self.islands.insert(new_island_id, new_island);
+    }
+
+    pub(super) fn merge_islands(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        island_id1: usize,
+        island_id2: usize,
+    ) {
+        if island_id1 == island_id2 {
+            return;
+        }
+
+        let island1 = &self.islands[island_id1];
+        let island2 = &self.islands[island_id2];
+
+        assert_eq!(
+            island1.id_in_awake_list.is_some(),
+            island2.id_in_awake_list.is_some(),
+            "Internal error: cannot merge two island with different sleeping statuses."
+        );
+
+        // Prefer removing the smallest island to reduce the amount of memory to move.
+        let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() {
+            (island_id2, island_id1)
+        } else {
+            (island_id1, island_id2)
+        };
+
+        // println!("Merging: {} <- {}", to_keep, to_remove);
+
+        let Some(removed_island) = self.islands.remove(to_remove) else {
+            // TODO: the island doesn’t exist is that an internal error?
+            return;
+        };
+
+        self.free_islands.push(to_remove);
+
+        // TODO: if we switched to linked list, we could avoid moving around all this memory.
+        let target_island = &mut self.islands[to_keep];
+        for handle in &removed_island.bodies {
+            let Some(rb) = bodies.get_mut_internal(*handle) else {
+                // This body no longer exists.
+                continue;
+            };
+            rb.wake_up(false);
+            rb.ids.active_island_id = to_keep;
+            rb.ids.active_set_id = target_island.bodies.len();
+            target_island.bodies.push(*handle);
+            target_island.additional_solver_iterations = target_island
+                .additional_solver_iterations
+                .max(rb.additional_solver_iterations);
+        }
+
+        // Update the awake_islands list.
+        if let Some(awake_id_to_remove) = removed_island.id_in_awake_list {
+            self.awake_islands.swap_remove(awake_id_to_remove);
+            // Update the awake list index of the awake island id we moved.
+            if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) {
+                self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove);
+            }
+        }
+    }
+}