Split rigid-bodies and colliders into multiple components

1 files changed, 19 insertions, 12 deletions

diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index 2fa4aee..48b71aa 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -1,5 +1,7 @@
 use super::AnyJointPositionConstraint;
-use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
+use crate::data::{ComponentSet, ComponentSetMut};
+use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters};
+use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition};
 use crate::math::{Isometry, Real};
 
 pub(crate) struct PositionSolver {
@@ -13,25 +15,28 @@ impl PositionSolver {
         }
     }
 
-    pub fn solve(
+    pub fn solve<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         contact_constraints: &[AnyPositionConstraint],
         joint_constraints: &[AnyJointPositionConstraint],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         if contact_constraints.is_empty() && joint_constraints.is_empty() {
             // Nothing to do.
             return;
         }
 
         self.positions.clear();
-        self.positions.extend(
-            bodies
-                .iter_active_island(island_id)
-                .map(|(_, b)| b.next_position),
-        );
+        self.positions
+            .extend(islands.active_island(island_id).iter().map(|h| {
+                let poss: &RigidBodyPosition = bodies.index(h.0);
+                poss.next_position
+            }));
 
         for _ in 0..params.max_position_iterations {
             for constraint in joint_constraints {
@@ -43,8 +48,10 @@ impl PositionSolver {
             }
         }
 
-        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_next_position(self.positions[rb.active_set_offset])
-        });
+        for handle in islands.active_island(island_id) {
+            let ids: &RigidBodyIds = bodies.index(handle.0);
+            let next_pos = &self.positions[ids.active_set_offset];
+            bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos);
+        }
     }
 }