feat: implement new "small-steps" solver + joint improvements

1 files changed, 27 insertions, 3 deletions

diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index ef5d50a..4d14147 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -4,7 +4,7 @@ use crate::dynamics::{
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
-use crate::utils::WDot;
+use crate::utils::SimdDot;
 
 /// Structure responsible for maintaining the set of active rigid-bodies, and
 /// putting non-moving rigid-bodies to sleep to save computation times.
@@ -14,6 +14,7 @@ pub struct IslandManager {
     pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
     pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
     pub(crate) active_islands: Vec<usize>,
+    pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
     active_set_timestamp: u32,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     can_sleep: Vec<RigidBodyHandle>, // Workspace.
@@ -28,6 +29,7 @@ impl IslandManager {
             active_dynamic_set: vec![],
             active_kinematic_set: vec![],
             active_islands: vec![],
+            active_islands_additional_solver_iterations: vec![],
             active_set_timestamp: 0,
             can_sleep: vec![],
             stack: vec![],
@@ -127,6 +129,10 @@ impl IslandManager {
         &self.active_dynamic_set[island_range]
     }
 
+    pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
+        self.active_islands_additional_solver_iterations[island_id]
+    }
+
     #[inline(always)]
     pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
         self.active_dynamic_set
@@ -232,12 +238,21 @@ impl IslandManager {
         //        let t = instant::now();
         // Propagation of awake state and awake island computation through the
         // traversal of the interaction graph.
+        self.active_islands_additional_solver_iterations.clear();
         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;
 
+        // NOTE: islands containing a body with non-standard number of iterations won’t
+        //       be merged with another island, unless another island with standard
+        //       iterations number already started before and got continued due to the
+        //       `min_island_size`. That could be avoided by pushing bodies with non-standard
+        //       iterations on top of the stack (and other bodies on the back). Not sure it’s
+        //       worth it though.
+        let mut additional_solver_iterations = 0;
+
         while let Some(handle) = self.stack.pop() {
             let rb = bodies.index_mut_internal(handle);
 
@@ -248,16 +263,23 @@ impl IslandManager {
             }
 
             if self.stack.len() < island_marker {
-                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
-                    >= min_island_size
+                if additional_solver_iterations != rb.additional_solver_iterations
+                    || self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                        >= min_island_size
                 {
                     // We are starting a new island.
+                    self.active_islands_additional_solver_iterations
+                        .push(additional_solver_iterations);
                     self.active_islands.push(self.active_dynamic_set.len());
+                    additional_solver_iterations = 0;
                 }
 
                 island_marker = self.stack.len();
             }
 
+            additional_solver_iterations =
+                additional_solver_iterations.max(rb.additional_solver_iterations);
+
             // Transmit the active state to all the rigid-bodies with colliders
             // in contact or joined with this collider.
             push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
@@ -281,6 +303,8 @@ impl IslandManager {
             self.active_dynamic_set.push(handle);
         }
 
+        self.active_islands_additional_solver_iterations
+            .push(additional_solver_iterations);
         self.active_islands.push(self.active_dynamic_set.len());
         //        println!(
         //            "Extraction: {}, num islands: {}",