Split rigid-bodies and colliders into multiple components

1 files changed, 68 insertions, 31 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index 635e0b1..56e19fc 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -13,7 +13,11 @@ use super::{
     WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
     WPrismaticPositionGroundConstraint,
 };
-use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::{
+    IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+    RigidBodyType,
+};
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
 use crate::math::{Isometry, Real};
@@ -56,9 +60,12 @@ pub(crate) enum AnyJointPositionConstraint {
 }
 
 impl AnyJointPositionConstraint {
-    pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
+    pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
+    {
+        let rb1 = bodies.index_bundle(joint.body1.0);
+        let rb2 = bodies.index_bundle(joint.body2.0);
 
         match &joint.params {
             JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
@@ -81,40 +88,47 @@ impl AnyJointPositionConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
-        let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
-        let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+    pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
+    {
+        let rbs1 = (
+            gather![|ii| bodies.index(joints[ii].body1.0)],
+            gather![|ii| bodies.index(joints[ii].body1.0)],
+        );
+        let rbs2 = (
+            gather![|ii| bodies.index(joints[ii].body2.0)],
+            gather![|ii| bodies.index(joints[ii].body2.0)],
+        );
 
         match &joints[0].params {
             JointParams::BallJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
                 AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
                     rbs1, rbs2, joints,
                 ))
             }
             JointParams::FixedJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
                 AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
                     rbs1, rbs2, joints,
                 ))
             }
             // JointParams::GenericJoint(_) => {
-            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
             //     AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
             //         rbs1, rbs2, joints,
             //     ))
             // }
             JointParams::PrismaticJoint(_) => {
-                let joints =
-                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
                 AnyJointPositionConstraint::WPrismaticJoint(
                     WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
                 )
             }
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(_) => {
-                let joints =
-                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
                 AnyJointPositionConstraint::WRevoluteJoint(
                     WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
                 )
@@ -122,15 +136,26 @@ impl AnyJointPositionConstraint {
         }
     }
 
-    pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
-        let mut rb1 = &bodies[joint.body1];
-        let mut rb2 = &bodies[joint.body2];
-        let flipped = !rb2.is_dynamic();
+    pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let mut handle1 = joint.body1;
+        let mut handle2 = joint.body2;
+
+        let status2: &RigidBodyType = bodies.index(handle2.0);
+        let flipped = !status2.is_dynamic();
 
         if flipped {
-            std::mem::swap(&mut rb1, &mut rb2);
+            std::mem::swap(&mut handle1, &mut handle2);
         }
 
+        let rb1 = bodies.index(handle1.0);
+        let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
+
         match &joint.params {
             JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
                 BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -154,48 +179,60 @@ impl AnyJointPositionConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
-        let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
-        let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+    pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let mut handles1 = gather![|ii| joints[ii].body1];
+        let mut handles2 = gather![|ii| joints[ii].body2];
+        let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
+
         let mut flipped = [false; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
-            if !rbs2[ii].is_dynamic() {
-                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+            if !status2[ii].is_dynamic() {
+                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
                 flipped[ii] = true;
             }
         }
 
+        let rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
+        let rbs2 = (
+            gather![|ii| bodies.index(handles2[ii].0)],
+            gather![|ii| bodies.index(handles2[ii].0)],
+        );
+
         match &joints[0].params {
             JointParams::BallJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
                 AnyJointPositionConstraint::WBallGroundConstraint(
                     WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
             JointParams::FixedJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
                 AnyJointPositionConstraint::WFixedGroundConstraint(
                     WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
             // JointParams::GenericJoint(_) => {
-            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
             //     AnyJointPositionConstraint::WGenericGroundConstraint(
             //         WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
             //     )
             // }
             JointParams::PrismaticJoint(_) => {
-                let joints =
-                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
                 AnyJointPositionConstraint::WPrismaticGroundConstraint(
                     WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(_) => {
-                let joints =
-                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
                 AnyJointPositionConstraint::WRevoluteGroundConstraint(
                     WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )