Merge pull request #183 from dimforge/bundles

Make Rapier accept any kind of data storage instead of RigidBodySet/ColliderSet

1 files changed, 100 insertions, 39 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index ed27e51..d723387 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -16,9 +16,11 @@ use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
 // use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
 //     GenericVelocityConstraint, GenericVelocityGroundConstraint,
 // };
+use crate::data::{BundleSet, ComponentSet};
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
+    IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 use crate::math::Real;
 #[cfg(feature = "simd-is-enabled")]
@@ -69,14 +71,30 @@ impl AnyJointVelocityConstraint {
         1
     }
 
-    pub fn from_joint(
+    pub fn from_joint<Bodies>(
         params: &IntegrationParameters,
         joint_id: JointIndex,
         joint: &Joint,
-        bodies: &RigidBodySet,
-    ) -> Self {
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
+        bodies: &Bodies,
+    ) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let rb1 = (
+            bodies.index(joint.body1.0),
+            bodies.index(joint.body1.0),
+            bodies.index(joint.body1.0),
+            bodies.index(joint.body1.0),
+        );
+        let rb2 = (
+            bodies.index(joint.body2.0),
+            bodies.index(joint.body2.0),
+            bodies.index(joint.body2.0),
+            bodies.index(joint.body2.0),
+        );
 
         match &joint.params {
             JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
@@ -99,45 +117,59 @@ impl AnyJointVelocityConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint(
+    pub fn from_wide_joint<Bodies>(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
         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];
+        bodies: &Bodies,
+    ) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let rbs1 = (
+            gather![|ii| bodies.index(joints[ii].body1.0)],
+            gather![|ii| bodies.index(joints[ii].body1.0)],
+            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)],
+            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()];
                 AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
                     params, joint_id, 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()];
                 AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
                     params, joint_id, 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()];
             //     AnyJointVelocityConstraint::WGenericConstraint(
             //         WGenericVelocityConstraint::from_params(params, joint_id, 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()];
                 AnyJointVelocityConstraint::WPrismaticConstraint(
                     WPrismaticVelocityConstraint::from_params(params, joint_id, 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()];
                 AnyJointVelocityConstraint::WRevoluteConstraint(
                     WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
                 )
@@ -145,20 +177,31 @@ impl AnyJointVelocityConstraint {
         }
     }
 
-    pub fn from_joint_ground(
+    pub fn from_joint_ground<Bodies>(
         params: &IntegrationParameters,
         joint_id: JointIndex,
         joint: &Joint,
-        bodies: &RigidBodySet,
-    ) -> Self {
-        let mut rb1 = &bodies[joint.body1];
-        let mut rb2 = &bodies[joint.body2];
-        let flipped = !rb2.is_dynamic();
+        bodies: &Bodies,
+    ) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyVelocity>
+            + 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_bundle(handle1.0);
+        let rb2 = bodies.index_bundle(handle2.0);
+
         match &joint.params {
             JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
                 BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
@@ -186,26 +229,46 @@ impl AnyJointVelocityConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground(
+    pub fn from_wide_joint_ground<Bodies>(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
         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];
+        bodies: &Bodies,
+    ) -> Self
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyVelocity>
+            + 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)],
+            gather![|ii| bodies.index(handles1[ii].0)],
+            gather![|ii| bodies.index(handles1[ii].0)],
+        );
+        let rbs2 = (
+            gather![|ii| bodies.index(handles2[ii].0)],
+            gather![|ii| bodies.index(handles2[ii].0)],
+            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()];
                 AnyJointVelocityConstraint::WBallGroundConstraint(
                     WBallVelocityGroundConstraint::from_params(
                         params, joint_id, rbs1, rbs2, joints, flipped,
@@ -213,7 +276,7 @@ impl AnyJointVelocityConstraint {
                 )
             }
             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()];
                 AnyJointVelocityConstraint::WFixedGroundConstraint(
                     WFixedVelocityGroundConstraint::from_params(
                         params, joint_id, rbs1, rbs2, joints, flipped,
@@ -221,7 +284,7 @@ impl AnyJointVelocityConstraint {
                 )
             }
             // 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()];
             //     AnyJointVelocityConstraint::WGenericGroundConstraint(
             //         WGenericVelocityGroundConstraint::from_params(
             //             params, joint_id, rbs1, rbs2, joints, flipped,
@@ -229,8 +292,7 @@ impl AnyJointVelocityConstraint {
             //     )
             // }
             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()];
                 AnyJointVelocityConstraint::WPrismaticGroundConstraint(
                     WPrismaticVelocityGroundConstraint::from_params(
                         params, joint_id, rbs1, rbs2, joints, flipped,
@@ -239,8 +301,7 @@ impl AnyJointVelocityConstraint {
             }
             #[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()];
                 AnyJointVelocityConstraint::WRevoluteGroundConstraint(
                     WRevoluteVelocityGroundConstraint::from_params(
                         params, joint_id, rbs1, rbs2, joints, flipped,