Split rigid-bodies and colliders into multiple components

1 files changed, 73 insertions, 19 deletions

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index c684cc5..1e65341 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,10 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
     AnyVelocityConstraint, SolverConstraints,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+};
+use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
@@ -24,17 +29,21 @@ impl IslandSolver {
         }
     }
 
-    pub fn solve_position_constraints(
+    pub fn solve_position_constraints<Bodies>(
         &mut self,
         island_id: usize,
+        islands: &IslandManager,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         counters.solver.position_resolution_time.resume();
         self.position_solver.solve(
             island_id,
             params,
+            islands,
             bodies,
             &self.contact_constraints.position_constraints,
             &self.joint_constraints.position_constraints,
@@ -42,31 +51,47 @@ impl IslandSolver {
         counters.solver.position_resolution_time.pause();
     }
 
-    pub fn init_constraints_and_solve_velocity_constraints(
+    pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
         &mut self,
         island_id: usize,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
         joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyForces>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
         let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
 
         if has_constraints {
             counters.solver.velocity_assembly_time.resume();
-            self.contact_constraints
-                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.contact_constraints.init(
+                island_id,
+                params,
+                islands,
+                bodies,
+                manifolds,
+                manifold_indices,
+            );
             self.joint_constraints
-                .init(island_id, params, bodies, joints, joint_indices);
+                .init(island_id, params, islands, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
             self.velocity_solver.solve(
                 island_id,
                 params,
+                islands,
                 bodies,
                 manifolds,
                 joints,
@@ -76,21 +101,50 @@ impl IslandSolver {
             counters.solver.velocity_resolution_time.pause();
 
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+
+            for handle in islands.active_island(island_id) {
+                let (poss, vels, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = vels.apply_damping(params.dt, damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
+
             counters.solver.velocity_update_time.pause();
         } else {
             self.contact_constraints.clear();
             self.joint_constraints.clear();
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+
+            for handle in islands.active_island(island_id) {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
-                rb.integrate_accelerations(params.dt);
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+                let (poss, vels, forces, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyForces,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = forces
+                    .integrate(params.dt, vels, mprops)
+                    .apply_damping(params.dt, &damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
             counters.solver.velocity_update_time.pause();
         }
     }