Split rigid-bodies and colliders into multiple components

CHANGELOG.md

@@ -1,3 +1,8 @@
+## v0.9.0
+
+### Modified
+- Renamed `BodyStatus` to `RigidBodyType`.
+
 ## v0.8.0
 ### Modified
 - Switch to nalgebra 0.26.

benchmarks2d/balls2.rs

@@ -1,5 +1,5 @@
 use na::Point2;
-use rapier2d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
@@ -42,9 +42,9 @@ pub fn init_world(testbed: &mut Testbed) {
             let y = j as f32 * shifty + centery;
 
             let status = if j == 0 {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             // Build the rigid body.

benchmarks2d/joint_ball2.rs

@@ -1,5 +1,5 @@
 use na::Point2;
-use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
@@ -28,9 +28,9 @@ pub fn init_world(testbed: &mut Testbed) {
             let fi = i as f32;
 
             let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             let rigid_body = RigidBodyBuilder::new(status)

benchmarks2d/joint_fixed2.rs

@@ -1,5 +1,5 @@
 use na::{Isometry2, Point2};
-use rapier2d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
@@ -33,9 +33,9 @@ pub fn init_world(testbed: &mut Testbed) {
                     let fi = i as f32;
 
                     let status = if k == 0 {
-                        BodyStatus::Static
+                        RigidBodyType::Static
                     } else {
-                        BodyStatus::Dynamic
+                        RigidBodyType::Dynamic
                     };
 
                     let rigid_body = RigidBodyBuilder::new(status)

benchmarks3d/balls3.rs

@@ -1,5 +1,5 @@
 use na::Point3;
-use rapier3d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
 
@@ -30,9 +30,9 @@ pub fn init_world(testbed: &mut Testbed) {
                 let z = k as f32 * shift - centerz;
 
                 let status = if j == 0 {
-                    BodyStatus::Static
+                    RigidBodyType::Static
                 } else {
-                    BodyStatus::Dynamic
+                    RigidBodyType::Dynamic
                 };
                 let density = 0.477;
 

benchmarks3d/joint_ball3.rs

@@ -1,5 +1,5 @@
 use na::Point3;
-use rapier3d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
 
@@ -23,9 +23,9 @@ pub fn init_world(testbed: &mut Testbed) {
             let fi = i as f32;
 
             let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             let rigid_body = RigidBodyBuilder::new(status)

benchmarks3d/joint_fixed3.rs

@@ -1,5 +1,5 @@
 use na::{Isometry3, Point3};
-use rapier3d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
 
@@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) {
                         // a joint between these.
 
                         let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
-                            BodyStatus::Static
+                            RigidBodyType::Static
                         } else {
-                            BodyStatus::Dynamic
+                            RigidBodyType::Dynamic
                         };
 
                         let rigid_body = RigidBodyBuilder::new(status)

build/rapier2d-f64/Cargo.toml

@@ -16,9 +16,10 @@ edition = "2018"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim2", "f64" ]
+default = [ "dim2", "f64", "default-sets" ]
 dim2    = [ ]
 f64     = [ ]
+default-sets = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]

build/rapier2d/Cargo.toml

@@ -16,9 +16,10 @@ edition = "2018"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim2", "f32" ]
+default = [ "dim2", "f32", "default-sets" ]
 dim2    = [ ]
 f32     = [ ]
+default-sets = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]

build/rapier3d-f64/Cargo.toml

@@ -16,9 +16,10 @@ edition = "2018"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim3", "f64" ]
+default = [ "dim3", "f64", "default-sets" ]
 dim3    = [ ]
 f64     = [ ]
+default-sets = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]

build/rapier3d/Cargo.toml

@@ -16,9 +16,10 @@ edition = "2018"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim3", "f32" ]
+default = [ "dim3", "f32", "default-sets" ]
 dim3    = [ ]
 f32     = [ ]
+default-sets = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]

examples2d/add_remove2.rs

@@ -31,9 +31,12 @@ pub fn init_world(testbed: &mut Testbed) {
             .map(|e| e.0)
             .collect();
         for handle in to_remove {
-            physics
-                .bodies
-                .remove(handle, &mut physics.colliders, &mut physics.joints);
+            physics.bodies.remove(
+                handle,
+                &mut physics.islands,
+                &mut physics.colliders,
+                &mut physics.joints,
+            );
 
             if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
                 graphics.remove_body_nodes(*window, handle);

examples2d/joints2.rs

@@ -1,5 +1,5 @@
 use na::Point2;
-use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType};
 use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
@@ -31,9 +31,9 @@ pub fn init_world(testbed: &mut Testbed) {
             let fi = i as f32;
 
             let status = if i == 0 && k == 0 {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             let rigid_body = RigidBodyBuilder::new(status)

examples2d/one_way_platforms2.rs

@@ -9,12 +9,15 @@ struct OneWayPlatformHook {
     platform2: ColliderHandle,
 }
 
-impl PhysicsHooks for OneWayPlatformHook {
+impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook {
     fn active_hooks(&self) -> PhysicsHooksFlags {
         PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
     }
 
-    fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
+    fn modify_solver_contacts(
+        &self,
+        context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
+    ) {
         // The allowed normal for the first platform is its local +y axis, and the
         // allowed normal for the second platform is its local -y axis.
         //
@@ -29,16 +32,16 @@ impl PhysicsHooks for OneWayPlatformHook {
         // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
         let mut allowed_local_n1 = Vector2::zeros();
 
-        if context.collider_handle1 == self.platform1 {
+        if context.collider1 == self.platform1 {
             allowed_local_n1 = Vector2::y();
-        } else if context.collider_handle2 == self.platform1 {
+        } else if context.collider2 == self.platform1 {
             // Flip the allowed direction.
             allowed_local_n1 = -Vector2::y();
         }
 
-        if context.collider_handle1 == self.platform2 {
+        if context.collider1 == self.platform2 {
             allowed_local_n1 = -Vector2::y();
-        } else if context.collider_handle2 == self.platform2 {
+        } else if context.collider2 == self.platform2 {
             // Flip the allowed direction.
             allowed_local_n1 = Vector2::y();
         }
@@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook {
         context.update_as_oneway_platform(&allowed_local_n1, 0.1);
 
         // Set the surface velocity of the accepted contacts.
-        let tangent_velocity = if context.collider_handle1 == self.platform1
-            || context.collider_handle2 == self.platform2
-        {
-            -12.0
-        } else {
-            12.0
-        };
+        let tangent_velocity =
+            if context.collider1 == self.platform1 || context.collider2 == self.platform2 {
+                -12.0
+            } else {
+                12.0
+            };
 
         for contact in context.solver_contacts.iter_mut() {
             contact.tangent_velocity.x = tangent_velocity;
@@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) {
             }
         }
 
-        physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
+        for handle in physics.islands.active_dynamic_bodies() {
+            let body = &mut physics.bodies[*handle];
             if body.position().translation.y > 1.0 {
                 body.set_gravity_scale(1.0, false);
             } else if body.position().translation.y < -1.0 {
                 body.set_gravity_scale(-1.0, false);
             }
-        });
+        }
     });
 
     /*

examples3d/debug_add_remove_collider3.rs

@@ -38,7 +38,12 @@ pub fn init_world(testbed: &mut Testbed) {
         // Remove then re-add the ground collider.
         let coll = physics
             .colliders
-            .remove(ground_collider_handle, &mut physics.bodies, true)
+            .remove(
+                ground_collider_handle,
+                &mut physics.islands,
+                &mut physics.bodies,
+                true,
+            )
             .unwrap();
         ground_collider_handle = physics
             .colliders

examples3d/debug_dynamic_collider_add3.rs

@@ -79,7 +79,9 @@ pub fn init_world(testbed: &mut Testbed) {
             step = snapped_frame;
 
             for handle in &extra_colliders {
-                physics.colliders.remove(*handle, &mut physics.bodies, true);
+                physics
+                    .colliders
+                    .remove(*handle, &mut physics.islands, &mut physics.bodies, true);
             }
 
             extra_colliders.clear();

examples3d/fountain3.rs

@@ -61,9 +61,12 @@ pub fn init_world(testbed: &mut Testbed) {
 
             let num_to_remove = to_remove.len() - MAX_NUMBER_OF_BODIES;
             for (handle, _) in &to_remove[..num_to_remove] {
-                physics
-                    .bodies
-                    .remove(*handle, &mut physics.colliders, &mut physics.joints);
+                physics.bodies.remove(
+                    *handle,
+                    &mut physics.islands,
+                    &mut physics.colliders,
+                    &mut physics.joints,
+                );
 
                 if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
                     graphics.remove_body_nodes(window, *handle);

examples3d/joints3.rs

@@ -1,7 +1,7 @@
 use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3};
 use rapier3d::dynamics::{
-    BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
-    RigidBodyHandle, RigidBodySet,
+    BallJoint, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
+    RigidBodyHandle, RigidBodySet, RigidBodyType,
 };
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -203,9 +203,9 @@ fn create_fixed_joints(
             // fixed bodies. Because physx will crash if we add
             // a joint between these.
             let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             let rigid_body = RigidBodyBuilder::new(status)
@@ -258,9 +258,9 @@ fn create_ball_joints(
             let fi = i as f32;
 
             let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
-                BodyStatus::Static
+                RigidBodyType::Static
             } else {
-                BodyStatus::Dynamic
+                RigidBodyType::Dynamic
             };
 
             let rigid_body = RigidBodyBuilder::new(status)
@@ -317,9 +317,9 @@ fn create_actuated_revolute_joints(
         // fixed bodies. Because physx will crash if we add
         // a joint between these.
         let status = if i == 0 {
-            BodyStatus::Static
+            RigidBodyType::Static
         } else {
-            BodyStatus::Dynamic
+            RigidBodyType::Dynamic
         };
 
         let shifty = (i >= 1) as u32 as f32 * -2.0;
@@ -378,9 +378,9 @@ fn create_actuated_ball_joints(
         // fixed bodies. Because physx will crash if we add
         // a joint between these.
         let status = if i == 0 {
-            BodyStatus::Static
+            RigidBodyType::Static
         } else {
-            BodyStatus::Dynamic
+            RigidBodyType::Dynamic
         };
 
         let rigid_body = RigidBodyBuilder::new(status)

examples3d/one_way_platforms3.rs

@@ -9,36 +9,39 @@ struct OneWayPlatformHook {
     platform2: ColliderHandle,
 }
 
-impl PhysicsHooks for OneWayPlatformHook {
+impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook {
     fn active_hooks(&self) -> PhysicsHooksFlags {
         PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
     }
 
-    fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
+    fn modify_solver_contacts(
+        &self,
+        context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
+    ) {
         // The allowed normal for the first platform is its local +y axis, and the
         // allowed normal for the second platform is its local -y axis.
         //
         // Now we have to be careful because the `manifold.local_n1` normal points
         // toward the outside of the shape of `context.co1`. So we need to flip the
-        // allowed normal direction if the platform is in `context.collider_handle2`.
+        // allowed normal direction if the platform is in `context.collider2`.
         //
         // Therefore:
-        // - If context.collider_handle1 == self.platform1 then the allowed normal is +y.
-        // - If context.collider_handle2 == self.platform1 then the allowed normal is -y.
-        // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
-        // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
+        // - If context.collider1 == self.platform1 then the allowed normal is +y.
+        // - If context.collider2 == self.platform1 then the allowed normal is -y.
+        // - If context.collider1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
+        // - If context.collider2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
         let mut allowed_local_n1 = Vector3::zeros();
 
-        if context.collider_handle1 == self.platform1 {
+        if context.collider1 == self.platform1 {
             allowed_local_n1 = Vector3::y();
-        } else if context.collider_handle2 == self.platform1 {
+        } else if context.collider2 == self.platform1 {
             // Flip the allowed direction.
             allowed_local_n1 = -Vector3::y();
         }
 
-        if context.collider_handle1 == self.platform2 {
+        if context.collider1 == self.platform2 {
             allowed_local_n1 = -Vector3::y();
-        } else if context.collider_handle2 == self.platform2 {
+        } else if context.collider2 == self.platform2 {
             // Flip the allowed direction.
             allowed_local_n1 = Vector3::y();
         }
@@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook {
         context.update_as_oneway_platform(&allowed_local_n1, 0.1);
 
         // Set the surface velocity of the accepted contacts.
-        let tangent_velocity = if context.collider_handle1 == self.platform1
-            || context.collider_handle2 == self.platform2
-        {
-            -12.0
-        } else {
-            12.0
-        };
+        let tangent_velocity =
+            if context.collider1 == self.platform1 || context.collider2 == self.platform2 {
+                -12.0
+            } else {
+                12.0
+            };
 
         for contact in context.solver_contacts.iter_mut() {
             contact.tangent_velocity.z = tangent_velocity;
@@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) {
             }
         }
 
-        physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
+        for handle in physics.islands.active_dynamic_bodies() {
+            let body = physics.bodies.get_mut(*handle).unwrap();
             if body.position().translation.y > 1.0 {
                 body.set_gravity_scale(1.0, false);
             } else if body.position().translation.y < -1.0 {
                 body.set_gravity_scale(-1.0, false);
             }
-        });
+        }
     });
 
     /*

examples3d/platform3.rs

@@ -1,4 +1,4 @@
-use na::Point3;
+use na::{Isometry3, Point3, UnitQuaternion, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -14,7 +14,7 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground.
      */
-    let ground_size = 10.0;
+    let ground_size = 20.0;
     let ground_height = 0.1;
 
     let rigid_body = RigidBodyBuilder::new_static()
@@ -27,25 +27,28 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Create the boxes
      */
-    let num = 6;
+    let num = 1;
     let rad = 0.2;
 
     let shift = rad * 2.0;
     let centerx = shift * num as f32 / 2.0;
-    let centery = shift / 2.0 + 3.04;
+    let centery = shift / 2.0;
     let centerz = shift * num as f32 / 2.0;
 
-    for i in 0usize..num {
+    for i in 0usize..20 {
         for j in 0usize..num {
             for k in 0usize..num {
-                let x = i as f32 * shift - centerx;
+                let x = i as f32 * (shift + rad / 4.0) - centerx;
                 let y = j as f32 * shift + centery;
                 let z = k as f32 * shift - centerz;
 
                 // Build the rigid body.
-                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let rigid_body = RigidBodyBuilder::new_dynamic()
+                    .translation(x, y + rad, z)
+                    .ccd_enabled(true)
+                    .build();
                 let handle = bodies.insert(rigid_body);
-                let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                let collider = ColliderBuilder::cuboid(rad, rad * 2.0, rad).build();
                 colliders.insert(collider, handle, &mut bodies);
             }
         }
@@ -55,11 +58,16 @@ pub fn init_world(testbed: &mut Testbed) {
      * Setup a kinematic rigid body.
      */
     let platform_body = RigidBodyBuilder::new_kinematic()
-        .translation(0.0, 1.5 + 0.8, -10.0 * rad)
+        .translation(0.2, 0.4, -40.0 * rad)
         .build();
     let platform_handle = bodies.insert(platform_body);
-    let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0).build();
-    colliders.insert(collider, platform_handle, &mut bodies);
+    let collider1 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0).build();
+    let collider2 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0)
+        .position_wrt_parent(Isometry3::translation(0.0, rad * 2.1, 0.0))
+        .build();
+    colliders.insert(collider1, platform_handle, &mut bodies);
+    colliders.insert(collider2, platform_handle, &mut bodies);
+    testbed.set_body_color(platform_handle, Point3::new(1.0, 1.0, 0.0));
 
     /*
      * Setup a callback to control the platform.
@@ -67,23 +75,27 @@ pub fn init_world(testbed: &mut Testbed) {
     let mut count = 0;
     testbed.add_callback(move |_, _, physics, _, run_state| {
         count += 1;
-        if count % 100 > 50 {
-            return;
-        }
+        // if count % 100 > 50 {
+        //     return;
+        // }
 
         if let Some(platform) = physics.bodies.get_mut(platform_handle) {
             let mut next_pos = *platform.position();
 
             let dt = 0.016;
-            next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt;
-            next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt;
+            // next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt;
+            // next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt;
 
-            if next_pos.translation.vector.z >= rad * 10.0 {
-                next_pos.translation.vector.z -= dt;
-            }
-            if next_pos.translation.vector.z <= -rad * 10.0 {
-                next_pos.translation.vector.z += dt;
-            }
+            let drot = UnitQuaternion::new(Vector3::y() * 0.01);
+            next_pos.rotation = drot * next_pos.rotation;
+            next_pos.translation.vector += next_pos.rotation * Vector3::z() * 0.1;
+
+            // if next_pos.translation.vector.z >= rad * 10.0 {
+            //     next_pos.translation.vector.z -= dt;
+            // }
+            // if next_pos.translation.vector.z <= -rad * 10.0 {
+            //     next_pos.translation.vector.z += dt;
+            // }
 
             platform.set_next_kinematic_position(next_pos);
         }

src/data/arena.rs

@@ -19,16 +19,16 @@ use std::vec;
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct Arena<T> {
     items: Vec<Entry<T>>,
-    generation: u64,
-    free_list_head: Option<usize>,
+    generation: u32,
+    free_list_head: Option<u32>,
     len: usize,
 }
 
 #[derive(Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 enum Entry<T> {
-    Free { next_free: Option<usize> },
-    Occupied { generation: u64, value: T },
+    Free { next_free: Option<u32> },
+    Occupied { generation: u32, value: T },
 }
 
 /// An index (and generation) into an `Arena`.
@@ -48,17 +48,17 @@ enum Entry<T> {
 #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct Index {
-    index: usize,
-    generation: u64,
+    index: u32,
+    generation: u32,
 }
 
 impl IndexedData for Index {
     fn default() -> Self {
-        Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+        Self::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32)
     }
 
     fn index(&self) -> usize {
-        self.into_raw_parts().0
+        self.into_raw_parts().0 as usize
     }
 }
 
@@ -70,7 +70,7 @@ impl Index {
     ///
     /// Providing arbitrary values will lead to malformed indices and ultimately
     /// panics.
-    pub fn from_raw_parts(a: usize, b: u64) -> Index {
+    pub fn from_raw_parts(a: u32, b: u32) -> Index {
         Index {
             index: a,
             generation: b,
@@ -84,7 +84,7 @@ impl Index {
     /// `Index` like `pub struct MyIdentifier(Index);`.  However, for external
     /// types whose definition you can't customize, but which you can construct
     /// instances of, this method can be useful.
-    pub fn into_raw_parts(self) -> (usize, u64) {
+    pub fn into_raw_parts(self) -> (u32, u32) {
         (self.index, self.generation)
     }
 }
@@ -161,7 +161,7 @@ impl<T> Arena<T> {
     pub fn clear(&mut self) {
         self.items.clear();
 
-        let end = self.items.capacity();
+        let end = self.items.capacity() as u32;
         self.items.extend((0..end).map(|i| {
             if i == end - 1 {
                 Entry::Free { next_free: None }
@@ -206,7 +206,7 @@ impl<T> Arena<T> {
         match self.try_alloc_next_index() {
             None => Err(value),
             Some(index) => {
-                self.items[index.index] = Entry::Occupied {
+                self.items[index.index as usize] = Entry::Occupied {
                     generation: self.generation,
                     value,
                 };
@@ -247,7 +247,7 @@ impl<T> Arena<T> {
         match self.try_alloc_next_index() {
             None => Err(create),
             Some(index) => {
-                self.items[index.index] = Entry::Occupied {
+                self.items[index.index as usize] = Entry::Occupied {
                     generation: self.generation,
                     value: create(index),
                 };
@@ -260,13 +260,13 @@ impl<T> Arena<T> {
     fn try_alloc_next_index(&mut self) -> Option<Index> {
         match self.free_list_head {
             None => None,
-            Some(i) => match self.items[i] {
+            Some(i) => match self.items[i as usize] {
                 Entry::Occupied { .. } => panic!("corrupt free list"),
                 Entry::Free { next_free } => {
                     self.free_list_head = next_free;
                     self.len += 1;
                     Some(Index {
-                        index: i,
+                        index: i as u32,
                         generation: self.generation,
                     })
                 }
@@ -355,14 +355,14 @@ impl<T> Arena<T> {
     /// assert_eq!(arena.remove(idx), None);
     /// ```
     pub fn remove(&mut self, i: Index) -> Option<T> {
-        if i.index >= self.items.len() {
+        if i.index >= self.items.len() as u32 {
             return None;
         }
 
-        match self.items[i.index] {
+        match self.items[i.index as usize] {
             Entry::Occupied { generation, .. } if i.generation == generation => {
                 let entry = mem::replace(
-                    &mut self.items[i.index],
+                    &mut self.items[i.index as usize],
                     Entry::Free {
                         next_free: self.free_list_head,
                     },
@@ -402,8 +402,8 @@ impl<T> Arena<T> {
     /// assert!(crew_members.next().is_none());
     /// ```
     pub fn retain(&mut self, mut predicate: impl FnMut(Index, &mut T) -> bool) {
-        for i in 0..self.capacity() {
-            let remove = match &mut self.items[i] {
+        for i in 0..self.capacity() as u32 {
+            let remove = match &mut self.items[i as usize] {
                 Entry::Occupied { generation, value } => {
                     let index = Index {
                         index: i,
@@ -462,7 +462,7 @@ impl<T> Arena<T> {
     /// assert!(arena.get(idx).is_none());
     /// ```
     pub fn get(&self, i: Index) -> Option<&T> {
-        match self.items.get(i.index) {
+        match self.items.get(i.index as usize) {
             Some(Entry::Occupied { generation, value }) if *generation == i.generation => {
                 Some(value)
             }
@@ -488,7 +488,7 @@ impl<T> Arena<T> {
     /// assert!(arena.get_mut(idx).is_none());
     /// ```
     pub fn get_mut(&mut self, i: Index) -> Option<&mut T> {
-        match self.items.get_mut(i.index) {
+        match self.items.get_mut(i.index as usize) {
             Some(Entry::Occupied { generation, value }) if *generation == i.generation => {
                 Some(value)
             }
@@ -526,7 +526,7 @@ impl<T> Arena<T> {
     /// assert_eq!(arena[idx2], 4);
     /// ```
     pub fn get2_mut(&mut self, i1: Index, i2: Index) -> (Option<&mut T>, Option<&mut T>) {
-        let len = self.items.len();
+        let len = self.items.len() as u32;
 
         if i1.index == i2.index {
             assert!(i1.generation != i2.generation);
@@ -544,11 +544,13 @@ impl<T> Arena<T> {
         }
 
         let (raw_item1, raw_item2) = {
-            let (xs, ys) = self.items.split_at_mut(cmp::max(i1.index, i2.index));
+            let (xs, ys) = self
+                .items
+                .split_at_mut(cmp::max(i1.index, i2.index) as usize);
             if i1.index < i2.index {
-                (&mut xs[i1.index], &mut ys[0])
+                (&mut xs[i1.index as usize], &mut ys[0])
             } else {
-                (&mut ys[0], &mut xs[i2.index])
+                (&mut ys[0], &mut xs[i2.index as usize])
             }
         };
 
@@ -666,11 +668,11 @@ impl<T> Arena<T> {
                 }
             } else {
                 Entry::Free {
-                    next_free: Some(i + 1),
+                    next_free: Some(i as u32 + 1),
                 }
             }
         }));
-        self.free_list_head = Some(start);
+        self.free_list_head = Some(start as u32);
     }
 
     /// Iterate over shared references to the elements in this arena.
@@ -774,7 +776,7 @@ impl<T> Arena<T> {
                 value,
                 Index {
                     generation: *generation,
-                    index: i,
+                    index: i as u32,
                 },
             )),
             _ => None,
@@ -797,7 +799,7 @@ impl<T> Arena<T> {
                 value,
                 Index {
                     generation: *generation,
-                    index: i,
+                    index: i as u32,
                 },
             )),
             _ => None,
@@ -941,7 +943,10 @@ impl<'a, T> Iterator for Iter<'a, T> {
                     },
                 )) => {
                     self.len -= 1;
-                    let idx = Index { index, generation };
+                    let idx = Index {
+                        index: index as u32,
+                        generation,
+                    };
                     return Some((idx, value));
                 }
                 None => {
@@ -970,7 +975,10 @@ impl<'a, T> DoubleEndedIterator for Iter<'a, T> {
                     },
                 )) => {
                     self.len -= 1;
-                    let idx = Index { index, generation };
+                    let idx = Index {
+                        index: index as u32,
+                        generation,
+                    };
                     return Some((idx, value));
                 }
                 None => {
@@ -1039,7 +1047,10 @@ impl<'a, T> Iterator for IterMut<'a, T> {
                     },
                 )) => {
                     self.len -= 1;
-                    let idx = Index { index, generation };
+                    let idx = Index {
+                        index: index as u32,
+                        generation,
+                    };
                     return Some((idx, value));
                 }
                 None => {
@@ -1068,7 +1079,10 @@ impl<'a, T> DoubleEndedIterator for IterMut<'a, T> {
                     },
                 )) => {
                     self.len -= 1;
-                    let idx = Index { index, generation };
+                    let idx = Index {
+                        index: index as u32,
+                        generation,
+                    };
                     return Some((idx, value));
                 }
                 None => {
@@ -1126,7 +1140,10 @@ impl<'a, T> Iterator for Drain<'a, T> {
             match self.inner.next() {
                 Some((_, Entry::Free { .. })) => continue,
                 Some((index, Entry::Occupied { generation, value })) => {
-                    let idx = Index { index, generation };
+                    let idx = Index {
+                        index: index as u32,
+                        generation,
+                    };
                     return Some((idx, value));
                 }
                 None => return None,

src/data/coarena.rs

@@ -4,7 +4,7 @@ use crate::data::arena::Index;
 #[derive(Clone, Debug)]
 /// A container for data associated to item existing into another Arena.
 pub struct Coarena<T> {
-    data: Vec<(u64, T)>,
+    data: Vec<(u32, T)>,
 }
 
 impl<T> Coarena<T> {
@@ -17,7 +17,7 @@ impl<T> Coarena<T> {
     pub fn get(&self, index: Index) -> Option<&T> {
         let (i, g) = index.into_raw_parts();
         self.data
-            .get(i)
+            .get(i as usize)
             .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
     }
 
@@ -25,7 +25,7 @@ impl<T> Coarena<T> {
     pub fn get_mut(&mut self, index: Index) -> Option<&mut T> {
         let (i, g) = index.into_raw_parts();
         self.data
-            .get_mut(i)
+            .get_mut(i as usize)
             .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
     }
 
@@ -36,11 +36,11 @@ impl<T> Coarena<T> {
     {
         let (i1, g1) = a.into_raw_parts();
 
-        if self.data.len() <= i1 {
-            self.data.resize(i1 + 1, (u32::MAX as u64, T::default()));
+        if self.data.len() <= i1 as usize {
+            self.data.resize(i1 as usize + 1, (u32::MAX, T::default()));
         }
 
-        self.data[i1] = (g1, value);
+        self.data[i1 as usize] = (g1, value);
     }
 
     /// Ensure that elements at the two given indices exist in this coarena, and return their reference.
@@ -56,20 +56,22 @@ impl<T> Coarena<T> {
         assert_ne!(i1, i2, "Cannot index the same object twice.");
 
         let (elt1, elt2) = if i1 > i2 {
-            if self.data.len() <= i1 {
-                self.data.resize(i1 + 1, (u32::MAX as u64, default.clone()));
+            if self.data.len() <= i1 as usize {
+                self.data
+                    .resize(i1 as usize + 1, (u32::MAX, default.clone()));
             }
 
-            let (left, right) = self.data.split_at_mut(i1);
-            (&mut right[0], &mut left[i2])
+            let (left, right) = self.data.split_at_mut(i1 as usize);
+            (&mut right[0], &mut left[i2 as usize])
         } else {
             // i2 > i1
-            if self.data.len() <= i2 {
-                self.data.resize(i2 + 1, (u32::MAX as u64, default.clone()));
+            if self.data.len() <= i2 as usize {
+                self.data
+                    .resize(i2 as usize + 1, (u32::MAX, default.clone()));
             }
 
-            let (left, right) = self.data.split_at_mut(i2);
-            (&mut left[i1], &mut right[0])
+            let (left, right) = self.data.split_at_mut(i2 as usize);
+            (&mut left[i1 as usize], &mut right[0])
         };
 
         if elt1.0 != g1 {

src/data/component_set.rs

@@ -0,0 +1,106 @@
+use crate::data::Index;
+
+// TODO ECS: use this to handle optional components properly.
+// pub trait OptionalComponentSet<T> {
+//     fn get(&self, handle: Index) -> Option<&T>;
+// }
+
+pub trait ComponentSetOption<T> {
+    fn get(&self, handle: Index) -> Option<&T>;
+}
+
+pub trait ComponentSet<T>: ComponentSetOption<T> {
+    fn size_hint(&self) -> usize;
+    // TODO ECS: remove this, its only needed by the query pipeline update
+    //           which should only take the modified colliders into account.
+    fn for_each(&self, f: impl FnMut(Index, &T));
+    fn index(&self, handle: Index) -> &T {
+        self.get(handle).unwrap()
+    }
+}
+
+pub trait ComponentSetMut<T>: ComponentSet<T> {
+    fn map_mut_internal<Result>(
+        &mut self,
+        handle: crate::data::Index,
+        f: impl FnOnce(&mut T) -> Result,
+    ) -> Option<Result>;
+    fn set_internal(&mut self, handle: crate::data::Index, val: T);
+}
+
+pub trait BundleSet<'a, T> {
+    fn index_bundle(&'a self, handle: Index) -> T;
+}
+
+impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T
+where
+    T: ComponentSet<A> + ComponentSet<B>,
+{
+    #[inline(always)]
+    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) {
+        (self.index(handle), self.index(handle))
+    }
+}
+
+impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T
+where
+    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C>,
+{
+    #[inline(always)]
+    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) {
+        (self.index(handle), self.index(handle), self.index(handle))
+    }
+}
+
+impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T
+where
+    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D>,
+{
+    #[inline(always)]
+    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) {
+        (
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+        )
+    }
+}
+
+impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T
+where
+    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D> + ComponentSet<E>,
+{
+    #[inline(always)]
+    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) {
+        (
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+        )
+    }
+}
+
+impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T
+where
+    T: ComponentSet<A>
+        + ComponentSet<B>
+        + ComponentSet<C>
+        + ComponentSet<D>
+        + ComponentSet<E>
+        + ComponentSet<F>,
+{
+    #[inline(always)]
+    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) {
+        (
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+            self.index(handle),
+        )
+    }
+}

src/data/mod.rs

@@ -1,8 +1,11 @@
 //! Data structures modified with guaranteed deterministic behavior after deserialization.
 
+pub use self::arena::{Arena, Index};
 pub use self::coarena::Coarena;
+pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 
 pub mod arena;
 mod coarena;
+mod component_set;
 pub(crate) mod graph;
 pub mod pubsub;

src/dynamics/ccd/ccd_solver.rs

@@ -1,6 +1,12 @@
 use super::TOIEntry;
-use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase};
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces};
+use crate::dynamics::{
+    RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+};
+use crate::geometry::{
+    ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase,
+};
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
@@ -44,19 +50,34 @@ impl CCDSolver {
     /// Apply motion-clamping to the bodies affected by the given `impacts`.
     ///
     /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`.
-    pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
+    pub fn clamp_motions<Bodies>(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts)
+    where
+        Bodies: ComponentSet<RigidBodyCcd>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         match impacts {
             PredictedImpacts::Impacts(tois) => {
                 // println!("Num to clamp: {}", tois.len());
                 for (handle, toi) in tois {
-                    if let Some(body) = bodies.get_mut_internal(*handle) {
-                        let min_toi = (body.ccd_thickness
-                            * 0.15
-                            * crate::utils::inv(body.max_point_velocity()))
-                        .min(dt);
-                        // println!("Min toi: {}, Toi: {}", min_toi, toi);
-                        body.integrate_next_position(toi.max(min_toi));
-                    }
+                    let (rb_poss, vels, ccd, mprops): (
+                        &RigidBodyPosition,
+                        &RigidBodyVelocity,
+                        &RigidBodyCcd,
+                        &RigidBodyMassProps,
+                    ) = bodies.index_bundle(handle.0);
+                    let local_com = &mprops.mass_properties.local_com;
+
+                    let min_toi = (ccd.ccd_thickness
+                        * 0.15
+                        * crate::utils::inv(ccd.max_point_velocity(vels)))
+                    .min(dt);
+                    // println!("Min toi: {}, Toi: {}", min_toi, toi);
+                    let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com);
+                    bodies.map_mut_internal(handle.0, |rb_poss| {
+                        rb_poss.next_position = new_pos;
+                    });
                 }
             }
             _ => {}
@@ -66,34 +87,64 @@ impl CCDSolver {
     /// Updates the set of bodies that needs CCD to be resolved.
     ///
     /// Returns `true` if any rigid-body must have CCD resolved.
-    pub fn update_ccd_active_flags(
+    pub fn update_ccd_active_flags<Bodies>(
         &self,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         dt: Real,
         include_forces: bool,
-    ) -> bool {
+    ) -> bool
+    where
+        Bodies: ComponentSetMut<RigidBodyCcd>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyForces>,
+    {
         let mut ccd_active = false;
 
         // println!("Checking CCD activation");
-        bodies.foreach_active_dynamic_body_mut_internal(|_, body| {
-            body.update_ccd_active_flag(dt, include_forces);
-            // println!("CCD is active: {}, for {:?}", ccd_active, handle);
-            ccd_active = ccd_active || body.is_ccd_active();
-        });
+        for handle in islands.active_dynamic_bodies() {
+            let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) =
+                bodies.index_bundle(handle.0);
+
+            if ccd.ccd_enabled {
+                let forces = if include_forces { Some(forces) } else { None };
+                let moving_fast = ccd.is_moving_fast(dt, vels, forces);
+
+                bodies.map_mut_internal(handle.0, |ccd| {
+                    ccd.ccd_active = moving_fast;
+                });
+
+                ccd_active = ccd_active || moving_fast;
+            }
+        }
 
         ccd_active
     }
 
     /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
-    pub fn find_first_impact(
+    pub fn find_first_impact<Bodies, Colliders>(
         &mut self,
         dt: Real,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
+        islands: &IslandManager,
+        bodies: &Bodies,
+        colliders: &Colliders,
         narrow_phase: &NarrowPhase,
-    ) -> Option<Real> {
+    ) -> Option<Real>
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyMassProps>,
+        Colliders: ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderType>,
+    {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
+            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -102,19 +153,37 @@ impl CCDSolver {
         let mut pairs_seen = HashMap::default();
         let mut min_toi = dt;
 
-        for (_, rb1) in bodies.iter_active_dynamic() {
-            if rb1.is_ccd_active() {
-                let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt);
+        for handle in islands.active_dynamic_bodies() {
+            let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
 
-                for ch1 in &rb1.colliders {
-                    let co1 = &colliders[*ch1];
+            if rb_ccd1.ccd_active {
+                let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyForces,
+                    &RigidBodyMassProps,
+                    &RigidBodyColliders,
+                ) = bodies.index_bundle(handle.0);
 
-                    if co1.is_sensor() {
+                let predicted_body_pos1 =
+                    rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
+
+                for ch1 in &rb_colliders1.0 {
+                    let co_parent1: &ColliderParent = colliders
+                        .get(ch1.0)
+                        .expect("Could not find the ColliderParent component.");
+                    let (co_shape1, co_pos1, co_type1): (
+                        &ColliderShape,
+                        &ColliderPosition,
+                        &ColliderType,
+                    ) = colliders.index_bundle(ch1.0);
+
+                    if co_type1.is_sensor() {
                         continue; // Ignore sensors.
                     }
 
-                    let aabb1 =
-                        co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent()));
+                    let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
+                    let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
 
                     self.query_pipeline
                         .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -130,12 +199,17 @@ impl CCDSolver {
                                 )
                                 .is_none()
                             {
-                                let c1 = colliders.get(*ch1).unwrap();
-                                let c2 = colliders.get(*ch2).unwrap();
-                                let bh1 = c1.parent();
-                                let bh2 = c2.parent();
+                                let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
+                                let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
+                                let c1: (_, _, _) = colliders.index_bundle(ch1.0);
+                                let c2: (_, _, _) = colliders.index_bundle(ch2.0);
+                                let co_type1: &ColliderType = colliders.index(ch1.0);
+                                let co_type2: &ColliderType = colliders.index(ch1.0);
 
-                                if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) {
+                                let bh1 = co_parent1.map(|p| p.handle);
+                                let bh2 = co_parent2.map(|p| p.handle);
+
+                                if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) {
                                     // Ignore self-intersection and sensors.
                                     return true;
                                 }
@@ -146,16 +220,15 @@ impl CCDSolver {
                                     .map(|c| c.1.dist)
                                     .unwrap_or(0.0);
 
-                                let b1 = bodies.get(bh1).unwrap();
-                                let b2 = bodies.get(bh2).unwrap();
+                                let b2 = bh2.map(|h| bodies.index_bundle(h.0));
 
                                 if let Some(toi) = TOIEntry::try_from_colliders(
                                     self.query_pipeline.query_dispatcher(),
                                     *ch1,
                                     *ch2,
-                                    c1,
-                                    c2,
-                                    b1,
+                                    (c1.0, c1.1, c1.2, co_parent1),
+                                    (c2.0, c2.1, c2.2, co_parent2),
+                                    Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
                                     b2,
                                     None,
                                     None,
@@ -181,14 +254,27 @@ impl CCDSolver {
     }
 
     /// Outputs the set of bodies as well as their first time-of-impact event.
-    pub fn predict_impacts_at_next_positions(
+    pub fn predict_impacts_at_next_positions<Bodies, Colliders>(
         &mut self,
         dt: Real,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
+        islands: &IslandManager,
+        bodies: &Bodies,
+        colliders: &Colliders,
         narrow_phase: &NarrowPhase,
         events: &dyn EventHandler,
-    ) -> PredictedImpacts {
+    ) -> PredictedImpacts
+    where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyMassProps>,
+        Colliders: ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderType>,
+    {
         let mut frozen = HashMap::<_, Real>::default();
         let mut all_toi = BinaryHeap::new();
         let mut pairs_seen = HashMap::default();
@@ -196,6 +282,7 @@ impl CCDSolver {
 
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
+            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithNextPosition,
@@ -207,71 +294,94 @@ impl CCDSolver {
          *
          */
         // TODO: don't iterate through all the colliders.
-        for (ch1, co1) in colliders.iter() {
-            let rb1 = &bodies[co1.parent()];
-            if rb1.is_ccd_active() {
-                let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent()));
+        for handle in islands.active_dynamic_bodies() {
+            let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
 
-                self.query_pipeline
-                    .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
-                        if ch1 == *ch2 {
-                            // Ignore self-intersection.
-                            return true;
-                        }
+            if rb_ccd1.ccd_active {
+                let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyForces,
+                    &RigidBodyMassProps,
+                    &RigidBodyColliders,
+                ) = bodies.index_bundle(handle.0);
 
-                        if pairs_seen
-                            .insert(
-                                SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
-                                (),
-                            )
-                            .is_none()
-                        {
-                            let c1 = colliders.get(ch1).unwrap();
-                            let c2 = colliders.get(*ch2).unwrap();
-                            let bh1 = c1.parent();
-                            let bh2 = c2.parent();
+                let predicted_body_pos1 =
+                    rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
 
-                            if bh1 == bh2 {
+                for ch1 in &rb_colliders1.0 {
+                    let co_parent1: &ColliderParent = colliders
+                        .get(ch1.0)
+                        .expect("Could not find the ColliderParent component.");
+                    let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
+                        colliders.index_bundle(ch1.0);
+
+                    let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
+                    let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
+
+                    self.query_pipeline
+                        .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
+                            if *ch1 == *ch2 {
                                 // Ignore self-intersection.
                                 return true;
                             }
 
-                            let b1 = bodies.get(bh1).unwrap();
-                            let b2 = bodies.get(bh2).unwrap();
+                            if pairs_seen
+                                .insert(
+                                    SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
+                                    (),
+                                )
+                                .is_none()
+                            {
+                                let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
+                                let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
+                                let c1: (_, _, _) = colliders.index_bundle(ch1.0);
+                                let c2: (_, _, _) = colliders.index_bundle(ch2.0);
 
-                            let smallest_dist = narrow_phase
-                                .contact_pair(ch1, *ch2)
-                                .and_then(|p| p.find_deepest_contact())
-                                .map(|c| c.1.dist)
-                                .unwrap_or(0.0);
+                                let bh1 = co_parent1.map(|p| p.handle);
+                                let bh2 = co_parent2.map(|p| p.handle);
 
-                            if let Some(toi) = TOIEntry::try_from_colliders(
-                                self.query_pipeline.query_dispatcher(),
-                                ch1,
-                                *ch2,
-                                c1,
-                                c2,
-                                b1,
-                                b2,
-                                None,
-                                None,
-                                0.0,
-                                // NOTE: we use dt here only once we know that
-                                // there is at least one TOI before dt.
-                                min_overstep,
-                                smallest_dist,
-                            ) {
-                                if toi.toi > dt {
-                                    min_overstep = min_overstep.min(toi.toi);
-                                } else {
-                                    min_overstep = dt;
-                                    all_toi.push(toi);
+                                if bh1 == bh2 {
+                                    // Ignore self-intersection.
+                                    return true;
+                                }
+
+                                let smallest_dist = narrow_phase
+                                    .contact_pair(*ch1, *ch2)
+                                    .and_then(|p| p.find_deepest_contact())
+                                    .map(|c| c.1.dist)
+                                    .unwrap_or(0.0);
+
+                                let b2 = bh2.map(|h| bodies.index_bundle(h.0));
+
+                                if let Some(toi) = TOIEntry::try_from_colliders(
+                                    self.query_pipeline.query_dispatcher(),
+                                    *ch1,
+                                    *ch2,
+                                    (c1.0, c1.1, c1.2, co_parent1),
+                                    (c2.0, c2.1, c2.2, co_parent2),
+                                    Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
+                                    b2,
+                                    None,
+                                    None,
+                                    0.0,
+                                    // NOTE: we use dt here only once we know that
+                                    // there is at least one TOI before dt.
+                                    min_overstep,
+                                    smallest_dist,
+                                ) {
+                                    if toi.toi > dt {
+                                        min_overstep = min_overstep.min(toi.toi);
+                                    } else {
+                                        min_overstep = dt;
+                                        all_toi.push(toi);
+                                    }
                                 }
                             }
-                        }
 
-                        true
-                    });
+                            true
+                        });
+                }
             }
         }
 
@@ -293,19 +403,25 @@ impl CCDSolver {
         while let Some(toi) = all_toi.pop() {
             assert!(toi.toi <= dt);
 
-            let body1 = bodies.get(toi.b1).unwrap();
-            let body2 = bodies.get(toi.b2).unwrap();
+            let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
+                toi.b1.map(|b| bodies.index_bundle(b.0));
+            let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
+                toi.b2.map(|b| bodies.index_bundle(b.0));
 
             let mut colliders_to_check = Vec::new();
-            let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1);
-            let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2);
+            let should_freeze1 = rb1.is_some()
+                && rb1.unwrap().0.ccd_active
+                && !frozen.contains_key(&toi.b1.unwrap());
+            let should_freeze2 = rb2.is_some()
+                && rb2.unwrap().0.ccd_active
+                && !frozen.contains_key(&toi.b2.unwrap());
 
             if !should_freeze1 && !should_freeze2 {
                 continue;
             }
 
             if toi.is_intersection_test {
-                // NOTE: this test is rendundant with the previous `if !should_freeze && ...`
+                // NOTE: this test is redundant with the previous `if !should_freeze && ...`
                 //       but let's keep it to avoid tricky regressions if we end up swapping both
                 //       `if` for some reasons in the future.
                 if should_freeze1 || should_freeze2 {
@@ -318,42 +434,51 @@ impl CCDSolver {
             }
 
             if should_freeze1 {
-                let _ = frozen.insert(toi.b1, toi.toi);
-                colliders_to_check.extend_from_slice(&body1.colliders);
+                let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
+                colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0);
             }
 
             if should_freeze2 {
-                let _ = frozen.insert(toi.b2, toi.toi);
-                colliders_to_check.extend_from_slice(&body2.colliders);
+                let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
+                colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0);
             }
 
             let start_time = toi.toi;
 
             for ch1 in &colliders_to_check {
-                let co1 = &colliders[*ch1];
-                let rb1 = &bodies[co1.parent];
-                let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent()));
+                let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap();
+                let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
+                    colliders.index_bundle(ch1.0);
+
+                let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0);
+                let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent;
+                let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1);
 
                 self.query_pipeline
                     .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
-                        let c1 = colliders.get(*ch1).unwrap();
-                        let c2 = colliders.get(*ch2).unwrap();
-                        let bh1 = c1.parent();
-                        let bh2 = c2.parent();
+                        let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
+                        let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
+                        let c1: (_, _, _) = colliders.index_bundle(ch1.0);
+                        let c2: (_, _, _) = colliders.index_bundle(ch2.0);
+
+                        let bh1 = co_parent1.map(|p| p.handle);
+                        let bh2 = co_parent2.map(|p| p.handle);
 
                         if bh1 == bh2 {
                             // Ignore self-intersection.
                             return true;
                         }
 
-                        let frozen1 = frozen.get(&bh1);
-                        let frozen2 = frozen.get(&bh2);
+                        let frozen1 = bh1.and_then(|h| frozen.get(&h));
+                        let frozen2 = bh2.and_then(|h| frozen.get(&h));
 
-                        let b1 = bodies.get(bh1).unwrap();
-                        let b2 = bodies.get(bh2).unwrap();
+                        let b1: Option<(_, _, _, &RigidBodyCcd)> =
+                            bh1.map(|h| bodies.index_bundle(h.0));
+                        let b2: Option<(_, _, _, &RigidBodyCcd)> =
+                            bh1.map(|h| bodies.index_bundle(h.0));
 
-                        if (frozen1.is_some() || !b1.is_ccd_active())
-                            && (frozen2.is_some() || !b2.is_ccd_active())
+                        if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false))
+                            && (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false))
                         {
                             // We already did a resweep.
                             return true;
@@ -369,8 +494,8 @@ impl CCDSolver {
                             self.query_pipeline.query_dispatcher(),
                             *ch1,
                             *ch2,
-                            c1,
-                            c2,
+                            (c1.0, c1.1, c1.2, co_parent1),
+                            (c2.0, c2.1, c2.2, co_parent2),
                             b1,
                             b2,
                             frozen1.copied(),
@@ -395,30 +520,57 @@ impl CCDSolver {
             // - If the intersection isn't active anymore, and it wasn't intersecting
             //   before, then we need to generate one interaction-start and one interaction-stop
             //   events because it will never be detected by the narrow-phase because of tunneling.
-            let body1 = &bodies[toi.b1];
-            let body2 = &bodies[toi.b2];
-            let co1 = &colliders[toi.c1];
-            let co2 = &colliders[toi.c2];
-            let frozen1 = frozen.get(&toi.b1);
-            let frozen2 = frozen.get(&toi.b2);
-            let pos1 = frozen1
-                .map(|t| body1.integrate_velocity(*t))
-                .unwrap_or(body1.next_position);
-            let pos2 = frozen2
-                .map(|t| body2.integrate_velocity(*t))
-                .unwrap_or(body2.next_position);
+            let (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) =
+                colliders.index_bundle(toi.c1.0);
+            let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) =
+                colliders.index_bundle(toi.c2.0);
 
-            let prev_coll_pos12 = co1.position.inv_mul(&co2.position);
-            let next_coll_pos12 =
-                (pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent());
+            let co_next_pos1 = if let Some(b1) = toi.b1 {
+                let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap();
+                let (rb_pos1, rb_vels1, rb_mprops1): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(b1.0);
+
+                let local_com1 = &rb_mprops1.mass_properties.local_com;
+                let frozen1 = frozen.get(&b1);
+                let pos1 = frozen1
+                    .map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1))
+                    .unwrap_or(rb_pos1.next_position);
+                pos1 * co_parent1.pos_wrt_parent
+            } else {
+                co_pos1.0
+            };
+
+            let co_next_pos2 = if let Some(b2) = toi.b2 {
+                let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap();
+                let (rb_pos2, rb_vels2, rb_mprops2): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(b2.0);
+
+                let local_com2 = &rb_mprops2.mass_properties.local_com;
+                let frozen2 = frozen.get(&b2);
+                let pos2 = frozen2
+                    .map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2))
+                    .unwrap_or(rb_pos2.next_position);
+                pos2 * co_parent2.pos_wrt_parent
+            } else {
+                co_pos2.0
+            };
+
+            let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2);
+            let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
 
             let query_dispatcher = self.query_pipeline.query_dispatcher();
             let intersect_before = query_dispatcher
-                .intersection_test(&prev_coll_pos12, co1.shape(), co2.shape())
+                .intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
                 .unwrap_or(false);
 
             let intersect_after = query_dispatcher
-                .intersection_test(&next_coll_pos12, co1.shape(), co2.shape())
+                .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
                 .unwrap_or(false);
 
             if !intersect_before && !intersect_after {

src/dynamics/ccd/toi_entry.rs

@@ -1,5 +1,9 @@
-use crate::dynamics::{RigidBody, RigidBodyHandle};
-use crate::geometry::{Collider, ColliderHandle};
+use crate::dynamics::{
+    RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+};
+use crate::geometry::{
+    ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
+};
 use crate::math::Real;
 use parry::query::{NonlinearRigidMotion, QueryDispatcher};
 
@@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher};
 pub struct TOIEntry {
     pub toi: Real,
     pub c1: ColliderHandle,
-    pub b1: RigidBodyHandle,
+    pub b1: Option<RigidBodyHandle>,
     pub c2: ColliderHandle,
-    pub b2: RigidBodyHandle,
+    pub b2: Option<RigidBodyHandle>,
     pub is_intersection_test: bool,
     pub timestamp: usize,
 }
@@ -18,9 +22,9 @@ impl TOIEntry {
     fn new(
         toi: Real,
         c1: ColliderHandle,
-        b1: RigidBodyHandle,
+        b1: Option<RigidBodyHandle>,
         c2: ColliderHandle,
-        b2: RigidBodyHandle,
+        b2: Option<RigidBodyHandle>,
         is_intersection_test: bool,
         timestamp: usize,
     ) -> Self {
@@ -39,10 +43,30 @@ impl TOIEntry {
         query_dispatcher: &QD,
         ch1: ColliderHandle,
         ch2: ColliderHandle,
-        c1: &Collider,
-        c2: &Collider,
-        b1: &RigidBody,
-        b2: &RigidBody,
+        c1: (
+            &ColliderType,
+            &ColliderShape,
+            &ColliderPosition,
+            Option<&ColliderParent>,
+        ),
+        c2: (
+            &ColliderType,
+            &ColliderShape,
+            &ColliderPosition,
+            Option<&ColliderParent>,
+        ),
+        b1: Option<(
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyCcd,
+        )>,
+        b2: Option<(
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyCcd,
+        )>,
         frozen1: Option<Real>,
         frozen2: Option<Real>,
         start_time: Real,
@@ -50,35 +74,46 @@ impl TOIEntry {
         smallest_contact_dist: Real,
     ) -> Option<Self> {
         assert!(start_time <= end_time);
+        if b1.is_none() && b2.is_none() {
+            return None;
+        }
 
-        let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel();
-        let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel();
-        let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel();
-        let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel();
+        let (co_type1, co_shape1, co_pos1, co_parent1) = c1;
+        let (co_type2, co_shape2, co_pos2, co_parent2) = c2;
+
+        let linvel1 =
+            frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
+        let linvel2 =
+            frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
+        let angvel1 =
+            frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
+        let angvel2 =
+            frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
 
         #[cfg(feature = "dim2")]
         let vel12 = (linvel2 - linvel1).norm()
-            + angvel1.abs() * b1.ccd_max_dist
-            + angvel2.abs() * b2.ccd_max_dist;
+            + angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+            + angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
         #[cfg(feature = "dim3")]
         let vel12 = (linvel2 - linvel1).norm()
-            + angvel1.norm() * b1.ccd_max_dist
-            + angvel2.norm() * b2.ccd_max_dist;
+            + angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+            + angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
 
         // We may be slightly over-conservative by taking the `max(0.0)` here.
         // But removing the `max` doesn't really affect performances so let's
         // keep it since more conservatism is good at this stage.
-        let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness())
+        let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
             + smallest_contact_dist.max(0.0);
-        let is_intersection_test = c1.is_sensor() || c2.is_sensor();
+        let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor();
 
         if (end_time - start_time) * vel12 < thickness {
             return None;
         }
 
         // Compute the TOI.
-        let mut motion1 = Self::body_motion(b1);
-        let mut motion2 = Self::body_motion(b2);
+        let identity = NonlinearRigidMotion::identity();
+        let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity);
+        let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity);
 
         if let Some(t) = frozen1 {
             motion1.freeze(t);
@@ -88,8 +123,8 @@ impl TOIEntry {
             motion2.freeze(t);
         }
 
-        let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
-        let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
+        let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0));
+        let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0));
 
         // println!("start_time: {}", start_time);
 
@@ -105,9 +140,9 @@ impl TOIEntry {
         let res_toi = query_dispatcher
             .nonlinear_time_of_impact(
                 &motion_c1,
-                c1.shape(),
+                co_shape1.as_ref(),
                 &motion_c2,
-                c2.shape(),
+                co_shape2.as_ref(),
                 start_time,
                 end_time,
                 stop_at_penetration,
@@ -119,24 +154,31 @@ impl TOIEntry {
         Some(Self::new(
             toi.toi,
             ch1,
-            c1.parent(),
+            co_parent1.map(|p| p.handle),
             ch2,
-            c2.parent(),
+            co_parent2.map(|p| p.handle),
             is_intersection_test,
             0,
         ))
     }
 
-    fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
-        if body.is_ccd_active() {
+    fn body_motion(
+        (poss, vels, mprops, ccd): (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyCcd,
+        ),
+    ) -> NonlinearRigidMotion {
+        if ccd.ccd_active {
             NonlinearRigidMotion::new(
-                body.position,
-                body.mass_properties.local_com,
-                body.linvel,
-                body.angvel,
+                poss.position,
+                mprops.mass_properties.local_com,
+                vels.linvel,
+                vels.angvel,
             )
         } else {
-            NonlinearRigidMotion::constant_position(body.next_position)
+            NonlinearRigidMotion::constant_position(poss.next_position)
         }
     }
 }

src/dynamics/coefficient_combine_rule.rs

@@ -20,17 +20,13 @@ pub enum CoefficientCombineRule {
     Max,
 }
 
-impl CoefficientCombineRule {
-    pub(crate) fn from_value(val: u8) -> Self {
-        match val {
-            0 => CoefficientCombineRule::Average,
-            1 => CoefficientCombineRule::Min,
-            2 => CoefficientCombineRule::Multiply,
-            3 => CoefficientCombineRule::Max,
-            _ => panic!("Invalid coefficient combine rule."),
-        }
+impl Default for CoefficientCombineRule {
+    fn default() -> Self {
+        CoefficientCombineRule::Average
     }
+}
 
+impl CoefficientCombineRule {
     pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real {
         let effective_rule = rule_value1.max(rule_value2);
 

src/dynamics/island_manager.rs

@@ -0,0 +1,344 @@
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{
+    Joint, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodyType,
+    RigidBodyVelocity,
+};
+use crate::geometry::{ColliderParent, InteractionGraph, NarrowPhase};
+use crate::math::Real;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct IslandManager {
+    pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
+    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
+    pub(crate) active_islands: Vec<usize>,
+    active_set_timestamp: u32,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    can_sleep: Vec<RigidBodyHandle>, // Workspace.
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    stack: Vec<RigidBodyHandle>, // Workspace.
+}
+
+impl IslandManager {
+    pub fn new() -> Self {
+        Self {
+            active_dynamic_set: vec![],
+            active_kinematic_set: vec![],
+            active_islands: vec![],
+            active_set_timestamp: 0,
+            can_sleep: vec![],
+            stack: vec![],
+        }
+    }
+
+    pub(crate) fn num_islands(&self) -> usize {
+        self.active_islands.len() - 1
+    }
+
+    pub fn cleanup_removed_rigid_bodies(
+        &mut self,
+        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
+    ) {
+        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
+
+        for active_set in &mut active_sets {
+            let mut i = 0;
+
+            while i < active_set.len() {
+                let handle = active_set[i];
+                if bodies.get(handle.0).is_none() {
+                    // This rigid-body no longer exists, so we need to remove it from the active set.
+                    active_set.swap_remove(i);
+
+                    if i < active_set.len() {
+                        bodies.map_mut_internal(active_set[i].0, |rb_ids| rb_ids.active_set_id = i);
+                    }
+                } else {
+                    i += 1;
+                }
+            }
+        }
+    }
+
+    pub fn rigid_body_removed(
+        &mut self,
+        removed_handle: RigidBodyHandle,
+        removed_ids: &RigidBodyIds,
+        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
+    ) {
+        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
+
+        for active_set in &mut active_sets {
+            if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
+                active_set.swap_remove(removed_ids.active_set_id);
+
+                if let Some(replacement) = active_set.get(removed_ids.active_set_id) {
+                    bodies.map_mut_internal(replacement.0, |ids| {
+                        ids.active_set_id = removed_ids.active_set_id;
+                    });
+                }
+            }
+        }
+    }
+
+    /// Forces the specified rigid-body to wake up if it is dynamic.
+    ///
+    /// If `strong` is `true` then it is assured that the rigid-body will
+    /// remain awake during multiple subsequent timesteps.
+    pub fn wake_up<Bodies>(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool)
+    where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        // TODO: what about kinematic bodies?
+        let status: RigidBodyType = *bodies.index(handle.0);
+        if status.is_dynamic() {
+            bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
+                activation.wake_up(strong)
+            });
+            bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| {
+                if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) {
+                    ids.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
+            });
+        }
+    }
+
+    /// Iter through all the active kinematic rigid-bodies on this set.
+    pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] {
+        &self.active_kinematic_set[..]
+    }
+
+    /// Iter through all the active dynamic rigid-bodies on this set.
+    pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] {
+        &self.active_dynamic_set[..]
+    }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        &self.active_dynamic_set[island_range]
+    }
+
+    #[inline(always)]
+    pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.active_dynamic_set
+            .iter()
+            .copied()
+            .chain(self.active_kinematic_set.iter().copied())
+    }
+
+    /*
+    #[cfg(feature = "parallel")]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub(crate) fn foreach_active_island_body_mut_internal_parallel<Set>(
+        &self,
+        island_id: usize,
+        bodies: &mut Set,
+        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
+    ) where
+        Set: ComponentSet<T>,
+    {
+        use std::sync::atomic::Ordering;
+
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        let bodies = std::sync::atomic::AtomicPtr::new(&mut bodies as *mut _);
+        self.active_dynamic_set[island_range]
+            .par_iter()
+            .for_each_init(
+                || bodies.load(Ordering::Relaxed),
+                |bodies, handle| {
+                    let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) };
+                    if let Some(rb) = bodies.get_mut_internal(handle.0) {
+                        f(*handle, rb)
+                    }
+                },
+            );
+    }
+     */
+
+    #[cfg(feature = "parallel")]
+    pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
+        self.active_islands[island_id]..self.active_islands[island_id + 1]
+    }
+
+    pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
+        &mut self,
+        bodies: &mut Bodies,
+        colliders: &Colliders,
+        narrow_phase: &NarrowPhase,
+        joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
+        min_island_size: usize,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetOption<ColliderParent>,
+    {
+        assert!(
+            min_island_size > 0,
+            "The minimum island size must be at least 1."
+        );
+
+        // Update the energy of every rigid body and
+        // keep only those that may not sleep.
+        //        let t = instant::now();
+        self.active_set_timestamp += 1;
+        self.stack.clear();
+        self.can_sleep.clear();
+
+        // NOTE: the `.rev()` is here so that two successive timesteps preserve
+        // the order of the bodies in the `active_dynamic_set` vec. This reversal
+        // does not seem to affect performances nor stability. However it makes
+        // debugging slightly nicer so we keep this rev.
+        for h in self.active_dynamic_set.drain(..).rev() {
+            let can_sleep = &mut self.can_sleep;
+            let stack = &mut self.stack;
+
+            let vels: &RigidBodyVelocity = bodies.index(h.0);
+            let pseudo_kinetic_energy = vels.pseudo_kinetic_energy();
+
+            bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
+                update_energy(activation, pseudo_kinetic_energy);
+
+                if activation.energy <= activation.threshold {
+                    // Mark them as sleeping for now. This will
+                    // be set to false during the graph traversal
+                    // if it should not be put to sleep.
+                    activation.sleeping = true;
+                    can_sleep.push(h);
+                } else {
+                    stack.push(h);
+                }
+            });
+        }
+
+        // Read all the contacts and push objects touching touching this rigid-body.
+        #[inline(always)]
+        fn push_contacting_bodies(
+            rb_colliders: &RigidBodyColliders,
+            colliders: &impl ComponentSetOption<ColliderParent>,
+            narrow_phase: &NarrowPhase,
+            stack: &mut Vec<RigidBodyHandle>,
+        ) {
+            for collider_handle in &rb_colliders.0 {
+                if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
+                    for inter in contacts {
+                        for manifold in &inter.2.manifolds {
+                            if !manifold.data.solver_contacts.is_empty() {
+                                let other = crate::utils::select_other(
+                                    (inter.0, inter.1),
+                                    *collider_handle,
+                                );
+                                if let Some(other_body) = colliders.get(other.0) {
+                                    stack.push(other_body.handle);
+                                }
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        // Now iterate on all active kinematic bodies and push all the bodies
+        // touching them to the stack so they can be woken up.
+        for h in self.active_kinematic_set.iter() {
+            let (vels, rb_colliders): (&RigidBodyVelocity, _) = bodies.index_bundle(h.0);
+
+            if vels.is_zero() {
+                // If the kinematic body does not move, it does not have
+                // to wake up any dynamic body.
+                continue;
+            }
+
+            push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
+        }
+
+        //        println!("Selection: {}", instant::now() - t);
+
+        //        let t = instant::now();
+        // Propagation of awake state and awake island computation through the
+        // traversal of the interaction graph.
+        self.active_islands.clear();
+        self.active_islands.push(0);
+
+        // The max avoid underflow when the stack is empty.
+        let mut island_marker = self.stack.len().max(1) - 1;
+
+        while let Some(handle) = self.stack.pop() {
+            let (rb_status, rb_ids, rb_colliders): (
+                &RigidBodyType,
+                &RigidBodyIds,
+                &RigidBodyColliders,
+            ) = bodies.index_bundle(handle.0);
+
+            if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() {
+                // We already visited this body and its neighbors.
+                // Also, we don't propagate awake state through static bodies.
+                continue;
+            }
+
+            if self.stack.len() < island_marker {
+                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                    >= min_island_size
+                {
+                    // We are starting a new island.
+                    self.active_islands.push(self.active_dynamic_set.len());
+                }
+
+                island_marker = self.stack.len();
+            }
+
+            // Transmit the active state to all the rigid-bodies with colliders
+            // in contact or joined with this collider.
+            push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
+
+            for inter in joint_graph.interactions_with(rb_ids.joint_graph_index) {
+                let other = crate::utils::select_other((inter.0, inter.1), handle);
+                self.stack.push(other);
+            }
+
+            bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
+                activation.wake_up(false);
+            });
+            bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| {
+                ids.active_island_id = self.active_islands.len() - 1;
+                ids.active_set_id = self.active_dynamic_set.len();
+                ids.active_set_offset =
+                    ids.active_set_id - self.active_islands[ids.active_island_id];
+                ids.active_set_timestamp = self.active_set_timestamp;
+            });
+
+            self.active_dynamic_set.push(handle);
+        }
+
+        self.active_islands.push(self.active_dynamic_set.len());
+        //        println!(
+        //            "Extraction: {}, num islands: {}",
+        //            instant::now() - t,
+        //            self.active_islands.len() - 1
+        //        );
+
+        // Actually put to sleep bodies which have not been detected as awake.
+        for h in &self.can_sleep {
+            let activation: &RigidBodyActivation = bodies.index(h.0);
+            if activation.sleeping {
+                bodies.set_internal(h.0, RigidBodyVelocity::zero());
+                bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
+                    activation.sleep()
+                });
+            }
+        }
+    }
+}
+
+fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) {
+    let mix_factor = 0.01;
+    let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy;
+    activation.energy = new_energy.min(activation.threshold.abs() * 4.0);
+}

src/dynamics/joint/joint_set.rs

@@ -2,31 +2,33 @@ use super::Joint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
 use crate::data::arena::Arena;
-use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
+use crate::dynamics::{JointParams, RigidBodyHandle};
 
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[repr(transparent)]
-pub struct JointHandle(pub(crate) crate::data::arena::Index);
+pub struct JointHandle(pub crate::data::arena::Index);
 
 impl JointHandle {
     /// Converts this handle into its (index, generation) components.
-    pub fn into_raw_parts(self) -> (usize, u64) {
+    pub fn into_raw_parts(self) -> (u32, u32) {
         self.0.into_raw_parts()
     }
 
     /// Reconstructs an handle from its (index, generation) components.
-    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
         Self(crate::data::arena::Index::from_raw_parts(id, generation))
     }
 
     /// An always-invalid joint handle.
     pub fn invalid() -> Self {
         Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_USIZE,
-            crate::INVALID_U64,
+            crate::INVALID_U32,
+            crate::INVALID_U32,
         ))
     }
 }
@@ -157,7 +159,7 @@ impl JointSet {
     /// Inserts a new joint into this set and retrieve its handle.
     pub fn insert<J>(
         &mut self,
-        bodies: &mut RigidBodySet,
+        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         joint_params: J,
@@ -177,57 +179,64 @@ impl JointSet {
             params: joint_params.into(),
         };
 
-        let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
-        let (rb1, rb2) = (
-            rb1.expect("Attempt to attach a joint to a non-existing body."),
-            rb2.expect("Attempt to attach a joint to a non-existing body."),
-        );
+        let mut graph_index1 = bodies.index(joint.body1.0).joint_graph_index;
+        let mut graph_index2 = bodies.index(joint.body2.0).joint_graph_index;
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
-        {
-            rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index1) {
+            graph_index1 = self.joint_graph.graph.add_node(joint.body1);
+            bodies.map_mut_internal(joint.body1.0, |ids| ids.joint_graph_index = graph_index1);
         }
 
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
-        {
-            rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index2) {
+            graph_index2 = self.joint_graph.graph.add_node(joint.body2);
+            bodies.map_mut_internal(joint.body2.0, |ids| ids.joint_graph_index = graph_index2);
         }
 
-        let id = self
-            .joint_graph
-            .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
-
-        self.joint_ids[handle] = id;
+        self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
         JointHandle(handle)
     }
 
     /// Retrieve all the joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
-    pub(crate) fn select_active_interactions(
+    pub(crate) fn select_active_interactions<Bodies>(
         &self,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         out: &mut Vec<Vec<JointIndex>>,
-    ) {
-        for out_island in &mut out[..bodies.num_islands()] {
+    ) where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyActivation>
+            + ComponentSet<RigidBodyIds>,
+    {
+        for out_island in &mut out[..islands.num_islands()] {
             out_island.clear();
         }
 
         // FIXME: don't iterate through all the interactions.
         for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
             let joint = &edge.weight;
-            let rb1 = &bodies[joint.body1];
-            let rb2 = &bodies[joint.body2];
 
-            if (rb1.is_dynamic() || rb2.is_dynamic())
-                && (!rb1.is_dynamic() || !rb1.is_sleeping())
-                && (!rb2.is_dynamic() || !rb2.is_sleeping())
+            let (status1, activation1, ids1): (
+                &RigidBodyType,
+                &RigidBodyActivation,
+                &RigidBodyIds,
+            ) = bodies.index_bundle(joint.body1.0);
+            let (status2, activation2, ids2): (
+                &RigidBodyType,
+                &RigidBodyActivation,
+                &RigidBodyIds,
+            ) = bodies.index_bundle(joint.body2.0);
+
+            if (status1.is_dynamic() || status2.is_dynamic())
+                && (!status1.is_dynamic() || !activation1.sleeping)
+                && (!status2.is_dynamic() || !activation2.sleeping)
             {
-                let island_index = if !rb1.is_dynamic() {
-                    rb2.active_island_id
+                let island_index = if !status1.is_dynamic() {
+                    ids2.active_island_id
                 } else {
-                    rb1.active_island_id
+                    ids1.active_island_id
                 };
 
                 out[island_index].push(i);
@@ -239,22 +248,28 @@ impl JointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
-    pub fn remove(
+    pub fn remove<Bodies>(
         &mut self,
         handle: JointHandle,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
         wake_up: bool,
-    ) -> Option<Joint> {
+    ) -> Option<Joint>
+    where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
         let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
             // Wake-up the bodies attached to this joint.
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
-                bodies.wake_up(*rb_handle, true);
+                islands.wake_up(bodies, *rb_handle, true);
             }
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
-                bodies.wake_up(*rb_handle, true);
+                islands.wake_up(bodies, *rb_handle, true);
             }
         }
 
@@ -267,11 +282,16 @@ impl JointSet {
         removed_joint
     }
 
-    pub(crate) fn remove_rigid_body(
+    pub(crate) fn remove_rigid_body<Bodies>(
         &mut self,
         deleted_id: RigidBodyGraphIndex,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
         if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
             // We have to delete each joint one by one in order to:
             // - Wake-up the attached bodies.
@@ -292,16 +312,16 @@ impl JointSet {
                 }
 
                 // Wake up the attached bodies.
-                bodies.wake_up(h1, true);
-                bodies.wake_up(h2, true);
+                islands.wake_up(bodies, h1, true);
+                islands.wake_up(bodies, h2, true);
             }
 
             if let Some(other) = self.joint_graph.remove_node(deleted_id) {
                 // One rigid-body joint graph index may have been invalidated
                 // so we need to update it.
-                if let Some(replacement) = bodies.get_mut_internal(other) {
-                    replacement.joint_graph_index = deleted_id;
-                }
+                bodies.map_mut_internal(other.0, |ids: &mut RigidBodyIds| {
+                    ids.joint_graph_index = deleted_id;
+                });
             }
         }
     }

src/dynamics/mod.rs

@@ -3,6 +3,7 @@
 pub use self::ccd::CCDSolver;
 pub use self::coefficient_combine_rule::CoefficientCombineRule;
 pub use self::integration_parameters::IntegrationParameters;
+pub use self::island_manager::IslandManager;
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::joint::JointIndex;
 #[cfg(feature = "dim3")]
@@ -17,19 +18,27 @@ pub use self::joint::{
     PrismaticJoint,
     SpringModel, // GenericJoint
 };
-pub(crate) use self::rigid_body::RigidBodyChanges;
-pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
-pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
+pub use self::rigid_body_components::*;
 #[cfg(not(feature = "parallel"))]
 pub(crate) use self::solver::IslandSolver;
 #[cfg(feature = "parallel")]
 pub(crate) use self::solver::ParallelIslandSolver;
 pub use parry::mass_properties::MassProperties;
 
+#[cfg(feature = "default-sets")]
+pub use self::rigid_body::{RigidBody, RigidBodyBuilder};
+#[cfg(feature = "default-sets")]
+pub use self::rigid_body_set::{BodyPair, RigidBodySet};
+
 mod ccd;
 mod coefficient_combine_rule;
 mod integration_parameters;
+mod island_manager;
 mod joint;
-mod rigid_body;
-mod rigid_body_set;
+mod rigid_body_components;
 mod solver;
+
+#[cfg(feature = "default-sets")]
+mod rigid_body;
+#[cfg(feature = "default-sets")]
+mod rigid_body_set;

src/dynamics/rigid_body_components.rs

@@ -0,0 +1,659 @@
+use crate::data::{ComponentSetMut, ComponentSetOption};
+use crate::dynamics::MassProperties;
+use crate::geometry::{
+    ColliderChanges, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition,
+    ColliderShape, InteractionGraph, RigidBodyGraphIndex,
+};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Translation, Vector};
+use crate::parry::partitioning::IndexedData;
+use crate::utils::WDot;
+use num::Zero;
+
+/// The unique handle of a rigid body added to a `RigidBodySet`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct RigidBodyHandle(pub crate::data::arena::Index);
+
+impl RigidBodyHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (u32, u32) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_U32,
+            crate::INVALID_U32,
+        ))
+    }
+}
+
+impl IndexedData for RigidBodyHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
+
+/// The type of a body, governing the way it is affected by external forces.
+#[deprecated(note = "renamed as RigidBodyType")]
+pub type BodyStatus = RigidBodyType;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The status of a body, governing the way it is affected by external forces.
+pub enum RigidBodyType {
+    /// A `RigidBodyType::Dynamic` body can be affected by all external forces.
+    Dynamic,
+    /// A `RigidBodyType::Static` body cannot be affected by external forces.
+    Static,
+    /// A `RigidBodyType::Kinematic` body cannot be affected by any external forces but can be controlled
+    /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
+    ///
+    /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
+    /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
+    /// modified by the user and is independent from any contact or joint it is involved in.
+    Kinematic,
+    // Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it?
+    // Disabled,
+}
+
+impl RigidBodyType {
+    pub fn is_static(self) -> bool {
+        self == RigidBodyType::Static
+    }
+
+    pub fn is_dynamic(self) -> bool {
+        self == RigidBodyType::Dynamic
+    }
+
+    pub fn is_kinematic(self) -> bool {
+        self == RigidBodyType::Kinematic
+    }
+}
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags describing how the rigid-body has been modified by the user.
+    pub struct RigidBodyChanges: u32 {
+        const MODIFIED    = 1 << 0;
+        const POSITION    = 1 << 1;
+        const SLEEP       = 1 << 2;
+        const COLLIDERS   = 1 << 3;
+        const TYPE        = 1 << 4;
+    }
+}
+
+impl Default for RigidBodyChanges {
+    fn default() -> Self {
+        RigidBodyChanges::empty()
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyPosition {
+    /// The world-space position of the rigid-body.
+    pub position: Isometry<Real>,
+    /// The next position of the rigid-body.
+    ///
+    /// At the beginning of the timestep, and when the
+    /// timestep is complete we must have position == next_position
+    /// except for kinematic bodies.
+    ///
+    /// The next_position is updated after the velocity and position
+    /// resolution. Then it is either validated (ie. we set position := set_position)
+    /// or clamped by CCD.
+    pub next_position: Isometry<Real>,
+}
+
+impl Default for RigidBodyPosition {
+    fn default() -> Self {
+        Self {
+            position: Isometry::identity(),
+            next_position: Isometry::identity(),
+        }
+    }
+}
+
+impl RigidBodyPosition {
+    #[must_use]
+    pub fn interpolate_velocity(&self, inv_dt: Real) -> RigidBodyVelocity {
+        let dpos = self.next_position * self.position.inverse();
+        let angvel;
+        #[cfg(feature = "dim2")]
+        {
+            angvel = dpos.rotation.angle() * inv_dt;
+        }
+        #[cfg(feature = "dim3")]
+        {
+            angvel = dpos.rotation.scaled_axis() * inv_dt;
+        }
+        let linvel = dpos.translation.vector * inv_dt;
+        RigidBodyVelocity { linvel, angvel }
+    }
+
+    #[must_use]
+    pub fn integrate_force_and_velocity(
+        &self,
+        dt: Real,
+        forces: &RigidBodyForces,
+        vels: &RigidBodyVelocity,
+        mprops: &RigidBodyMassProps,
+    ) -> Isometry<Real> {
+        let new_vels = forces.integrate(dt, vels, mprops);
+        new_vels.integrate(dt, &self.position, &mprops.mass_properties.local_com)
+    }
+}
+
+impl<T> From<T> for RigidBodyPosition
+where
+    Isometry<Real>: From<T>,
+{
+    fn from(position: T) -> Self {
+        let position = position.into();
+        Self {
+            position,
+            next_position: position,
+        }
+    }
+}
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub struct RigidBodyMassPropsFlags: u8 {
+        const TRANSLATION_LOCKED = 1 << 0;
+        const ROTATION_LOCKED_X = 1 << 1;
+        const ROTATION_LOCKED_Y = 1 << 2;
+        const ROTATION_LOCKED_Z = 1 << 3;
+        const ROTATION_LOCKED = Self::ROTATION_LOCKED_X.bits | Self::ROTATION_LOCKED_Y.bits | Self::ROTATION_LOCKED_Z.bits;
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyMassProps {
+    /// Flags for locking rotation and translation.
+    pub flags: RigidBodyMassPropsFlags,
+    /// The local mass properties of the rigid-body.
+    pub mass_properties: MassProperties,
+    /// The world-space center of mass of the rigid-body.
+    pub world_com: Point<Real>,
+    /// The inverse mass taking into account translation locking.
+    pub effective_inv_mass: Real,
+    /// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
+    /// taking into account rotation locking.
+    pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
+}
+
+impl Default for RigidBodyMassProps {
+    fn default() -> Self {
+        Self {
+            flags: RigidBodyMassPropsFlags::empty(),
+            mass_properties: MassProperties::zero(),
+            world_com: Point::origin(),
+            effective_inv_mass: 0.0,
+            effective_world_inv_inertia_sqrt: AngularInertia::zero(),
+        }
+    }
+}
+
+impl From<RigidBodyMassPropsFlags> for RigidBodyMassProps {
+    fn from(flags: RigidBodyMassPropsFlags) -> Self {
+        Self {
+            flags,
+            ..Self::default()
+        }
+    }
+}
+
+impl RigidBodyMassProps {
+    #[must_use]
+    pub fn with_translations_locked(mut self) -> Self {
+        self.flags |= RigidBodyMassPropsFlags::TRANSLATION_LOCKED;
+        self
+    }
+
+    pub fn effective_mass(&self) -> Real {
+        crate::utils::inv(self.effective_inv_mass)
+    }
+
+    pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
+        self.world_com = self.mass_properties.world_com(&position);
+        self.effective_inv_mass = self.mass_properties.inv_mass;
+        self.effective_world_inv_inertia_sqrt = self
+            .mass_properties
+            .world_inv_inertia_sqrt(&position.rotation);
+
+        // Take into account translation/rotation locking.
+        if self
+            .flags
+            .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED)
+        {
+            self.effective_inv_mass = 0.0;
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            if self
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
+            {
+                self.effective_world_inv_inertia_sqrt = 0.0;
+            }
+        }
+        #[cfg(feature = "dim3")]
+        {
+            if self
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X)
+            {
+                self.effective_world_inv_inertia_sqrt.m11 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+            }
+
+            if self
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y)
+            {
+                self.effective_world_inv_inertia_sqrt.m22 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+            if self
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
+            {
+                self.effective_world_inv_inertia_sqrt.m33 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyVelocity {
+    /// The linear velocity of the rigid-body.
+    pub linvel: Vector<Real>,
+    /// The angular velocity of the rigid-body.
+    pub angvel: AngVector<Real>,
+}
+
+impl Default for RigidBodyVelocity {
+    fn default() -> Self {
+        Self::zero()
+    }
+}
+
+impl RigidBodyVelocity {
+    #[must_use]
+    pub fn zero() -> Self {
+        Self {
+            linvel: na::zero(),
+            angvel: na::zero(),
+        }
+    }
+
+    #[must_use]
+    pub fn pseudo_kinetic_energy(&self) -> Real {
+        self.linvel.norm_squared() + self.angvel.gdot(self.angvel)
+    }
+
+    #[must_use]
+    pub fn apply_damping(&self, dt: Real, damping: &RigidBodyDamping) -> Self {
+        RigidBodyVelocity {
+            linvel: self.linvel * (1.0 / (1.0 + dt * damping.linear_damping)),
+            angvel: self.angvel * (1.0 / (1.0 + dt * damping.angular_damping)),
+        }
+    }
+
+    #[must_use]
+    pub fn integrate(
+        &self,
+        dt: Real,
+        init_pos: &Isometry<Real>,
+        local_com: &Point<Real>,
+    ) -> Isometry<Real> {
+        let com = init_pos * local_com;
+        let shift = Translation::from(com.coords);
+        let mut result =
+            shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos;
+        result.rotation.renormalize_fast();
+        result
+    }
+
+    #[must_use]
+    pub fn is_zero(&self) -> bool {
+        self.linvel.is_zero() && self.angvel.is_zero()
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyDamping {
+    /// Damping factor for gradually slowing down the translational motion of the rigid-body.
+    pub linear_damping: Real,
+    /// Damping factor for gradually slowing down the angular motion of the rigid-body.
+    pub angular_damping: Real,
+}
+
+impl Default for RigidBodyDamping {
+    fn default() -> Self {
+        Self {
+            linear_damping: 0.0,
+            angular_damping: 0.0,
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyForces {
+    /// Accumulation of external forces (only for dynamic bodies).
+    pub force: Vector<Real>,
+    /// Accumulation of external torques (only for dynamic bodies).
+    pub torque: AngVector<Real>,
+    pub gravity_scale: Real,
+}
+
+impl Default for RigidBodyForces {
+    fn default() -> Self {
+        Self {
+            force: na::zero(),
+            torque: na::zero(),
+            gravity_scale: 1.0,
+        }
+    }
+}
+
+impl RigidBodyForces {
+    #[must_use]
+    pub fn integrate(
+        &self,
+        dt: Real,
+        init_vels: &RigidBodyVelocity,
+        mprops: &RigidBodyMassProps,
+    ) -> RigidBodyVelocity {
+        let linear_acc = self.force * mprops.effective_inv_mass;
+        let angular_acc = mprops.effective_world_inv_inertia_sqrt
+            * (mprops.effective_world_inv_inertia_sqrt * self.torque);
+
+        RigidBodyVelocity {
+            linvel: init_vels.linvel + linear_acc * dt,
+            angvel: init_vels.angvel + angular_acc * dt,
+        }
+    }
+
+    pub fn add_linear_acceleration(&mut self, gravity: &Vector<Real>, mass: Real) {
+        self.force += gravity * self.gravity_scale * mass;
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyCcd {
+    pub ccd_thickness: Real,
+    pub ccd_max_dist: Real,
+    pub ccd_active: bool,
+    pub ccd_enabled: bool,
+}
+
+impl Default for RigidBodyCcd {
+    fn default() -> Self {
+        Self {
+            ccd_thickness: 0.0,
+            ccd_max_dist: 0.0,
+            ccd_active: false,
+            ccd_enabled: false,
+        }
+    }
+}
+
+impl RigidBodyCcd {
+    pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real {
+        #[cfg(feature = "dim2")]
+        return vels.linvel.norm() + vels.angvel.abs() * self.ccd_max_dist;
+        #[cfg(feature = "dim3")]
+        return vels.linvel.norm() + vels.angvel.norm() * self.ccd_max_dist;
+    }
+
+    pub fn is_moving_fast(
+        &self,
+        dt: Real,
+        vels: &RigidBodyVelocity,
+        forces: Option<&RigidBodyForces>,
+    ) -> bool {
+        // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
+        //       should use `self.rb_ccd.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
+        //       is the deepest contact (the contact with the largest penetration depth, i.e., the
+        //       negative `dist` with the largest absolute value.
+        //       However, getting this penetration depth assumes querying the contact graph from
+        //       the narrow-phase, which can be pretty expensive. So we use the CCD thickness
+        //       divided by 10 right now. We will see in practice if this value is OK or if we
+        //       should use a smaller (to be less conservative) or larger divisor (to be more conservative).
+        let threshold = self.ccd_thickness / 10.0;
+
+        if let Some(forces) = forces {
+            let linear_part = (vels.linvel + forces.force * dt).norm();
+            #[cfg(feature = "dim2")]
+            let angular_part = (vels.angvel + forces.torque * dt).abs() * self.ccd_max_dist;
+            #[cfg(feature = "dim3")]
+            let angular_part = (vels.angvel + forces.torque * dt).norm() * self.ccd_max_dist;
+            let vel_with_forces = linear_part + angular_part;
+            vel_with_forces > threshold
+        } else {
+            self.max_point_velocity(vels) * dt > threshold
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyIds {
+    pub joint_graph_index: RigidBodyGraphIndex,
+    pub active_island_id: usize,
+    pub active_set_id: usize,
+    pub active_set_offset: usize,
+    pub active_set_timestamp: u32,
+}
+
+impl Default for RigidBodyIds {
+    fn default() -> Self {
+        Self {
+            joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
+            active_island_id: 0,
+            active_set_id: 0,
+            active_set_offset: 0,
+            active_set_timestamp: 0,
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug)]
+pub struct RigidBodyColliders(pub Vec<ColliderHandle>);
+
+impl Default for RigidBodyColliders {
+    fn default() -> Self {
+        Self(vec![])
+    }
+}
+
+impl RigidBodyColliders {
+    pub fn detach_collider(
+        &mut self,
+        rb_changes: &mut RigidBodyChanges,
+        co_handle: ColliderHandle,
+    ) {
+        if let Some(i) = self.0.iter().position(|e| *e == co_handle) {
+            rb_changes.set(
+                RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
+                true,
+            );
+            self.0.swap_remove(i);
+        }
+    }
+
+    pub fn attach_collider(
+        &mut self,
+        rb_changes: &mut RigidBodyChanges,
+        rb_ccd: &mut RigidBodyCcd,
+        rb_mprops: &mut RigidBodyMassProps,
+        rb_pos: &RigidBodyPosition,
+        co_handle: ColliderHandle,
+        co_pos: &mut ColliderPosition,
+        co_parent: &ColliderParent,
+        co_shape: &ColliderShape,
+        co_mprops: &ColliderMassProperties,
+    ) {
+        rb_changes.set(
+            RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
+            true,
+        );
+
+        co_pos.0 = rb_pos.position * co_parent.pos_wrt_parent;
+        rb_ccd.ccd_thickness = rb_ccd.ccd_thickness.min(co_shape.ccd_thickness());
+
+        let shape_bsphere = co_shape.compute_bounding_sphere(&co_parent.pos_wrt_parent);
+        rb_ccd.ccd_max_dist = rb_ccd
+            .ccd_max_dist
+            .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
+
+        let mass_properties = co_mprops
+            .mass_properties(&**co_shape)
+            .transform_by(&co_parent.pos_wrt_parent);
+        self.0.push(co_handle);
+        rb_mprops.mass_properties += mass_properties;
+        rb_mprops.update_world_mass_properties(&rb_pos.position);
+    }
+
+    pub fn update_positions<Colliders>(
+        &self,
+        colliders: &mut Colliders,
+        modified_colliders: &mut Vec<ColliderHandle>,
+        parent_pos: &Isometry<Real>,
+    ) where
+        Colliders: ComponentSetMut<ColliderPosition>
+            + ComponentSetMut<ColliderChanges>
+            + ComponentSetOption<ColliderParent>,
+    {
+        for handle in &self.0 {
+            // NOTE: the ColliderParent component must exist if we enter this method.
+            let co_parent: &ColliderParent = colliders
+                .get(handle.0)
+                .expect("Could not find the ColliderParent component.");
+            let new_pos = parent_pos * co_parent.pos_wrt_parent;
+
+            // Set the modification flag so we can benefit from the modification-tracking
+            // when updating the narrow-phase/broad-phase afterwards.
+            colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| {
+                if !co_changes.contains(ColliderChanges::MODIFIED) {
+                    modified_colliders.push(*handle);
+                }
+
+                *co_changes |= ColliderChanges::POSITION;
+            });
+            colliders.set_internal(handle.0, ColliderPosition(new_pos));
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, Copy)]
+pub struct RigidBodyDominance(pub i8);
+
+impl Default for RigidBodyDominance {
+    fn default() -> Self {
+        RigidBodyDominance(0)
+    }
+}
+
+impl RigidBodyDominance {
+    pub fn effective_group(&self, status: &RigidBodyType) -> i16 {
+        if status.is_dynamic() {
+            self.0 as i16
+        } else {
+            i8::MAX as i16 + 1
+        }
+    }
+}
+
+/// The rb_activation status of a body.
+///
+/// This controls whether a body is sleeping or not.
+/// If the threshold is negative, the body never sleeps.
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct RigidBodyActivation {
+    /// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
+    pub threshold: Real,
+    /// The current pseudo-kinetic energy of the body.
+    pub energy: Real,
+    /// Is this body already sleeping?
+    pub sleeping: bool,
+}
+
+impl Default for RigidBodyActivation {
+    fn default() -> Self {
+        Self::new_active()
+    }
+}
+
+impl RigidBodyActivation {
+    /// The default amount of energy bellow which a body can be put to sleep by nphysics.
+    pub fn default_threshold() -> Real {
+        0.01
+    }
+
+    /// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
+    pub fn new_active() -> Self {
+        RigidBodyActivation {
+            threshold: Self::default_threshold(),
+            energy: Self::default_threshold() * 4.0,
+            sleeping: false,
+        }
+    }
+
+    /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
+    pub fn new_inactive() -> Self {
+        RigidBodyActivation {
+            threshold: Self::default_threshold(),
+            energy: 0.0,
+            sleeping: true,
+        }
+    }
+
+    /// Returns `true` if the body is not asleep.
+    #[inline]
+    pub fn is_active(&self) -> bool {
+        self.energy != 0.0
+    }
+
+    #[inline]
+    pub fn wake_up(&mut self, strong: bool) {
+        self.sleeping = false;
+        if strong || self.energy == 0.0 {
+            self.energy = self.threshold.abs() * 2.0;
+        }
+    }
+
+    #[inline]
+    pub fn sleep(&mut self) {
+        self.energy = 0.0;
+        self.sleeping = true;
+    }
+}

src/dynamics/rigid_body_set.rs

@@ -1,48 +1,19 @@
 #[cfg(feature = "parallel")]
 use rayon::prelude::*;
 
-use crate::data::arena::Arena;
-use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges};
-use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
+use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{
+    IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
+    RigidBodyType,
+};
+use crate::dynamics::{
+    JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+};
+use crate::geometry::ColliderSet;
 use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
 
-/// The unique handle of a rigid body added to a `RigidBodySet`.
-#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[repr(transparent)]
-pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
-
-impl RigidBodyHandle {
-    /// Converts this handle into its (index, generation) components.
-    pub fn into_raw_parts(self) -> (usize, u64) {
-        self.0.into_raw_parts()
-    }
-
-    /// Reconstructs an handle from its (index, generation) components.
-    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(id, generation))
-    }
-
-    /// An always-invalid rigid-body handle.
-    pub fn invalid() -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_USIZE,
-            crate::INVALID_U64,
-        ))
-    }
-}
-
-impl IndexedData for RigidBodyHandle {
-    fn default() -> Self {
-        Self(IndexedData::default())
-    }
-
-    fn index(&self) -> usize {
-        self.0.index()
-    }
-}
-
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A pair of rigid body handles.
@@ -69,38 +40,75 @@ pub struct RigidBodySet {
     // parallelism because the `Receiver` breaks the Sync impl.
     // Could we avoid this?
     pub(crate) bodies: Arena<RigidBody>,
-    pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
-    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
-    // Set of inactive bodies which have been modified.
-    // This typically include static bodies which have been modified.
-    pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
-    pub(crate) active_islands: Vec<usize>,
-    active_set_timestamp: u32,
     pub(crate) modified_bodies: Vec<RigidBodyHandle>,
-    pub(crate) modified_all_bodies: bool,
-    #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    can_sleep: Vec<RigidBodyHandle>, // Workspace.
-    #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    stack: Vec<RigidBodyHandle>, // Workspace.
 }
 
+macro_rules! impl_field_component_set(
+    ($T: ty, $field: ident) => {
+        impl ComponentSetOption<$T> for RigidBodySet {
+            fn get(&self, handle: crate::data::Index) -> Option<&$T> {
+                self.get(RigidBodyHandle(handle)).map(|b| &b.$field)
+            }
+        }
+
+        impl ComponentSet<$T> for RigidBodySet {
+            fn size_hint(&self) -> usize {
+                self.len()
+            }
+
+            #[inline(always)]
+            fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
+                for (handle, body) in self.bodies.iter() {
+                    f(handle, &body.$field)
+                }
+            }
+        }
+
+        impl ComponentSetMut<$T> for RigidBodySet {
+            fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
+                if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) {
+                    rb.$field = val;
+                }
+            }
+
+            #[inline(always)]
+            fn map_mut_internal<Result>(
+                &mut self,
+                handle: crate::data::Index,
+                f: impl FnOnce(&mut $T) -> Result,
+            ) -> Option<Result> {
+                self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field))
+            }
+        }
+    }
+);
+
+impl_field_component_set!(RigidBodyPosition, rb_pos);
+impl_field_component_set!(RigidBodyMassProps, rb_mprops);
+impl_field_component_set!(RigidBodyVelocity, rb_vels);
+impl_field_component_set!(RigidBodyDamping, rb_damping);
+impl_field_component_set!(RigidBodyForces, rb_forces);
+impl_field_component_set!(RigidBodyCcd, rb_ccd);
+impl_field_component_set!(RigidBodyIds, rb_ids);
+impl_field_component_set!(RigidBodyType, rb_type);
+impl_field_component_set!(RigidBodyActivation, rb_activation);
+impl_field_component_set!(RigidBodyColliders, rb_colliders);
+impl_field_component_set!(RigidBodyDominance, rb_dominance);
+impl_field_component_set!(RigidBodyChanges, changes);
+
 impl RigidBodySet {
     /// Create a new empty set of rigid bodies.
     pub fn new() -> Self {
         RigidBodySet {
             bodies: Arena::new(),
-            active_dynamic_set: Vec::new(),
-            active_kinematic_set: Vec::new(),
-            modified_inactive_set: Vec::new(),
-            active_islands: Vec::new(),
-            active_set_timestamp: 0,
             modified_bodies: Vec::new(),
-            modified_all_bodies: false,
-            can_sleep: Vec::new(),
-            stack: Vec::new(),
         }
     }
 
+    pub(crate) fn take_modified(&mut self) -> Vec<RigidBodyHandle> {
+        std::mem::replace(&mut self.modified_bodies, vec![])
+    }
+
     /// The number of rigid bodies on this set.
     pub fn len(&self) -> usize {
         self.bodies.len()
@@ -121,18 +129,10 @@ impl RigidBodySet {
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         rb.reset_internal_references();
-        rb.changes.set(RigidBodyChanges::all(), true);
+        rb.changes_mut_internal().set(RigidBodyChanges::all(), true);
 
         let handle = RigidBodyHandle(self.bodies.insert(rb));
         self.modified_bodies.push(handle);
-
-        let rb = &mut self.bodies[handle.0];
-
-        if rb.is_kinematic() {
-            rb.active_set_id = self.active_kinematic_set.len();
-            self.active_kinematic_set.push(handle);
-        }
-
         handle
     }
 
@@ -140,6 +140,7 @@ impl RigidBodySet {
     pub fn remove(
         &mut self,
         handle: RigidBodyHandle,
+        islands: &mut IslandManager,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
     ) -> Option<RigidBody> {
@@ -147,55 +148,23 @@ impl RigidBodySet {
         /*
          * Update active sets.
          */
-        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
-
-        for active_set in &mut active_sets {
-            if active_set.get(rb.active_set_id) == Some(&handle) {
-                active_set.swap_remove(rb.active_set_id);
-
-                if let Some(replacement) = active_set.get(rb.active_set_id) {
-                    self.bodies[replacement.0].active_set_id = rb.active_set_id;
-                }
-            }
-        }
+        islands.rigid_body_removed(handle, &rb.rb_ids, self);
 
         /*
          * Remove colliders attached to this rigid-body.
          */
-        for collider in &rb.colliders {
-            colliders.remove(*collider, self, false);
+        for collider in rb.colliders() {
+            colliders.remove(*collider, islands, self, false);
         }
 
         /*
          * Remove joints attached to this rigid-body.
          */
-        joints.remove_rigid_body(rb.joint_graph_index, self);
+        joints.remove_rigid_body(rb.rb_ids.joint_graph_index, islands, self);
 
         Some(rb)
     }
 
-    pub(crate) fn num_islands(&self) -> usize {
-        self.active_islands.len() - 1
-    }
-
-    /// Forces the specified rigid-body to wake up if it is dynamic.
-    ///
-    /// If `strong` is `true` then it is assured that the rigid-body will
-    /// remain awake during multiple subsequent timesteps.
-    pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
-        if let Some(rb) = self.bodies.get_mut(handle.0) {
-            // TODO: what about kinematic bodies?
-            if rb.is_dynamic() {
-                rb.wake_up(strong);
-
-                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
-                    rb.active_set_id = self.active_dynamic_set.len();
-                    self.active_dynamic_set.push(handle);
-                }
-            }
-        }
-    }
-
     /// Gets the rigid-body with the given handle without a known generation.
     ///
     /// This is useful when you know you want the rigid-body at position `i` but
@@ -224,12 +193,7 @@ impl RigidBodySet {
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
         let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
         let handle = RigidBodyHandle(handle);
-        Self::mark_as_modified(
-            handle,
-            rb,
-            &mut self.modified_bodies,
-            self.modified_all_bodies,
-        );
+        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
         Some((rb, handle))
     }
 
@@ -238,14 +202,13 @@ impl RigidBodySet {
         self.bodies.get(handle.0)
     }
 
-    fn mark_as_modified(
+    pub(crate) fn mark_as_modified(
         handle: RigidBodyHandle,
         rb: &mut RigidBody,
         modified_bodies: &mut Vec<RigidBodyHandle>,
-        modified_all_bodies: bool,
     ) {
-        if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
-            rb.changes = RigidBodyChanges::MODIFIED;
+        if !rb.changes().contains(RigidBodyChanges::MODIFIED) {
+            *rb.changes_mut_internal() = RigidBodyChanges::MODIFIED;
             modified_bodies.push(handle);
         }
     }
@@ -254,12 +217,7 @@ impl RigidBodySet {
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
-        Self::mark_as_modified(
-            handle,
-            result,
-            &mut self.modified_bodies,
-            self.modified_all_bodies,
-        );
+        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
         Some(result)
     }
 
@@ -274,23 +232,10 @@ impl RigidBodySet {
         handle: RigidBodyHandle,
     ) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
-        Self::mark_as_modified(
-            handle,
-            result,
-            &mut self.modified_bodies,
-            self.modified_all_bodies,
-        );
+        Self::mark_as_modified(handle, result, &mut self.modified_bodies);
         Some(result)
     }
 
-    pub(crate) fn get2_mut_internal(
-        &mut self,
-        h1: RigidBodyHandle,
-        h2: RigidBodyHandle,
-    ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
-        self.bodies.get2_mut(h1.0, h2.0)
-    }
-
     /// Iterates through all the rigid-bodies on this set.
     pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
         self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
@@ -300,431 +245,11 @@ impl RigidBodySet {
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
         self.modified_bodies.clear();
-        self.modified_all_bodies = true;
-        self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
-    }
-
-    /// Iter through all the active kinematic rigid-bodies on this set.
-    pub fn iter_active_kinematic<'a>(
-        &'a self,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let bodies: &'a _ = &self.bodies;
-        self.active_kinematic_set
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    /// Iter through all the active dynamic rigid-bodies on this set.
-    pub fn iter_active_dynamic<'a>(
-        &'a self,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let bodies: &'a _ = &self.bodies;
-        self.active_dynamic_set
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    #[cfg(not(feature = "parallel"))]
-    pub(crate) fn iter_active_island<'a>(
-        &'a self,
-        island_id: usize,
-    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        let bodies: &'a _ = &self.bodies;
-        self.active_dynamic_set[island_range]
-            .iter()
-            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
-    }
-
-    /// Applies the given function on all the active dynamic rigid-bodies
-    /// contained by this set.
-    #[inline(always)]
-    #[cfg(not(feature = "dev-remove-slow-accessors"))]
-    pub fn foreach_active_dynamic_body_mut(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                Self::mark_as_modified(
-                    *handle,
-                    rb,
-                    &mut self.modified_bodies,
-                    self.modified_all_bodies,
-                );
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-
-        for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_dynamic_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_active_kinematic_body_mut_internal(
-        &mut self,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    #[cfg(not(feature = "parallel"))]
-    pub(crate) fn foreach_active_island_body_mut_internal(
-        &mut self,
-        island_id: usize,
-        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
-    ) {
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        for handle in &self.active_dynamic_set[island_range] {
-            if let Some(rb) = self.bodies.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[cfg(feature = "parallel")]
-    #[inline(always)]
-    #[allow(dead_code)]
-    pub(crate) fn foreach_active_island_body_mut_internal_parallel(
-        &mut self,
-        island_id: usize,
-        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
-    ) {
-        use std::sync::atomic::Ordering;
-
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
-        self.active_dynamic_set[island_range]
-            .par_iter()
-            .for_each_init(
-                || bodies.load(Ordering::Relaxed),
-                |bodies, handle| {
-                    let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
-                    if let Some(rb) = bodies.get_mut(handle.0) {
-                        f(*handle, rb)
-                    }
-                },
-            );
-    }
-
-    // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
-    //     &self.active_dynamic_set
-    // }
-
-    pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
-        self.active_islands[island_id]..self.active_islands[island_id + 1]
-    }
-
-    pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
-        &self.active_dynamic_set[self.active_island_range(island_id)]
-    }
-
-    // Utility function to avoid some borrowing issue in the `maintain` method.
-    fn maintain_one(
-        bodies: &mut Arena<RigidBody>,
-        colliders: &mut ColliderSet,
-        handle: RigidBodyHandle,
-        modified_inactive_set: &mut Vec<RigidBodyHandle>,
-        active_kinematic_set: &mut Vec<RigidBodyHandle>,
-        active_dynamic_set: &mut Vec<RigidBodyHandle>,
-    ) {
-        enum FinalAction {
-            UpdateActiveKinematicSetId,
-            UpdateActiveDynamicSetId,
-        }
-
-        if let Some(rb) = bodies.get_mut(handle.0) {
-            let mut final_action = None;
-
-            // The body's status changed. We need to make sure
-            // it is on the correct active set.
-            if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
-                match rb.body_status() {
-                    BodyStatus::Dynamic => {
-                        // Remove from the active kinematic set if it was there.
-                        if active_kinematic_set.get(rb.active_set_id) == Some(&handle) {
-                            active_kinematic_set.swap_remove(rb.active_set_id);
-                            final_action =
-                                Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id));
-                        }
-
-                        // Add to the active dynamic set.
-                        rb.wake_up(true);
-                        // Make sure the sleep change flag is set (even if for some
-                        // reasons the rigid-body was already awake) to make
-                        // sure the code handling sleeping change adds the body to
-                        // the active_dynamic_set.
-                        rb.changes.set(RigidBodyChanges::SLEEP, true);
-                    }
-                    BodyStatus::Kinematic => {
-                        // Remove from the active dynamic set if it was there.
-                        if active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
-                            active_dynamic_set.swap_remove(rb.active_set_id);
-                            final_action =
-                                Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id));
-                        }
-
-                        // Add to the active kinematic set.
-                        if active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
-                            rb.active_set_id = active_kinematic_set.len();
-                            active_kinematic_set.push(handle);
-                        }
-                    }
-                    BodyStatus::Static => {}
-                }
-            }
-
-            // Update the positions of the colliders.
-            if rb.changes.contains(RigidBodyChanges::POSITION)
-                || rb.changes.contains(RigidBodyChanges::COLLIDERS)
-            {
-                rb.update_colliders_positions(colliders);
-
-                if rb.is_static() {
-                    modified_inactive_set.push(handle);
-                }
-
-                if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle)
-                {
-                    rb.active_set_id = active_kinematic_set.len();
-                    active_kinematic_set.push(handle);
-                }
-            }
-
-            // Push the body to the active set if it is not
-            // sleeping and if it is not already inside of the active set.
-            if rb.changes.contains(RigidBodyChanges::SLEEP)
-                && !rb.is_sleeping() // May happen if the body was put to sleep manually.
-                && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
-                && active_dynamic_set.get(rb.active_set_id) != Some(&handle)
-            {
-                rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
-                active_dynamic_set.push(handle);
-            }
-
-            rb.changes = RigidBodyChanges::empty();
-
-            // Adjust some ids, if needed.
-            if let Some((action, id)) = final_action {
-                let active_set = match action {
-                    FinalAction::UpdateActiveKinematicSetId => active_kinematic_set,
-                    FinalAction::UpdateActiveDynamicSetId => active_dynamic_set,
-                };
-
-                if id < active_set.len() {
-                    if let Some(rb2) = bodies.get_mut(active_set[id].0) {
-                        rb2.active_set_id = id;
-                    }
-                }
-            }
-        }
-    }
-
-    pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
-        if self.modified_all_bodies {
-            // Unfortunately, we have to push all the bodies to `modified_bodies`
-            // instead of just calling `maintain_one` on each element i
-            // `self.bodies.iter_mut()` because otherwise it would be difficult to
-            // handle the final  change of active_set_id in Self::maintain_one
-            // (because it has  to modify another rigid-body because of the swap-remove.
-            // So this causes borrowing problems if we do this while iterating
-            // through self.bodies.iter_mut()).
-            for (handle, _) in self.bodies.iter_mut() {
-                self.modified_bodies.push(RigidBodyHandle(handle));
-            }
-        }
-
-        for handle in self.modified_bodies.drain(..) {
-            Self::maintain_one(
-                &mut self.bodies,
-                colliders,
-                handle,
-                &mut self.modified_inactive_set,
-                &mut self.active_kinematic_set,
-                &mut self.active_dynamic_set,
-            )
-        }
-
-        if self.modified_all_bodies {
-            self.modified_bodies.shrink_to_fit(); // save some memory.
-            self.modified_all_bodies = false;
-        }
-    }
-
-    pub(crate) fn update_active_set_with_contacts(
-        &mut self,
-        colliders: &ColliderSet,
-        narrow_phase: &NarrowPhase,
-        joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
-        min_island_size: usize,
-    ) {
-        assert!(
-            min_island_size > 0,
-            "The minimum island size must be at least 1."
-        );
-
-        // Update the energy of every rigid body and
-        // keep only those that may not sleep.
-        //        let t = instant::now();
-        self.active_set_timestamp += 1;
-        self.stack.clear();
-        self.can_sleep.clear();
-
-        // NOTE: the `.rev()` is here so that two successive timesteps preserve
-        // the order of the bodies in the `active_dynamic_set` vec. This reversal
-        // does not seem to affect performances nor stability. However it makes
-        // debugging slightly nicer so we keep this rev.
-        for h in self.active_dynamic_set.drain(..).rev() {
-            let rb = &mut self.bodies[h.0];
-            rb.update_energy();
-            if rb.activation.energy <= rb.activation.threshold {
-                // Mark them as sleeping for now. This will
-                // be set to false during the graph traversal
-                // if it should not be put to sleep.
-                rb.activation.sleeping = true;
-                self.can_sleep.push(h);
-            } else {
-                self.stack.push(h);
-            }
-        }
-
-        // Read all the contacts and push objects touching touching this rigid-body.
-        #[inline(always)]
-        fn push_contacting_bodies(
-            rb: &RigidBody,
-            colliders: &ColliderSet,
-            narrow_phase: &NarrowPhase,
-            stack: &mut Vec<RigidBodyHandle>,
-        ) {
-            for collider_handle in &rb.colliders {
-                if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
-                    for inter in contacts {
-                        for manifold in &inter.2.manifolds {
-                            if !manifold.data.solver_contacts.is_empty() {
-                                let other = crate::utils::select_other(
-                                    (inter.0, inter.1),
-                                    *collider_handle,
-                                );
-                                let other_body = colliders[other].parent;
-                                stack.push(other_body);
-                                break;
-                            }
-                        }
-                    }
-                }
-            }
-        }
-
-        // Now iterate on all active kinematic bodies and push all the bodies
-        // touching them to the stack so they can be woken up.
-        for h in self.active_kinematic_set.iter() {
-            let rb = &self.bodies[h.0];
-
-            if !rb.is_moving() {
-                // If the kinematic body does not move, it does not have
-                // to wake up any dynamic body.
-                continue;
-            }
-
-            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
-        }
-
-        //        println!("Selection: {}", instant::now() - t);
-
-        //        let t = instant::now();
-        // Propagation of awake state and awake island computation through the
-        // traversal of the interaction graph.
-        self.active_islands.clear();
-        self.active_islands.push(0);
-
-        // The max avoid underflow when the stack is empty.
-        let mut island_marker = self.stack.len().max(1) - 1;
-
-        while let Some(handle) = self.stack.pop() {
-            let rb = &mut self.bodies[handle.0];
-
-            if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
-                // We already visited this body and its neighbors.
-                // Also, we don't propagate awake state through static bodies.
-                continue;
-            }
-
-            if self.stack.len() < island_marker {
-                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
-                    >= min_island_size
-                {
-                    // We are starting a new island.
-                    self.active_islands.push(self.active_dynamic_set.len());
-                }
-
-                island_marker = self.stack.len();
-            }
-
-            rb.wake_up(false);
-            rb.active_island_id = self.active_islands.len() - 1;
-            rb.active_set_id = self.active_dynamic_set.len();
-            rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
-            rb.active_set_timestamp = self.active_set_timestamp;
-            self.active_dynamic_set.push(handle);
-
-            // Transmit the active state to all the rigid-bodies with colliders
-            // in contact or joined with this collider.
-            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
-
-            for inter in joint_graph.interactions_with(rb.joint_graph_index) {
-                let other = crate::utils::select_other((inter.0, inter.1), handle);
-                self.stack.push(other);
-            }
-        }
-
-        self.active_islands.push(self.active_dynamic_set.len());
-        //        println!(
-        //            "Extraction: {}, num islands: {}",
-        //            instant::now() - t,
-        //            self.active_islands.len() - 1
-        //        );
-
-        // Actually put to sleep bodies which have not been detected as awake.
-        //        let t = instant::now();
-        for h in &self.can_sleep {
-            let b = &mut self.bodies[h.0];
-            if b.activation.sleeping {
-                b.sleep();
-            }
-        }
-        //        println!("Activation: {}", instant::now() - t);
+        let modified_bodies = &mut self.modified_bodies;
+        self.bodies.iter_mut().map(move |(h, b)| {
+            modified_bodies.push(RigidBodyHandle(h));
+            (RigidBodyHandle(h), b)
+        })
     }
 }
 
@@ -736,16 +261,19 @@ impl Index<RigidBodyHandle> for RigidBodySet {
     }
 }
 
+impl Index<crate::data::Index> for RigidBodySet {
+    type Output = RigidBody;
+
+    fn index(&self, index: crate::data::Index) -> &RigidBody {
+        &self.bodies[index]
+    }
+}
+
 #[cfg(not(feature = "dev-remove-slow-accessors"))]
 impl IndexMut<RigidBodyHandle> for RigidBodySet {
     fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
         let rb = &mut self.bodies[handle.0];
-        Self::mark_as_modified(
-            handle,
-            rb,
-            &mut self.modified_bodies,
-            self.modified_all_bodies,
-        );
+        Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
         rb
     }
 }

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);

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)
+                };
 
-                // FIXME: don't generate interactions between static bodies in the first place.
+                let is_static1 = !status1.is_dynamic();
+                let is_static2 = !status2.is_dynamic();
+
+                // 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;

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();
         }
     }

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,
             }
         }
     }

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;

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,
         }
     }
 

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 (poss1, vels1, mprops1, ids1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 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 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]);
-        let local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
-        );
+        let (poss1, vels1, mprops1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor2
+        } else {
+            cparams[ii].local_anchor1
+        }]);
 
-        let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
-        );
-        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        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_anchor2 = Point::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            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;
@@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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_lambda2.linear -= self.impulse * self.im2;
@@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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 angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -1,4 +1,6 @@
-use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
 use crate::utils::WAngularInertia;
 
@@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint {
 }
 
 impl FixedPositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
+    pub fn from_params(
+        rb1: (&RigidBodyMassProps, &RigidBodyIds),
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
+        cparams: &FixedJoint,
+    ) -> Self {
+        let (mprops1, ids1) = rb1;
+        let (mprops2, ids2) = rb2;
+
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
         Self {
             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,
             im1,
             im2,
             ii1,
             ii2,
-            local_com1: rb1.mass_properties.local_com,
-            local_com2: rb2.mass_properties.local_com,
+            local_com1: mprops1.mass_properties.local_com,
+            local_com2: mprops2.mass_properties.local_com,
             lin_inv_lhs,
             ang_inv_lhs,
         }
@@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint {
 
 impl FixedPositionGroundConstraint {
     pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: &RigidBodyPosition,
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
         cparams: &FixedJoint,
         flipped: bool,
     ) -> Self {
+        let poss1 = rb1;
+        let (mprops2, ids2) = rb2;
+
         let anchor1;
         let local_anchor2;
 
         if flipped {
-            anchor1 = rb1.next_position * cparams.local_anchor2;
+            anchor1 = poss1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
         } else {
-            anchor1 = rb1.next_position * cparams.local_anchor1;
+            anchor1 = poss1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
         };
 
         Self {
             anchor1,
             local_anchor2,
-            position2: rb2.active_set_offset,
-            im2: rb2.effective_inv_mass,
-            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
-            local_com2: rb2.mass_properties.local_com,
+            position2: ids2.active_set_offset,
+            im2: mprops2.effective_inv_mass,
+            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: mprops2.mass_properties.local_com,
             impulse: 0.0,
         }
     }

src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs

@@ -1,5 +1,7 @@
 use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
-use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, 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 WFixedPositionConstraint {
 
 impl WFixedPositionConstraint {
     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: [&FixedJoint; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| FixedPositionConstraint::from_params(
+                (rbs1.0[ii], rbs1.1[ii]),
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii]
+            )],
         }
     }
 
@@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint {
 
 impl WFixedPositionGroundConstraint {
     pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
+        rbs2: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&FixedJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
+                rbs1[ii],
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii],
+                flipped[ii]
+            )],
         }
     }
 

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -1,6 +1,7 @@
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -45,18 +46,31 @@ impl FixedVelocityConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         cparams: &FixedJoint,
     ) -> Self {
-        let anchor1 = rb1.position * cparams.local_anchor1;
-        let anchor2 = rb2.position * cparams.local_anchor2;
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1.translation.vector - rb1.world_com.coords;
-        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let (poss1, vels1, mprops1, ids1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
+        let anchor1 = poss1.position * cparams.local_anchor1;
+        let anchor2 = poss2.position * cparams.local_anchor2;
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - mprops1.world_com.coords;
+        let r2 = anchor2.translation.vector - mprops2.world_com.coords;
         let rmat1 = r1.gcross_matrix();
         let rmat2 = r2.gcross_matrix();
 
@@ -99,8 +113,9 @@ impl FixedVelocityConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
 
-        let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
-        let ang_dvel = -rb1.angvel + rb2.angvel;
+        let lin_dvel =
+            -vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
+        let ang_dvel = -vels1.angvel + vels2.angvel;
 
         #[cfg(feature = "dim2")]
         let mut rhs =
@@ -133,14 +148,14 @@ impl FixedVelocityConstraint {
 
         FixedVelocityConstraint {
             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,
             ii1,
             ii2,
-            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,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r1,
@@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         cparams: &FixedJoint,
         flipped: bool,
     ) -> Self {
+        let (poss1, vels1, mprops1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         let (anchor1, anchor2) = if flipped {
             (
-                rb1.position * cparams.local_anchor2,
-                rb2.position * cparams.local_anchor1,
+                poss1.position * cparams.local_anchor2,
+                poss2.position * cparams.local_anchor1,
             )
         } else {
             (
-                rb1.position * cparams.local_anchor1,
-                rb2.position * cparams.local_anchor2,
+                poss1.position * cparams.local_anchor1,
+                poss2.position * cparams.local_anchor2,
             )
         };
 
-        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r1 = anchor1.translation.vector - mprops1.world_com.coords;
 
-        let im2 = rb2.effective_inv_mass;
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let im2 = mprops2.effective_inv_mass;
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r2 = anchor2.translation.vector - mprops2.world_com.coords;
         let rmat2 = r2.gcross_matrix();
 
         #[allow(unused_mut)] // For 2D.
@@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
 
-        let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
-        let ang_dvel = rb2.angvel - rb1.angvel;
+        let lin_dvel =
+            vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
+        let ang_dvel = vels2.angvel - vels1.angvel;
 
         #[cfg(feature = "dim2")]
         let mut rhs =
@@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint {
 
         FixedVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: ids2.active_set_offset,
             im2,
             ii2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r2,

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
 
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{
     AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
@@ -53,33 +54,46 @@ impl WFixedVelocityConstraint {
     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: [&FixedJoint; 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 (poss1, vels1, mprops1, ids1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
-        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        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 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
+        let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
+        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -157,8 +171,7 @@ impl WFixedVelocityConstraint {
 
             #[cfg(feature = "dim3")]
             {
-                let ang_err =
-                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
                 rhs += Vector6::new(
                     lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
                 ) * velocity_based_erp_inv_dt;
@@ -185,20 +198,16 @@ impl WFixedVelocityConstraint {
 
     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
+            ]),
         };
 
         let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -229,20 +238,16 @@ impl WFixedVelocityConstraint {
 
     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);
@@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint {
     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: [&FixedJoint; 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]);
+        let (poss1, vels1, mprops1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 local_anchor1 = Isometry::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
-        );
-        let local_anchor2 = Isometry::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
-        );
-        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        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 = Isometry::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor2
+        } else {
+            cparams[ii].local_anchor1
+        }]);
+        let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            cparams[ii].local_anchor2
+        }]);
+        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint {
 
             #[cfg(feature = "dim3")]
             {
-                let ang_err =
-                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
                 rhs += Vector6::new(
                     lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
                 ) * velocity_based_erp_inv_dt;
@@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/joint_constraint/generic_position_constraint.rs

@@ -1,6 +1,6 @@
 use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
 use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{GenericJoint, IntegrationParameters};
 use crate::math::{
     AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
     DIM,

src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs

@@ -15,7 +15,11 @@ impl WGenericPositionConstraint {
         cparams: [&GenericJoint; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| GenericPositionConstraint::from_params(
+                rbs1[ii],
+                rbs2[ii],
+                cparams[ii]
+            )],
         }
     }
 
@@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint {
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| GenericPositionGroundConstraint::from_params(
+                rbs1[ii],
+                rbs2[ii],
+                cparams[ii],
+                flipped[ii]
+            )],
         }
     }
 

src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs

@@ -50,8 +50,8 @@ impl GenericVelocityConstraint {
         rb1: &RigidBody,
         rb2: &RigidBody,
     ) -> SpatialVector<Real> {
-        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
-            + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
+        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
+            + basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
         let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
             + basis2.inverse_transform_vector(&rb2.angvel);
 
@@ -203,8 +203,8 @@ impl GenericVelocityConstraint {
         rb2: &RigidBody,
         joint: &GenericJoint,
     ) -> Self {
-        let anchor1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * joint.local_anchor2;
+        let anchor1 = rb1.position() * joint.local_anchor1;
+        let anchor2 = rb2.position() * joint.local_anchor2;
         let basis1 = anchor1.rotation;
         let basis2 = anchor2.rotation;
         let im1 = rb1.effective_inv_mass;
@@ -405,13 +405,13 @@ impl GenericVelocityGroundConstraint {
     ) -> Self {
         let (anchor1, anchor2) = if flipped {
             (
-                rb1.position * joint.local_anchor2,
-                rb2.position * joint.local_anchor1,
+                rb1.position() * joint.local_anchor2,
+                rb2.position() * joint.local_anchor1,
             )
         } else {
             (
-                rb1.position * joint.local_anchor1,
-                rb2.position * joint.local_anchor2,
+                rb1.position() * joint.local_anchor1,
+                rb2.position() * joint.local_anchor2,
             )
         };
 

src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs

@@ -57,29 +57,29 @@ impl WGenericVelocityConstraint {
         rbs2: [&RigidBody; SIMD_WIDTH],
         cparams: [&GenericJoint; 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 position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
+        let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
+        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
+        let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
+        let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
+            |ii| rbs1[ii].effective_world_inv_inertia_sqrt
+        ]);
+        let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
 
-        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 position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
+        let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
+        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
+        let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
+        let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
+            |ii| rbs2[ii].effective_world_inv_inertia_sqrt
+        ]);
+        let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
 
-        let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
-        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
+        let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
+        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -160,20 +160,16 @@ impl WGenericVelocityConstraint {
 
     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
+            ]),
         };
 
         let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -204,20 +200,16 @@ impl WGenericVelocityConstraint {
 
     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);
@@ -306,28 +298,32 @@ impl WGenericVelocityGroundConstraint {
         cparams: [&GenericJoint; 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]);
+        let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
+        let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
+        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
+        let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
 
-        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 position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
+        let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
+        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
+        let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
+        let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
+            |ii| rbs2[ii].effective_world_inv_inertia_sqrt
+        ]);
+        let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
 
-        let local_anchor1 = Isometry::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
-        );
-        let local_anchor2 = Isometry::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
-        );
-        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor2
+        } else {
+            cparams[ii].local_anchor1
+        }]);
+        let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            cparams[ii].local_anchor2
+        }]);
+        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -395,12 +391,10 @@ impl WGenericVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -422,12 +416,10 @@ impl WGenericVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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,

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),
                 )

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -1,4 +1,6 @@
-use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
+use crate::dynamics::{
+    IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::WAngularInertia;
 use na::Unit;
@@ -27,11 +29,18 @@ pub(crate) struct PrismaticPositionConstraint {
 }
 
 impl PrismaticPositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
+    pub fn from_params(
+        rb1: (&RigidBodyMassProps, &RigidBodyIds),
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
+        cparams: &PrismaticJoint,
+    ) -> Self {
+        let (mprops1, ids1) = rb1;
+        let (mprops2, ids2) = rb2;
+
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
@@ -46,8 +55,8 @@ impl PrismaticPositionConstraint {
             local_frame2: cparams.local_frame2(),
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: ids1.active_set_offset,
+            position2: ids2.active_set_offset,
             limits: cparams.limits,
         }
     }
@@ -108,25 +117,28 @@ pub(crate) struct PrismaticPositionGroundConstraint {
 
 impl PrismaticPositionGroundConstraint {
     pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: &RigidBodyPosition,
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
         cparams: &PrismaticJoint,
         flipped: bool,
     ) -> Self {
+        let poss1 = rb1;
+        let (_, ids2) = rb2;
+
         let frame1;
         let local_frame2;
         let axis1;
         let local_axis2;
 
         if flipped {
-            frame1 = rb1.next_position * cparams.local_frame2();
+            frame1 = poss1.next_position * cparams.local_frame2();
             local_frame2 = cparams.local_frame1();
-            axis1 = rb1.next_position * cparams.local_axis2;
+            axis1 = poss1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
         } else {
-            frame1 = rb1.next_position * cparams.local_frame1();
+            frame1 = poss1.next_position * cparams.local_frame1();
             local_frame2 = cparams.local_frame2();
-            axis1 = rb1.next_position * cparams.local_axis1;
+            axis1 = poss1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
         };
 
@@ -135,7 +147,7 @@ impl PrismaticPositionGroundConstraint {
             local_frame2,
             axis1,
             local_axis2,
-            position2: rb2.active_set_offset,
+            position2: ids2.active_set_offset,
             limits: cparams.limits,
         }
     }

src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs

@@ -1,5 +1,7 @@
 use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
-use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
+use crate::dynamics::{
+    IntegrationParameters, PrismaticJoint, 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 WPrismaticPositionConstraint {
 
 impl WPrismaticPositionConstraint {
     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: [&PrismaticJoint; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| PrismaticPositionConstraint::from_params(
+                (rbs1.0[ii], rbs1.1[ii]),
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii]
+            )],
         }
     }
 
@@ -33,13 +45,21 @@ pub(crate) struct WPrismaticPositionGroundConstraint {
 
 impl WPrismaticPositionGroundConstraint {
     pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
+        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
+        rbs2: (
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ),
         cparams: [&PrismaticJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         Self {
-            constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+            constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
+                rbs1[ii],
+                (rbs2.0[ii], rbs2.1[ii]),
+                cparams[ii],
+                flipped[ii]
+            )],
         }
     }
 

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -1,6 +1,7 @@
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{AngularInertia, Real, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
@@ -74,32 +75,45 @@ impl PrismaticVelocityConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &PrismaticJoint,
     ) -> Self {
+        let (poss1, vels1, mprops1, ids1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         // Linear part.
-        let anchor1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * joint.local_anchor2;
-        let axis1 = rb1.position * joint.local_axis1;
-        let axis2 = rb2.position * joint.local_axis2;
+        let anchor1 = poss1.position * joint.local_anchor1;
+        let anchor2 = poss2.position * joint.local_anchor2;
+        let axis1 = poss1.position * joint.local_axis1;
+        let axis2 = poss2.position * joint.local_axis2;
 
         #[cfg(feature = "dim2")]
-        let basis1 = rb1.position * joint.basis1[0];
+        let basis1 = poss1.position * joint.basis1[0];
         #[cfg(feature = "dim3")]
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * joint.basis1[0],
-            rb1.position * joint.basis1[1],
+            poss1.position * joint.basis1[0],
+            poss1.position * joint.basis1[1],
         ]);
 
-        let im1 = rb1.effective_inv_mass;
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - rb1.world_com;
+        let im1 = mprops1.effective_inv_mass;
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - mprops1.world_com;
         let r1_mat = r1.gcross_matrix();
 
-        let im2 = rb2.effective_inv_mass;
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2 - rb2.world_com;
+        let im2 = mprops2.effective_inv_mass;
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - mprops2.world_com;
         let r2_mat = r2.gcross_matrix();
 
         #[allow(unused_mut)] // For 2D.
@@ -131,8 +145,8 @@ impl PrismaticVelocityConstraint {
             lhs = SdpMatrix2::new(m11, m12, m22);
         }
 
-        let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
-        let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
+        let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
+        let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
 
         // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
         // for which a textbook inverse is still efficient.
@@ -142,7 +156,7 @@ impl PrismaticVelocityConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = rb2.angvel - rb1.angvel;
+        let angvel_err = vels2.angvel - vels1.angvel;
 
         #[cfg(feature = "dim2")]
         let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -159,8 +173,8 @@ impl PrismaticVelocityConstraint {
         if velocity_based_erp_inv_dt != 0.0 {
             let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
 
-            let frame1 = rb1.position * joint.local_frame1();
-            let frame2 = rb2.position * joint.local_frame2();
+            let frame1 = poss1.position * joint.local_frame1();
+            let frame2 = poss2.position * joint.local_frame2();
             let ang_err = frame2.rotation * frame1.rotation.inverse();
 
             #[cfg(feature = "dim2")]
@@ -195,9 +209,9 @@ impl PrismaticVelocityConstraint {
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2)
-                - rb1.linvel.dot(&axis1)
-                - rb1.angvel.gdot(gcross1);
+            let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
+                - vels1.linvel.dot(&axis1)
+                - vels1.angvel.gdot(gcross1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
@@ -266,12 +280,12 @@ impl PrismaticVelocityConstraint {
 
         PrismaticVelocityConstraint {
             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,
-            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
             im2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
             impulse: joint.impulse * params.warmstart_coeff,
             limits_active,
             limits_impulse: limits_impulse * params.warmstart_coeff,
@@ -501,11 +515,19 @@ impl PrismaticVelocityGroundConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &PrismaticJoint,
         flipped: bool,
     ) -> Self {
+        let (poss1, vels1, mprops1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         let anchor2;
         let anchor1;
         let axis2;
@@ -513,35 +535,35 @@ impl PrismaticVelocityGroundConstraint {
         let basis1;
 
         if flipped {
-            anchor2 = rb2.position * joint.local_anchor1;
-            anchor1 = rb1.position * joint.local_anchor2;
-            axis2 = rb2.position * joint.local_axis1;
-            axis1 = rb1.position * joint.local_axis2;
+            anchor2 = poss2.position * joint.local_anchor1;
+            anchor1 = poss1.position * joint.local_anchor2;
+            axis2 = poss2.position * joint.local_axis1;
+            axis1 = poss1.position * joint.local_axis2;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * joint.basis2[0];
+                basis1 = poss1.position * joint.basis2[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * joint.basis2[0],
-                    rb1.position * joint.basis2[1],
+                    poss1.position * joint.basis2[0],
+                    poss1.position * joint.basis2[1],
                 ]);
             }
         } else {
-            anchor2 = rb2.position * joint.local_anchor2;
-            anchor1 = rb1.position * joint.local_anchor1;
-            axis2 = rb2.position * joint.local_axis2;
-            axis1 = rb1.position * joint.local_axis1;
+            anchor2 = poss2.position * joint.local_anchor2;
+            anchor1 = poss1.position * joint.local_anchor1;
+            axis2 = poss2.position * joint.local_axis2;
+            axis1 = poss1.position * joint.local_axis1;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * joint.basis1[0];
+                basis1 = poss1.position * joint.basis1[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * joint.basis1[0],
-                    rb1.position * joint.basis1[1],
+                    poss1.position * joint.basis1[0],
+                    poss1.position * joint.basis1[1],
                 ]);
             }
         };
@@ -560,10 +582,10 @@ impl PrismaticVelocityGroundConstraint {
         // simplifications of the computation without introducing
         // much instabilities.
 
-        let im2 = rb2.effective_inv_mass;
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - rb1.world_com;
-        let r2 = anchor2 - rb2.world_com;
+        let im2 = mprops2.effective_inv_mass;
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - mprops1.world_com;
+        let r2 = anchor2 - mprops2.world_com;
         let r2_mat = r2.gcross_matrix();
 
         #[allow(unused_mut)] // For 2D.
@@ -592,8 +614,8 @@ impl PrismaticVelocityGroundConstraint {
             lhs = SdpMatrix2::new(m11, m12, m22);
         }
 
-        let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
-        let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
+        let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
+        let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
 
         // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
         // for which a textbook inverse is still efficient.
@@ -603,7 +625,7 @@ impl PrismaticVelocityGroundConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = rb2.angvel - rb1.angvel;
+        let angvel_err = vels2.angvel - vels1.angvel;
 
         #[cfg(feature = "dim2")]
         let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -622,11 +644,11 @@ impl PrismaticVelocityGroundConstraint {
 
             let (frame1, frame2);
             if flipped {
-                frame1 = rb1.position * joint.local_frame2();
-                frame2 = rb2.position * joint.local_frame1();
+                frame1 = poss1.position * joint.local_frame2();
+                frame2 = poss2.position * joint.local_frame1();
             } else {
-                frame1 = rb1.position * joint.local_frame1();
-                frame2 = rb2.position * joint.local_frame2();
+                frame1 = poss1.position * joint.local_frame1();
+                frame2 = poss2.position * joint.local_frame2();
             }
 
             let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -660,7 +682,7 @@ impl PrismaticVelocityGroundConstraint {
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
@@ -714,9 +736,9 @@ impl PrismaticVelocityGroundConstraint {
 
         PrismaticVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: ids2.active_set_offset,
             im2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
             impulse: joint.impulse * params.warmstart_coeff,
             limits_active,
             limits_forcedir2,

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -2,7 +2,8 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
@@ -71,47 +72,60 @@ impl WPrismaticVelocityConstraint {
     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: [&PrismaticJoint; 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 (poss1, vels1, mprops1, ids1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 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 local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
-        let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
+        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 local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
+        let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
 
         #[cfg(feature = "dim2")]
-        let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
+        let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
         #[cfg(feature = "dim3")]
         let local_basis1 = [
-            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+            Vector::from(gather![|ii| cparams[ii].basis1[0]]),
+            Vector::from(gather![|ii| cparams[ii].basis1[1]]),
         ];
 
         #[cfg(feature = "dim2")]
-        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
         #[cfg(feature = "dim3")]
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -207,8 +221,8 @@ impl WPrismaticVelocityConstraint {
 
             let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
 
-            let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
-            let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
+            let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
+            let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
 
             let frame1 = position1 * local_frame1;
             let frame2 = position2 * local_frame2;
@@ -221,8 +235,7 @@ impl WPrismaticVelocityConstraint {
 
             #[cfg(feature = "dim3")]
             {
-                let ang_err =
-                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
                 rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
                     * velocity_based_erp_inv_dt;
             }
@@ -237,15 +250,15 @@ impl WPrismaticVelocityConstraint {
         let mut limits_inv_lhs = zero;
         let mut limits_impulse_limits = (zero, zero);
 
-        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+        let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
         if limits_enabled.any() {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
             // TODO: we should allow predictive constraint activation.
 
-            let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
-            let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
+            let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
 
             let min_enabled = dist.simd_lt(min_limit);
             let max_enabled = dist.simd_gt(max_limit);
@@ -265,10 +278,9 @@ impl WPrismaticVelocityConstraint {
                     - (min_limit - dist).simd_max(zero))
                     * SimdReal::splat(velocity_based_erp_inv_dt);
 
-                limits_impulse =
-                    SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
-                        .simd_max(limits_impulse_limits.0)
-                        .simd_min(limits_impulse_limits.1);
+                limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
+                    .simd_max(limits_impulse_limits.0)
+                    .simd_min(limits_impulse_limits.1);
 
                 limits_inv_lhs = SimdReal::splat(1.0)
                     / (im1
@@ -303,20 +315,16 @@ impl WPrismaticVelocityConstraint {
 
     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
+            ]),
         };
 
         let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -428,20 +436,16 @@ impl WPrismaticVelocityConstraint {
 
     pub fn solve(&mut 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
+            ]),
         };
 
         self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
@@ -510,59 +514,85 @@ impl WPrismaticVelocityGroundConstraint {
     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: [&PrismaticJoint; 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]);
+        let (poss1, vels1, mprops1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 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];
 
         #[cfg(feature = "dim2")]
-        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
         #[cfg(feature = "dim3")]
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
 
-        let local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
-        );
-        let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
-        );
-        let local_axis1 = Vector::from(
-            array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
-        );
-        let local_axis2 = Vector::from(
-            array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
-        );
+        let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor2
+        } else {
+            cparams[ii].local_anchor1
+        }]);
+        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            cparams[ii].local_anchor2
+        }]);
+        let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
+            *cparams[ii].local_axis2
+        } else {
+            *cparams[ii].local_axis1
+        }]);
+        let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
+            *cparams[ii].local_axis1
+        } else {
+            *cparams[ii].local_axis2
+        }]);
 
         #[cfg(feature = "dim2")]
         let basis1 = position1
-            * Vector::from(
-                array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
-            );
+            * Vector::from(gather![|ii| if flipped[ii] {
+                cparams[ii].basis2[0]
+            } else {
+                cparams[ii].basis1[0]
+            }]);
         #[cfg(feature = "dim3")]
         let basis1 = Matrix3x2::from_columns(&[
             position1
-                * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    cparams[ii].basis2[0]
+                } else {
+                    cparams[ii].basis1[0]
+                }]),
             position1
-                * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    cparams[ii].basis2[1]
+                } else {
+                    cparams[ii].basis1[1]
+                }]),
         ]);
 
         let anchor1 = position1 * local_anchor1;
@@ -634,13 +664,17 @@ impl WPrismaticVelocityGroundConstraint {
             let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
 
             let frame1 = position1
-                * Isometry::from(
-                    array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH],
-                );
+                * Isometry::from(gather![|ii| if flipped[ii] {
+                    cparams[ii].local_frame2()
+                } else {
+                    cparams[ii].local_frame1()
+                }]);
             let frame2 = position2
-                * Isometry::from(
-                    array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH],
-                );
+                * Isometry::from(gather![|ii| if flipped[ii] {
+                    cparams[ii].local_frame1()
+                } else {
+                    cparams[ii].local_frame2()
+                }]);
 
             let ang_err = frame2.rotation * frame1.rotation.inverse();
 
@@ -651,8 +685,7 @@ impl WPrismaticVelocityGroundConstraint {
 
             #[cfg(feature = "dim3")]
             {
-                let ang_err =
-                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
                 rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
                     * velocity_based_erp_inv_dt;
             }
@@ -666,14 +699,14 @@ impl WPrismaticVelocityGroundConstraint {
         let mut limits_impulse = zero;
         let mut limits_impulse_limits = (zero, zero);
 
-        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+        let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
         if limits_enabled.any() {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
             // TODO: we should allow predictive constraint activation.
-            let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
-            let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
+            let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
 
             let min_enabled = dist.simd_lt(min_limit);
             let max_enabled = dist.simd_gt(max_limit);
@@ -690,10 +723,9 @@ impl WPrismaticVelocityGroundConstraint {
                     - (min_limit - dist).simd_max(zero))
                     * SimdReal::splat(velocity_based_erp_inv_dt);
 
-                limits_impulse =
-                    SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
-                        .simd_max(limits_impulse_limits.0)
-                        .simd_min(limits_impulse_limits.1);
+                limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
+                    .simd_max(limits_impulse_limits.0)
+                    .simd_min(limits_impulse_limits.1);
             }
         }
 
@@ -718,12 +750,10 @@ impl WPrismaticVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -791,12 +821,10 @@ impl WPrismaticVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         self.solve_dofs(&mut mj_lambda2);

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -1,4 +1,6 @@
-use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
+use crate::dynamics::{
+    IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::Unit;
@@ -29,11 +31,18 @@ pub(crate) struct RevolutePositionConstraint {
 }
 
 impl RevolutePositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
+    pub fn from_params(
+        rb1: (&RigidBodyMassProps, &RigidBodyIds),
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
+        cparams: &RevoluteJoint,
+    ) -> Self {
+        let (mprops1, ids1) = rb1;
+        let (mprops2, ids2) = rb2;
+
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
         Self {
@@ -42,14 +51,14 @@ impl RevolutePositionConstraint {
             ii1,
             ii2,
             ang_inv_lhs,
-            local_com1: rb1.mass_properties.local_com,
-            local_com2: rb2.mass_properties.local_com,
+            local_com1: mprops1.mass_properties.local_com,
+            local_com2: mprops2.mass_properties.local_com,
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: ids1.active_set_offset,
+            position2: ids2.active_set_offset,
             local_basis1: cparams.basis1,
             local_basis2: cparams.basis2,
         }
@@ -132,11 +141,14 @@ pub(crate) struct RevolutePositionGroundConstraint {
 
 impl RevolutePositionGroundConstraint {
     pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: &RigidBodyPosition,
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
         cparams: &RevoluteJoint,
         flipped: bool,
     ) -> Self {
+        let poss1 = rb1;
+        let (mprops2, ids2) = rb2;
+
         let anchor1;
         let local_anchor2;
         let axis1;
@@ -145,23 +157,23 @@ impl RevolutePositionGroundConstraint {
         let local_basis2;
 
         if flipped {
-            anchor1 = rb1.next_position * cparams.local_anchor2;
+            anchor1 = poss1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
-            axis1 = rb1.next_position * cparams.local_axis2;
+            axis1 = poss1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
             basis1 = [
-                rb1.next_position * cparams.basis2[0],
-                rb1.next_position * cparams.basis2[1],
+                poss1.next_position * cparams.basis2[0],
+                poss1.next_position * cparams.basis2[1],
             ];
             local_basis2 = cparams.basis1;
         } else {
-            anchor1 = rb1.next_position * cparams.local_anchor1;
+            anchor1 = poss1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
-            axis1 = rb1.next_position * cparams.local_axis1;
+            axis1 = poss1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
             basis1 = [
-                rb1.next_position * cparams.basis1[0],
-                rb1.next_position * cparams.basis1[1],
+                poss1.next_position * cparams.basis1[0],
+                poss1.next_position * cparams.basis1[1],
             ];
             local_basis2 = cparams.basis2;
         };
@@ -169,12 +181,12 @@ impl RevolutePositionGroundConstraint {
         Self {
             anchor1,
             local_anchor2,
-            im2: rb2.effective_inv_mass,
-            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
-            local_com2: rb2.mass_properties.local_com,
+            im2: mprops2.effective_inv_mass,
+            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: mprops2.mass_properties.local_com,
             axis1,
             local_axis2,
-            position2: rb2.active_set_offset,
+            position2: ids2.active_set_offset,
             basis1,
             local_basis2,
         }

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]
+            )],
         }
     }
 

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -1,6 +1,7 @@
 use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
 use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{AngularInertia, Real, Rotation, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -43,34 +44,47 @@ impl RevoluteVelocityConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &RevoluteJoint,
     ) -> Self {
+        let (poss1, vels1, mprops1, ids1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         // Linear part.
-        let anchor1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * joint.local_anchor2;
+        let anchor1 = poss1.position * joint.local_anchor1;
+        let anchor2 = poss2.position * joint.local_anchor2;
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * joint.basis1[0],
-            rb1.position * joint.basis1[1],
+            poss1.position * joint.basis1[0],
+            poss1.position * joint.basis1[1],
         ]);
 
         let basis2 = Matrix3x2::from_columns(&[
-            rb2.position * joint.basis2[0],
-            rb2.position * joint.basis2[1],
+            poss2.position * joint.basis2[0],
+            poss2.position * joint.basis2[1],
         ]);
         let basis_projection2 = basis2 * basis2.transpose();
         let basis2 = basis_projection2 * basis1;
 
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
+        let im1 = mprops1.effective_inv_mass;
+        let im2 = mprops2.effective_inv_mass;
 
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - rb1.world_com;
+        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - mprops1.world_com;
         let r1_mat = r1.gcross_matrix();
 
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2 - rb2.world_com;
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - mprops2.world_com;
         let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
@@ -90,8 +104,8 @@ impl RevoluteVelocityConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let linvel_err =
-            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+            (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
+        let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
 
         let mut rhs = Vector5::new(
             linvel_err.x,
@@ -105,8 +119,8 @@ impl RevoluteVelocityConstraint {
         if velocity_based_erp_inv_dt != 0.0 {
             let lin_err = anchor2 - anchor1;
 
-            let axis1 = rb1.position * joint.local_axis1;
-            let axis2 = rb2.position * joint.local_axis2;
+            let axis1 = poss1.position * joint.local_axis1;
+            let axis2 = poss2.position * joint.local_axis2;
 
             let axis_error = axis1.cross(&axis2);
             let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
@@ -118,8 +132,8 @@ impl RevoluteVelocityConstraint {
         /*
          * Motor.
          */
-        let motor_axis1 = rb1.position * *joint.local_axis1;
-        let motor_axis2 = rb2.position * *joint.local_axis2;
+        let motor_axis1 = poss1.position * *joint.local_axis1;
+        let motor_axis2 = poss2.position * *joint.local_axis2;
         let mut motor_rhs = 0.0;
         let mut motor_inv_lhs = 0.0;
         let mut motor_angle = 0.0;
@@ -132,12 +146,12 @@ impl RevoluteVelocityConstraint {
         );
 
         if stiffness != 0.0 {
-            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
             motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
+            let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
@@ -171,14 +185,14 @@ impl RevoluteVelocityConstraint {
 
         RevoluteVelocityConstraint {
             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,
-            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
             basis1,
             basis2,
             im2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
             impulse,
             inv_lhs,
             rhs,
@@ -330,11 +344,19 @@ impl RevoluteVelocityGroundConstraint {
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
+        rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ),
         joint: &RevoluteJoint,
         flipped: bool,
     ) -> AnyJointVelocityConstraint {
+        let (poss1, vels1, mprops1) = rb1;
+        let (poss2, vels2, mprops2, ids2) = rb2;
+
         let anchor2;
         let anchor1;
         let axis1;
@@ -343,39 +365,39 @@ impl RevoluteVelocityGroundConstraint {
         let basis2;
 
         if flipped {
-            axis1 = rb1.position * *joint.local_axis2;
-            axis2 = rb2.position * *joint.local_axis1;
-            anchor1 = rb1.position * joint.local_anchor2;
-            anchor2 = rb2.position * joint.local_anchor1;
+            axis1 = poss1.position * *joint.local_axis2;
+            axis2 = poss2.position * *joint.local_axis1;
+            anchor1 = poss1.position * joint.local_anchor2;
+            anchor2 = poss2.position * joint.local_anchor1;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * joint.basis2[0],
-                rb1.position * joint.basis2[1],
+                poss1.position * joint.basis2[0],
+                poss1.position * joint.basis2[1],
             ]);
             basis2 = Matrix3x2::from_columns(&[
-                rb2.position * joint.basis1[0],
-                rb2.position * joint.basis1[1],
+                poss2.position * joint.basis1[0],
+                poss2.position * joint.basis1[1],
             ]);
         } else {
-            axis1 = rb1.position * *joint.local_axis1;
-            axis2 = rb2.position * *joint.local_axis2;
-            anchor1 = rb1.position * joint.local_anchor1;
-            anchor2 = rb2.position * joint.local_anchor2;
+            axis1 = poss1.position * *joint.local_axis1;
+            axis2 = poss2.position * *joint.local_axis2;
+            anchor1 = poss1.position * joint.local_anchor1;
+            anchor2 = poss2.position * joint.local_anchor2;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * joint.basis1[0],
-                rb1.position * joint.basis1[1],
+                poss1.position * joint.basis1[0],
+                poss1.position * joint.basis1[1],
             ]);
             basis2 = Matrix3x2::from_columns(&[
-                rb2.position * joint.basis2[0],
-                rb2.position * joint.basis2[1],
+                poss2.position * joint.basis2[0],
+                poss2.position * joint.basis2[1],
             ]);
         };
 
         let basis_projection2 = basis2 * basis2.transpose();
         let basis2 = basis_projection2 * basis1;
-        let im2 = rb2.effective_inv_mass;
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - rb1.world_com;
-        let r2 = anchor2 - rb2.world_com;
+        let im2 = mprops2.effective_inv_mass;
+        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - mprops1.world_com;
+        let r2 = anchor2 - mprops2.world_com;
         let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
@@ -393,8 +415,8 @@ impl RevoluteVelocityGroundConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let linvel_err =
-            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+            (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
+        let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
         let mut rhs = Vector5::new(
             linvel_err.x,
             linvel_err.y,
@@ -409,11 +431,11 @@ impl RevoluteVelocityGroundConstraint {
 
             let (axis1, axis2);
             if flipped {
-                axis1 = rb1.position * joint.local_axis2;
-                axis2 = rb2.position * joint.local_axis1;
+                axis1 = poss1.position * joint.local_axis2;
+                axis2 = poss2.position * joint.local_axis1;
             } else {
-                axis1 = rb1.position * joint.local_axis1;
-                axis2 = rb2.position * joint.local_axis2;
+                axis1 = poss1.position * joint.local_axis1;
+                axis2 = poss2.position * joint.local_axis2;
             }
             let axis_error = axis1.cross(&axis2);
             let ang_err = basis2.tr_mul(&axis_error);
@@ -437,12 +459,12 @@ impl RevoluteVelocityGroundConstraint {
         );
 
         if stiffness != 0.0 {
-            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
             motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
+            let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
@@ -460,9 +482,9 @@ impl RevoluteVelocityGroundConstraint {
 
         let result = RevoluteVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: ids2.active_set_offset,
             im2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
             impulse: joint.impulse * params.warmstart_coeff,
             basis2,
             inv_lhs,

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
 
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
@@ -39,41 +40,54 @@ impl WRevoluteVelocityConstraint {
     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],
+        ),
         joints: [&RevoluteJoint; 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 (poss1, vels1, mprops1, ids1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 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 local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
+        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| joints[ii].local_anchor1]);
+        let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
         let local_basis1 = [
-            Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
+            Vector::from(gather![|ii| joints[ii].basis1[0]]),
+            Vector::from(gather![|ii| joints[ii].basis1[1]]),
         ];
         let local_basis2 = [
-            Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
+            Vector::from(gather![|ii| joints[ii].basis2[0]]),
+            Vector::from(gather![|ii| joints[ii].basis2[1]]),
         ];
-        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
@@ -124,10 +138,8 @@ impl WRevoluteVelocityConstraint {
 
             let lin_err = anchor2 - anchor1;
 
-            let local_axis1 =
-                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
-            let local_axis2 =
-                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
+            let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
+            let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
 
             let axis1 = position1 * local_axis1;
             let axis2 = position2 * local_axis2;
@@ -150,12 +162,12 @@ impl WRevoluteVelocityConstraint {
         let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
         let mut impulse = impulse * warmstart_coeff;
 
-        let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
-        let rotated_impulse = Vector::from(array![|ii| {
+        let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
+        let rotated_impulse = Vector::from(gather![|ii| {
             let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
                 .unwrap_or_else(Rotation::identity);
             axis_rot * joints[ii].world_ang_impulse
-        }; SIMD_WIDTH]);
+        }]);
 
         let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
         impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
@@ -182,20 +194,16 @@ impl WRevoluteVelocityConstraint {
 
     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
+            ]),
         };
 
         let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -225,20 +233,16 @@ impl WRevoluteVelocityConstraint {
 
     pub fn solve(&mut 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
+            ]),
         };
 
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -314,52 +318,76 @@ impl WRevoluteVelocityGroundConstraint {
     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],
+        ),
         joints: [&RevoluteJoint; 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]);
+        let (poss1, vels1, mprops1) = rbs1;
+        let (poss2, vels2, mprops2, ids2) = rbs2;
 
-        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 impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
+        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 local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
-        );
-        let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
-        );
+        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 impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
+
+        let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
+            joints[ii].local_anchor2
+        } else {
+            joints[ii].local_anchor1
+        }]);
+        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
+            joints[ii].local_anchor1
+        } else {
+            joints[ii].local_anchor2
+        }]);
         let basis1 = Matrix3x2::from_columns(&[
             position1
-                * Vector::from(
-                    array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    joints[ii].basis2[0]
+                } else {
+                    joints[ii].basis1[0]
+                }]),
             position1
-                * Vector::from(
-                    array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    joints[ii].basis2[1]
+                } else {
+                    joints[ii].basis1[1]
+                }]),
         ]);
         let basis2 = Matrix3x2::from_columns(&[
             position2
-                * Vector::from(
-                    array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    joints[ii].basis1[0]
+                } else {
+                    joints[ii].basis2[0]
+                }]),
             position2
-                * Vector::from(
-                    array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
-                ),
+                * Vector::from(gather![|ii| if flipped[ii] {
+                    joints[ii].basis1[1]
+                } else {
+                    joints[ii].basis2[1]
+                }]),
         ]);
         let basis_projection2 = basis2 * basis2.transpose();
         let basis2 = basis_projection2 * basis1;
@@ -403,12 +431,16 @@ impl WRevoluteVelocityGroundConstraint {
 
             let lin_err = anchor2 - anchor1;
 
-            let local_axis1 = Unit::<Vector<_>>::from(
-                array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
-            );
-            let local_axis2 = Unit::<Vector<_>>::from(
-                array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
-            );
+            let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
+                joints[ii].local_axis2
+            } else {
+                joints[ii].local_axis1
+            }]);
+            let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
+                joints[ii].local_axis1
+            } else {
+                joints[ii].local_axis2
+            }]);
             let axis1 = position1 * local_axis1;
             let axis2 = position2 * local_axis2;
 
@@ -434,12 +466,10 @@ impl WRevoluteVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -458,12 +488,10 @@ impl WRevoluteVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/parallel_island_solver.rs

@@ -1,9 +1,10 @@
 use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
+use crate::data::ComponentSet;
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
     AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real};
 use crate::utils::WAngularInertia;
@@ -150,13 +151,15 @@ impl ParallelIslandSolver {
         }
     }
 
-    pub fn solve_position_constraints<'s>(
+    pub fn solve_position_constraints<'s, Bodies>(
         &'s mut self,
         scope: &Scope<'s>,
         island_id: usize,
         params: &'s IntegrationParameters,
-        bodies: &'s mut RigidBodySet,
-    ) {
+        bodies: &'s mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBody>,
+    {
         let num_threads = rayon::current_num_threads();
         let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -179,7 +182,7 @@ impl ParallelIslandSolver {
                 // Transmute *mut -> &mut
                 let positions: &mut Vec<Isometry<Real>> =
                     unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
-                let bodies: &mut RigidBodySet =
+                let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                 let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
                     std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
@@ -197,8 +200,8 @@ impl ParallelIslandSolver {
 
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
+                        let rb = &mut bodies.index(handle.0);
                         positions[rb.active_set_offset] = rb.next_position;
                     }
                 }
@@ -216,8 +219,8 @@ impl ParallelIslandSolver {
                 // Write results back to rigid bodies.
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.position_writeback_index] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.position_writeback_index) {
+                        let rb = &mut bodies.index(handle.0);
                         rb.set_next_position(positions[rb.active_set_offset]);
                     }
                 }
@@ -225,17 +228,19 @@ impl ParallelIslandSolver {
         }
     }
 
-    pub fn init_constraints_and_solve_velocity_constraints<'s>(
+    pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>(
         &'s mut self,
         scope: &Scope<'s>,
         island_id: usize,
         params: &'s IntegrationParameters,
-        bodies: &'s mut RigidBodySet,
+        bodies: &'s mut Bodies,
         manifolds: &'s mut Vec<&'s mut ContactManifold>,
         manifold_indices: &'s [ContactManifoldIndex],
         joints: &'s mut Vec<JointGraphEdge>,
         joint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBody>,
+    {
         let num_threads = rayon::current_num_threads();
         let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -280,7 +285,7 @@ impl ParallelIslandSolver {
                 // Transmute *mut -> &mut
                 let mj_lambdas: &mut Vec<DeltaVel<Real>> =
                     unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
-                let bodies: &mut RigidBodySet =
+                let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                 let manifolds: &mut Vec<&mut ContactManifold> =
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -303,8 +308,8 @@ impl ParallelIslandSolver {
 
                     concurrent_loop! {
                         let batch_size = thread.batch_size;
-                        for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
-                            let rb = &mut bodies[handle.0];
+                        for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) {
+                            let rb = &mut bodies.index(handle.0);
                             let dvel = &mut mj_lambdas[rb.active_set_offset];
 
                             // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
@@ -348,8 +353,8 @@ impl ParallelIslandSolver {
 
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let rb = &mut bodies[handle.0];
+                    for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
+                        let rb = &mut bodies.index(handle.0);
                         let dvel = mj_lambdas[rb.active_set_offset];
                         rb.linvel += dvel.linear;
                         rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);

src/dynamics/solver/parallel_solver_constraints.rs

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
     PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge};
 use crate::geometry::ContactManifold;
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
@@ -20,7 +20,7 @@ use std::sync::atomic::Ordering;
 // pub fn init_constraint_groups(
 //     &mut self,
 //     island_id: usize,
-//     bodies: &RigidBodySet,
+//     bodies: &impl ComponentSet<RigidBody>,
 //     manifolds: &mut [&mut ContactManifold],
 //     manifold_groups: &ParallelInteractionGroups,
 //     joints: &mut [JointGraphEdge],
@@ -36,9 +36,9 @@ pub(crate) enum ConstraintDesc {
     NongroundNongrouped(usize),
     GroundNongrouped(usize),
     #[cfg(feature = "simd-is-enabled")]
-    NongroundGrouped([usize; SIMD_WIDTH]),
+    NongroundGrouped([usize]),
     #[cfg(feature = "simd-is-enabled")]
-    GroundGrouped([usize; SIMD_WIDTH]),
+    GroundGrouped([usize]),
 }
 
 pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
@@ -78,7 +78,7 @@ macro_rules! impl_init_constraints_group {
             pub fn init_constraint_groups(
                 &mut self,
                 island_id: usize,
-                bodies: &RigidBodySet,
+                bodies: &impl ComponentSet<RigidBody>,
                 interactions: &mut [$Interaction],
                 interaction_groups: &ParallelInteractionGroups,
             ) {
@@ -144,7 +144,7 @@ macro_rules! impl_init_constraints_group {
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::NongroundGrouped(
-                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                                gather![|ii| interaction_i[ii]],
                             ),
                         ));
                         total_num_constraints += $num_active_constraints(interaction);
@@ -172,7 +172,7 @@ macro_rules! impl_init_constraints_group {
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GroundGrouped(
-                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                                gather![|ii| interaction_i[ii]],
                             ),
                         ));
                         total_num_constraints += $num_active_constraints(interaction);
@@ -223,7 +223,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &impl ComponentSet<RigidBody>,
         manifolds_all: &[&mut ContactManifold],
     ) {
         let descs = &self.constraint_descs;
@@ -244,13 +244,13 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
                         WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
                         WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
@@ -265,7 +265,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &impl ComponentSet<RigidBody>,
         joints_all: &[JointGraphEdge],
     ) {
         let descs = &self.constraint_descs;
@@ -290,7 +290,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
                         let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
                         let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
                         self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
@@ -298,7 +298,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
                         let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
                         let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
                         self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;

src/dynamics/solver/position_constraint.rs

@@ -1,7 +1,8 @@
+use crate::data::ComponentSet;
 use crate::dynamics::solver::PositionGroundConstraint;
 #[cfg(feature = "simd-is-enabled")]
 use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
 use crate::geometry::ContactManifold;
 use crate::math::{
     AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -51,15 +52,26 @@ pub(crate) struct PositionConstraint {
 }
 
 impl PositionConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold: &ContactManifold,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
-    ) {
-        let rb1 = &bodies[manifold.data.body_pair.body1];
-        let rb2 = &bodies[manifold.data.body_pair.body2];
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let handle1 = manifold.data.rigid_body1.unwrap();
+        let handle2 = manifold.data.rigid_body2.unwrap();
+
+        let ids1: &RigidBodyIds = bodies.index(handle1.0);
+        let ids2: &RigidBodyIds = bodies.index(handle2.0);
+        let poss1: &RigidBodyPosition = bodies.index(handle1.0);
+        let poss2: &RigidBodyPosition = bodies.index(handle2.0);
+        let mprops1: &RigidBodyMassProps = bodies.index(handle1.0);
+        let mprops2: &RigidBodyMassProps = bodies.index(handle2.0);
 
         for (l, manifold_points) in manifold
             .data
@@ -72,26 +84,28 @@ impl PositionConstraint {
             let mut dists = [0.0; MAX_MANIFOLD_POINTS];
 
             for l in 0..manifold_points.len() {
-                local_p1[l] = rb1
+                local_p1[l] = poss1
                     .position
                     .inverse_transform_point(&manifold_points[l].point);
-                local_p2[l] = rb2
+                local_p2[l] = poss2
                     .position
                     .inverse_transform_point(&manifold_points[l].point);
                 dists[l] = manifold_points[l].dist;
             }
 
             let constraint = PositionConstraint {
-                rb1: rb1.active_set_offset,
-                rb2: rb2.active_set_offset,
+                rb1: ids1.active_set_offset,
+                rb2: ids2.active_set_offset,
                 local_p1,
                 local_p2,
-                local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
+                local_n1: poss1
+                    .position
+                    .inverse_transform_vector(&manifold.data.normal),
                 dists,
-                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(),
+                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(),
                 num_contacts: manifold_points.len() as u8,
                 erp: params.erp,
                 max_linear_correction: params.max_linear_correction,

src/dynamics/solver/position_constraint_wide.rs

@@ -1,5 +1,5 @@
 use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
 use crate::geometry::ContactManifold;
 use crate::math::{
     AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
 };
 use crate::utils::{WAngularInertia, WCross, WDot};
 
+use crate::data::ComponentSet;
 use num::Zero;
 use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
@@ -28,39 +29,47 @@ pub(crate) struct WPositionConstraint {
 }
 
 impl WPositionConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
-    ) {
-        let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
-        let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
+        let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
 
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
-        let sqrt_ii1: AngularInertia<SimdReal> = AngularInertia::from(
-            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
+        let poss1: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let poss2: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
+        let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
+        let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
 
-        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
+        let sqrt_ii1: AngularInertia<SimdReal> =
+            AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let sqrt_ii2: AngularInertia<SimdReal> =
+            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
-        let local_n1 = pos1.inverse_transform_vector(&Vector::from(
-            array![|ii| manifolds[ii].data.normal; SIMD_WIDTH],
-        ));
+        let pos1 = Isometry::from(gather![|ii| poss1[ii].position]);
+        let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
 
-        let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
-        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let local_n1 =
+            pos1.inverse_transform_vector(&Vector::from(gather![|ii| manifolds[ii].data.normal]));
+
+        let rb1 = gather![|ii| ids1[ii].active_set_offset];
+        let rb2 = gather![|ii| ids2[ii].active_set_offset];
 
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
+            let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionConstraint {
@@ -80,8 +89,8 @@ impl WPositionConstraint {
             };
 
             for i in 0..num_points {
-                let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
-                let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
+                let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
+                let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
                 constraint.local_p1[i] = pos1.inverse_transform_point(&point);
                 constraint.local_p2[i] = pos2.inverse_transform_point(&point);
                 constraint.dists[i] = dist;
@@ -99,8 +108,8 @@ impl WPositionConstraint {
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         // FIXME: can we avoid most of the multiplications by pos1/pos2?
         // Compute jacobians.
-        let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
-        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let mut pos1 = Isometry::from(gather![|ii| positions[self.rb1[ii]]]);
+        let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
 
         for k in 0..self.num_contacts as usize {

src/dynamics/solver/position_ground_constraint.rs

@@ -1,5 +1,6 @@
 use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
 use crate::geometry::ContactManifold;
 use crate::math::{
     AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -21,24 +22,28 @@ pub(crate) struct PositionGroundConstraint {
 }
 
 impl PositionGroundConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold: &ContactManifold,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
-    ) {
-        let mut rb1 = &bodies[manifold.data.body_pair.body1];
-        let mut rb2 = &bodies[manifold.data.body_pair.body2];
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         let flip = manifold.data.relative_dominance < 0;
 
-        let n1 = if flip {
-            std::mem::swap(&mut rb1, &mut rb2);
-            -manifold.data.normal
+        let (handle2, n1) = if flip {
+            (manifold.data.rigid_body1.unwrap(), -manifold.data.normal)
         } else {
-            manifold.data.normal
+            (manifold.data.rigid_body2.unwrap(), manifold.data.normal)
         };
 
+        let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) =
+            bodies.index_bundle(handle2.0);
+
         for (l, manifold_contacts) in manifold
             .data
             .solver_contacts
@@ -51,20 +56,20 @@ impl PositionGroundConstraint {
 
             for k in 0..manifold_contacts.len() {
                 p1[k] = manifold_contacts[k].point;
-                local_p2[k] = rb2
+                local_p2[k] = poss2
                     .position
                     .inverse_transform_point(&manifold_contacts[k].point);
                 dists[k] = manifold_contacts[k].dist;
             }
 
             let constraint = PositionGroundConstraint {
-                rb2: rb2.active_set_offset,
+                rb2: ids2.active_set_offset,
                 p1,
                 local_p2,
                 n1,
                 dists,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_contacts.len() as u8,
                 erp: params.erp,
                 max_linear_correction: params.max_linear_correction,

src/dynamics/solver/position_ground_constraint_wide.rs

@@ -1,5 +1,5 @@
 use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
 use crate::geometry::ContactManifold;
 use crate::math::{
     AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
 };
 use crate::utils::{WAngularInertia, WCross, WDot};
 
+use crate::data::ComponentSet;
 use num::Zero;
 use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
@@ -25,42 +26,51 @@ pub(crate) struct WPositionGroundConstraint {
 }
 
 impl WPositionGroundConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
-    ) {
-        let mut rbs1 =
-            array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
-        let mut rbs2 =
-            array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>,
+    {
+        let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
+        let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
         let mut flipped = [false; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
             if manifolds[ii].data.relative_dominance < 0 {
                 flipped[ii] = true;
-                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
             }
         }
 
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
+        let poss2: [&RigidBodyPosition; SIMD_WIDTH] =
+            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
+            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
 
-        let n1 = Vector::from(
-            array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],
-        );
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let sqrt_ii2: AngularInertia<SimdReal> =
+            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
-        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let n1 = Vector::from(gather![|ii| if flipped[ii] {
+            -manifolds[ii].data.normal
+        } else {
+            manifolds[ii].data.normal
+        }]);
+
+        let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
+        let rb2 = gather![|ii| ids2[ii].active_set_offset];
 
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
+            let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionGroundConstraint {
@@ -77,8 +87,8 @@ impl WPositionGroundConstraint {
             };
 
             for i in 0..num_points {
-                let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
-                let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
+                let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
+                let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
                 constraint.p1[i] = point;
                 constraint.local_p2[i] = pos2.inverse_transform_point(&point);
                 constraint.dists[i] = dist;
@@ -96,7 +106,7 @@ impl WPositionGroundConstraint {
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         // FIXME: can we avoid most of the multiplications by pos1/pos2?
         // Compute jacobians.
-        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
 
         for k in 0..self.num_contacts as usize {

src/dynamics/solver/position_solver.rs

@@ -1,5 +1,7 @@
 use super::AnyJointPositionConstraint;
-use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
+use crate::data::{ComponentSet, ComponentSetMut};
+use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters};
+use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition};
 use crate::math::{Isometry, Real};
 
 pub(crate) struct PositionSolver {
@@ -13,25 +15,28 @@ impl PositionSolver {
         }
     }
 
-    pub fn solve(
+    pub fn solve<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         contact_constraints: &[AnyPositionConstraint],
         joint_constraints: &[AnyJointPositionConstraint],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         if contact_constraints.is_empty() && joint_constraints.is_empty() {
             // Nothing to do.
             return;
         }
 
         self.positions.clear();
-        self.positions.extend(
-            bodies
-                .iter_active_island(island_id)
-                .map(|(_, b)| b.next_position),
-        );
+        self.positions
+            .extend(islands.active_island(island_id).iter().map(|h| {
+                let poss: &RigidBodyPosition = bodies.index(h.0);
+                poss.next_position
+            }));
 
         for _ in 0..params.max_position_iterations {
             for constraint in joint_constraints {
@@ -43,8 +48,10 @@ impl PositionSolver {
             }
         }
 
-        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_next_position(self.positions[rb.active_set_offset])
-        });
+        for handle in islands.active_island(island_id) {
+            let ids: &RigidBodyIds = bodies.index(handle.0);
+            let next_pos = &self.positions[ids.active_set_offset];
+            bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos);
+        }
     }
 }

src/dynamics/solver/solver_constraints.rs

@@ -5,13 +5,16 @@ use super::{
 use super::{
     WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
 };
+use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
 };
 use crate::dynamics::{
-    solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+    solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
 };
+use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
@@ -50,13 +53,16 @@ impl<VelocityConstraint, PositionConstraint>
 }
 
 impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
-    pub fn init_constraint_groups(
+    pub fn init_constraint_groups<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         manifolds: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         self.not_ground_interactions.clear();
         self.ground_interactions.clear();
         categorize_contacts(
@@ -70,6 +76,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         self.interaction_groups.clear_groups();
         self.interaction_groups.group_manifolds(
             island_id,
+            islands,
             bodies,
             manifolds,
             &self.not_ground_interactions,
@@ -78,6 +85,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         self.ground_interaction_groups.clear_groups();
         self.ground_interaction_groups.group_manifolds(
             island_id,
+            islands,
             bodies,
             manifolds,
             &self.ground_interactions,
@@ -92,18 +100,25 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         //            .append(&mut self.ground_interaction_groups.grouped_interactions);
     }
 
-    pub fn init(
+    pub fn init<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         manifolds: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
         self.velocity_constraints.clear();
         self.position_constraints.clear();
 
-        self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
+        self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices);
 
         #[cfg(feature = "simd-is-enabled")]
         {
@@ -118,19 +133,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_constraints(
+    fn compute_grouped_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         manifolds_all: &[&mut ContactManifold],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         for manifolds_i in self
             .interaction_groups
             .grouped_interactions
             .chunks_exact(SIMD_WIDTH)
         {
-            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
-            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            let manifold_id = gather![|ii| manifolds_i[ii]];
+            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
             WVelocityConstraint::generate(
                 params,
                 manifold_id,
@@ -149,12 +169,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         }
     }
 
-    fn compute_nongrouped_constraints(
+    fn compute_nongrouped_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         manifolds_all: &[&mut ContactManifold],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         for manifold_i in &self.interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
             VelocityConstraint::generate(
@@ -176,19 +201,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_ground_constraints(
+    fn compute_grouped_ground_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         manifolds_all: &[&mut ContactManifold],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         for manifolds_i in self
             .ground_interaction_groups
             .grouped_interactions
             .chunks_exact(SIMD_WIDTH)
         {
-            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
-            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            let manifold_id = gather![|ii| manifolds_i[ii]];
+            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
             WVelocityGroundConstraint::generate(
                 params,
                 manifold_id,
@@ -207,12 +237,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         }
     }
 
-    fn compute_nongrouped_ground_constraints(
+    fn compute_nongrouped_ground_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         manifolds_all: &[&mut ContactManifold],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
             VelocityGroundConstraint::generate(
@@ -235,14 +270,21 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
 }
 
 impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
-    pub fn init(
+    pub fn init<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         joints: &[JointGraphEdge],
         joint_constraint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         // Generate constraints for joints.
         self.not_ground_interactions.clear();
         self.ground_interactions.clear();
@@ -260,6 +302,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         self.interaction_groups.clear_groups();
         self.interaction_groups.group_joints(
             island_id,
+            islands,
             bodies,
             joints,
             &self.not_ground_interactions,
@@ -268,6 +311,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         self.ground_interaction_groups.clear_groups();
         self.ground_interaction_groups.group_joints(
             island_id,
+            islands,
             bodies,
             joints,
             &self.ground_interactions,
@@ -292,12 +336,18 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         }
     }
 
-    fn compute_nongrouped_joint_ground_constraints(
+    fn compute_nongrouped_joint_ground_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         joints_all: &[JointGraphEdge],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyIds>,
+    {
         for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
             let joint = &joints_all[*joint_i].weight;
             let vel_constraint =
@@ -309,19 +359,25 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_joint_ground_constraints(
+    fn compute_grouped_joint_ground_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         joints_all: &[JointGraphEdge],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         for joints_i in self
             .ground_interaction_groups
             .grouped_interactions
             .chunks_exact(SIMD_WIDTH)
         {
-            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
-            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let joints_id = gather![|ii| joints_i[ii]];
+            let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
             let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
                 params, joints_id, joints, bodies,
             );
@@ -332,12 +388,17 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         }
     }
 
-    fn compute_nongrouped_joint_constraints(
+    fn compute_nongrouped_joint_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         joints_all: &[JointGraphEdge],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         for joint_i in &self.interaction_groups.nongrouped_interactions {
             let joint = &joints_all[*joint_i].weight;
             let vel_constraint =
@@ -349,19 +410,24 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_joint_constraints(
+    fn compute_grouped_joint_constraints<Bodies>(
         &mut self,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         joints_all: &[JointGraphEdge],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         for joints_i in self
             .interaction_groups
             .grouped_interactions
             .chunks_exact(SIMD_WIDTH)
         {
-            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
-            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let joints_id = gather![|ii| joints_i[ii]];
+            let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
             let vel_constraint =
                 AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
             self.velocity_constraints.push(vel_constraint);

src/dynamics/solver/velocity_constraint.rs

@@ -1,7 +1,8 @@
+use crate::data::{BundleSet, ComponentSet};
 use crate::dynamics::solver::VelocityGroundConstraint;
 #[cfg(feature = "simd-is-enabled")]
 use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -102,23 +103,32 @@ impl VelocityConstraint {
         manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
     }
 
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold_id: ContactManifoldIndex,
         manifold: &ContactManifold,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         assert_eq!(manifold.data.relative_dominance, 0);
 
         let inv_dt = params.inv_dt();
         let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
 
-        let rb1 = &bodies[manifold.data.body_pair.body1];
-        let rb2 = &bodies[manifold.data.body_pair.body2];
-        let mj_lambda1 = rb1.active_set_offset;
-        let mj_lambda2 = rb2.active_set_offset;
+        let handle1 = manifold.data.rigid_body1.unwrap();
+        let handle2 = manifold.data.rigid_body2.unwrap();
+        let (ids1, vels1, mprops1): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
+            bodies.index_bundle(handle1.0);
+        let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
+            bodies.index_bundle(handle2.0);
+
+        let mj_lambda1 = ids1.active_set_offset;
+        let mj_lambda2 = ids2.active_set_offset;
         let force_dir1 = -manifold.data.normal;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
@@ -126,7 +136,7 @@ impl VelocityConstraint {
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
         let (tangents1, tangent_rot1) =
-            super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
+            super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
 
         for (_l, manifold_points) in manifold
             .data
@@ -142,8 +152,8 @@ impl VelocityConstraint {
                 #[cfg(feature = "dim3")]
                 tangent_rot1,
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im1: rb1.effective_inv_mass,
-                im2: rb2.effective_inv_mass,
+                im1: mprops1.effective_inv_mass,
+                im2: mprops2.effective_inv_mass,
                 limit: 0.0,
                 mj_lambda1,
                 mj_lambda2,
@@ -190,8 +200,8 @@ impl VelocityConstraint {
                     constraint.tangent1 = tangents1[0];
                     constraint.tangent_rot1 = tangent_rot1;
                 }
-                constraint.im1 = rb1.effective_inv_mass;
-                constraint.im2 = rb2.effective_inv_mass;
+                constraint.im1 = mprops1.effective_inv_mass;
+                constraint.im2 = mprops2.effective_inv_mass;
                 constraint.limit = 0.0;
                 constraint.mj_lambda1 = mj_lambda1;
                 constraint.mj_lambda2 = mj_lambda2;
@@ -202,11 +212,11 @@ impl VelocityConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let dp1 = manifold_point.point - rb1.world_com;
-                let dp2 = manifold_point.point - rb2.world_com;
+                let dp1 = manifold_point.point - mprops1.world_com;
+                let dp2 = manifold_point.point - mprops2.world_com;
 
-                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
-                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
+                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
 
                 let warmstart_correction;
 
@@ -215,16 +225,16 @@ impl VelocityConstraint {
 
                 // Normal part.
                 {
-                    let gcross1 = rb1
+                    let gcross1 = mprops1
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp1.gcross(force_dir1));
-                    let gcross2 = rb2
+                    let gcross2 = mprops2
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let r = 1.0
-                        / (rb1.effective_inv_mass
-                            + rb2.effective_inv_mass
+                        / (mprops1.effective_inv_mass
+                            + mprops2.effective_inv_mass
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
 
@@ -261,15 +271,15 @@ impl VelocityConstraint {
                     constraint.elements[k].tangent_part.impulse = impulse;
 
                     for j in 0..DIM - 1 {
-                        let gcross1 = rb1
+                        let gcross1 = mprops1
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp1.gcross(tangents1[j]));
-                        let gcross2 = rb2
+                        let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
                         let r = 1.0
-                            / (rb1.effective_inv_mass
-                                + rb2.effective_inv_mass
+                            / (mprops1.effective_inv_mass
+                                + mprops2.effective_inv_mass
                                 + gcross1.gdot(gcross1)
                                 + gcross2.gdot(gcross2));
                         let rhs =

src/dynamics/solver/velocity_constraint_wide.rs

@@ -1,7 +1,8 @@
 use super::{
     AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
 };
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::data::ComponentSet;
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
     AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -32,14 +33,18 @@ pub(crate) struct WVelocityConstraint {
 }
 
 impl WVelocityConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
         manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         for ii in 0..SIMD_WIDTH {
             assert_eq!(manifolds[ii].data.relative_dominance, 0);
         }
@@ -49,36 +54,39 @@ impl WVelocityConstraint {
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
 
-        let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
-        let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
+        let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
+        let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
 
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii1: AngularInertia<SimdReal> = AngularInertia::from(
-            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
+        let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
+        let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
+        let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
 
-        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(gather![|ii| mprops1[ii].world_com]);
+        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
+        let ii1: AngularInertia<SimdReal> =
+            AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
 
-        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
+        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
 
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii2: AngularInertia<SimdReal> = AngularInertia::from(
-            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
-        );
+        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
+        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let ii2: AngularInertia<SimdReal> =
+            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
-        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
-        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
+        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
 
-        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
 
-        let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
-
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
+        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
 
         let warmstart_multiplier =
-            SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
+            SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
@@ -89,9 +97,8 @@ impl WVelocityConstraint {
             super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii|
-                &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH
-            ];
+            let manifold_points =
+                gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WVelocityConstraint {
@@ -112,24 +119,20 @@ impl WVelocityConstraint {
             };
 
             for k in 0..num_points {
-                let friction =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
-                let restitution =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
-                let is_bouncy = SimdReal::from(
-                    array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
-                );
+                let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
+                let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
+                let is_bouncy = SimdReal::from(gather![
+                    |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
+                ]);
                 let is_resting = SimdReal::splat(1.0) - is_bouncy;
-                let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
-                let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+                let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
                 let tangent_velocity =
-                    Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
+                    Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
 
-                let impulse = SimdReal::from(
-                    array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
-                );
-                let prev_rhs =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
+                let impulse =
+                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
+                let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
 
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
@@ -140,8 +143,7 @@ impl WVelocityConstraint {
                 let warmstart_correction;
 
                 constraint.limit = friction;
-                constraint.manifold_contact_id[k] =
-                    array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
+                constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
 
                 // Normal part.
                 {
@@ -172,15 +174,15 @@ impl WVelocityConstraint {
 
                 // tangent parts.
                 #[cfg(feature = "dim2")]
-                let impulse = [SimdReal::from(
-                    array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
-                ) * warmstart_correction];
+                let impulse = [SimdReal::from(gather![
+                    |ii| manifold_points[ii][k].warmstart_tangent_impulse
+                ]) * warmstart_correction];
 
                 #[cfg(feature = "dim3")]
                 let impulse = tangent_rot1
-                    * na::Vector2::from(
-                        array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
-                    )
+                    * na::Vector2::from(gather![
+                        |ii| manifold_points[ii][k].warmstart_tangent_impulse
+                    ])
                     * warmstart_correction;
 
                 constraint.elements[k].tangent_part.impulse = impulse;
@@ -210,21 +212,17 @@ impl WVelocityConstraint {
 
     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
+            ]),
         };
 
         VelocityConstraintElement::warmstart_group(
@@ -250,21 +248,17 @@ impl WVelocityConstraint {
 
     pub fn solve(&mut 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
+            ]),
         };
 
         VelocityConstraintElement::solve_group(

src/dynamics/solver/velocity_ground_constraint.rs

@@ -2,12 +2,13 @@ use super::{
     AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
     VelocityGroundConstraintNormalPart,
 };
-use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{WAngularInertia, WCross, WDot};
 
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 #[derive(Copy, Clone, Debug)]
@@ -28,35 +29,50 @@ pub(crate) struct VelocityGroundConstraint {
 }
 
 impl VelocityGroundConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold_id: ContactManifoldIndex,
         manifold: &ContactManifold,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         let inv_dt = params.inv_dt();
         let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
 
-        let mut rb1 = &bodies[manifold.data.body_pair.body1];
-        let mut rb2 = &bodies[manifold.data.body_pair.body2];
+        let mut handle1 = manifold.data.rigid_body1;
+        let mut handle2 = manifold.data.rigid_body2;
         let flipped = manifold.data.relative_dominance < 0;
 
         let (force_dir1, flipped_multiplier) = if flipped {
-            std::mem::swap(&mut rb1, &mut rb2);
+            std::mem::swap(&mut handle1, &mut handle2);
             (manifold.data.normal, -1.0)
         } else {
             (-manifold.data.normal, 1.0)
         };
 
+        let (vels1, world_com1) = if let Some(handle1) = handle1 {
+            let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                bodies.index_bundle(handle1.0);
+            (*vels1, mprops1.world_com)
+        } else {
+            (RigidBodyVelocity::zero(), Point::origin())
+        };
+
+        let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
+            bodies.index_bundle(handle2.unwrap().0);
+
         #[cfg(feature = "dim2")]
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
         let (tangents1, tangent_rot1) =
-            super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
+            super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
 
-        let mj_lambda2 = rb2.active_set_offset;
+        let mj_lambda2 = ids2.active_set_offset;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
         for (_l, manifold_points) in manifold
@@ -73,7 +89,7 @@ impl VelocityGroundConstraint {
                 #[cfg(feature = "dim3")]
                 tangent_rot1,
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im2: rb2.effective_inv_mass,
+                im2: mprops2.effective_inv_mass,
                 limit: 0.0,
                 mj_lambda2,
                 manifold_id,
@@ -119,7 +135,7 @@ impl VelocityGroundConstraint {
                     constraint.tangent1 = tangents1[0];
                     constraint.tangent_rot1 = tangent_rot1;
                 }
-                constraint.im2 = rb2.effective_inv_mass;
+                constraint.im2 = mprops2.effective_inv_mass;
                 constraint.limit = 0.0;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
@@ -129,10 +145,10 @@ impl VelocityGroundConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let dp2 = manifold_point.point - rb2.world_com;
-                let dp1 = manifold_point.point - rb1.world_com;
-                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
-                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+                let dp2 = manifold_point.point - mprops2.world_com;
+                let dp1 = manifold_point.point - world_com1;
+                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
+                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
                 let warmstart_correction;
 
                 constraint.limit = manifold_point.friction;
@@ -140,11 +156,11 @@ impl VelocityGroundConstraint {
 
                 // Normal part.
                 {
-                    let gcross2 = rb2
+                    let gcross2 = mprops2
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
+                    let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
@@ -178,10 +194,10 @@ impl VelocityGroundConstraint {
                     constraint.elements[k].tangent_part.impulse = impulse;
 
                     for j in 0..DIM - 1 {
-                        let gcross2 = rb2
+                        let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
-                        let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
+                        let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
                         let rhs = (vel1 - vel2
                             + flipped_multiplier * manifold_point.tangent_velocity)
                             .dot(&tangents1[j]);

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -2,7 +2,8 @@ use super::{
     AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
     VelocityGroundConstraintNormalPart,
 };
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::data::ComponentSet;
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
     AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -31,52 +32,71 @@ pub(crate) struct WVelocityGroundConstraint {
 }
 
 impl WVelocityGroundConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
         manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         let inv_dt = SimdReal::splat(params.inv_dt());
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
 
-        let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
-        let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
+        let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
+        let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
         let mut flipped = [1.0; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
             if manifolds[ii].data.relative_dominance < 0 {
-                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
                 flipped[ii] = -1.0;
             }
         }
 
+        let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
+            handles1[ii]
+                .map(|h| *bodies.index(h.0))
+                .unwrap_or_else(RigidBodyVelocity::zero)
+        }];
+        let world_com1 = Point::from(gather![|ii| {
+            handles1[ii]
+                .map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).world_com)
+                .unwrap_or_else(Point::origin)
+        }]);
+
+        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
+            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
+            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
+
         let flipped_sign = SimdReal::from(flipped);
 
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
-        let ii2: AngularInertia<SimdReal> = AngularInertia::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> =
+            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
-        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
-        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
+        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
 
-        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
-        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
+        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
 
-        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
-        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
 
-        let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
+        let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
         let force_dir1 = normal1 * -flipped_sign;
 
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
 
         let warmstart_multiplier =
-            SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
+            SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
         let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
         let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -88,7 +108,7 @@ impl WVelocityGroundConstraint {
             super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
+            let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WVelocityGroundConstraint {
@@ -107,24 +127,20 @@ impl WVelocityGroundConstraint {
             };
 
             for k in 0..num_points {
-                let friction =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
-                let restitution =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
-                let is_bouncy = SimdReal::from(
-                    array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
-                );
+                let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
+                let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
+                let is_bouncy = SimdReal::from(gather![
+                    |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
+                ]);
                 let is_resting = SimdReal::splat(1.0) - is_bouncy;
-                let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
-                let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+                let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
                 let tangent_velocity =
-                    Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
+                    Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
 
-                let impulse = SimdReal::from(
-                    array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
-                );
-                let prev_rhs =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
+                let impulse =
+                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
+                let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
 
@@ -133,8 +149,7 @@ impl WVelocityGroundConstraint {
                 let warmstart_correction;
 
                 constraint.limit = friction;
-                constraint.manifold_contact_id[k] =
-                    array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
+                constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
 
                 // Normal part.
                 {
@@ -162,14 +177,14 @@ impl WVelocityGroundConstraint {
 
                 // tangent parts.
                 #[cfg(feature = "dim2")]
-                let impulse = [SimdReal::from(
-                    array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
-                ) * warmstart_correction];
+                let impulse = [SimdReal::from(gather![
+                    |ii| manifold_points[ii][k].warmstart_tangent_impulse
+                ]) * warmstart_correction];
                 #[cfg(feature = "dim3")]
                 let impulse = tangent_rot1
-                    * na::Vector2::from(
-                        array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
-                    )
+                    * na::Vector2::from(gather![
+                        |ii| manifold_points[ii][k].warmstart_tangent_impulse
+                    ])
                     * warmstart_correction;
                 constraint.elements[k].tangent_part.impulse = impulse;
 
@@ -195,12 +210,10 @@ impl WVelocityGroundConstraint {
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         VelocityGroundConstraintElement::warmstart_group(
@@ -220,12 +233,10 @@ impl WVelocityGroundConstraint {
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         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
+            ]),
         };
 
         VelocityGroundConstraintElement::solve_group(

src/dynamics/solver/velocity_solver.rs

@@ -1,8 +1,10 @@
 use super::AnyJointVelocityConstraint;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, RigidBodySet,
+    IntegrationParameters, JointGraphEdge, RigidBodyForces, RigidBodyVelocity,
 };
+use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
 use crate::utils::WAngularInertia;
@@ -18,31 +20,38 @@ impl VelocitySolver {
         }
     }
 
-    pub fn solve(
+    pub fn solve<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         contact_constraints: &mut [AnyVelocityConstraint],
         joint_constraints: &mut [AnyJointVelocityConstraint],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         self.mj_lambdas.clear();
         self.mj_lambdas
-            .resize(bodies.active_island(island_id).len(), DeltaVel::zero());
+            .resize(islands.active_island(island_id).len(), DeltaVel::zero());
 
         // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
-        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            let dvel = &mut self.mj_lambdas[rb.active_set_offset];
+        for handle in islands.active_island(island_id) {
+            let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
+                bodies.index_bundle(handle.0);
 
-            dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
-            rb.force = na::zero();
+            let dvel = &mut self.mj_lambdas[ids.active_set_offset];
 
-            // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
-            dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
-            rb.torque = na::zero();
-        });
+            // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
+            //       by the square root of the inertia tensor:
+            dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
+            dvel.linear += forces.force * (mprops.effective_inv_mass * params.dt);
+        }
 
         /*
          * Warmstart constraints.
@@ -69,13 +78,19 @@ impl VelocitySolver {
         }
 
         // Update velocities.
-        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            let dvel = self.mj_lambdas[rb.active_set_offset];
-            rb.linvel += dvel.linear;
-            rb.angvel += rb
+        for handle in islands.active_island(island_id) {
+            let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
+
+            let dvel = self.mj_lambdas[ids.active_set_offset];
+            let dangvel = mprops
                 .effective_world_inv_inertia_sqrt
                 .transform_vector(dvel.angular);
-        });
+
+            bodies.map_mut_internal(handle.0, |vels| {
+                vels.linvel += dvel.linear;
+                vels.angvel += dangvel;
+            });
+        }
 
         // Write impulses back into the manifold structures.
         for constraint in &*joint_constraints {

src/geometry/broad_phase_multi_sap/broad_phase.rs

@@ -1,15 +1,17 @@
 use super::{
     BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
 };
-use crate::data::pubsub::Subscription;
 use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
-use crate::geometry::collider::ColliderChanges;
-use crate::geometry::{ColliderSet, RemovedCollider};
+use crate::geometry::{
+    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape,
+};
 use crate::math::Real;
 use crate::utils::IndexMut2;
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::HashMap;
 
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+
 /// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune.
 ///
 /// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws:
@@ -78,8 +80,19 @@ pub struct BroadPhase {
     layers: Vec<SAPLayer>,
     smallest_layer: u8,
     largest_layer: u8,
-    removed_colliders: Option<Subscription<RemovedCollider>>,
     deleted_any: bool,
+    // NOTE: we maintain this hashmap to simplify collider removal.
+    //       This information is also present in the ColliderProxyId
+    //       component. However if that component is removed, we need
+    //       a way to access it to do some cleanup.
+    //       Note that we could just remove the ColliderProxyId component
+    //       altogether but that would be slow because of the need to
+    //       always access this hashmap. Instead, we access this hashmap
+    //       only when the collider has been added/removed.
+    //       Another alternative would be to remove ColliderProxyId and
+    //       just use a Coarena. But this seems like it could use too
+    //       much memory.
+    colliders_proxy_ids: HashMap<ColliderHandle, SAPProxyIndex>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     region_pool: SAPRegionPool, // To avoid repeated allocations.
     // We could think serializing this workspace is useless.
@@ -107,13 +120,13 @@ impl BroadPhase {
     /// Create a new empty broad-phase.
     pub fn new() -> Self {
         BroadPhase {
-            removed_colliders: None,
             proxies: SAPProxies::new(),
             layers: Vec::new(),
             smallest_layer: 0,
             largest_layer: 0,
             region_pool: Vec::new(),
             reporting: HashMap::default(),
+            colliders_proxy_ids: HashMap::default(),
             deleted_any: false,
         }
     }
@@ -123,26 +136,13 @@ impl BroadPhase {
     /// For each colliders marked as removed, we make their containing layer mark
     /// its proxy as pre-deleted. The actual proxy removal will happen at the end
     /// of the `BroadPhase::update`.
-    fn handle_removed_colliders(&mut self, colliders: &mut ColliderSet) {
-        // Ensure we already subscribed the collider-removed events.
-        if self.removed_colliders.is_none() {
-            self.removed_colliders = Some(colliders.removed_colliders.subscribe());
+    fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
+        // For each removed collider, remove the corresponding proxy.
+        for removed in removed_colliders {
+            if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() {
+                self.predelete_proxy(proxy_id);
+            }
         }
-
-        // Extract the cursor to avoid borrowing issues.
-        let cursor = self.removed_colliders.take().unwrap();
-
-        // Read all the collider-removed events, and remove the corresponding proxy.
-        for collider in colliders.removed_colliders.read(&cursor) {
-            self.predelete_proxy(collider.proxy_index);
-        }
-
-        // NOTE: We don't acknowledge the cursor just yet because we need
-        // to traverse the set of removed colliders one more time after
-        // the broad-phase update.
-
-        // Re-insert the cursor we extracted to avoid borrowing issues.
-        self.removed_colliders = Some(cursor);
     }
 
     /// Pre-deletes a proxy from this broad-phase.
@@ -173,7 +173,7 @@ impl BroadPhase {
     /// This method will actually remove from the proxy list all the proxies
     /// marked as deletable by `self.predelete_proxy`, making their proxy
     /// handles re-usable by new proxies.
-    fn complete_removals(&mut self, colliders: &mut ColliderSet) {
+    fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) {
         // If there is no layer, there is nothing to remove.
         if self.layers.is_empty() {
             return;
@@ -215,13 +215,13 @@ impl BroadPhase {
         /*
          * Actually remove the colliders proxies.
          */
-        let cursor = self.removed_colliders.as_ref().unwrap();
-        for collider in colliders.removed_colliders.read(&cursor) {
-            if collider.proxy_index != crate::INVALID_U32 {
-                self.proxies.remove(collider.proxy_index);
+        for removed in removed_colliders {
+            if let Some(proxy_id) = self.colliders_proxy_ids.remove(removed) {
+                if proxy_id != crate::INVALID_U32 {
+                    self.proxies.remove(proxy_id);
+                }
             }
         }
-        colliders.removed_colliders.ack(&cursor);
     }
 
     /// Finalize the insertion of the layer identified by `layer_id`.
@@ -336,67 +336,127 @@ impl BroadPhase {
         }
     }
 
-    /// Updates the broad-phase, taking into account the new collider positions.
-    pub fn update(
+    fn handle_modified_collider(
         &mut self,
         prediction_distance: Real,
-        colliders: &mut ColliderSet,
+        handle: ColliderHandle,
+        proxy_index: &mut u32,
+        collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
+    ) -> bool {
+        let (co_pos, co_shape, co_changes) = collider;
+
+        let mut aabb = co_shape
+            .compute_aabb(co_pos)
+            .loosened(prediction_distance / 2.0);
+
+        aabb.mins = super::clamp_point(aabb.mins);
+        aabb.maxs = super::clamp_point(aabb.maxs);
+
+        let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) {
+            let mut layer_id = proxy.layer_id;
+            proxy.aabb = aabb;
+
+            if co_changes.contains(ColliderChanges::SHAPE) {
+                // If the shape was changed, then we need to see if this proxy should be
+                // migrated to a larger layer. Indeed, if the shape was replaced by
+                // a much larger shape, we need to promote the proxy to a bigger layer
+                // to avoid the O(n²) discretization problem.
+                let new_layer_depth = super::layer_containing_aabb(&aabb);
+                if new_layer_depth > proxy.layer_depth {
+                    self.layers[proxy.layer_id as usize]
+                        .proper_proxy_moved_to_bigger_layer(&mut self.proxies, *proxy_index);
+
+                    // We need to promote the proxy to the bigger layer.
+                    layer_id = self.ensure_layer_exists(new_layer_depth);
+                    self.proxies[*proxy_index].layer_id = layer_id;
+                }
+            }
+
+            layer_id
+        } else {
+            let layer_depth = super::layer_containing_aabb(&aabb);
+            let layer_id = self.ensure_layer_exists(layer_depth);
+
+            // Create the proxy.
+            let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
+            *proxy_index = self.proxies.insert(proxy);
+            layer_id
+        };
+
+        let layer = &mut self.layers[layer_id as usize];
+
+        // Preupdate the collider in the layer.
+        layer.preupdate_collider(
+            *proxy_index,
+            &aabb,
+            &mut self.proxies,
+            &mut self.region_pool,
+        );
+        let need_region_propagation = !layer.created_regions.is_empty();
+
+        need_region_propagation
+    }
+
+    /// Updates the broad-phase, taking into account the new collider positions.
+    pub fn update<Colliders>(
+        &mut self,
+        prediction_distance: Real,
+        colliders: &mut Colliders,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
         events: &mut Vec<BroadPhasePairEvent>,
-    ) {
+    ) where
+        Colliders: ComponentSetMut<ColliderBroadPhaseData>
+            + ComponentSet<ColliderChanges>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         // Phase 1: pre-delete the collisions that have been deleted.
-        self.handle_removed_colliders(colliders);
+        self.handle_removed_colliders(removed_colliders);
 
         let mut need_region_propagation = false;
 
         // Phase 2: pre-delete the collisions that have been deleted.
-        colliders.foreach_modified_colliders_mut_internal(|handle, collider| {
-            if !collider.changes.needs_broad_phase_update() {
-                return;
-            }
+        for handle in modified_colliders {
+            // NOTE: we use `get` because the collider may no longer
+            //       exist if it has been removed.
+            let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
 
-            let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
-            aabb.mins = super::clamp_point(aabb.mins);
-            aabb.maxs = super::clamp_point(aabb.maxs);
+            if let Some(co_changes) = co_changes {
+                let (co_bf_data, co_pos, co_shape): (
+                    &ColliderBroadPhaseData,
+                    &ColliderPosition,
+                    &ColliderShape,
+                ) = colliders.index_bundle(handle.0);
 
-            let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
-                let mut layer_id = proxy.layer_id;
-                proxy.aabb = aabb;
+                if !co_changes.needs_broad_phase_update() {
+                    return;
+                }
+                let mut new_proxy_id = co_bf_data.proxy_index;
 
-                if collider.changes.contains(ColliderChanges::SHAPE) {
-                    // If the shape was changed, then we need to see if this proxy should be
-                    // migrated to a larger layer. Indeed, if the shape was replaced by
-                    // a much larger shape, we need to promote the proxy to a bigger layer
-                    // to avoid the O(n²) discretization problem.
-                    let new_layer_depth = super::layer_containing_aabb(&aabb);
-                    if new_layer_depth > proxy.layer_depth {
-                        self.layers[proxy.layer_id as usize].proper_proxy_moved_to_bigger_layer(
-                            &mut self.proxies,
-                            collider.proxy_index,
-                        );
-
-                        // We need to promote the proxy to the bigger layer.
-                        layer_id = self.ensure_layer_exists(new_layer_depth);
-                        self.proxies[collider.proxy_index].layer_id = layer_id;
-                    }
+                if self.handle_modified_collider(
+                    prediction_distance,
+                    *handle,
+                    &mut new_proxy_id,
+                    (co_pos, co_shape, co_changes),
+                ) {
+                    need_region_propagation = true;
                 }
 
-                layer_id
-            } else {
-                let layer_depth = super::layer_containing_aabb(&aabb);
-                let layer_id = self.ensure_layer_exists(layer_depth);
+                if co_bf_data.proxy_index != new_proxy_id {
+                    self.colliders_proxy_ids.insert(*handle, new_proxy_id);
 
-                // Create the proxy.
-                let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
-                collider.proxy_index = self.proxies.insert(proxy);
-                layer_id
-            };
-
-            let layer = &mut self.layers[layer_id as usize];
-
-            // Preupdate the collider in the layer.
-            layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool);
-            need_region_propagation = need_region_propagation || !layer.created_regions.is_empty();
-        });
+                    // Make sure we have the new proxy index in case
+                    // the collider was added for the first time.
+                    colliders.set_internal(
+                        handle.0,
+                        ColliderBroadPhaseData {
+                            proxy_index: new_proxy_id,
+                        },
+                    );
+                }
+            }
+        }
 
         // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
         if need_region_propagation {
@@ -408,7 +468,7 @@ impl BroadPhase {
 
         // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
         // layers to possible remove regions from larger layers that would become empty that way.
-        self.complete_removals(colliders);
+        self.complete_removals(removed_colliders);
     }
 
     /// Propagate regions from the smallest layers up to the larger layers.

src/geometry/broad_phase_multi_sap/sap_layer.rs

@@ -1,6 +1,6 @@
 use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
-use crate::geometry::{Collider, SAPProxyIndex, AABB};
+use crate::geometry::{SAPProxyIndex, AABB};
 use crate::math::{Point, Real};
 use parry::utils::hashmap::{Entry, HashMap};
 
@@ -213,12 +213,11 @@ impl SAPLayer {
 
     pub fn preupdate_collider(
         &mut self,
-        collider: &Collider,
+        proxy_id: u32,
         aabb: &AABB,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
     ) {
-        let proxy_id = collider.proxy_index;
         let start = super::point_key(aabb.mins, self.region_width);
         let end = super::point_key(aabb.maxs, self.region_width);
 

src/geometry/collider.rs

@@ -1,231 +1,160 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
-use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
+use crate::geometry::{
+    ColliderBroadPhaseData, ColliderChanges, ColliderGroups, ColliderMassProperties,
+    ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
+    InteractionGroups, SharedShape, SolverFlags,
+};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use na::Unit;
 use parry::bounding_volume::{BoundingVolume, AABB};
 use parry::shape::Shape;
 
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
-    pub(crate) struct ColliderFlags: u8 {
-        const SENSOR = 1 << 0;
-        const FRICTION_COMBINE_RULE_01 = 1 << 1;
-        const FRICTION_COMBINE_RULE_10 = 1 << 2;
-        const RESTITUTION_COMBINE_RULE_01 = 1 << 3;
-        const RESTITUTION_COMBINE_RULE_10 = 1 << 4;
-    }
-}
-
-impl ColliderFlags {
-    pub fn is_sensor(self) -> bool {
-        self.contains(ColliderFlags::SENSOR)
-    }
-
-    pub fn friction_combine_rule_value(self) -> u8 {
-        (self.bits & 0b0000_0110) >> 1
-    }
-
-    pub fn restitution_combine_rule_value(self) -> u8 {
-        (self.bits & 0b0001_1000) >> 3
-    }
-
-    pub fn with_friction_combine_rule(mut self, rule: CoefficientCombineRule) -> Self {
-        self.bits = (self.bits & !0b0000_0110) | ((rule as u8) << 1);
-        self
-    }
-
-    pub fn with_restitution_combine_rule(mut self, rule: CoefficientCombineRule) -> Self {
-        self.bits = (self.bits & !0b0001_1000) | ((rule as u8) << 3);
-        self
-    }
-}
-
-#[derive(Clone)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-enum MassInfo {
-    /// `MassProperties` are computed with the help of [`SharedShape::mass_properties`].
-    Density(Real),
-    MassProperties(Box<MassProperties>),
-}
-
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    /// Flags describing how the collider has been modified by the user.
-    pub(crate) struct ColliderChanges: u32 {
-        const MODIFIED             = 1 << 0;
-        const POSITION_WRT_PARENT  = 1 << 1; // => BF & NF updates.
-        const POSITION             = 1 << 2; // => BF & NF updates.
-        const COLLISION_GROUPS     = 1 << 3; // => NF update.
-        const SOLVER_GROUPS        = 1 << 4; // => NF update.
-        const SHAPE                = 1 << 5; // => BF & NF update. NF pair workspace invalidation.
-        const SENSOR               = 1 << 6; // => NF update. NF pair invalidation.
-    }
-}
-
-impl ColliderChanges {
-    pub fn needs_broad_phase_update(self) -> bool {
-        self.intersects(
-            ColliderChanges::POSITION_WRT_PARENT
-                | ColliderChanges::POSITION
-                | ColliderChanges::SHAPE,
-        )
-    }
-
-    pub fn needs_narrow_phase_update(self) -> bool {
-        self.bits() > 1
-    }
-}
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
 ///
 /// To build a new collider, use the `ColliderBuilder` structure.
 pub struct Collider {
-    shape: SharedShape,
-    mass_info: MassInfo,
-    pub(crate) flags: ColliderFlags,
-    pub(crate) solver_flags: SolverFlags,
-    pub(crate) changes: ColliderChanges,
-    pub(crate) parent: RigidBodyHandle,
-    pub(crate) delta: Isometry<Real>,
-    pub(crate) position: Isometry<Real>,
-    /// The friction coefficient of this collider.
-    pub friction: Real,
-    /// The restitution coefficient of this collider.
-    pub restitution: Real,
-    pub(crate) collision_groups: InteractionGroups,
-    pub(crate) solver_groups: InteractionGroups,
-    pub(crate) proxy_index: SAPProxyIndex,
+    pub co_type: ColliderType,
+    pub co_shape: ColliderShape, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_mprops: ColliderMassProperties, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_changes: ColliderChanges, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_parent: ColliderParent, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_pos: ColliderPosition,  // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_material: ColliderMaterial, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_groups: ColliderGroups, // TODO ECS: this is public only for our bevy_rapier experiments.
+    pub co_bf_data: ColliderBroadPhaseData, // TODO ECS: this is public only for our bevy_rapier experiments.
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
 
 impl Collider {
-    pub(crate) fn reset_internal_references(&mut self) {
-        self.parent = RigidBodyHandle::invalid();
-        self.proxy_index = crate::INVALID_U32;
-        self.changes = ColliderChanges::empty();
+    // TODO ECS: exists only for our bevy_ecs tests.
+    pub fn reset_internal_references(&mut self) {
+        self.co_parent.handle = RigidBodyHandle::invalid();
+        self.co_bf_data.proxy_index = crate::INVALID_U32;
+        self.co_changes = ColliderChanges::all();
     }
 
     /// The rigid body this collider is attached to.
     pub fn parent(&self) -> RigidBodyHandle {
-        self.parent
+        self.co_parent.handle
     }
 
     /// Is this collider a sensor?
     pub fn is_sensor(&self) -> bool {
-        self.flags.is_sensor()
+        self.co_type.is_sensor()
     }
 
     /// The combine rule used by this collider to combine its friction
     /// coefficient with the friction coefficient of the other collider it
     /// is in contact with.
     pub fn friction_combine_rule(&self) -> CoefficientCombineRule {
-        CoefficientCombineRule::from_value(self.flags.friction_combine_rule_value())
+        self.co_material.friction_combine_rule
     }
 
     /// Sets the combine rule used by this collider to combine its friction
     /// coefficient with the friction coefficient of the other collider it
     /// is in contact with.
     pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) {
-        self.flags = self.flags.with_friction_combine_rule(rule);
+        self.co_material.friction_combine_rule = rule;
     }
 
     /// The combine rule used by this collider to combine its restitution
     /// coefficient with the restitution coefficient of the other collider it
     /// is in contact with.
     pub fn restitution_combine_rule(&self) -> CoefficientCombineRule {
-        CoefficientCombineRule::from_value(self.flags.restitution_combine_rule_value())
+        self.co_material.restitution_combine_rule
     }
 
     /// Sets the combine rule used by this collider to combine its restitution
     /// coefficient with the restitution coefficient of the other collider it
     /// is in contact with.
     pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) {
-        self.flags = self.flags.with_restitution_combine_rule(rule)
+        self.co_material.restitution_combine_rule = rule;
     }
 
     /// Sets whether or not this is a sensor collider.
     pub fn set_sensor(&mut self, is_sensor: bool) {
         if is_sensor != self.is_sensor() {
-            self.changes.insert(ColliderChanges::SENSOR);
-            self.flags.set(ColliderFlags::SENSOR, is_sensor);
+            self.co_changes.insert(ColliderChanges::TYPE);
+            self.co_type = if is_sensor {
+                ColliderType::Sensor
+            } else {
+                ColliderType::Solid
+            };
         }
     }
 
     #[doc(hidden)]
     pub fn set_position_debug(&mut self, position: Isometry<Real>) {
-        self.position = position;
+        self.co_pos.0 = position;
     }
 
     /// The position of this collider expressed in the local-space of the rigid-body it is attached to.
     #[deprecated(note = "use `.position_wrt_parent()` instead.")]
     pub fn delta(&self) -> &Isometry<Real> {
-        &self.delta
+        &self.co_parent.pos_wrt_parent
     }
 
     /// The world-space position of this collider.
     pub fn position(&self) -> &Isometry<Real> {
-        &self.position
-    }
-
-    /// Sets the position of this collider wrt. its parent rigid-body.
-    pub(crate) fn set_position(&mut self, position: Isometry<Real>) {
-        self.changes.insert(ColliderChanges::POSITION);
-        self.position = position;
+        &self.co_pos
     }
 
     /// The position of this collider wrt the body it is attached to.
     pub fn position_wrt_parent(&self) -> &Isometry<Real> {
-        &self.delta
+        &self.co_parent.pos_wrt_parent
     }
 
     /// Sets the position of this collider wrt. its parent rigid-body.
     pub fn set_position_wrt_parent(&mut self, position: Isometry<Real>) {
-        self.changes.insert(ColliderChanges::POSITION_WRT_PARENT);
-        self.delta = position;
+        self.co_changes.insert(ColliderChanges::PARENT);
+        self.co_parent.pos_wrt_parent = position;
     }
 
     /// The collision groups used by this collider.
     pub fn collision_groups(&self) -> InteractionGroups {
-        self.collision_groups
+        self.co_groups.collision_groups
     }
 
     /// Sets the collision groups of this collider.
     pub fn set_collision_groups(&mut self, groups: InteractionGroups) {
-        if self.collision_groups != groups {
-            self.changes.insert(ColliderChanges::COLLISION_GROUPS);
-            self.collision_groups = groups;
+        if self.co_groups.collision_groups != groups {
+            self.co_changes.insert(ColliderChanges::GROUPS);
+            self.co_groups.collision_groups = groups;
         }
     }
 
     /// The solver groups used by this collider.
     pub fn solver_groups(&self) -> InteractionGroups {
-        self.solver_groups
+        self.co_groups.solver_groups
     }
 
     /// Sets the solver groups of this collider.
     pub fn set_solver_groups(&mut self, groups: InteractionGroups) {
-        if self.solver_groups != groups {
-            self.changes.insert(ColliderChanges::SOLVER_GROUPS);
-            self.solver_groups = groups;
+        if self.co_groups.solver_groups != groups {
+            self.co_changes.insert(ColliderChanges::GROUPS);
+            self.co_groups.solver_groups = groups;
         }
     }
 
+    pub fn material(&self) -> &ColliderMaterial {
+        &self.co_material
+    }
+
     /// The density of this collider, if set.
     pub fn density(&self) -> Option<Real> {
-        match &self.mass_info {
-            MassInfo::Density(density) => Some(*density),
-            MassInfo::MassProperties(_) => None,
+        match &self.co_mprops {
+            ColliderMassProperties::Density(density) => Some(*density),
+            ColliderMassProperties::MassProperties(_) => None,
         }
     }
 
     /// The geometric shape of this collider.
     pub fn shape(&self) -> &dyn Shape {
-        &*self.shape.0
+        self.co_shape.as_ref()
     }
 
     /// A mutable reference to the geometric shape of this collider.
@@ -234,33 +163,33 @@ impl Collider {
     /// cloned first so that `self` contains a unique copy of that
     /// shape that you can modify.
     pub fn shape_mut(&mut self) -> &mut dyn Shape {
-        self.changes.insert(ColliderChanges::SHAPE);
-        self.shape.make_mut()
+        self.co_changes.insert(ColliderChanges::SHAPE);
+        self.co_shape.make_mut()
     }
 
     /// Sets the shape of this collider.
     pub fn set_shape(&mut self, shape: SharedShape) {
-        self.changes.insert(ColliderChanges::SHAPE);
-        self.shape = shape;
+        self.co_changes.insert(ColliderChanges::SHAPE);
+        self.co_shape = shape;
     }
 
     /// Compute the axis-aligned bounding box of this collider.
     pub fn compute_aabb(&self) -> AABB {
-        self.shape.compute_aabb(&self.position)
+        self.co_shape.compute_aabb(&self.co_pos)
     }
 
     /// Compute the axis-aligned bounding box of this collider.
     pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
-        let aabb1 = self.shape.compute_aabb(&self.position);
-        let aabb2 = self.shape.compute_aabb(next_position);
+        let aabb1 = self.co_shape.compute_aabb(&self.co_pos);
+        let aabb2 = self.co_shape.compute_aabb(next_position);
         aabb1.merged(&aabb2)
     }
 
     /// Compute the local-space mass properties of this collider.
     pub fn mass_properties(&self) -> MassProperties {
-        match &self.mass_info {
-            MassInfo::Density(density) => self.shape.mass_properties(*density),
-            MassInfo::MassProperties(mass_properties) => **mass_properties,
+        match &self.co_mprops {
+            ColliderMassProperties::Density(density) => self.co_shape.mass_properties(*density),
+            ColliderMassProperties::MassProperties(mass_properties) => **mass_properties,
         }
     }
 }
@@ -272,10 +201,10 @@ pub struct ColliderBuilder {
     /// The shape of the collider to be built.
     pub shape: SharedShape,
     /// The uniform density of the collider to be built.
-    density: Option<Real>,
+    pub density: Option<Real>,
     /// Overrides automatic computation of `MassProperties`.
     /// If None, it will be computed based on shape and density.
-    mass_properties: Option<MassProperties>,
+    pub mass_properties: Option<MassProperties>,
     /// The friction coefficient of the collider to be built.
     pub friction: Real,
     /// The rule used to combine two friction coefficients.
@@ -285,7 +214,7 @@ pub struct ColliderBuilder {
     /// The rule used to combine two restitution coefficients.
     pub restitution_combine_rule: CoefficientCombineRule,
     /// The position of this collider relative to the local frame of the rigid-body it is attached to.
-    pub delta: Isometry<Real>,
+    pub pos_wrt_parent: Isometry<Real>,
     /// Is this collider a sensor?
     pub is_sensor: bool,
     /// Do we have to always call the contact modifier
@@ -308,7 +237,7 @@ impl ColliderBuilder {
             mass_properties: None,
             friction: Self::default_friction(),
             restitution: 0.0,
-            delta: Isometry::identity(),
+            pos_wrt_parent: Isometry::identity(),
             is_sensor: false,
             user_data: 0,
             collision_groups: InteractionGroups::all(),
@@ -646,8 +575,8 @@ impl ColliderBuilder {
     /// relative to the rigid-body it is attached to.
     #[cfg(feature = "dim2")]
     pub fn translation(mut self, x: Real, y: Real) -> Self {
-        self.delta.translation.x = x;
-        self.delta.translation.y = y;
+        self.pos_wrt_parent.translation.x = x;
+        self.pos_wrt_parent.translation.y = y;
         self
     }
 
@@ -655,23 +584,23 @@ impl ColliderBuilder {
     /// relative to the rigid-body it is attached to.
     #[cfg(feature = "dim3")]
     pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self {
-        self.delta.translation.x = x;
-        self.delta.translation.y = y;
-        self.delta.translation.z = z;
+        self.pos_wrt_parent.translation.x = x;
+        self.pos_wrt_parent.translation.y = y;
+        self.pos_wrt_parent.translation.z = z;
         self
     }
 
     /// Sets the initial orientation of the collider to be created,
     /// relative to the rigid-body it is attached to.
     pub fn rotation(mut self, angle: AngVector<Real>) -> Self {
-        self.delta.rotation = Rotation::new(angle);
+        self.pos_wrt_parent.rotation = Rotation::new(angle);
         self
     }
 
     /// Sets the initial position (translation and orientation) of the collider to be created,
     /// relative to the rigid-body it is attached to.
     pub fn position_wrt_parent(mut self, pos: Isometry<Real>) -> Self {
-        self.delta = pos;
+        self.pos_wrt_parent = pos;
         self
     }
 
@@ -679,53 +608,97 @@ impl ColliderBuilder {
     /// relative to the rigid-body it is attached to.
     #[deprecated(note = "Use `.position_wrt_parent` instead.")]
     pub fn position(mut self, pos: Isometry<Real>) -> Self {
-        self.delta = pos;
+        self.pos_wrt_parent = pos;
         self
     }
 
     /// Set the position of this collider in the local-space of the rigid-body it is attached to.
-    #[deprecated(note = "Use `.position` instead.")]
+    #[deprecated(note = "Use `.position_wrt_parent` instead.")]
     pub fn delta(mut self, delta: Isometry<Real>) -> Self {
-        self.delta = delta;
+        self.pos_wrt_parent = delta;
         self
     }
 
     /// Builds a new collider attached to the given rigid-body.
     pub fn build(&self) -> Collider {
+        let (co_changes, co_pos, co_bf_data, co_shape, co_type, co_groups, co_material, co_mprops) =
+            self.components();
+        let co_parent = ColliderParent {
+            pos_wrt_parent: co_pos.0,
+            handle: RigidBodyHandle::invalid(),
+        };
+        Collider {
+            co_shape,
+            co_mprops,
+            co_material,
+            co_parent,
+            co_changes,
+            co_pos,
+            co_bf_data,
+            co_groups,
+            co_type,
+            user_data: self.user_data,
+        }
+    }
+
+    /// Builds all the components required by a collider.
+    pub fn components(
+        &self,
+    ) -> (
+        ColliderChanges,
+        ColliderPosition,
+        ColliderBroadPhaseData,
+        ColliderShape,
+        ColliderType,
+        ColliderGroups,
+        ColliderMaterial,
+        ColliderMassProperties,
+    ) {
         let mass_info = if let Some(mp) = self.mass_properties {
-            MassInfo::MassProperties(Box::new(mp))
+            ColliderMassProperties::MassProperties(Box::new(mp))
         } else {
             let default_density = if self.is_sensor { 0.0 } else { 1.0 };
             let density = self.density.unwrap_or(default_density);
-            MassInfo::Density(density)
+            ColliderMassProperties::Density(density)
         };
 
-        let mut flags = ColliderFlags::empty();
-        flags.set(ColliderFlags::SENSOR, self.is_sensor);
-        flags = flags
-            .with_friction_combine_rule(self.friction_combine_rule)
-            .with_restitution_combine_rule(self.restitution_combine_rule);
         let mut solver_flags = SolverFlags::default();
         solver_flags.set(
             SolverFlags::MODIFY_SOLVER_CONTACTS,
             self.modify_solver_contacts,
         );
 
-        Collider {
-            shape: self.shape.clone(),
-            mass_info,
+        let co_shape = self.shape.clone();
+        let co_mprops = mass_info;
+        let co_material = ColliderMaterial {
             friction: self.friction,
             restitution: self.restitution,
-            delta: self.delta,
-            flags,
+            friction_combine_rule: self.friction_combine_rule,
+            restitution_combine_rule: self.restitution_combine_rule,
             solver_flags,
-            changes: ColliderChanges::all(),
-            parent: RigidBodyHandle::invalid(),
-            position: Isometry::identity(),
-            proxy_index: crate::INVALID_U32,
+        };
+        let co_changes = ColliderChanges::all();
+        let co_pos = ColliderPosition(self.pos_wrt_parent);
+        let co_bf_data = ColliderBroadPhaseData::default();
+        let co_groups = ColliderGroups {
             collision_groups: self.collision_groups,
             solver_groups: self.solver_groups,
-            user_data: self.user_data,
-        }
+        };
+        let co_type = if self.is_sensor {
+            ColliderType::Sensor
+        } else {
+            ColliderType::Solid
+        };
+
+        (
+            co_changes,
+            co_pos,
+            co_bf_data,
+            co_shape,
+            co_type,
+            co_groups,
+            co_material,
+            co_mprops,
+        )
     }
 }

src/geometry/collider_components.rs

@@ -0,0 +1,220 @@
+use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
+use crate::geometry::{InteractionGroups, SAPProxyIndex, Shape, SharedShape, SolverFlags};
+use crate::math::{Isometry, Real};
+use crate::parry::partitioning::IndexedData;
+use std::ops::Deref;
+
+/// The unique identifier of a collider added to a collider set.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct ColliderHandle(pub crate::data::arena::Index);
+
+impl ColliderHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (u32, u32) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid collider handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_U32,
+            crate::INVALID_U32,
+        ))
+    }
+}
+
+impl IndexedData for ColliderHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags describing how the collider has been modified by the user.
+    pub struct ColliderChanges: u32 {
+        const MODIFIED             = 1 << 0;
+        const PARENT               = 1 << 1; // => BF & NF updates.
+        const POSITION             = 1 << 2; // => BF & NF updates.
+        const GROUPS               = 1 << 3; // => NF update.
+        const SHAPE                = 1 << 4; // => BF & NF update. NF pair workspace invalidation.
+        const TYPE                 = 1 << 5; // => NF update. NF pair invalidation.
+    }
+}
+
+impl Default for ColliderChanges {
+    fn default() -> Self {
+        ColliderChanges::empty()
+    }
+}
+
+impl ColliderChanges {
+    pub fn needs_broad_phase_update(self) -> bool {
+        self.intersects(
+            ColliderChanges::PARENT | ColliderChanges::POSITION | ColliderChanges::SHAPE,
+        )
+    }
+
+    pub fn needs_narrow_phase_update(self) -> bool {
+        self.bits() > 1
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum ColliderType {
+    Solid,
+    Sensor,
+}
+
+impl ColliderType {
+    pub fn is_sensor(self) -> bool {
+        self == ColliderType::Sensor
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderBroadPhaseData {
+    pub(crate) proxy_index: SAPProxyIndex,
+}
+
+impl Default for ColliderBroadPhaseData {
+    fn default() -> Self {
+        ColliderBroadPhaseData {
+            proxy_index: crate::INVALID_U32,
+        }
+    }
+}
+
+pub type ColliderShape = SharedShape;
+
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum ColliderMassProperties {
+    /// `MassProperties` are computed with the help of [`SharedShape::mass_properties`].
+    Density(Real),
+    MassProperties(Box<MassProperties>),
+}
+
+impl Default for ColliderMassProperties {
+    fn default() -> Self {
+        ColliderMassProperties::Density(1.0)
+    }
+}
+
+impl ColliderMassProperties {
+    pub fn mass_properties(&self, shape: &dyn Shape) -> MassProperties {
+        match self {
+            Self::Density(density) => shape.mass_properties(*density),
+            Self::MassProperties(mprops) => **mprops,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderParent {
+    pub handle: RigidBodyHandle,
+    pub pos_wrt_parent: Isometry<Real>,
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderPosition(pub Isometry<Real>);
+
+impl AsRef<Isometry<Real>> for ColliderPosition {
+    #[inline]
+    fn as_ref(&self) -> &Isometry<Real> {
+        &self.0
+    }
+}
+
+impl Deref for ColliderPosition {
+    type Target = Isometry<Real>;
+    #[inline]
+    fn deref(&self) -> &Isometry<Real> {
+        &self.0
+    }
+}
+
+impl Default for ColliderPosition {
+    fn default() -> Self {
+        Self::identity()
+    }
+}
+
+impl ColliderPosition {
+    #[must_use]
+    fn identity() -> Self {
+        ColliderPosition(Isometry::identity())
+    }
+}
+
+impl<T> From<T> for ColliderPosition
+where
+    Isometry<Real>: From<T>,
+{
+    fn from(position: T) -> Self {
+        Self(position.into())
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderGroups {
+    pub collision_groups: InteractionGroups,
+    pub solver_groups: InteractionGroups,
+}
+
+impl Default for ColliderGroups {
+    fn default() -> Self {
+        Self {
+            collision_groups: InteractionGroups::default(),
+            solver_groups: InteractionGroups::default(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderMaterial {
+    pub friction: Real,
+    pub restitution: Real,
+    pub friction_combine_rule: CoefficientCombineRule,
+    pub restitution_combine_rule: CoefficientCombineRule,
+    pub solver_flags: SolverFlags,
+}
+
+impl ColliderMaterial {
+    pub fn new(friction: Real, restitution: Real) -> Self {
+        Self {
+            friction,
+            restitution,
+            ..Default::default()
+        }
+    }
+}
+
+impl Default for ColliderMaterial {
+    fn default() -> Self {
+        Self {
+            friction: 1.0,
+            restitution: 0.0,
+            friction_combine_rule: CoefficientCombineRule::default(),
+            restitution_combine_rule: CoefficientCombineRule::default(),
+            solver_flags: SolverFlags::default(),
+        }
+    }
+}

src/geometry/collider_set.rs

@@ -1,78 +1,93 @@
 use crate::data::arena::Arena;
-use crate::data::pubsub::PubSub;
-use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::collider::ColliderChanges;
-use crate::geometry::{Collider, SAPProxyIndex};
-use parry::partitioning::IndexedData;
+use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
+use crate::geometry::{
+    Collider, ColliderBroadPhaseData, ColliderGroups, ColliderMassProperties, ColliderMaterial,
+    ColliderParent, ColliderPosition, ColliderShape, ColliderType,
+};
+use crate::geometry::{ColliderChanges, ColliderHandle};
 use std::ops::{Index, IndexMut};
 
-/// The unique identifier of a collider added to a collider set.
-#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[repr(transparent)]
-pub struct ColliderHandle(pub(crate) crate::data::arena::Index);
-
-impl ColliderHandle {
-    /// Converts this handle into its (index, generation) components.
-    pub fn into_raw_parts(self) -> (usize, u64) {
-        self.0.into_raw_parts()
-    }
-
-    /// Reconstructs an handle from its (index, generation) components.
-    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(id, generation))
-    }
-
-    /// An always-invalid collider handle.
-    pub fn invalid() -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_USIZE,
-            crate::INVALID_U64,
-        ))
-    }
-}
-
-impl IndexedData for ColliderHandle {
-    fn default() -> Self {
-        Self(IndexedData::default())
-    }
-
-    fn index(&self) -> usize {
-        self.0.index()
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-pub(crate) struct RemovedCollider {
-    pub handle: ColliderHandle,
-    pub(crate) proxy_index: SAPProxyIndex,
-}
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A set of colliders that can be handled by a physics `World`.
 pub struct ColliderSet {
-    pub(crate) removed_colliders: PubSub<RemovedCollider>,
     pub(crate) colliders: Arena<Collider>,
     pub(crate) modified_colliders: Vec<ColliderHandle>,
-    pub(crate) modified_all_colliders: bool,
+    pub(crate) removed_colliders: Vec<ColliderHandle>,
 }
 
+macro_rules! impl_field_component_set(
+    ($T: ty, $field: ident) => {
+        impl ComponentSetOption<$T> for ColliderSet {
+            fn get(&self, handle: crate::data::Index) -> Option<&$T> {
+                self.get(ColliderHandle(handle)).map(|b| &b.$field)
+            }
+        }
+
+        impl ComponentSet<$T> for ColliderSet {
+            fn size_hint(&self) -> usize {
+                self.len()
+            }
+
+            #[inline(always)]
+            fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
+                for (handle, body) in self.colliders.iter() {
+                    f(handle, &body.$field)
+                }
+            }
+        }
+
+        impl ComponentSetMut<$T> for ColliderSet {
+            fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
+                if let Some(rb) = self.get_mut_internal(ColliderHandle(handle)) {
+                    rb.$field = val;
+                }
+            }
+
+            #[inline(always)]
+            fn map_mut_internal<Result>(
+                &mut self,
+                handle: crate::data::Index,
+                f: impl FnOnce(&mut $T) -> Result,
+            ) -> Option<Result> {
+                self.get_mut_internal(ColliderHandle(handle)).map(|rb| f(&mut rb.$field))
+            }
+        }
+    }
+);
+
+impl_field_component_set!(ColliderType, co_type);
+impl_field_component_set!(ColliderShape, co_shape);
+impl_field_component_set!(ColliderMassProperties, co_mprops);
+impl_field_component_set!(ColliderChanges, co_changes);
+impl_field_component_set!(ColliderParent, co_parent);
+impl_field_component_set!(ColliderPosition, co_pos);
+impl_field_component_set!(ColliderMaterial, co_material);
+impl_field_component_set!(ColliderGroups, co_groups);
+impl_field_component_set!(ColliderBroadPhaseData, co_bf_data);
+
 impl ColliderSet {
     /// Create a new empty set of colliders.
     pub fn new() -> Self {
         ColliderSet {
-            removed_colliders: PubSub::new(),
             colliders: Arena::new(),
             modified_colliders: Vec::new(),
-            modified_all_colliders: false,
+            removed_colliders: Vec::new(),
         }
     }
 
+    pub(crate) fn take_modified(&mut self) -> Vec<ColliderHandle> {
+        std::mem::replace(&mut self.modified_colliders, vec![])
+    }
+
+    pub(crate) fn take_removed(&mut self) -> Vec<ColliderHandle> {
+        std::mem::replace(&mut self.removed_colliders, vec![])
+    }
+
     /// An always-invalid collider handle.
     pub fn invalid_handle() -> ColliderHandle {
-        ColliderHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+        ColliderHandle::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32)
     }
 
     /// Iterate through all the colliders on this set.
@@ -84,31 +99,11 @@ impl ColliderSet {
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
         self.modified_colliders.clear();
-        self.modified_all_colliders = true;
-        self.colliders
-            .iter_mut()
-            .map(|(h, b)| (ColliderHandle(h), b))
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_modified_colliders(&self, mut f: impl FnMut(ColliderHandle, &Collider)) {
-        for handle in &self.modified_colliders {
-            if let Some(rb) = self.colliders.get(handle.0) {
-                f(*handle, rb)
-            }
-        }
-    }
-
-    #[inline(always)]
-    pub(crate) fn foreach_modified_colliders_mut_internal(
-        &mut self,
-        mut f: impl FnMut(ColliderHandle, &mut Collider),
-    ) {
-        for handle in &self.modified_colliders {
-            if let Some(rb) = self.colliders.get_mut(handle.0) {
-                f(*handle, rb)
-            }
-        }
+        let modified_colliders = &mut self.modified_colliders;
+        self.colliders.iter_mut().map(move |(h, b)| {
+            modified_colliders.push(ColliderHandle(h));
+            (ColliderHandle(h), b)
+        })
     }
 
     /// The number of colliders on this set.
@@ -126,29 +121,6 @@ impl ColliderSet {
         self.colliders.contains(handle.0)
     }
 
-    pub(crate) fn contains_any_modified_collider(&self) -> bool {
-        self.modified_all_colliders || !self.modified_colliders.is_empty()
-    }
-
-    pub(crate) fn clear_modified_colliders(&mut self) {
-        if self.modified_all_colliders {
-            for collider in self.colliders.iter_mut() {
-                collider.1.changes = ColliderChanges::empty();
-            }
-            self.modified_colliders.clear();
-            self.modified_all_colliders = false;
-        } else {
-            for handle in self.modified_colliders.drain(..) {
-                // NOTE: if the collider was added, then removed from this set before
-                //       a an update, then it will no longer exist in `self.colliders`
-                //       so we need to do this `if let`.
-                if let Some(co) = self.colliders.get_mut(handle.0) {
-                    co.changes = ColliderChanges::empty();
-                }
-            }
-        }
-    }
-
     /// Inserts a new collider to this set and retrieve its handle.
     pub fn insert(
         &mut self,
@@ -159,20 +131,24 @@ impl ColliderSet {
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         coll.reset_internal_references();
-
-        coll.parent = parent_handle;
+        coll.co_parent.handle = parent_handle;
 
         // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
         // modification flag is updated properly.
         let parent = bodies
             .get_mut_internal_with_modification_tracking(parent_handle)
             .expect("Parent rigid body not found.");
-        coll.position = parent.position * coll.delta;
         let handle = ColliderHandle(self.colliders.insert(coll));
         self.modified_colliders.push(handle);
 
-        let coll = self.colliders.get(handle.0).unwrap();
-        parent.add_collider(handle, &coll);
+        let coll = self.colliders.get_mut(handle.0).unwrap();
+        parent.add_collider(
+            handle,
+            &mut coll.co_parent,
+            &mut coll.co_pos,
+            &coll.co_shape,
+            &coll.co_mprops,
+        );
         handle
     }
 
@@ -183,6 +159,7 @@ impl ColliderSet {
     pub fn remove(
         &mut self,
         handle: ColliderHandle,
+        islands: &mut IslandManager,
         bodies: &mut RigidBodySet,
         wake_up: bool,
     ) -> Option<Collider> {
@@ -191,25 +168,22 @@ impl ColliderSet {
         /*
          * Delete the collider from its parent body.
          */
-        // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
+        // NOTE: we use `get_mut_internal_with_modification_tracking` instead of `get_mut_internal` so that the
         // modification flag is updated properly.
-        if let Some(parent) = bodies.get_mut_internal_with_modification_tracking(collider.parent) {
+        if let Some(parent) =
+            bodies.get_mut_internal_with_modification_tracking(collider.co_parent.handle)
+        {
             parent.remove_collider_internal(handle, &collider);
 
             if wake_up {
-                bodies.wake_up(collider.parent, true);
+                islands.wake_up(bodies, collider.co_parent.handle, true);
             }
         }
 
         /*
          * Publish removal.
          */
-        let message = RemovedCollider {
-            handle,
-            proxy_index: collider.proxy_index,
-        };
-
-        self.removed_colliders.publish(message);
+        self.removed_colliders.push(handle);
 
         Some(collider)
     }
@@ -242,12 +216,7 @@ impl ColliderSet {
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> {
         let (collider, handle) = self.colliders.get_unknown_gen_mut(i)?;
         let handle = ColliderHandle(handle);
-        Self::mark_as_modified(
-            handle,
-            collider,
-            &mut self.modified_colliders,
-            self.modified_all_colliders,
-        );
+        Self::mark_as_modified(handle, collider, &mut self.modified_colliders);
         Some((collider, handle))
     }
 
@@ -260,10 +229,9 @@ impl ColliderSet {
         handle: ColliderHandle,
         collider: &mut Collider,
         modified_colliders: &mut Vec<ColliderHandle>,
-        modified_all_colliders: bool,
     ) {
-        if !modified_all_colliders && !collider.changes.contains(ColliderChanges::MODIFIED) {
-            collider.changes = ColliderChanges::MODIFIED;
+        if !collider.co_changes.contains(ColliderChanges::MODIFIED) {
+            collider.co_changes = ColliderChanges::MODIFIED;
             modified_colliders.push(handle);
         }
     }
@@ -272,62 +240,20 @@ impl ColliderSet {
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         let result = self.colliders.get_mut(handle.0)?;
-        Self::mark_as_modified(
-            handle,
-            result,
-            &mut self.modified_colliders,
-            self.modified_all_colliders,
-        );
+        Self::mark_as_modified(handle, result, &mut self.modified_colliders);
         Some(result)
     }
 
     pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         self.colliders.get_mut(handle.0)
     }
+}
 
-    // Just a very long name instead of `.get_mut` to make sure
-    // this is really the method we wanted to use instead of `get_mut_internal`.
-    pub(crate) fn get_mut_internal_with_modification_tracking(
-        &mut self,
-        handle: ColliderHandle,
-    ) -> Option<&mut Collider> {
-        let result = self.colliders.get_mut(handle.0)?;
-        Self::mark_as_modified(
-            handle,
-            result,
-            &mut self.modified_colliders,
-            self.modified_all_colliders,
-        );
-        Some(result)
-    }
+impl Index<crate::data::Index> for ColliderSet {
+    type Output = Collider;
 
-    // Utility function to avoid some borrowing issue in the `maintain` method.
-    fn maintain_one(bodies: &mut RigidBodySet, collider: &mut Collider) {
-        if collider
-            .changes
-            .contains(ColliderChanges::POSITION_WRT_PARENT)
-        {
-            if let Some(parent) = bodies.get_mut_internal(collider.parent()) {
-                let position = parent.position * collider.position_wrt_parent();
-                // NOTE: the set_position method will add the ColliderChanges::POSITION flag,
-                //       which is needed for the broad-phase/narrow-phase to detect the change.
-                collider.set_position(position);
-            }
-        }
-    }
-
-    pub(crate) fn handle_user_changes(&mut self, bodies: &mut RigidBodySet) {
-        if self.modified_all_colliders {
-            for (_, rb) in self.colliders.iter_mut() {
-                Self::maintain_one(bodies, rb)
-            }
-        } else {
-            for handle in self.modified_colliders.iter() {
-                if let Some(rb) = self.colliders.get_mut(handle.0) {
-                    Self::maintain_one(bodies, rb)
-                }
-            }
-        }
+    fn index(&self, index: crate::data::Index) -> &Collider {
+        &self.colliders[index]
     }
 }
 
@@ -343,12 +269,7 @@ impl Index<ColliderHandle> for ColliderSet {
 impl IndexMut<ColliderHandle> for ColliderSet {
     fn index_mut(&mut self, handle: ColliderHandle) -> &mut Collider {
         let collider = &mut self.colliders[handle.0];
-        Self::mark_as_modified(
-            handle,
-            collider,
-            &mut self.modified_colliders,
-            self.modified_all_colliders,
-        );
+        Self::mark_as_modified(handle, collider, &mut self.modified_colliders);
         collider
     }
 }

src/geometry/contact_pair.rs

@@ -1,4 +1,4 @@
-use crate::dynamics::{BodyPair, RigidBodyHandle};
+use crate::dynamics::RigidBodyHandle;
 use crate::geometry::{ColliderPair, Contact, ContactManifold};
 use crate::math::{Point, Real, Vector};
 use parry::query::ContactManifoldsWorkspace;
@@ -115,8 +115,10 @@ impl ContactPair {
 /// part of the same contact manifold share the same contact normal and contact kinematics.
 pub struct ContactManifoldData {
     // The following are set by the narrow-phase.
-    /// The pair of body involved in this contact manifold.
-    pub body_pair: BodyPair,
+    /// The first rigid-body involved in this contact manifold.
+    pub rigid_body1: Option<RigidBodyHandle>,
+    /// The second rigid-body involved in this contact manifold.
+    pub rigid_body2: Option<RigidBodyHandle>,
     pub(crate) warmstart_multiplier: Real,
     // The two following are set by the constraints solver.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
@@ -207,17 +209,19 @@ impl SolverContact {
 
 impl Default for ContactManifoldData {
     fn default() -> Self {
-        Self::new(
-            BodyPair::new(RigidBodyHandle::invalid(), RigidBodyHandle::invalid()),
-            SolverFlags::empty(),
-        )
+        Self::new(None, None, SolverFlags::empty())
     }
 }
 
 impl ContactManifoldData {
-    pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData {
+    pub(crate) fn new(
+        rigid_body1: Option<RigidBodyHandle>,
+        rigid_body2: Option<RigidBodyHandle>,
+        solver_flags: SolverFlags,
+    ) -> ContactManifoldData {
         Self {
-            body_pair,
+            rigid_body1,
+            rigid_body2,
             warmstart_multiplier: Self::min_warmstart_multiplier(),
             constraint_index: 0,
             position_constraint_index: 0,

src/geometry/mod.rs

@@ -1,8 +1,7 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
 pub use self::broad_phase_multi_sap::BroadPhase;
-pub use self::collider::{Collider, ColliderBuilder};
-pub use self::collider_set::{ColliderHandle, ColliderSet};
+pub use self::collider_components::*;
 pub use self::contact_pair::{ContactData, ContactManifoldData};
 pub use self::contact_pair::{ContactPair, SolverContact, SolverFlags};
 pub use self::interaction_graph::{
@@ -11,6 +10,11 @@ pub use self::interaction_graph::{
 pub use self::interaction_groups::InteractionGroups;
 pub use self::narrow_phase::NarrowPhase;
 
+#[cfg(feature = "default-sets")]
+pub use self::collider::{Collider, ColliderBuilder};
+#[cfg(feature = "default-sets")]
+pub use self::collider_set::ColliderSet;
+
 pub use parry::query::TrackedContact;
 
 /// A contact between two colliders.
@@ -85,7 +89,6 @@ impl IntersectionEvent {
 }
 
 pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair, SAPProxyIndex};
-pub(crate) use self::collider_set::RemovedCollider;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 pub(crate) use parry::partitioning::SimdQuadTree;
 pub use parry::shape::*;
@@ -102,9 +105,13 @@ pub(crate) fn default_query_dispatcher() -> std::sync::Arc<dyn parry::query::Que
 }
 
 mod broad_phase_multi_sap;
-mod collider;
-mod collider_set;
+mod collider_components;
 mod contact_pair;
 mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
+
+#[cfg(feature = "default-sets")]
+mod collider;
+#[cfg(feature = "default-sets")]
+mod collider_set;

src/geometry/narrow_phase.rs

@@ -1,14 +1,16 @@
 #[cfg(feature = "parallel")]
 use rayon::prelude::*;
 
-use crate::data::pubsub::Subscription;
-use crate::data::Coarena;
-use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
-use crate::geometry::collider::ColliderChanges;
+use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::CoefficientCombineRule;
+use crate::dynamics::{
+    IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, RigidBodyType,
+};
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet,
+    BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderGroups, ColliderHandle,
+    ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
     ContactData, ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
-    IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
+    IntersectionEvent, SolverContact, SolverFlags,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{
@@ -54,7 +56,6 @@ pub struct NarrowPhase {
     contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
     intersection_graph: InteractionGraph<ColliderHandle, bool>,
     graph_indices: Coarena<ColliderGraphIndices>,
-    removed_colliders: Option<Subscription<RemovedCollider>>,
 }
 
 pub(crate) type ContactManifoldIndex = usize;
@@ -75,7 +76,6 @@ impl NarrowPhase {
             contact_graph: InteractionGraph::new(),
             intersection_graph: InteractionGraph::new(),
             graph_indices: Coarena::new(),
-            removed_colliders: None,
         }
     }
 
@@ -172,75 +172,79 @@ impl NarrowPhase {
     // }
 
     /// Maintain the narrow-phase internal state by taking collider removal into account.
-    pub fn handle_user_changes(
+    pub fn handle_user_changes<Bodies, Colliders>(
         &mut self,
-        colliders: &mut ColliderSet,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        colliders: &mut Colliders,
+        bodies: &mut Bodies,
         events: &dyn EventHandler,
-    ) {
-        // Ensure we already subscribed.
-        if self.removed_colliders.is_none() {
-            self.removed_colliders = Some(colliders.removed_colliders.subscribe());
-        }
-
-        let cursor = self.removed_colliders.take().unwrap();
-
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+        Colliders: ComponentSet<ColliderChanges>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>,
+    {
         // TODO: avoid these hash-maps.
         // They are necessary to handle the swap-remove done internally
         // by the contact/intersection graphs when a node is removed.
         let mut prox_id_remap = HashMap::new();
         let mut contact_id_remap = HashMap::new();
-        let mut i = 0;
 
-        while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
+        for collider in removed_colliders {
             // NOTE: if the collider does not have any graph indices currently, there is nothing
             // to remove in the narrow-phase for this collider.
-            if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) {
+            if let Some(graph_idx) = self.graph_indices.get(collider.0) {
                 let intersection_graph_id = prox_id_remap
-                    .get(&collider.handle)
+                    .get(collider)
                     .copied()
                     .unwrap_or(graph_idx.intersection_graph_index);
                 let contact_graph_id = contact_id_remap
-                    .get(&collider.handle)
+                    .get(collider)
                     .copied()
                     .unwrap_or(graph_idx.contact_graph_index);
 
                 self.remove_collider(
                     intersection_graph_id,
                     contact_graph_id,
+                    islands,
                     colliders,
                     bodies,
                     &mut prox_id_remap,
                     &mut contact_id_remap,
                 );
             }
-
-            i += 1;
         }
 
-        colliders.removed_colliders.ack(&cursor);
-        self.removed_colliders = Some(cursor);
-
-        self.handle_modified_colliders(colliders, bodies, events);
+        self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
     }
 
-    pub(crate) fn remove_collider(
+    pub(crate) fn remove_collider<Bodies, Colliders>(
         &mut self,
         intersection_graph_id: ColliderGraphIndex,
         contact_graph_id: ColliderGraphIndex,
-        colliders: &mut ColliderSet,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        colliders: &mut Colliders,
+        bodies: &mut Bodies,
         prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
         contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+        Colliders: ComponentSetOption<ColliderParent>,
+    {
         // Wake up every body in contact with the deleted collider.
         for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
-            if let Some(parent) = colliders.get(a).map(|c| c.parent) {
-                bodies.wake_up(parent, true)
+            if let Some(parent) = colliders.get(a.0).map(|c| c.handle) {
+                islands.wake_up(bodies, parent, true)
             }
 
-            if let Some(parent) = colliders.get(b).map(|c| c.parent) {
-                bodies.wake_up(parent, true)
+            if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
+                islands.wake_up(bodies, parent, true)
             }
         }
 
@@ -263,78 +267,104 @@ impl NarrowPhase {
         }
     }
 
-    pub(crate) fn handle_modified_colliders(
+    pub(crate) fn handle_modified_colliders<Bodies, Colliders>(
         &mut self,
-        colliders: &mut ColliderSet,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        modified_colliders: &[ColliderHandle],
+        colliders: &Colliders,
+        bodies: &mut Bodies,
         events: &dyn EventHandler,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+        Colliders: ComponentSet<ColliderChanges>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>,
+    {
         let mut pairs_to_remove = vec![];
 
-        colliders.foreach_modified_colliders(|handle, collider| {
-            if collider.changes.needs_narrow_phase_update() {
-                // No flag relevant to the narrow-phase is enabled for this collider.
-                return;
-            }
+        for handle in modified_colliders {
+            // NOTE: we use `get` because the collider may no longer
+            //       exist if it has been removed.
+            let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
 
-            if let Some(gid) = self.graph_indices.get(handle.0) {
-                // For each modified colliders, we need to wake-up the bodies it is in contact with
-                // so that the narrow-phase properly takes into account the change in, e.g.,
-                // collision groups. Waking up the modified collider's parent isn't enough because
-                // it could be a static or kinematic body which don't propagate the wake-up state.
-                bodies.wake_up(collider.parent, true);
-
-                for inter in self
-                    .contact_graph
-                    .interactions_with(gid.contact_graph_index)
-                {
-                    let other_handle = if handle == inter.0 { inter.1 } else { inter.0 };
-                    if let Some(other_collider) = colliders.get(other_handle) {
-                        bodies.wake_up(other_collider.parent, true);
-                    }
+            if let Some(co_changes) = co_changes {
+                if co_changes.needs_narrow_phase_update() {
+                    // No flag relevant to the narrow-phase is enabled for this collider.
+                    return;
                 }
 
-                // For each collider which had their sensor status modified, we need
-                // to transfer their contact/intersection graph edges to the intersection/contact graph.
-                // To achieve this we will remove the relevant contact/intersection pairs form the
-                // contact/intersection graphs, and then add them into the other graph.
-                if collider.changes.contains(ColliderChanges::SENSOR) {
-                    if collider.is_sensor() {
-                        // Find the contact pairs for this collider and
-                        // push them to `pairs_to_remove`.
-                        for inter in self
-                            .contact_graph
-                            .interactions_with(gid.contact_graph_index)
-                        {
-                            pairs_to_remove.push((
-                                ColliderPair::new(inter.0, inter.1),
-                                PairRemovalMode::FromContactGraph,
-                            ));
+                if let Some(gid) = self.graph_indices.get(handle.0) {
+                    // For each modified colliders, we need to wake-up the bodies it is in contact with
+                    // so that the narrow-phase properly takes into account the change in, e.g.,
+                    // collision groups. Waking up the modified collider's parent isn't enough because
+                    // it could be a static or kinematic body which don't propagate the wake-up state.
+
+                    let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+                    let (co_changes, co_type): (&ColliderChanges, &ColliderType) =
+                        colliders.index_bundle(handle.0);
+
+                    if let Some(co_parent) = co_parent {
+                        islands.wake_up(bodies, co_parent.handle, true);
+                    }
+
+                    for inter in self
+                        .contact_graph
+                        .interactions_with(gid.contact_graph_index)
+                    {
+                        let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 };
+                        let other_parent: Option<&ColliderParent> = colliders.get(other_handle.0);
+
+                        if let Some(other_parent) = other_parent {
+                            islands.wake_up(bodies, other_parent.handle, true);
                         }
-                    } else {
-                        // Find the contact pairs for this collider and
-                        // push them to `pairs_to_remove` if both involved
-                        // colliders are not sensors.
-                        for inter in self
-                            .intersection_graph
-                            .interactions_with(gid.intersection_graph_index)
-                            .filter(|(h1, h2, _)| {
-                                !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
-                            })
-                        {
-                            pairs_to_remove.push((
-                                ColliderPair::new(inter.0, inter.1),
-                                PairRemovalMode::FromIntersectionGraph,
-                            ));
+                    }
+
+                    // For each collider which had their sensor status modified, we need
+                    // to transfer their contact/intersection graph edges to the intersection/contact graph.
+                    // To achieve this we will remove the relevant contact/intersection pairs form the
+                    // contact/intersection graphs, and then add them into the other graph.
+                    if co_changes.contains(ColliderChanges::TYPE) {
+                        if co_type.is_sensor() {
+                            // Find the contact pairs for this collider and
+                            // push them to `pairs_to_remove`.
+                            for inter in self
+                                .contact_graph
+                                .interactions_with(gid.contact_graph_index)
+                            {
+                                pairs_to_remove.push((
+                                    ColliderPair::new(inter.0, inter.1),
+                                    PairRemovalMode::FromContactGraph,
+                                ));
+                            }
+                        } else {
+                            // Find the contact pairs for this collider and
+                            // push them to `pairs_to_remove` if both involved
+                            // colliders are not sensors.
+                            for inter in self
+                                .intersection_graph
+                                .interactions_with(gid.intersection_graph_index)
+                                .filter(|(h1, h2, _)| {
+                                    let co_type1: &ColliderType = colliders.index(h1.0);
+                                    let co_type2: &ColliderType = colliders.index(h2.0);
+                                    !co_type1.is_sensor() && !co_type2.is_sensor()
+                                })
+                            {
+                                pairs_to_remove.push((
+                                    ColliderPair::new(inter.0, inter.1),
+                                    PairRemovalMode::FromIntersectionGraph,
+                                ));
+                            }
                         }
                     }
                 }
             }
-        });
+        }
 
         // Remove the pair from the relevant graph.
         for pair in &pairs_to_remove {
-            self.remove_pair(colliders, bodies, &pair.0, events, pair.1);
+            self.remove_pair(islands, colliders, bodies, &pair.0, events, pair.1);
         }
 
         // Add the paid removed pair to the relevant graph.
@@ -343,17 +373,24 @@ impl NarrowPhase {
         }
     }
 
-    fn remove_pair(
+    fn remove_pair<Bodies, Colliders>(
         &mut self,
-        colliders: &mut ColliderSet,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        colliders: &Colliders,
+        bodies: &mut Bodies,
         pair: &ColliderPair,
         events: &dyn EventHandler,
         mode: PairRemovalMode,
-    ) {
-        if let (Some(co1), Some(co2)) =
-            (colliders.get(pair.collider1), colliders.get(pair.collider2))
-        {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+        Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
+    {
+        let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
+        let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
+
+        if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
             // TODO: could we just unwrap here?
             // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
             if let (Some(gid1), Some(gid2)) = (
@@ -361,7 +398,8 @@ impl NarrowPhase {
                 self.graph_indices.get(pair.collider2.0),
             ) {
                 if mode == PairRemovalMode::FromIntersectionGraph
-                    || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
+                    || (mode == PairRemovalMode::Auto
+                        && (co_type1.is_sensor() || co_type2.is_sensor()))
                 {
                     let was_intersecting = self
                         .intersection_graph
@@ -382,8 +420,18 @@ impl NarrowPhase {
                     // Also wake up the dynamic bodies that were in contact.
                     if let Some(ctct) = contact_pair {
                         if ctct.has_any_active_contact {
-                            bodies.wake_up(co1.parent, true);
-                            bodies.wake_up(co2.parent, true);
+                            let co_parent1: Option<&ColliderParent> =
+                                colliders.get(pair.collider1.0);
+                            let co_parent2: Option<&ColliderParent> =
+                                colliders.get(pair.collider2.0);
+
+                            if let Some(co_parent1) = co_parent1 {
+                                islands.wake_up(bodies, co_parent1.handle, true);
+                            }
+
+                            if let Some(co_parent2) = co_parent2 {
+                                islands.wake_up(bodies, co_parent2.handle, true);
+                            }
 
                             events.handle_contact_event(ContactEvent::Stopped(
                                 pair.collider1,
@@ -396,11 +444,18 @@ impl NarrowPhase {
         }
     }
 
-    fn add_pair(&mut self, colliders: &mut ColliderSet, pair: &ColliderPair) {
-        if let (Some(co1), Some(co2)) =
-            (colliders.get(pair.collider1), colliders.get(pair.collider2))
-        {
-            if co1.parent == co2.parent {
+    fn add_pair<Colliders>(&mut self, colliders: &Colliders, pair: &ColliderPair)
+    where
+        Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
+    {
+        let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
+        let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
+
+        if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
+            let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
+            let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
+
+            if co_parent1.map(|p| p.handle) == co_parent2.map(|p| p.handle) {
                 // Same parents. Ignore collisions.
                 return;
             }
@@ -411,7 +466,7 @@ impl NarrowPhase {
                 ColliderGraphIndices::invalid(),
             );
 
-            if co1.is_sensor() || co2.is_sensor() {
+            if co_type1.is_sensor() || co_type2.is_sensor() {
                 // NOTE: the collider won't have a graph index as long
                 // as it does not interact with anything.
                 if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
@@ -469,33 +524,56 @@ impl NarrowPhase {
         }
     }
 
-    pub(crate) fn register_pairs(
+    pub(crate) fn register_pairs<Bodies, Colliders>(
         &mut self,
-        colliders: &mut ColliderSet,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        colliders: &Colliders,
+        bodies: &mut Bodies,
         broad_phase_events: &[BroadPhasePairEvent],
         events: &dyn EventHandler,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+        Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
+    {
         for event in broad_phase_events {
             match event {
                 BroadPhasePairEvent::AddPair(pair) => {
                     self.add_pair(colliders, pair);
                 }
                 BroadPhasePairEvent::DeletePair(pair) => {
-                    self.remove_pair(colliders, bodies, pair, events, PairRemovalMode::Auto);
+                    self.remove_pair(
+                        islands,
+                        colliders,
+                        bodies,
+                        pair,
+                        events,
+                        PairRemovalMode::Auto,
+                    );
                 }
             }
         }
     }
 
-    pub(crate) fn compute_intersections(
+    pub(crate) fn compute_intersections<Bodies, Colliders>(
         &mut self,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
-        hooks: &dyn PhysicsHooks,
+        bodies: &Bodies,
+        colliders: &Colliders,
+        modified_colliders: &[ColliderHandle],
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
         events: &dyn EventHandler,
-    ) {
-        if !colliders.contains_any_modified_collider() {
+    ) where
+        Bodies: ComponentSet<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyDominance>,
+        Colliders: ComponentSet<ColliderChanges>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderPosition>,
+    {
+        if modified_colliders.is_empty() {
             return;
         }
 
@@ -507,35 +585,66 @@ impl NarrowPhase {
         par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
             let handle1 = nodes[edge.source().index()].weight;
             let handle2 = nodes[edge.target().index()].weight;
-            let co1 = &colliders[handle1];
-            let co2 = &colliders[handle2];
 
-            if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
+            let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
+            let (co_changes1, co_groups1, co_shape1, co_pos1): (
+                &ColliderChanges,
+                &ColliderGroups,
+                &ColliderShape,
+                &ColliderPosition,
+            ) = colliders.index_bundle(handle1.0);
+
+            let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
+            let (co_changes2, co_groups2, co_shape2, co_pos2): (
+                &ColliderChanges,
+                &ColliderGroups,
+                &ColliderShape,
+                &ColliderPosition,
+            ) = colliders.index_bundle(handle2.0);
+
+            if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
             {
                 // No update needed for these colliders.
                 return;
             }
 
             // TODO: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
+            let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
+            let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
 
-            if (rb1.is_sleeping() && rb2.is_static())
-                || (rb2.is_sleeping() && rb1.is_static())
-                || (rb1.is_sleeping() && rb2.is_sleeping())
+            if let Some(co_parent1) = co_parent1 {
+                let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
+                    bodies.index_bundle(co_parent1.handle.0);
+                status1 = *rb_type1;
+                sleeping1 = rb_activation1.sleeping;
+            }
+
+            if let Some(co_parent2) = co_parent2 {
+                let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
+                    bodies.index_bundle(co_parent2.handle.0);
+                status2 = *rb_type2;
+                sleeping2 = rb_activation2.sleeping;
+            }
+
+            if (sleeping1 && status2.is_static())
+                || (sleeping2 && status1.is_static())
+                || (sleeping1 && sleeping2)
             {
                 // No need to update this intersection because nothing moved.
                 return;
             }
 
-            if !co1.collision_groups.test(co2.collision_groups) {
+            if !co_groups1
+                .collision_groups
+                .test(co_groups2.collision_groups)
+            {
                 // The intersection is not allowed.
                 return;
             }
 
             if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR)
-                && !rb1.is_dynamic()
-                && !rb2.is_dynamic()
+                && !status1.is_dynamic()
+                && !status2.is_dynamic()
             {
                 // Default filtering rule: no intersection between two non-dynamic bodies.
                 return;
@@ -543,12 +652,12 @@ impl NarrowPhase {
 
             if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) {
                 let context = PairFilterContext {
-                    rigid_body1: rb1,
-                    rigid_body2: rb2,
-                    collider_handle1: handle1,
-                    collider_handle2: handle2,
-                    collider1: co1,
-                    collider2: co2,
+                    bodies,
+                    colliders,
+                    rigid_body1: co_parent1.map(|p| p.handle),
+                    rigid_body2: co_parent2.map(|p| p.handle),
+                    collider1: handle1,
+                    collider2: handle2,
                 };
 
                 if !hooks.filter_intersection_pair(&context) {
@@ -557,10 +666,10 @@ impl NarrowPhase {
                 }
             }
 
-            let pos12 = co1.position().inv_mul(co2.position());
+            let pos12 = co_pos1.inv_mul(co_pos2);
 
             if let Ok(intersection) =
-                query_dispatcher.intersection_test(&pos12, co1.shape(), co2.shape())
+                query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2)
             {
                 if intersection != edge.weight {
                     edge.weight = intersection;
@@ -574,15 +683,26 @@ impl NarrowPhase {
         });
     }
 
-    pub(crate) fn compute_contacts(
+    pub(crate) fn compute_contacts<Bodies, Colliders>(
         &mut self,
         prediction_distance: Real,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
-        hooks: &dyn PhysicsHooks,
+        bodies: &Bodies,
+        colliders: &Colliders,
+        modified_colliders: &[ColliderHandle],
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
         events: &dyn EventHandler,
-    ) {
-        if !colliders.contains_any_modified_collider() {
+    ) where
+        Bodies: ComponentSet<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyDominance>,
+        Colliders: ComponentSet<ColliderChanges>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderMaterial>,
+    {
+        if modified_colliders.is_empty() {
             return;
         }
 
@@ -592,35 +712,68 @@ impl NarrowPhase {
         // TODO: don't iterate on all the edges.
         par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
             let pair = &mut edge.weight;
-            let co1 = &colliders[pair.pair.collider1];
-            let co2 = &colliders[pair.pair.collider2];
 
-            if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
+            let co_parent1: Option<&ColliderParent> = colliders.get(pair.pair.collider1.0);
+            let (co_changes1, co_groups1, co_shape1, co_pos1, co_material1): (
+                &ColliderChanges,
+                &ColliderGroups,
+                &ColliderShape,
+                &ColliderPosition,
+                &ColliderMaterial,
+            ) = colliders.index_bundle(pair.pair.collider1.0);
+
+            let co_parent2: Option<&ColliderParent> = colliders.get(pair.pair.collider2.0);
+            let (co_changes2, co_groups2, co_shape2, co_pos2, co_material2): (
+                &ColliderChanges,
+                &ColliderGroups,
+                &ColliderShape,
+                &ColliderPosition,
+                &ColliderMaterial,
+            ) = colliders.index_bundle(pair.pair.collider2.0);
+
+            if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
             {
                 // No update needed for these colliders.
                 return;
             }
 
             // TODO: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
+            let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
+            let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
 
-            if (rb1.is_sleeping() && rb2.is_static())
-                || (rb2.is_sleeping() && rb1.is_static())
-                || (rb1.is_sleeping() && rb2.is_sleeping())
+            if let Some(co_parent1) = co_parent1 {
+                let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
+                    bodies.index_bundle(co_parent1.handle.0);
+                status1 = *rb_type1;
+                sleeping1 = rb_activation1.sleeping;
+            }
+
+            if let Some(co_parent2) = co_parent2 {
+                let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
+                    bodies.index_bundle(co_parent2.handle.0);
+                status2 = *rb_type2;
+                sleeping2 = rb_activation2.sleeping;
+            }
+
+            if (sleeping1 && status2.is_static())
+                || (sleeping2 && status1.is_static())
+                || (sleeping1 && sleeping2)
             {
-                // No need to update this contact because nothing moved.
+                // No need to update this intersection because nothing moved.
                 return;
             }
 
-            if !co1.collision_groups.test(co2.collision_groups) {
+            if !co_groups1
+                .collision_groups
+                .test(co_groups2.collision_groups)
+            {
                 // The collision is not allowed.
                 return;
             }
 
             if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
-                && !rb1.is_dynamic()
-                && !rb2.is_dynamic()
+                && !status1.is_dynamic()
+                && !status2.is_dynamic()
             {
                 // Default filtering rule: no contact between two non-dynamic bodies.
                 return;
@@ -629,12 +782,12 @@ impl NarrowPhase {
             let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
             {
                 let context = PairFilterContext {
-                    rigid_body1: rb1,
-                    rigid_body2: rb2,
-                    collider_handle1: pair.pair.collider1,
-                    collider_handle2: pair.pair.collider2,
-                    collider1: co1,
-                    collider2: co2,
+                    bodies,
+                    colliders,
+                    rigid_body1: co_parent1.map(|p| p.handle),
+                    rigid_body2: co_parent2.map(|p| p.handle),
+                    collider1: pair.pair.collider1,
+                    collider2: pair.pair.collider2,
                 };
 
                 if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
@@ -644,25 +797,25 @@ impl NarrowPhase {
                     return;
                 }
             } else {
-                co1.solver_flags | co2.solver_flags
+                co_material1.solver_flags | co_material2.solver_flags
             };
 
-            if !co1.solver_groups.test(co2.solver_groups) {
+            if !co_groups1.solver_groups.test(co_groups2.solver_groups) {
                 solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
             }
 
-            if co1.changes.contains(ColliderChanges::SHAPE)
-                || co2.changes.contains(ColliderChanges::SHAPE)
+            if co_changes1.contains(ColliderChanges::SHAPE)
+                || co_changes2.contains(ColliderChanges::SHAPE)
             {
                 // The shape changed so the workspace is no longer valid.
                 pair.workspace = None;
             }
 
-            let pos12 = co1.position().inv_mul(co2.position());
+            let pos12 = co_pos1.inv_mul(co_pos2);
             let _ = query_dispatcher.contact_manifolds(
                 &pos12,
-                co1.shape(),
-                co2.shape(),
+                &**co_shape1,
+                &**co_shape2,
                 prediction_distance,
                 &mut pair.manifolds,
                 &mut pair.workspace,
@@ -671,25 +824,34 @@ impl NarrowPhase {
             let mut has_any_active_contact = false;
 
             let friction = CoefficientCombineRule::combine(
-                co1.friction,
-                co2.friction,
-                co1.flags.friction_combine_rule_value(),
-                co2.flags.friction_combine_rule_value(),
+                co_material1.friction,
+                co_material2.friction,
+                co_material1.friction_combine_rule as u8,
+                co_material2.friction_combine_rule as u8,
             );
             let restitution = CoefficientCombineRule::combine(
-                co1.restitution,
-                co2.restitution,
-                co1.flags.restitution_combine_rule_value(),
-                co2.flags.restitution_combine_rule_value(),
+                co_material1.restitution,
+                co_material2.restitution,
+                co_material1.restitution_combine_rule as u8,
+                co_material2.restitution_combine_rule as u8,
             );
 
+            let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
+            let dominance1 = co_parent1
+                .map(|p1| *bodies.index(p1.handle.0))
+                .unwrap_or(zero);
+            let dominance2 = co_parent2
+                .map(|p2| *bodies.index(p2.handle.0))
+                .unwrap_or(zero);
+
             for manifold in &mut pair.manifolds {
-                let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position());
+                let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
                 manifold.data.solver_contacts.clear();
-                manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
+                manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
+                manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
                 manifold.data.solver_flags = solver_flags;
                 manifold.data.relative_dominance =
-                    rb1.effective_dominance_group() - rb2.effective_dominance_group();
+                    dominance1.effective_group(&status1) - dominance2.effective_group(&status2);
                 manifold.data.normal = world_pos1 * manifold.local_n1;
 
                 // Generate solver contacts.
@@ -732,12 +894,12 @@ impl NarrowPhase {
                     let mut modifiable_normal = manifold.data.normal;
 
                     let mut context = ContactModificationContext {
-                        rigid_body1: rb1,
-                        rigid_body2: rb2,
-                        collider_handle1: pair.pair.collider1,
-                        collider_handle2: pair.pair.collider2,
-                        collider1: co1,
-                        collider2: co2,
+                        bodies,
+                        colliders,
+                        rigid_body1: co_parent1.map(|p| p.handle),
+                        rigid_body2: co_parent2.map(|p| p.handle),
+                        collider1: pair.pair.collider1,
+                        collider2: pair.pair.collider2,
                         manifold,
                         solver_contacts: &mut modifiable_solver_contacts,
                         normal: &mut modifiable_normal,
@@ -772,38 +934,61 @@ impl NarrowPhase {
 
     /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
     // NOTE: this is very similar to the code from JointSet::select_active_interactions.
-    pub(crate) fn select_active_contacts<'a>(
+    pub(crate) fn select_active_contacts<'a, Bodies>(
         &'a mut self,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         out_manifolds: &mut Vec<&'a mut ContactManifold>,
         out: &mut Vec<Vec<ContactManifoldIndex>>,
-    ) {
-        for out_island in &mut out[..bodies.num_islands()] {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyActivation>,
+    {
+        for out_island in &mut out[..islands.num_islands()] {
             out_island.clear();
         }
 
         // TODO: don't iterate through all the interactions.
         for inter in self.contact_graph.graph.edges.iter_mut() {
             for manifold in &mut inter.weight.manifolds {
-                let rb1 = &bodies[manifold.data.body_pair.body1];
-                let rb2 = &bodies[manifold.data.body_pair.body2];
                 if manifold
                     .data
                     .solver_flags
                     .contains(SolverFlags::COMPUTE_IMPULSES)
                     && manifold.data.num_active_contacts() != 0
-                    && (rb1.is_dynamic() || rb2.is_dynamic())
-                    && (!rb1.is_dynamic() || !rb1.is_sleeping())
-                    && (!rb2.is_dynamic() || !rb2.is_sleeping())
                 {
-                    let island_index = if !rb1.is_dynamic() {
-                        rb2.active_island_id
-                    } else {
-                        rb1.active_island_id
-                    };
+                    let (active_island_id1, status1, sleeping1) =
+                        if let Some(handle1) = manifold.data.rigid_body1 {
+                            let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
+                                bodies.index_bundle(handle1.0);
+                            (data.0.active_island_id, *data.1, data.2.sleeping)
+                        } else {
+                            (0, RigidBodyType::Static, true)
+                        };
 
-                    out[island_index].push(out_manifolds.len());
-                    out_manifolds.push(manifold);
+                    let (active_island_id2, status2, sleeping2) =
+                        if let Some(handle2) = manifold.data.rigid_body2 {
+                            let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
+                                bodies.index_bundle(handle2.0);
+                            (data.0.active_island_id, *data.1, data.2.sleeping)
+                        } else {
+                            (0, RigidBodyType::Static, true)
+                        };
+
+                    if (status1.is_dynamic() || status2.is_dynamic())
+                        && (!status1.is_dynamic() || !sleeping1)
+                        && (!status2.is_dynamic() || !sleeping2)
+                    {
+                        let island_index = if !status1.is_dynamic() {
+                            active_island_id2
+                        } else {
+                            active_island_id1
+                        };
+
+                        out[island_index].push(out_manifolds.len());
+                        out_manifolds.push(manifold);
+                    }
                 }
             }
         }

src/lib.rs

@@ -8,7 +8,8 @@
 //! - The ability to run a perfectly deterministic simulation on different machine, as long as they
 //! are compliant with the IEEE 754-2008 floating point standard.
 
-#![warn(missing_docs)]
+#![deny(bare_trait_objects)]
+// #![warn(missing_docs)] // TODO: re-enable this
 
 #[cfg(all(feature = "dim2", feature = "f32"))]
 pub extern crate parry2d as parry;
@@ -49,8 +50,8 @@ macro_rules! enable_flush_to_zero(
 );
 
 #[cfg(feature = "simd-is-enabled")]
-macro_rules! array(
-    ($callback: expr; SIMD_WIDTH) => {
+macro_rules! gather(
+    ($callback: expr) => {
         {
             #[inline(always)]
             #[allow(dead_code)]
@@ -122,7 +123,6 @@ macro_rules! try_ret {
 // }
 
 pub(crate) const INVALID_U32: u32 = u32::MAX;
-pub(crate) const INVALID_U64: u64 = u64::MAX;
 pub(crate) const INVALID_USIZE: usize = INVALID_U32 as usize;
 
 /// The string version of Rapier.

src/pipeline/collision_pipeline.rs

@@ -1,7 +1,11 @@
 //! Physics pipeline structures.
 
-use crate::dynamics::{JointSet, RigidBodySet};
-use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
+use crate::data::{ComponentSet, ComponentSetMut};
+use crate::dynamics::{
+    IslandManager, JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance,
+    RigidBodyIds, RigidBodyType, RigidBodyVelocity,
+};
+use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderShape, NarrowPhase};
 use crate::math::Real;
 use crate::pipeline::{EventHandler, PhysicsHooks};
 
@@ -34,46 +38,25 @@ impl CollisionPipeline {
     }
 
     /// Executes one step of the collision detection.
-    pub fn step(
+    pub fn step<Bodies, Colliders>(
         &mut self,
-        prediction_distance: Real,
-        broad_phase: &mut BroadPhase,
-        narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
-        hooks: &dyn PhysicsHooks,
-        events: &dyn EventHandler,
-    ) {
-        colliders.handle_user_changes(bodies);
-        bodies.handle_user_changes(colliders);
-        self.broadphase_collider_pairs.clear();
-
-        self.broad_phase_events.clear();
-        broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events);
-
-        narrow_phase.handle_user_changes(colliders, bodies, events);
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
-        narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
-        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
-
-        bodies.update_active_set_with_contacts(
-            colliders,
-            narrow_phase,
-            self.empty_joints.joint_graph(),
-            128,
-        );
-
-        // Update colliders positions and kinematic bodies positions.
-        bodies.foreach_active_body_mut_internal(|_, rb| {
-            rb.position = rb.next_position;
-            rb.update_colliders_positions(colliders);
-
-            for handle in &rb.colliders {
-                let collider = colliders.get_mut_internal(*handle).unwrap();
-                collider.position = rb.position * collider.delta;
-            }
-        });
-
-        bodies.modified_inactive_set.clear();
+        _prediction_distance: Real,
+        _broad_phase: &mut BroadPhase,
+        _narrow_phase: &mut NarrowPhase,
+        _islands: &mut IslandManager,
+        _bodies: &mut Bodies,
+        _colliders: &mut Colliders,
+        _hooks: &dyn PhysicsHooks<Bodies, Colliders>,
+        _events: &dyn EventHandler,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyDominance>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetMut<ColliderShape>,
+    {
+        unimplemented!()
     }
 }

src/pipeline/mod.rs

@@ -13,3 +13,4 @@ mod event_handler;
 mod physics_hooks;
 mod physics_pipeline;
 mod query_pipeline;
+mod user_changes;

src/pipeline/physics_hooks.rs

@@ -1,38 +1,38 @@
-use crate::dynamics::RigidBody;
-use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags};
+use crate::dynamics::RigidBodyHandle;
+use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags};
 use crate::math::{Real, Vector};
 use na::ComplexField;
 
 /// Context given to custom collision filters to filter-out collisions.
-pub struct PairFilterContext<'a> {
-    /// The first collider involved in the potential collision.
-    pub rigid_body1: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub rigid_body2: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub collider_handle1: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider_handle2: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider1: &'a Collider,
-    /// The first collider involved in the potential collision.
-    pub collider2: &'a Collider,
+pub struct PairFilterContext<'a, Bodies, Colliders> {
+    /// The set of rigid-bodies.
+    pub bodies: &'a Bodies,
+    /// The set of colliders.
+    pub colliders: &'a Colliders,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider1: ColliderHandle,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider2: ColliderHandle,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body1: Option<RigidBodyHandle>,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body2: Option<RigidBodyHandle>,
 }
 
 /// Context given to custom contact modifiers to modify the contacts seen by the constraints solver.
-pub struct ContactModificationContext<'a> {
-    /// The first collider involved in the potential collision.
-    pub rigid_body1: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub rigid_body2: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub collider_handle1: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider_handle2: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider1: &'a Collider,
-    /// The first collider involved in the potential collision.
-    pub collider2: &'a Collider,
+pub struct ContactModificationContext<'a, Bodies, Colliders> {
+    /// The set of rigid-bodies.
+    pub bodies: &'a Bodies,
+    /// The set of colliders.
+    pub colliders: &'a Colliders,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider1: ColliderHandle,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider2: ColliderHandle,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body1: Option<RigidBodyHandle>,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body2: Option<RigidBodyHandle>,
     /// The contact manifold.
     pub manifold: &'a ContactManifold,
     /// The solver contacts that can be modified.
@@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a> {
     pub user_data: &'a mut u32,
 }
 
-impl<'a> ContactModificationContext<'a> {
+impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> {
     /// Helper function to update `self` to emulate a oneway-platform.
     ///
     /// The "oneway" behavior will only allow contacts between two colliders
@@ -127,9 +127,14 @@ bitflags::bitflags! {
         const MODIFY_SOLVER_CONTACTS = 0b0100;
     }
 }
+impl Default for PhysicsHooksFlags {
+    fn default() -> Self {
+        PhysicsHooksFlags::empty()
+    }
+}
 
 /// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
-pub trait PhysicsHooks: Send + Sync {
+pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
     /// The sets of hooks that must be taken into account.
     fn active_hooks(&self) -> PhysicsHooksFlags;
 
@@ -156,7 +161,10 @@ pub trait PhysicsHooks: Send + Sync {
     /// will be taken into account by the constraints solver. If this returns
     /// `Some(SolverFlags::empty())` then the constraints solver will ignore these
     /// contacts.
-    fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
+    fn filter_contact_pair(
+        &self,
+        _context: &PairFilterContext<Bodies, Colliders>,
+    ) -> Option<SolverFlags> {
         None
     }
 
@@ -179,7 +187,7 @@ pub trait PhysicsHooks: Send + Sync {
     /// not compute any intersection information for it.
     /// If this return `true` then the narrow-phase will compute intersection
     /// information for this pair.
-    fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool {
+    fn filter_intersection_pair(&self, _context: &PairFilterContext<Bodies, Colliders>) -> bool {
         false
     }
 
@@ -207,21 +215,22 @@ pub trait PhysicsHooks: Send + Sync {
     /// as 0 and can be modified in `context.user_data`.
     ///
     /// The world-space contact normal can be modified in `context.normal`.
-    fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
+    fn modify_solver_contacts(&self, _context: &mut ContactModificationContext<Bodies, Colliders>) {
+    }
 }
 
-impl PhysicsHooks for () {
+impl<Bodies, Colliders> PhysicsHooks<Bodies, Colliders> for () {
     fn active_hooks(&self) -> PhysicsHooksFlags {
         PhysicsHooksFlags::empty()
     }
 
-    fn filter_contact_pair(&self, _: &PairFilterContext) -> Option<SolverFlags> {
+    fn filter_contact_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> Option<SolverFlags> {
         None
     }
 
-    fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
+    fn filter_intersection_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> bool {
         false
     }
 
-    fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
+    fn modify_solver_contacts(&self, _: &mut ContactModificationContext<Bodies, Colliders>) {}
 }

src/pipeline/physics_pipeline.rs

@@ -1,17 +1,28 @@
 //! Physics pipeline structures.
 
 use crate::counters::Counters;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{
+    CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyActivation, RigidBodyCcd,
+    RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces,
+    RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    RigidBodyVelocity,
+};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
+    BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups,
+    ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
+    ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{EventHandler, PhysicsHooks};
 
+#[cfg(feature = "default-sets")]
+use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
+
 /// The physics pipeline, responsible for stepping the whole physics simulation.
 ///
 /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh
@@ -58,17 +69,43 @@ impl PhysicsPipeline {
         }
     }
 
-    fn detect_collisions(
+    fn clear_modified_colliders(
+        &mut self,
+        colliders: &mut impl ComponentSetMut<ColliderChanges>,
+        modified_colliders: &mut Vec<ColliderHandle>,
+    ) {
+        for handle in modified_colliders.drain(..) {
+            colliders.set_internal(handle.0, ColliderChanges::empty())
+        }
+    }
+
+    fn detect_collisions<Bodies, Colliders>(
         &mut self,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
-        hooks: &dyn PhysicsHooks,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
         events: &dyn EventHandler,
         handle_user_changes: bool,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSet<RigidBodyDominance>,
+        Colliders: ComponentSetMut<ColliderBroadPhaseData>
+            + ComponentSet<ColliderChanges>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderMaterial>,
+    {
         self.counters.stages.collision_detection_time.resume();
         self.counters.cd.broad_phase_time.resume();
 
@@ -78,6 +115,8 @@ impl PhysicsPipeline {
         broad_phase.update(
             integration_parameters.prediction_distance,
             colliders,
+            modified_colliders,
+            removed_colliders,
             &mut self.broad_phase_events,
         );
 
@@ -86,37 +125,46 @@ impl PhysicsPipeline {
 
         // Update narrow-phase.
         if handle_user_changes {
-            narrow_phase.handle_user_changes(colliders, bodies, events);
+            narrow_phase.handle_user_changes(
+                islands,
+                modified_colliders,
+                removed_colliders,
+                colliders,
+                bodies,
+                events,
+            );
         }
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events);
         narrow_phase.compute_contacts(
             integration_parameters.prediction_distance,
             bodies,
             colliders,
+            modified_colliders,
             hooks,
             events,
         );
-        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
-
-        // Clear colliders modification flags.
-        colliders.clear_modified_colliders();
+        narrow_phase.compute_intersections(bodies, colliders, modified_colliders, hooks, events);
 
         self.counters.cd.narrow_phase_time.pause();
         self.counters.stages.collision_detection_time.pause();
     }
 
-    fn solve_position_constraints(
+    fn solve_position_constraints<Bodies>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         #[cfg(not(feature = "parallel"))]
         {
             enable_flush_to_zero!();
 
-            for island_id in 0..bodies.num_islands() {
+            for island_id in 0..islands.num_islands() {
                 self.solvers[island_id].solve_position_constraints(
                     island_id,
+                    islands,
                     &mut self.counters,
                     integration_parameters,
                     bodies,
@@ -129,7 +177,7 @@ impl PhysicsPipeline {
             use rayon::prelude::*;
             use std::sync::atomic::Ordering;
 
-            let num_islands = bodies.num_islands();
+            let num_islands = ilands.num_islands();
             let solvers = &mut self.solvers[..num_islands];
             let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
 
@@ -140,7 +188,7 @@ impl PhysicsPipeline {
                     .par_iter_mut()
                     .enumerate()
                     .for_each(|(island_id, solver)| {
-                        let bodies: &mut RigidBodySet =
+                        let bodies: &mut Bodies =
                             unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
 
                         solver.solve_position_constraints(
@@ -154,17 +202,30 @@ impl PhysicsPipeline {
         }
     }
 
-    fn build_islands_and_solve_velocity_constraints(
+    fn build_islands_and_solve_velocity_constraints<Bodies, Colliders>(
         &mut self,
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
         joints: &mut JointSet,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetOption<ColliderParent>,
+    {
         self.counters.stages.island_construction_time.resume();
-        bodies.update_active_set_with_contacts(
+        islands.update_active_set_with_contacts(
+            bodies,
             colliders,
             narrow_phase,
             joints.joint_graph(),
@@ -172,42 +233,58 @@ impl PhysicsPipeline {
         );
         self.counters.stages.island_construction_time.pause();
 
-        if self.manifold_indices.len() < bodies.num_islands() {
+        if self.manifold_indices.len() < islands.num_islands() {
             self.manifold_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_islands(), Vec::new());
         }
 
-        if self.joint_constraint_indices.len() < bodies.num_islands() {
+        if self.joint_constraint_indices.len() < islands.num_islands() {
             self.joint_constraint_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_islands(), Vec::new());
         }
 
         let mut manifolds = Vec::new();
-        narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices);
-        joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
+        narrow_phase.select_active_contacts(
+            islands,
+            bodies,
+            &mut manifolds,
+            &mut self.manifold_indices,
+        );
+        joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices);
 
         self.counters.stages.update_time.resume();
-        bodies.foreach_active_dynamic_body_mut_internal(|_, b| {
-            b.update_world_mass_properties();
-            b.add_gravity(*gravity)
-        });
+        for handle in islands.active_dynamic_bodies() {
+            let poss: &RigidBodyPosition = bodies.index(handle.0);
+            let position = poss.position;
+
+            let effective_inv_mass = bodies
+                .map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| {
+                    mprops.update_world_mass_properties(&position);
+                    mprops.effective_mass()
+                })
+                .unwrap();
+            bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| {
+                forces.add_linear_acceleration(&gravity, effective_inv_mass)
+            });
+        }
         self.counters.stages.update_time.pause();
 
         self.counters.stages.solver_time.resume();
-        if self.solvers.len() < bodies.num_islands() {
+        if self.solvers.len() < islands.num_islands() {
             self.solvers
-                .resize_with(bodies.num_islands(), IslandSolver::new);
+                .resize_with(islands.num_islands(), IslandSolver::new);
         }
 
         #[cfg(not(feature = "parallel"))]
         {
             enable_flush_to_zero!();
 
-            for island_id in 0..bodies.num_islands() {
+            for island_id in 0..islands.num_islands() {
                 self.solvers[island_id].init_constraints_and_solve_velocity_constraints(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
+                    islands,
                     bodies,
                     &mut manifolds[..],
                     &self.manifold_indices[island_id],
@@ -238,7 +315,7 @@ impl PhysicsPipeline {
                     .par_iter_mut()
                     .enumerate()
                     .for_each(|(island_id, solver)| {
-                        let bodies: &mut RigidBodySet =
+                        let bodies: &mut Bodies =
                             unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                         let manifolds: &mut Vec<&mut ContactManifold> =
                             unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -261,19 +338,32 @@ impl PhysicsPipeline {
         self.counters.stages.solver_time.pause();
     }
 
-    fn run_ccd_motion_clamping(
+    fn run_ccd_motion_clamping<Bodies, Colliders>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
         narrow_phase: &NarrowPhase,
         ccd_solver: &mut CCDSolver,
         events: &dyn EventHandler,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyMassProps>,
+        Colliders: ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderType>,
+    {
         self.counters.ccd.toi_computation_time.start();
         // Handle CCD
         let impacts = ccd_solver.predict_impacts_at_next_positions(
             integration_parameters.dt,
+            islands,
             bodies,
             colliders,
             narrow_phase,
@@ -283,74 +373,176 @@ impl PhysicsPipeline {
         self.counters.ccd.toi_computation_time.pause();
     }
 
-    fn advance_to_final_positions(
+    fn advance_to_final_positions<Bodies, Colliders>(
         &mut self,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_colliders: &mut Vec<ColliderHandle>,
         clear_forces: bool,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyColliders>,
+        Colliders: ComponentSetMut<ColliderPosition>
+            + ComponentSetMut<ColliderChanges>
+            + ComponentSetOption<ColliderParent>,
+    {
         // Set the rigid-bodies and kinematic bodies to their final position.
-        bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
-                rb.linvel = na::zero();
-                rb.angvel = na::zero();
+        for handle in islands.iter_active_bodies() {
+            let status: &RigidBodyType = bodies.index(handle.0);
+            if status.is_kinematic() {
+                bodies.set_internal(handle.0, RigidBodyVelocity::zero());
             }
 
             if clear_forces {
-                rb.force = na::zero();
-                rb.torque = na::zero();
+                bodies.map_mut_internal(handle.0, |f: &mut RigidBodyForces| {
+                    f.torque = na::zero();
+                    f.force = na::zero();
+                });
             }
 
-            rb.position = rb.next_position;
-            rb.update_colliders_positions(colliders);
-        });
+            bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
+                poss.position = poss.next_position
+            });
+
+            let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) =
+                bodies.index_bundle(handle.0);
+            rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position);
+        }
     }
 
-    fn interpolate_kinematic_velocities(
+    fn interpolate_kinematic_velocities<Bodies>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyPosition>,
+    {
         // Update kinematic bodies velocities.
         // TODO: what is the best place for this? It should at least be
         // located before the island computation because we test the velocity
         // there to determine if this kinematic body should wake-up dynamic
         // bodies it is touching.
-        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_next_position(integration_parameters.inv_dt());
-        });
+        for handle in islands.active_kinematic_bodies() {
+            let ppos: &RigidBodyPosition = bodies.index(handle.0);
+            let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt());
+            bodies.set_internal(handle.0, new_vel);
+        }
     }
 
-    /// Executes one timestep of the physics simulation.
+    #[cfg(feature = "default-sets")]
     pub fn step(
         &mut self,
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
         ccd_solver: &mut CCDSolver,
-        hooks: &dyn PhysicsHooks,
+        hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>,
         events: &dyn EventHandler,
     ) {
-        self.counters.reset();
-        self.counters.step_started();
-        colliders.handle_user_changes(bodies);
-        bodies.handle_user_changes(colliders);
+        let mut modified_bodies = bodies.take_modified();
+        let mut modified_colliders = colliders.take_modified();
+        let mut removed_colliders = colliders.take_removed();
 
-        self.detect_collisions(
+        self.step_generic(
+            gravity,
             integration_parameters,
+            islands,
             broad_phase,
             narrow_phase,
             bodies,
             colliders,
+            &mut modified_bodies,
+            &mut modified_colliders,
+            &mut removed_colliders,
+            joints,
+            ccd_solver,
+            hooks,
+            events,
+        );
+    }
+
+    /// Executes one timestep of the physics simulation.
+    pub fn step_generic<Bodies, Colliders>(
+        &mut self,
+        gravity: &Vector<Real>,
+        integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
+        broad_phase: &mut BroadPhase,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_bodies: &mut Vec<RigidBodyHandle>,
+        modified_colliders: &mut Vec<ColliderHandle>,
+        removed_colliders: &mut Vec<ColliderHandle>,
+        joints: &mut JointSet,
+        ccd_solver: &mut CCDSolver,
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
+        events: &dyn EventHandler,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSetMut<RigidBodyChanges>
+            + ComponentSetMut<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyDominance>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetMut<ColliderBroadPhaseData>
+            + ComponentSetMut<ColliderChanges>
+            + ComponentSetMut<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderMaterial>,
+    {
+        self.counters.reset();
+        self.counters.step_started();
+
+        super::user_changes::handle_user_changes_to_colliders(
+            bodies,
+            colliders,
+            &modified_colliders[..],
+        );
+        super::user_changes::handle_user_changes_to_rigid_bodies(
+            islands,
+            bodies,
+            colliders,
+            &modified_bodies,
+            modified_colliders,
+        );
+
+        self.detect_collisions(
+            integration_parameters,
+            islands,
+            broad_phase,
+            narrow_phase,
+            bodies,
+            colliders,
+            &modified_colliders[..],
+            removed_colliders,
             hooks,
             events,
             true,
         );
 
+        self.clear_modified_colliders(colliders, modified_colliders);
+        removed_colliders.clear();
+
         let mut remaining_time = integration_parameters.dt;
         let mut integration_parameters = *integration_parameters;
 
@@ -375,9 +567,16 @@ impl PhysicsPipeline {
             if ccd_is_enabled && remaining_substeps > 1 {
                 // NOTE: Take forces into account when updating the bodies CCD activation flags
                 //       these forces have not been integrated to the body's velocity yet.
-                let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true);
+                let ccd_active =
+                    ccd_solver.update_ccd_active_flags(islands, bodies, remaining_time, true);
                 let first_impact = if ccd_active {
-                    ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase)
+                    ccd_solver.find_first_impact(
+                        remaining_time,
+                        islands,
+                        bodies,
+                        colliders,
+                        narrow_phase,
+                    )
                 } else {
                     None
                 };
@@ -414,10 +613,11 @@ impl PhysicsPipeline {
 
             self.counters.ccd.num_substeps += 1;
 
-            self.interpolate_kinematic_velocities(&integration_parameters, bodies);
+            self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies);
             self.build_islands_and_solve_velocity_constraints(
                 gravity,
                 &integration_parameters,
+                islands,
                 narrow_phase,
                 bodies,
                 colliders,
@@ -428,11 +628,16 @@ impl PhysicsPipeline {
             if ccd_is_enabled {
                 // NOTE: don't the forces into account when updating the CCD active flags because
                 //       they have already been integrated into the velocities by the solver.
-                let ccd_active =
-                    ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false);
+                let ccd_active = ccd_solver.update_ccd_active_flags(
+                    islands,
+                    bodies,
+                    integration_parameters.dt,
+                    false,
+                );
                 if ccd_active {
                     self.run_ccd_motion_clamping(
                         &integration_parameters,
+                        islands,
                         bodies,
                         colliders,
                         narrow_phase,
@@ -449,22 +654,31 @@ impl PhysicsPipeline {
             //       This happens because our CCD use the real rigid-body
             //       velocities instead of just interpolating between
             //       isometries.
-            self.solve_position_constraints(&integration_parameters, bodies);
+            self.solve_position_constraints(&integration_parameters, islands, bodies);
 
             let clear_forces = remaining_substeps == 0;
-            self.advance_to_final_positions(bodies, colliders, clear_forces);
+            self.advance_to_final_positions(
+                islands,
+                bodies,
+                colliders,
+                modified_colliders,
+                clear_forces,
+            );
             self.detect_collisions(
                 &integration_parameters,
+                islands,
                 broad_phase,
                 narrow_phase,
                 bodies,
                 colliders,
+                modified_colliders,
+                removed_colliders,
                 hooks,
                 events,
                 false,
             );
 
-            bodies.modified_inactive_set.clear();
+            self.clear_modified_colliders(colliders, modified_colliders);
         }
 
         self.counters.step_completed();

src/pipeline/query_pipeline.rs

@@ -1,9 +1,14 @@
-use crate::dynamics::RigidBodySet;
+use crate::data::{BundleSet, ComponentSet, ComponentSetOption};
+use crate::dynamics::{
+    IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
+    RigidBodyVelocity,
+};
 use crate::geometry::{
-    Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
-    RayIntersection, SimdQuadTree, AABB,
+    ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape,
+    InteractionGroups, PointProjection, Ray, RayIntersection, SimdQuadTree, AABB,
 };
 use crate::math::{Isometry, Point, Real, Vector};
+use parry::partitioning::SimdQuadtreeDataGenerator;
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -32,11 +37,11 @@ pub struct QueryPipeline {
     dilation_factor: Real,
 }
 
-struct QueryPipelineAsCompositeShape<'a> {
+struct QueryPipelineAsCompositeShape<'a, Colliders> {
     query_pipeline: &'a QueryPipeline,
-    colliders: &'a ColliderSet,
+    colliders: &'a Colliders,
     query_groups: InteractionGroups,
-    filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>,
+    filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
 }
 
 /// Indicates how the colliders position should be taken into account when
@@ -55,7 +60,12 @@ pub enum QueryPipelineMode {
     },
 }
 
-impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
+impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders>
+where
+    // TODO ECS: make everything optional but the shape?
+    Colliders:
+        ComponentSet<ColliderGroups> + ComponentSet<ColliderPosition> + ComponentSet<ColliderShape>,
+{
     type PartShape = dyn Shape;
     type PartId = ColliderHandle;
 
@@ -64,11 +74,15 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
         shape_id: Self::PartId,
         mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
     ) {
-        if let Some(collider) = self.colliders.get(shape_id) {
-            if collider.collision_groups.test(self.query_groups)
-                && self.filter.map(|f| f(shape_id, collider)).unwrap_or(true)
+        let co_groups: Option<&ColliderGroups> = self.colliders.get(shape_id.0);
+
+        if let Some(co_groups) = co_groups {
+            if co_groups.collision_groups.test(self.query_groups)
+                && self.filter.map(|f| f(shape_id)).unwrap_or(true)
             {
-                f(Some(collider.position()), collider.shape())
+                let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+                    self.colliders.index_bundle(shape_id.0);
+                f(Some(co_pos), &**co_shape)
             }
         }
     }
@@ -98,12 +112,12 @@ impl QueryPipeline {
         Self::with_query_dispatcher(DefaultQueryDispatcher)
     }
 
-    fn as_composite_shape<'a>(
+    fn as_composite_shape<'a, Colliders>(
         &'a self,
-        colliders: &'a ColliderSet,
+        colliders: &'a Colliders,
         query_groups: InteractionGroups,
-        filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> QueryPipelineAsCompositeShape<'a> {
+        filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
+    ) -> QueryPipelineAsCompositeShape<'a, Colliders> {
         QueryPipelineAsCompositeShape {
             query_pipeline: self,
             colliders,
@@ -134,52 +148,140 @@ impl QueryPipeline {
     }
 
     /// Update the acceleration structure on the query pipeline.
-    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
-        self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
+    pub fn update<Bodies, Colliders>(
+        &mut self,
+        islands: &IslandManager,
+        bodies: &Bodies,
+        colliders: &Colliders,
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyForces>,
+        Colliders: ComponentSet<ColliderShape>
+            + ComponentSet<ColliderPosition>
+            + ComponentSetOption<ColliderParent>,
+    {
+        self.update_with_mode(
+            islands,
+            bodies,
+            colliders,
+            QueryPipelineMode::CurrentPosition,
+        )
     }
 
     /// Update the acceleration structure on the query pipeline.
-    pub fn update_with_mode(
+    pub fn update_with_mode<Bodies, Colliders>(
         &mut self,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
+        islands: &IslandManager,
+        bodies: &Bodies,
+        colliders: &Colliders,
         mode: QueryPipelineMode,
-    ) {
-        if !self.tree_built {
-            match mode {
-                QueryPipelineMode::CurrentPosition => {
-                    let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
-                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
-                }
-                QueryPipelineMode::SweepTestWithNextPosition => {
-                    let data = colliders.iter().map(|(h, c)| {
-                        let next_position =
-                            bodies[c.parent()].next_position * c.position_wrt_parent();
-                        (h, c.compute_swept_aabb(&next_position))
-                    });
-                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
-                }
-                QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                    let data = colliders.iter().map(|(h, c)| {
-                        let next_position = bodies[c.parent()]
-                            .predict_position_using_velocity_and_forces(dt)
-                            * c.position_wrt_parent();
-                        (h, c.compute_swept_aabb(&next_position))
-                    });
-                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyForces>,
+        Colliders: ComponentSet<ColliderShape>
+            + ComponentSet<ColliderPosition>
+            + ComponentSetOption<ColliderParent>,
+    {
+        struct DataGenerator<'a, Bs, Cs> {
+            bodies: &'a Bs,
+            colliders: &'a Cs,
+            mode: QueryPipelineMode,
+        }
+
+        impl<'a, Bs, Cs> SimdQuadtreeDataGenerator<ColliderHandle> for DataGenerator<'a, Bs, Cs>
+        where
+            Bs: ComponentSet<RigidBodyPosition>
+                + ComponentSet<RigidBodyMassProps>
+                + ComponentSet<RigidBodyVelocity>
+                + ComponentSet<RigidBodyForces>,
+            Cs: ComponentSet<ColliderShape>
+                + ComponentSet<ColliderPosition>
+                + ComponentSetOption<ColliderParent>,
+        {
+            fn size_hint(&self) -> usize {
+                ComponentSet::<ColliderShape>::size_hint(self.colliders)
+            }
+
+            #[inline(always)]
+            fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) {
+                match self.mode {
+                    QueryPipelineMode::CurrentPosition => {
+                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
+                            let co_pos: &ColliderPosition = self.colliders.index(h);
+                            f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+                        })
+                    }
+                    QueryPipelineMode::SweepTestWithNextPosition => {
+                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
+                            let co_parent: Option<&ColliderParent> = self.colliders.get(h);
+                            let co_pos: &ColliderPosition = self.colliders.index(h);
+
+                            if let Some(co_parent) = co_parent {
+                                let rb_pos: &RigidBodyPosition =
+                                    self.bodies.index(co_parent.handle.0);
+                                let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
+
+                                f(
+                                    ColliderHandle(h),
+                                    co_shape.compute_swept_aabb(&co_pos, &next_position),
+                                )
+                            } else {
+                                f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+                            }
+                        })
+                    }
+                    QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
+                            let co_parent: Option<&ColliderParent> = self.colliders.get(h);
+                            let co_pos: &ColliderPosition = self.colliders.index(h);
+
+                            if let Some(co_parent) = co_parent {
+                                let (rb_pos, vels, forces, mprops): (
+                                    &RigidBodyPosition,
+                                    &RigidBodyVelocity,
+                                    &RigidBodyForces,
+                                    &RigidBodyMassProps,
+                                ) = self.bodies.index_bundle(co_parent.handle.0);
+                                let predicted_pos =
+                                    rb_pos.integrate_force_and_velocity(dt, forces, vels, mprops);
+
+                                let next_position = predicted_pos * co_parent.pos_wrt_parent;
+                                f(
+                                    ColliderHandle(h),
+                                    co_shape.compute_swept_aabb(&co_pos, &next_position),
+                                )
+                            } else {
+                                f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+                            }
+                        })
+                    }
                 }
             }
+        }
+
+        if !self.tree_built {
+            let generator = DataGenerator {
+                bodies,
+                colliders,
+                mode,
+            };
+            self.quadtree
+                .clear_and_rebuild(generator, self.dilation_factor);
 
             // FIXME: uncomment this once we handle insertion/removals properly.
             // self.tree_built = true;
             return;
         }
 
-        for (_, body) in bodies
-            .iter_active_dynamic()
-            .chain(bodies.iter_active_kinematic())
-        {
-            for handle in &body.colliders {
+        for handle in islands.iter_active_bodies() {
+            let body_colliders: &RigidBodyColliders = bodies.index(handle.0);
+            for handle in &body_colliders.0 {
                 self.quadtree.pre_update(*handle)
             }
         }
@@ -187,17 +289,28 @@ impl QueryPipeline {
         match mode {
             QueryPipelineMode::CurrentPosition => {
                 self.quadtree.update(
-                    |handle| colliders[*handle].compute_aabb(),
+                    |handle| {
+                        let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+                            colliders.index_bundle(handle.0);
+                        co_shape.compute_aabb(&co_pos)
+                    },
                     self.dilation_factor,
                 );
             }
             QueryPipelineMode::SweepTestWithNextPosition => {
                 self.quadtree.update(
                     |handle| {
-                        let co = &colliders[*handle];
-                        let next_position =
-                            bodies[co.parent()].next_position * co.position_wrt_parent();
-                        co.compute_swept_aabb(&next_position)
+                        let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+                        let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+                            colliders.index_bundle(handle.0);
+
+                        if let Some(co_parent) = co_parent {
+                            let rb_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
+                            let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
+                            co_shape.compute_swept_aabb(&co_pos, &next_position)
+                        } else {
+                            co_shape.compute_aabb(&co_pos)
+                        }
                     },
                     self.dilation_factor,
                 );
@@ -205,11 +318,26 @@ impl QueryPipeline {
             QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
                 self.quadtree.update(
                     |handle| {
-                        let co = &colliders[*handle];
-                        let next_position = bodies[co.parent()]
-                            .predict_position_using_velocity_and_forces(dt)
-                            * co.position_wrt_parent();
-                        co.compute_swept_aabb(&next_position)
+                        let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+                        let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+                            colliders.index_bundle(handle.0);
+
+                        if let Some(co_parent) = co_parent {
+                            let (rb_pos, vels, forces, mprops): (
+                                &RigidBodyPosition,
+                                &RigidBodyVelocity,
+                                &RigidBodyForces,
+                                &RigidBodyMassProps,
+                            ) = bodies.index_bundle(co_parent.handle.0);
+
+                            let predicted_pos =
+                                rb_pos.integrate_force_and_velocity(dt, forces, vels, mprops);
+
+                            let next_position = predicted_pos * co_parent.pos_wrt_parent;
+                            co_shape.compute_swept_aabb(&co_pos, &next_position)
+                        } else {
+                            co_shape.compute_aabb(&co_pos)
+                        }
                     },
                     self.dilation_factor,
                 );
@@ -232,15 +360,20 @@ impl QueryPipeline {
     /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn cast_ray(
+    pub fn cast_ray<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, Real)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, Real)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor =
             RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
@@ -263,15 +396,20 @@ impl QueryPipeline {
     /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn cast_ray_and_get_normal(
+    pub fn cast_ray_and_get_normal<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, RayIntersection)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, RayIntersection)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
             &pipeline_shape,
@@ -301,26 +439,31 @@ impl QueryPipeline {
     /// - `callback`: function executed on each collider for which a ray intersection has been found.
     ///               There is no guarantees on the order the results will be yielded. If this callback returns `false`,
     ///               this method will exit early, ignore any further raycast.
-    pub fn intersections_with_ray<'a>(
+    pub fn intersections_with_ray<'a, Colliders>(
         &self,
-        colliders: &'a ColliderSet,
+        colliders: &'a Colliders,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-        mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
-    ) {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
+    ) where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            if let Some(coll) = colliders.get(*handle) {
-                if coll.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle, coll)).unwrap_or(true)
+            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
+            if let Some(co_shape) = co_shape {
+                let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+                    colliders.index_bundle(handle.0);
+                if co_groups.collision_groups.test(query_groups)
+                    && filter.map(|f| f(*handle)).unwrap_or(true)
                 {
-                    if let Some(hit) =
-                        coll.shape()
-                            .cast_ray_and_get_normal(coll.position(), ray, max_toi, solid)
+                    if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid)
                     {
-                        return callback(*handle, coll, hit);
+                        return callback(*handle, hit);
                     }
                 }
             }
@@ -343,14 +486,19 @@ impl QueryPipeline {
     /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn intersection_with_shape(
+    pub fn intersection_with_shape<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         shape_pos: &Isometry<Real>,
         shape: &dyn Shape,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<ColliderHandle> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<ColliderHandle>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
@@ -379,14 +527,19 @@ impl QueryPipeline {
     /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn project_point(
+    pub fn project_point<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         point: &Point<Real>,
         solid: bool,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, PointProjection)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, PointProjection)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor =
             PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
@@ -408,21 +561,30 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     /// * `callback` - A function called with each collider with a shape
     ///                containing the `point`.
-    pub fn intersections_with_point<'a>(
+    pub fn intersections_with_point<'a, Colliders>(
         &self,
-        colliders: &'a ColliderSet,
+        colliders: &'a Colliders,
         point: &Point<Real>,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-        mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool,
-    ) {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        mut callback: impl FnMut(ColliderHandle) -> bool,
+    ) where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            if let Some(coll) = colliders.get(*handle) {
-                if coll.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle, coll)).unwrap_or(true)
-                    && coll.shape().contains_point(coll.position(), point)
+            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
+
+            if let Some(co_shape) = co_shape {
+                let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+                    colliders.index_bundle(handle.0);
+
+                if co_groups.collision_groups.test(query_groups)
+                    && filter.map(|f| f(*handle)).unwrap_or(true)
+                    && co_shape.contains_point(co_pos, point)
                 {
-                    return callback(*handle, coll);
+                    return callback(*handle);
                 }
             }
 
@@ -451,13 +613,18 @@ impl QueryPipeline {
     /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn project_point_and_get_feature(
+    pub fn project_point_and_get_feature<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         point: &Point<Real>,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, PointProjection, FeatureId)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor =
             PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
@@ -493,16 +660,21 @@ impl QueryPipeline {
     /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn cast_shape<'a>(
+    pub fn cast_shape<'a, Colliders>(
         &self,
-        colliders: &'a ColliderSet,
+        colliders: &'a Colliders,
         shape_pos: &Isometry<Real>,
         shape_vel: &Vector<Real>,
         shape: &dyn Shape,
         max_toi: Real,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, TOI)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, TOI)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
@@ -535,17 +707,22 @@ impl QueryPipeline {
     /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
-    pub fn nonlinear_cast_shape(
+    pub fn nonlinear_cast_shape<Colliders>(
         &self,
-        colliders: &ColliderSet,
+        colliders: &Colliders,
         shape_motion: &NonlinearRigidMotion,
         shape: &dyn Shape,
         start_time: Real,
         end_time: Real,
         stop_at_penetration: bool,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-    ) -> Option<(ColliderHandle, TOI)> {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+    ) -> Option<(ColliderHandle, TOI)>
+    where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
         let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
@@ -574,27 +751,36 @@ impl QueryPipeline {
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
     /// * `callback` - A function called with the handles of each collider intersecting the `shape`.
-    pub fn intersections_with_shape<'a>(
+    pub fn intersections_with_shape<'a, Colliders>(
         &self,
-        colliders: &'a ColliderSet,
+        colliders: &'a Colliders,
         shape_pos: &Isometry<Real>,
         shape: &dyn Shape,
         query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
-        mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool,
-    ) {
+        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        mut callback: impl FnMut(ColliderHandle) -> bool,
+    ) where
+        Colliders: ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>,
+    {
         let dispatcher = &*self.query_dispatcher;
         let inv_shape_pos = shape_pos.inverse();
 
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            if let Some(coll) = colliders.get(*handle) {
-                if coll.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle, coll)).unwrap_or(true)
-                {
-                    let pos12 = inv_shape_pos * coll.position();
+            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
 
-                    if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) {
-                        return callback(*handle, coll);
+            if let Some(co_shape) = co_shape {
+                let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+                    colliders.index_bundle(handle.0);
+
+                if co_groups.collision_groups.test(query_groups)
+                    && filter.map(|f| f(*handle)).unwrap_or(true)
+                {
+                    let pos12 = inv_shape_pos * co_pos.as_ref();
+
+                    if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) {
+                        return callback(*handle);
                     }
                 }
             }

src/pipeline/user_changes.rs

@@ -0,0 +1,156 @@
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
+use crate::dynamics::{
+    IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle,
+    RigidBodyIds, RigidBodyPosition, RigidBodyType,
+};
+use crate::geometry::{ColliderChanges, ColliderHandle, ColliderParent, ColliderPosition};
+
+pub(crate) fn handle_user_changes_to_colliders<Colliders>(
+    bodies: &mut impl ComponentSet<RigidBodyPosition>,
+    colliders: &mut Colliders,
+    modified_colliders: &[ColliderHandle],
+) where
+    Colliders: ComponentSetMut<ColliderChanges>
+        + ComponentSetMut<ColliderPosition>
+        + ComponentSetOption<ColliderParent>,
+{
+    for handle in modified_colliders {
+        // NOTE: we use `get` because the collider may no longer
+        //       exist if it has been removed.
+        let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
+
+        if let Some(co_changes) = co_changes {
+            if co_changes.contains(ColliderChanges::PARENT) {
+                let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+
+                if let Some(co_parent) = co_parent {
+                    let parent_pos = bodies.index(co_parent.handle.0);
+
+                    let new_pos = parent_pos.position * co_parent.pos_wrt_parent;
+                    let new_changes = *co_changes | ColliderChanges::POSITION;
+                    colliders.set_internal(handle.0, ColliderPosition(new_pos));
+                    colliders.set_internal(handle.0, new_changes);
+                }
+            }
+        }
+    }
+}
+
+pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
+    islands: &mut IslandManager,
+    bodies: &mut Bodies,
+    colliders: &mut Colliders,
+    modified_bodies: &[RigidBodyHandle],
+    modified_colliders: &mut Vec<ColliderHandle>,
+) where
+    Bodies: ComponentSetMut<RigidBodyChanges>
+        + ComponentSet<RigidBodyType>
+        + ComponentSetMut<RigidBodyIds>
+        + ComponentSetMut<RigidBodyActivation>
+        + ComponentSet<RigidBodyColliders>
+        + ComponentSet<RigidBodyPosition>,
+    Colliders: ComponentSetMut<ColliderPosition>
+        + ComponentSetMut<ColliderChanges>
+        + ComponentSetOption<ColliderParent>,
+{
+    enum FinalAction {
+        UpdateActiveKinematicSetId,
+        UpdateActiveDynamicSetId,
+    }
+
+    for handle in modified_bodies {
+        let mut final_action = None;
+
+        let mut changes: RigidBodyChanges = *bodies.index(handle.0);
+        let mut ids: RigidBodyIds = *bodies.index(handle.0);
+        let mut activation: RigidBodyActivation = *bodies.index(handle.0);
+        let (status, rb_colliders, poss): (
+            &RigidBodyType,
+            &RigidBodyColliders,
+            &RigidBodyPosition,
+        ) = bodies.index_bundle(handle.0);
+
+        {
+            // The body's status changed. We need to make sure
+            // it is on the correct active set.
+            if changes.contains(RigidBodyChanges::TYPE) {
+                match status {
+                    RigidBodyType::Dynamic => {
+                        // Remove from the active kinematic set if it was there.
+                        if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
+                            islands.active_kinematic_set.swap_remove(ids.active_set_id);
+                            final_action =
+                                Some((FinalAction::UpdateActiveKinematicSetId, ids.active_set_id));
+                        }
+
+                        // Add to the active dynamic set.
+                        activation.wake_up(true);
+                        // Make sure the sleep change flag is set (even if for some
+                        // reasons the rigid-body was already awake) to make
+                        // sure the code handling sleeping change adds the body to
+                        // the active_dynamic_set.
+                        changes.set(RigidBodyChanges::SLEEP, true);
+                    }
+                    RigidBodyType::Kinematic => {
+                        // Remove from the active dynamic set if it was there.
+                        if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) {
+                            islands.active_dynamic_set.swap_remove(ids.active_set_id);
+                            final_action =
+                                Some((FinalAction::UpdateActiveDynamicSetId, ids.active_set_id));
+                        }
+
+                        // Add to the active kinematic set.
+                        if islands.active_kinematic_set.get(ids.active_set_id) != Some(&handle) {
+                            ids.active_set_id = islands.active_kinematic_set.len();
+                            islands.active_kinematic_set.push(*handle);
+                        }
+                    }
+                    RigidBodyType::Static => {}
+                }
+            }
+
+            // Update the positions of the colliders.
+            if changes.contains(RigidBodyChanges::POSITION)
+                || changes.contains(RigidBodyChanges::COLLIDERS)
+            {
+                rb_colliders.update_positions(colliders, modified_colliders, &poss.position);
+
+                if status.is_kinematic()
+                    && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
+                {
+                    ids.active_set_id = islands.active_kinematic_set.len();
+                    islands.active_kinematic_set.push(*handle);
+                }
+            }
+
+            // Push the body to the active set if it is not
+            // sleeping and if it is not already inside of the active set.
+            if changes.contains(RigidBodyChanges::SLEEP)
+                && !activation.sleeping // May happen if the body was put to sleep manually.
+                && status.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
+            {
+                ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                islands.active_dynamic_set.push(*handle);
+            }
+
+            bodies.set_internal(handle.0, RigidBodyChanges::empty());
+            bodies.set_internal(handle.0, ids);
+            bodies.set_internal(handle.0, activation);
+        }
+
+        // Adjust some ids, if needed.
+        if let Some((action, id)) = final_action {
+            let active_set = match action {
+                FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set,
+                FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set,
+            };
+
+            if id < active_set.len() {
+                bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| {
+                    ids2.active_set_id = id;
+                });
+            }
+        }
+    }
+}

src_testbed/box2d_backend.rs

@@ -158,8 +158,8 @@ impl Box2dWorld {
         let center = na_vec_to_b2_vec(collider.position_wrt_parent().translation.vector);
         let mut fixture_def = b2::FixtureDef::new();
 
-        fixture_def.restitution = collider.restitution;
-        fixture_def.friction = collider.friction;
+        fixture_def.restitution = collider.material().restitution;
+        fixture_def.friction = collider.material().friction;
         fixture_def.density = collider.density().unwrap_or(1.0);
         fixture_def.is_sensor = collider.is_sensor();
         fixture_def.filter = b2::Filter::new();

src_testbed/harness/mod.rs

@@ -4,7 +4,7 @@ use crate::{
 };
 use kiss3d::window::Window;
 use plugin::HarnessPlugin;
-use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
+use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -120,7 +120,7 @@ impl Harness {
         colliders: ColliderSet,
         joints: JointSet,
         gravity: Vector<f32>,
-        hooks: impl PhysicsHooks + 'static,
+        hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         // println!("Num bodies: {}", bodies.len());
         // println!("Num joints: {}", joints.len());
@@ -130,6 +130,7 @@ impl Harness {
         self.physics.joints = joints;
         self.physics.hooks = Box::new(hooks);
 
+        self.physics.islands = IslandManager::new();
         self.physics.broad_phase = BroadPhase::new();
         self.physics.narrow_phase = NarrowPhase::new();
         self.state.timestep_id = 0;
@@ -191,6 +192,7 @@ impl Harness {
         self.physics.pipeline.step(
             &self.physics.gravity,
             &self.physics.integration_parameters,
+            &mut self.physics.islands,
             &mut self.physics.broad_phase,
             &mut self.physics.narrow_phase,
             &mut self.physics.bodies,
@@ -201,9 +203,11 @@ impl Harness {
             &self.event_handler,
         );
 
-        self.physics
-            .query_pipeline
-            .update(&self.physics.bodies, &self.physics.colliders);
+        self.physics.query_pipeline.update(
+            &self.physics.islands,
+            &self.physics.bodies,
+            &self.physics.colliders,
+        );
 
         for plugin in &mut self.plugins {
             plugin.step(&mut self.physics, &self.state)

src_testbed/physics/mod.rs

@@ -1,5 +1,5 @@
 use crossbeam::channel::Receiver;
-use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
+use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
 use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -68,6 +68,7 @@ impl PhysicsSnapshot {
 }
 
 pub struct PhysicsState {
+    pub islands: IslandManager,
     pub broad_phase: BroadPhase,
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
@@ -78,12 +79,13 @@ pub struct PhysicsState {
     pub query_pipeline: QueryPipeline,
     pub integration_parameters: IntegrationParameters,
     pub gravity: Vector<f32>,
-    pub hooks: Box<dyn PhysicsHooks>,
+    pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
 }
 
 impl PhysicsState {
     pub fn new() -> Self {
         Self {
+            islands: IslandManager::new(),
             broad_phase: BroadPhase::new(),
             narrow_phase: NarrowPhase::new(),
             bodies: RigidBodySet::new(),

src_testbed/physx_backend.rs

@@ -520,9 +520,9 @@ fn physx_collider_from_rapier_collider(
     };
     let mut material = physics
         .create_material(
-            collider.friction,
-            collider.friction,
-            collider.restitution,
+            collider.co_material.friction,
+            collider.co_material.friction,
+            collider.co_material.restitution,
             (),
         )
         .unwrap();

src_testbed/testbed.rs

@@ -19,7 +19,7 @@ use kiss3d::text::Font;
 use kiss3d::window::{State, Window};
 use na::{self, Point2, Point3, Vector3};
 use rapier::dynamics::{
-    ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
+    IntegrationParameters, JointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet,
 };
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
@@ -245,7 +245,7 @@ impl Testbed {
         colliders: ColliderSet,
         joints: JointSet,
         gravity: Vector<f32>,
-        hooks: impl PhysicsHooks + 'static,
+        hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         self.harness
             .set_world_with_params(bodies, colliders, joints, gravity, hooks);
@@ -586,6 +586,7 @@ impl Testbed {
                 for to_delete in &colliders[..num_to_delete] {
                     self.harness.physics.colliders.remove(
                         to_delete[0],
+                        &mut self.harness.physics.islands,
                         &mut self.harness.physics.bodies,
                         true,
                     );
@@ -605,6 +606,7 @@ impl Testbed {
                 for to_delete in &dynamic_bodies[..num_to_delete] {
                     self.harness.physics.bodies.remove(
                         *to_delete,
+                        &mut self.harness.physics.islands,
                         &mut self.harness.physics.colliders,
                         &mut self.harness.physics.joints,
                     );
@@ -617,6 +619,7 @@ impl Testbed {
                 for to_delete in &joints[..num_to_delete] {
                     self.harness.physics.joints.remove(
                         *to_delete,
+                        &mut self.harness.physics.islands,
                         &mut self.harness.physics.bodies,
                         true,
                     );
@@ -1205,13 +1208,13 @@ impl State for Testbed {
                 != self.state.flags.contains(TestbedStateFlags::SLEEP)
             {
                 if self.state.flags.contains(TestbedStateFlags::SLEEP) {
-                    for (_, mut body) in self.harness.physics.bodies.iter_mut() {
-                        body.activation.threshold = ActivationStatus::default_threshold();
+                    for (_, body) in self.harness.physics.bodies.iter_mut() {
+                        body.activation_mut().threshold = RigidBodyActivation::default_threshold();
                     }
                 } else {
-                    for (_, mut body) in self.harness.physics.bodies.iter_mut() {
+                    for (_, body) in self.harness.physics.bodies.iter_mut() {
                         body.wake_up(true);
-                        body.activation.threshold = -1.0;
+                        body.activation_mut().threshold = -1.0;
                     }
                 }
             }