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

Author: sebcrozet

crates/rapier2d-f64/Cargo.toml

@@ -43,6 +43,7 @@ serde-serialize = [
     "dep:serde",
     "bit-vec/serde",
     "arrayvec/serde",
+    "vec_map/serde"
 ]
 enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"]
 debug-render = []

crates/rapier2d/Cargo.toml

@@ -44,6 +44,7 @@ serde-serialize = [
     "dep:serde",
     "bit-vec/serde",
     "arrayvec/serde",
+    "vec_map/serde"
 ]
 enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"]
 debug-render = []

crates/rapier3d-f64/Cargo.toml

@@ -46,6 +46,7 @@ serde-serialize = [
     "parry3d-f64/serde-serialize",
     "dep:serde",
     "bit-vec/serde",
+    "vec_map/serde"
 ]
 enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"]
 debug-render = []

crates/rapier3d/Cargo.toml

@@ -48,6 +48,7 @@ serde-serialize = [
     "parry3d/serde-serialize",
     "dep:serde",
     "bit-vec/serde",
+    "vec_map/serde"
 ]
 enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"]
 debug-render = []

examples2d/one_way_platforms2.rs

@@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) {
         }
 
         for handle in physics.islands.active_bodies() {
-            let body = &mut physics.bodies[*handle];
+            let body = &mut physics.bodies[handle];
             if body.position().translation.y > 1.0 {
                 body.set_gravity_scale(1.0, false);
             } else if body.position().translation.y < -1.0 {

examples3d/one_way_platforms3.rs

@@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) {
         }
 
         for handle in physics.islands.active_bodies() {
-            let body = physics.bodies.get_mut(*handle).unwrap();
+            let body = physics.bodies.get_mut(handle).unwrap();
             if body.position().translation.y > 1.0 {
                 body.set_gravity_scale(1.0, false);
             } else if body.position().translation.y < -1.0 {

src/counters/collision_detection_counters.rs

@@ -8,6 +8,9 @@ pub struct CollisionDetectionCounters {
     pub ncontact_pairs: usize,
     /// Time spent for the broad-phase of the collision detection.
     pub broad_phase_time: Timer,
+    /// Time spent by the final broad-phase AABB update after body movement to keep
+    /// user scene queries valid.
+    pub final_broad_phase_time: Timer,
     /// Time spent for the narrow-phase of the collision detection.
     pub narrow_phase_time: Timer,
 }
@@ -18,6 +21,7 @@ impl CollisionDetectionCounters {
         CollisionDetectionCounters {
             ncontact_pairs: 0,
             broad_phase_time: Timer::new(),
+            final_broad_phase_time: Timer::new(),
             narrow_phase_time: Timer::new(),
         }
     }
@@ -26,6 +30,7 @@ impl CollisionDetectionCounters {
     pub fn reset(&mut self) {
         self.ncontact_pairs = 0;
         self.broad_phase_time.reset();
+        self.final_broad_phase_time.reset();
         self.narrow_phase_time.reset();
     }
 }
@@ -34,6 +39,7 @@ impl Display for CollisionDetectionCounters {
     fn fmt(&self, f: &mut Formatter) -> Result {
         writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?;
         writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
+        writeln!(f, "Final broad-phase time: {}", self.final_broad_phase_time)?;
         writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
     }
 }

src/counters/solver_counters.rs

@@ -12,6 +12,10 @@ pub struct SolverCounters {
     pub velocity_resolution_time: Timer,
     /// Time spent for the assembly of all the velocity constraints.
     pub velocity_assembly_time: Timer,
+    /// Time spent by the velocity assembly for initializing solver bodies.
+    pub velocity_assembly_time_solver_bodies: Timer,
+    /// Time spent by the velocity assemble for initializing the constraints.
+    pub velocity_assembly_time_constraints_init: Timer,
     /// Time spent for the update of the velocity of the bodies.
     pub velocity_update_time: Timer,
     /// Time spent to write force back to user-accessible data.
@@ -25,6 +29,8 @@ impl SolverCounters {
             nconstraints: 0,
             ncontacts: 0,
             velocity_assembly_time: Timer::new(),
+            velocity_assembly_time_solver_bodies: Timer::new(),
+            velocity_assembly_time_constraints_init: Timer::new(),
             velocity_resolution_time: Timer::new(),
             velocity_update_time: Timer::new(),
             velocity_writeback_time: Timer::new(),
@@ -37,6 +43,8 @@ impl SolverCounters {
         self.ncontacts = 0;
         self.velocity_resolution_time.reset();
         self.velocity_assembly_time.reset();
+        self.velocity_assembly_time_solver_bodies.reset();
+        self.velocity_assembly_time_constraints_init.reset();
         self.velocity_update_time.reset();
         self.velocity_writeback_time.reset();
     }

src/counters/stages_counters.rs

@@ -10,6 +10,8 @@ pub struct StagesCounters {
     pub collision_detection_time: Timer,
     /// Time spent for the computation of collision island and body activation/deactivation (sleeping).
     pub island_construction_time: Timer,
+    /// Time spent for collecting awake constraints from islands.
+    pub island_constraints_collection_time: Timer,
     /// Total time spent for the constraints resolution and position update.t
     pub solver_time: Timer,
     /// Total time spent for CCD and CCD resolution.
@@ -25,6 +27,7 @@ impl StagesCounters {
             update_time: Timer::new(),
             collision_detection_time: Timer::new(),
             island_construction_time: Timer::new(),
+            island_constraints_collection_time: Timer::new(),
             solver_time: Timer::new(),
             ccd_time: Timer::new(),
             user_changes: Timer::new(),
@@ -36,6 +39,7 @@ impl StagesCounters {
         self.update_time.reset();
         self.collision_detection_time.reset();
         self.island_construction_time.reset();
+        self.island_constraints_collection_time.reset();
         self.solver_time.reset();
         self.ccd_time.reset();
         self.user_changes.reset();
@@ -55,6 +59,11 @@ impl Display for StagesCounters {
             "Island construction time: {}",
             self.island_construction_time
         )?;
+        writeln!(
+            f,
+            "Island construction time: {}",
+            self.island_constraints_collection_time
+        )?;
         writeln!(f, "Solver time: {}", self.solver_time)?;
         writeln!(f, "CCD time: {}", self.ccd_time)?;
         writeln!(f, "User changes time: {}", self.user_changes)

src/dynamics/ccd/ccd_solver.rs

@@ -88,7 +88,7 @@ impl CCDSolver {
 
         // println!("Checking CCD activation");
         for handle in islands.active_bodies() {
-            let rb = bodies.index_mut_internal(*handle);
+            let rb = bodies.index_mut_internal(handle);
 
             if rb.ccd.ccd_enabled {
                 let forces = if include_forces {
@@ -143,7 +143,7 @@ impl CCDSolver {
         let mut min_toi = dt;
 
         for handle in islands.active_bodies() {
-            let rb1 = &bodies[*handle];
+            let rb1 = &bodies[handle];
 
             if rb1.ccd.ccd_active {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
@@ -276,7 +276,7 @@ impl CCDSolver {
          */
         // TODO: don't iterate through all the colliders.
         for handle in islands.active_bodies() {
-            let rb1 = &bodies[*handle];
+            let rb1 = &bodies[handle];
 
             if rb1.ccd.ccd_active {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(