Split rigid-bodies and colliders into multiple components

22 files changed, 1318 insertions, 914 deletions

diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 93996f4..159cc77 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -1,4 +1,6 @@
-use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
@@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint {
 }
 
 impl BallPositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
+    pub fn from_params(
+        rb1: (&RigidBodyMassProps, &RigidBodyIds),
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
+        cparams: &BallJoint,
+    ) -> Self {
+        let (mprops1, ids1) = rb1;
+        let (mprops2, ids2) = rb2;
+
         Self {
-            local_com1: rb1.mass_properties.local_com,
-            local_com2: rb2.mass_properties.local_com,
-            im1: rb1.effective_inv_mass,
-            im2: rb2.effective_inv_mass,
-            ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
-            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com1: mprops1.mass_properties.local_com,
+            local_com2: mprops2.mass_properties.local_com,
+            im1: mprops1.effective_inv_mass,
+            im2: mprops2.effective_inv_mass,
+            ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
+            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: ids1.active_set_offset,
+            position2: ids2.active_set_offset,
         }
     }
 
@@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint {
 
 impl BallPositionGroundConstraint {
     pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: &RigidBodyPosition,
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
         cparams: &BallJoint,
         flipped: bool,
     ) -> Self {
+        let poss1 = rb1;
+        let (mprops2, ids2) = rb2;
+
         if flipped {
             // Note the only thing that is flipped here
             // are the local_anchors. The rb1 and rb2 have
             // already been flipped by the caller.
             Self {
-                anchor1: rb1.next_position * cparams.local_anchor2,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                anchor1: poss1.next_position * cparams.local_anchor2,
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
-                position2: rb2.active_set_offset,
-                local_com2: rb2.mass_properties.local_com,
+                position2: ids2.active_set_offset,
+                local_com2: mprops2.mass_properties.local_com,
             }
         } else {
             Self {
-                anchor1: rb1.next_position * cparams.local_anchor1,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                anchor1: poss1.next_position * cparams.local_anchor1,
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
-                position2: rb2.active_set_offset,
-                local_com2: rb2.mass_properties.local_com,
+                position2: ids2.active_set_offset,
+                local_com2: mprops2.mass_properties.local_com,
             }
         }
     }
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
index e9162a4..6b00639 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
@@ -1,4 +1,6 @@
-use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
@@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint {
 
 impl WBallPositionConstraint {
     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: [&BallJoint; SIMD_WIDTH],
     ) -> Self {
-        let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
-        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii1 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        )
+        let (mprops1, ids1) = rbs1;
+        let (mprops2, ids2) = rbs2;
+
+        let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]);
+        let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
+        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let ii1 = AngularInertia::<SimdReal>::from(gather![
+            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
+        ])
         .squared();
-        let ii2 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        )
+        let ii2 = AngularInertia::<SimdReal>::from(gather![
+            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
+        ])
         .squared();
-        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
-        let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
-        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
+        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
+        let position1 = gather![|ii| ids1[ii].active_set_offset];
+        let position2 = gather![|ii| ids2[ii].active_set_offset];
 
         Self {
             local_com1,
@@ -61,8 +72,8 @@ impl WBallPositionConstraint {
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
-        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+        let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
+        let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
 
         let anchor1 = position1 * self.local_anchor1;
         let anchor2 = position2 * self.local_anchor2;
@@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint {
 
 impl WBallPositionGroundConstraint {
     pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
+        rbs2: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&BallJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
-        let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
+        let poss1 = rbs1;
+        let (mprops2, ids2) = rbs2;
+
+        let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
         let anchor1 = position1
-            * Point::from(array![|ii| if flipped[ii] {
+            * Point::from(gather![|ii| if flipped[ii] {
                 cparams[ii].local_anchor2
             } else {
                 cparams[ii].local_anchor1
-            }; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii2 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        )
+            }]);
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let ii2 = AngularInertia::<SimdReal>::from(gather![
+            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
+        ])
         .squared();
-        let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
+        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
             cparams[ii].local_anchor1
         } else {
             cparams[ii].local_anchor2
-        }; SIMD_WIDTH]);
-        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
-        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
+        }]);
+        let position2 = gather![|ii| ids2[ii].active_set_offset];
+        let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
 
         Self {
             anchor1,
@@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint {
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+        let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
 
         let anchor2 = position2 * self.local_anchor2;
         let com2 = position2 * self.local_com2;
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index 508ac65..9e5227e 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -1,6 +1,7 @@
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -36,19 +37,32 @@ impl BallVelocityConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &BallJoint,
     ) -> Self {
-        let anchor_world1 = rb1.position * joint.local_anchor1;
-        let anchor_world2 = rb2.position * joint.local_anchor2;
-        let anchor1 = anchor_world1 - rb1.world_com;
-        let anchor2 = anchor_world2 - rb2.world_com;
+        let (poss1, vels1, mprops1, ids1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
 
-        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
-        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
+        let anchor_world1 = poss1.position * joint.local_anchor1;
+        let anchor_world2 = poss2.position * joint.local_anchor2;
+        let anchor1 = anchor_world1 - mprops1.world_com;
+        let anchor2 = anchor_world2 - mprops2.world_com;
+
+        let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
+        let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
 
         let rhs = (vel2 - vel1) * params.velocity_solve_fraction
             + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -59,12 +73,12 @@ impl BallVelocityConstraint {
 
         #[cfg(feature = "dim3")]
         {
-            lhs = rb2
+            lhs = mprops2
                 .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2)
-                + rb1
+                + mprops1
                     .effective_world_inv_inertia_sqrt
                     .squared()
                     .quadform(&cmat1)
@@ -75,8 +89,8 @@ impl BallVelocityConstraint {
         // it's just easier that way.
         #[cfg(feature = "dim2")]
         {
-            let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+            let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+            let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
             let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -100,8 +114,8 @@ impl BallVelocityConstraint {
             );
 
             if stiffness != 0.0 {
-                let dpos = rb2.position.rotation
-                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                let dpos = poss2.position.rotation
+                    * (poss1.position.rotation * joint.motor_target_pos).inverse();
                 #[cfg(feature = "dim2")]
                 {
                     motor_rhs += dpos.angle() * stiffness;
@@ -113,15 +127,15 @@ impl BallVelocityConstraint {
             }
 
             if damping != 0.0 {
-                let curr_vel = rb2.angvel - rb1.angvel;
+                let curr_vel = vels2.angvel - vels1.angvel;
                 motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
             }
 
             #[cfg(feature = "dim2")]
             if stiffness != 0.0 || damping != 0.0 {
                 motor_inv_lhs = if keep_lhs {
-                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
                     Some(gamma / (ii1 + ii2))
                 } else {
                     Some(gamma)
@@ -132,8 +146,8 @@ impl BallVelocityConstraint {
             #[cfg(feature = "dim3")]
             if stiffness != 0.0 || damping != 0.0 {
                 motor_inv_lhs = if keep_lhs {
-                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
                     Some((ii1 + ii2).inverse_unchecked() * gamma)
                 } else {
                     Some(SdpMatrix::diagonal(gamma))
@@ -151,8 +165,8 @@ impl BallVelocityConstraint {
 
         BallVelocityConstraint {
             joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda1: ids1.active_set_offset,
+            mj_lambda2: ids2.active_set_offset,
             im1,
             im2,
             impulse: joint.impulse * params.warmstart_coeff,
@@ -164,8 +178,8 @@ impl BallVelocityConstraint {
             motor_impulse,
             motor_inv_lhs,
             motor_max_impulse: joint.motor_max_impulse,
-            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
         }
     }
 
@@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &BallJoint,
         flipped: bool,
     ) -> Self {
+        let (poss1, vels1, mprops1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         let (anchor_world1, anchor_world2) = if flipped {
             (
-                rb1.position * joint.local_anchor2,
-                rb2.position * joint.local_anchor1,
+                poss1.position * joint.local_anchor2,
+                poss2.position * joint.local_anchor1,
             )
         } else {
             (
-                rb1.position * joint.local_anchor1,
-                rb2.position * joint.local_anchor2,
+                poss1.position * joint.local_anchor1,
+                poss2.position * joint.local_anchor2,
             )
         };
 
-        let anchor1 = anchor_world1 - rb1.world_com;
-        let anchor2 = anchor_world2 - rb2.world_com;
+        let anchor1 = anchor_world1 - mprops1.world_com;
+        let anchor2 = anchor_world2 - mprops2.world_com;
 
-        let im2 = rb2.effective_inv_mass;
-        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
-        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
+        let im2 = mprops2.effective_inv_mass;
+        let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
+        let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
 
         let rhs = (vel2 - vel1) * params.velocity_solve_fraction
             + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint {
 
         #[cfg(feature = "dim3")]
         {
-            lhs = rb2
+            lhs = mprops2
                 .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
@@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint {
 
         #[cfg(feature = "dim2")]
         {
-            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+            let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im2 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat2.x * cmat2.y * ii2;
             let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint {
             );
 
             if stiffness != 0.0 {
-                let dpos = rb2.position.rotation
-                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                let dpos = poss2.position.rotation
+                    * (poss1.position.rotation * joint.motor_target_pos).inverse();
                 #[cfg(feature = "dim2")]
                 {
                     motor_rhs += dpos.angle() * stiffness;
@@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint {
             }
 
             if damping != 0.0 {
-                let curr_vel = rb2.angvel - rb1.angvel;
+                let curr_vel = vels2.angvel - vels1.angvel;
                 motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
             }
 
             #[cfg(feature = "dim2")]
             if stiffness != 0.0 || damping != 0.0 {
                 motor_inv_lhs = if keep_lhs {
-                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
                     Some(gamma / ii2)
                 } else {
                     Some(gamma)
@@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint {
             #[cfg(feature = "dim3")]
             if stiffness != 0.0 || damping != 0.0 {
                 motor_inv_lhs = if keep_lhs {
-                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
                     Some(ii2.inverse_unchecked() * gamma)
                 } else {
                     Some(SdpMatrix::diagonal(gamma))
@@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint {
 
         BallVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: ids2.active_set_offset,
             im2,
             impulse: joint.impulse * params.warmstart_coeff,
             r2: anchor2,
@@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint {
             motor_impulse,
             motor_inv_lhs,
             motor_max_impulse: joint.motor_max_impulse,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
         }
     }
 
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
index ff5e001..ee465cd 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
@@ -1,6 +1,7 @@
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
@@ -34,33 +35,46 @@ impl WBallVelocityConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
+        rbs2: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&BallJoint; SIMD_WIDTH],
     ) -> Self {
-        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
-        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
-        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
-        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
-
-        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
-        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
-        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
-        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
-
-        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
-        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let (poss1, vels1, mprops1, ids1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
+
+        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
+        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
+        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
+        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
+        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
+            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
+        ]);
+        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
+
+        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
+        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
+        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
+        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
+            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
+        ]);
+        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
+
+        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
+        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
+        let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor_world1 = position1 * local_anchor1;
         let anchor_world2 = position2 * local_anchor2;
@@ -114,20 +128,16 @@ impl WBallVelocityConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
-            ),
+            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
+            ]),
         };
         let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
+            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+            ]),
         };
 
         mj_lambda1.linear += self.impulse * self.im1;
@@ -147,20 +157,16 @@ impl WBallVelocityConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
-            ),
+            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
+            ]),
         };
         let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
+            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+            ]),
         };
 
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+        ),
+        rbs2: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&BallJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
-        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
-        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
-        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
-        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
-