Split rigid-bodies and colliders into multiple components

1 files changed, 22 insertions, 17 deletions

diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index add5f2c..115b099 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -1,9 +1,10 @@
 use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
+use crate::data::ComponentSet;
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
     AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real};
 use crate::utils::WAngularInertia;
@@ -150,13 +151,15 @@ impl ParallelIslandSolver {
         }
     }
 
-    pub fn solve_position_constraints<'s>(
+    pub fn solve_position_constraints<'s, Bodies>(
         &'s mut self,
         scope: &Scope<'s>,
         island_id: usize,
         params: &'s IntegrationParameters,
-        bodies: &'s mut RigidBodySet,
-    ) {
+        bodies: &'s mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBody>,
+    {
         let num_threads = rayon::current_num_threads();
         let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -179,7 +182,7 @@ impl ParallelIslandSolver {
                 // Transmute *mut -> &mut
                 let positions: &mut Vec<Isometry<Real>> =
                     unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
-                let bodies: &mut RigidBodySet =
+                let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                 let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
                     std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
@@ -197,8 +200,8 @@ impl ParallelIslandSolver {
 
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
+                        let rb = &mut bodies.index(handle.0);
                         positions[rb.active_set_offset] = rb.next_position;
                     }
                 }
@@ -216,8 +219,8 @@ impl ParallelIslandSolver {
                 // Write results back to rigid bodies.
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.position_writeback_index] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.position_writeback_index) {
+                        let rb = &mut bodies.index(handle.0);
                         rb.set_next_position(positions[rb.active_set_offset]);
                     }
                 }
@@ -225,17 +228,19 @@ impl ParallelIslandSolver {
         }
     }
 
-    pub fn init_constraints_and_solve_velocity_constraints<'s>(
+    pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>(
         &'s mut self,
         scope: &Scope<'s>,
         island_id: usize,
         params: &'s IntegrationParameters,
-        bodies: &'s mut RigidBodySet,
+        bodies: &'s mut Bodies,
         manifolds: &'s mut Vec<&'s mut ContactManifold>,
         manifold_indices: &'s [ContactManifoldIndex],
         joints: &'s mut Vec<JointGraphEdge>,
         joint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBody>,
+    {
         let num_threads = rayon::current_num_threads();
         let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -280,7 +285,7 @@ impl ParallelIslandSolver {
                 // Transmute *mut -> &mut
                 let mj_lambdas: &mut Vec<DeltaVel<Real>> =
                     unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
-                let bodies: &mut RigidBodySet =
+                let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                 let manifolds: &mut Vec<&mut ContactManifold> =
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -303,8 +308,8 @@ impl ParallelIslandSolver {
 
                     concurrent_loop! {
                         let batch_size = thread.batch_size;
-                        for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
-                            let rb = &mut bodies[handle.0];
+                        for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) {
+                            let rb = &mut bodies.index(handle.0);
                             let dvel = &mut mj_lambdas[rb.active_set_offset];
 
                             // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
@@ -348,8 +353,8 @@ impl ParallelIslandSolver {
 
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
+                        let rb = &mut bodies.index(handle.0);
                         let dvel = mj_lambdas[rb.active_set_offset];
                         rb.linvel += dvel.linear;
                         rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);