Experiment with incremental island having only one awake island.

4 files changed, 7 insertions, 10 deletions

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;