Split rigid-bodies and colliders into multiple components

38 files changed, 1984 insertions, 1331 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index e3327c6..366a5b3 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -1,8 +1,9 @@
-use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
+use crate::data::ComponentSet;
+use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub(crate) fn categorize_contacts(
-    _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
+    _bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
@@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts(
 }
 
 pub(crate) fn categorize_joints(
-    bodies: &RigidBodySet,
+    bodies: &impl ComponentSet<RigidBodyType>,
     joints: &[JointGraphEdge],
     joint_indices: &[JointIndex],
     ground_joints: &mut Vec<JointIndex>,
@@ -28,10 +29,10 @@ pub(crate) fn categorize_joints(
 ) {
     for joint_i in joint_indices {
         let joint = &joints[*joint_i].weight;
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
+        let status1 = bodies.index(joint.body1.0);
+        let status2 = bodies.index(joint.body2.0);
 
-        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+        if !status1.is_dynamic() || !status2.is_dynamic() {
             ground_joints.push(*joint_i);
         } else {
             nonground_joints.push(*joint_i);
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 6b8de5a..86f9cff 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -1,4 +1,8 @@
-use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet};
+#[cfg(feature = "parallel")]
+use crate::dynamics::BodyPair;
+use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyType};
+use crate::dynamics::{JointGraphEdge, JointIndex};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use {
@@ -6,16 +10,19 @@ use {
     vec_map::VecMap,
 };
 
+#[cfg(feature = "parallel")]
 pub(crate) trait PairInteraction {
     fn body_pair(&self) -> BodyPair;
 }
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for &'a mut ContactManifold {
     fn body_pair(&self) -> BodyPair {
         self.data.body_pair
     }
 }
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for JointGraphEdge {
     fn body_pair(&self) -> BodyPair {
         BodyPair::new(self.weight.body1, self.weight.body2)
@@ -54,7 +61,7 @@ impl ParallelInteractionGroups {
     pub fn group_interactions<Interaction: PairInteraction>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        bodies: &impl ComponentSet<RigidBody>,
         interactions: &[Interaction],
         interaction_indices: &[usize],
     ) {
@@ -78,8 +85,8 @@ impl ParallelInteractionGroups {
             .zip(self.interaction_colors.iter_mut())
         {
             let body_pair = interactions[*interaction_id].body_pair();
-            let rb1 = &bodies[body_pair.body1];
-            let rb2 = &bodies[body_pair.body2];
+            let rb1 = bodies.index(body_pair.body1);
+            let rb2 = bodies.index(body_pair.body2);
 
             match (rb1.is_static(), rb2.is_static()) {
                 (false, false) => {
@@ -168,14 +175,15 @@ impl InteractionGroups {
         self.nongrouped_interactions.clear();
     }
 
-    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // TODO: there is a lot of duplicated code with group_manifolds here.
     // But we don't refactor just now because we may end up with distinct
     // grouping strategies in the future.
     #[cfg(not(feature = "simd-is-enabled"))]
     pub fn group_joints(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
     ) {
@@ -184,13 +192,16 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_joints(
+    pub fn group_joints<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // NOTE: in 3D we have up to 10 different joint types.
         // In 2D we only have 5 joint types.
         #[cfg(feature = "dim3")]
@@ -204,11 +215,11 @@ impl InteractionGroups {
 
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped constraints.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -216,10 +227,14 @@ impl InteractionGroups {
 
         for interaction_i in interaction_indices {
             let interaction = &interactions[*interaction_i].weight;
-            let body1 = &bodies[interaction.body1];
-            let body2 = &bodies[interaction.body2];
-            let is_static1 = !body1.is_dynamic();
-            let is_static2 = !body2.is_dynamic();
+
+            let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body1.0);
+            let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body2.0);
+
+            let is_static1 = !status1.is_dynamic();
+            let is_static2 = !status2.is_dynamic();
 
             if is_static1 && is_static2 {
                 continue;
@@ -232,8 +247,8 @@ impl InteractionGroups {
             }
 
             let ijoint = interaction.params.type_id();
-            let i1 = body1.active_set_offset;
-            let i2 = body2.active_set_offset;
+            let i1 = ids1.active_set_offset;
+            let i2 = ids2.active_set_offset;
             let conflicts =
                 self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
             let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -325,7 +340,8 @@ impl InteractionGroups {
     pub fn group_manifolds(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
     ) {
@@ -334,21 +350,24 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_manifolds(
+    pub fn group_manifolds<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped contacts.
         // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -359,31 +378,44 @@ impl InteractionGroups {
             .max()
             .unwrap_or(1);
 
-        // FIXME: find a way to reduce the number of iteration.
+        // TODO: find a way to reduce the number of iteration.
         // There must be a way to iterate just once on every interaction indices
         // instead of MAX_MANIFOLD_POINTS times.
         for k in 1..=max_interaction_points {
             for interaction_i in interaction_indices {
                 let interaction = &interactions[*interaction_i];
 
-                // FIXME: how could we avoid iterating
+                // TODO: how could we avoid iterating
                 // on each interaction at every iteration on k?
                 if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.data.body_pair.body1];
-                let body2 = &bodies[interaction.data.body_pair.body2];
-                let is_static1 = !body1.is_dynamic();
-                let is_static2 = !body2.is_dynamic();
+                let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+                let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+
+                let is_static1 = !status1.is_dynamic();
+                let is_static2 = !status2.is_dynamic();
 
-                // FIXME: don't generate interactions between static bodies in the first place.
+                // TODO: don't generate interactions between static bodies in the first place.
                 if is_static1 && is_static2 {
                     continue;
                 }
 
-                let i1 = body1.active_set_offset;
-                let i2 = body2.active_set_offset;
+                let i1 = active_set_offset1;
+                let i2 = active_set_offset2;
                 let conflicts = self.body_masks[i1] | self.body_masks[i2];
                 let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
                 let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index c684cc5..1e65341 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,10 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
     AnyVelocityConstraint, SolverConstraints,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+};
+use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
@@ -24,17 +29,21 @@ impl IslandSolver {
         }
     }
 
-    pub fn solve_position_constraints(
+    pub fn solve_position_constraints<Bodies>(
         &mut self,
         island_id: usize,
+        islands: &IslandManager,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         counters.solver.position_resolution_time.resume();
         self.position_solver.solve(
             island_id,
             params,
+            islands,
             bodies,
             &self.contact_constraints.position_constraints,
             &self.joint_constraints.position_constraints,
@@ -42,31 +51,47 @@ impl IslandSolver {
         counters.solver.position_resolution_time.pause();
     }
 
-    pub fn init_constraints_and_solve_velocity_constraints(
+    pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
         &mut self,
         island_id: usize,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
         joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyForces>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
         let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
 
         if has_constraints {
             counters.solver.velocity_assembly_time.resume();
-            self.contact_constraints
-                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.contact_constraints.init(
+                island_id,
+                params,
+                islands,
+                bodies,
+                manifolds,
+                manifold_indices,
+            );
             self.joint_constraints
-                .init(island_id, params, bodies, joints, joint_indices);
+                .init(island_id, params, islands, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
             self.velocity_solver.solve(
                 island_id,
                 params,
+                islands,
                 bodies,
                 manifolds,
                 joints,
@@ -76,21 +101,50 @@ impl IslandSolver {
             counters.solver.velocity_resolution_time.pause();
 
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+
+            for handle in islands.active_island(island_id) {
+                let (poss, vels, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = vels.apply_damping(params.dt, damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
+
             counters.solver.velocity_update_time.pause();
         } else {
             self.contact_constraints.clear();
             self.joint_constraints.clear();
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+
+            for handle in islands.active_island(island_id) {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
-                rb.integrate_accelerations(params.dt);
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+                let (poss, vels, forces, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyForces,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = forces
+                    .integrate(params.dt, vels, mprops)
+                    .apply_damping(params.dt, &damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
             counters.solver.velocity_update_time.pause();
         }
     }
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];