Split rigid-bodies and colliders into multiple components

1 files changed, 27 insertions, 7 deletions

diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
index e2d140d..5881cb3 100644
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
@@ -1,5 +1,7 @@
 use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
-use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
+use crate::dynamics::{
+    IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 use crate::math::{Isometry, Real, SIMD_WIDTH};
 
 // TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WRevolutePositionConstraint {
 
 impl WRevolutePositionConstraint {
     pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
+        rbs2: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&RevoluteJoint; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| RevolutePositionConstraint::from_params(
+                (rbs1.0[ii], rbs1.1[ii]),
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii]
+            )],
         }
     }
 
@@ -33,13 +45,21 @@ pub(crate) struct WRevolutePositionGroundConstraint {
 
 impl WRevolutePositionGroundConstraint {
     pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
+        rbs2: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&RevoluteJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
+                rbs1[ii],
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii],
+                flipped[ii]
+            )],
         }
     }