Finalize refactoring

19 files changed, 475 insertions, 677 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 8fc5a7f..77a1ff7 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -1,16 +1,10 @@
 use super::TOIEntry;
-use crate::dynamics::{
-    IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity,
-};
-use crate::geometry::{
-    ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent,
-    NarrowPhase,
-};
+use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
+use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
-use crate::prelude::{ActiveEvents, ColliderFlags};
+use crate::prelude::ActiveEvents;
 use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
 use parry::utils::hashmap::HashMap;
 use std::collections::BinaryHeap;
@@ -61,23 +55,18 @@ impl CCDSolver {
         match impacts {
             PredictedImpacts::Impacts(tois) => {
                 for (handle, toi) in tois {
-                    let (rb_poss, vels, ccd, mprops): (
-                        &RigidBodyPosition,
-                        &RigidBodyVelocity,
-                        &RigidBodyCcd,
-                        &RigidBodyMassProps,
-                    ) = bodies.index_bundle(handle.0);
-                    let local_com = &mprops.local_mprops.local_com;
-
-                    let min_toi = (ccd.ccd_thickness
+                    let rb = bodies.index_mut_internal(*handle);
+                    let local_com = &rb.mprops.local_mprops.local_com;
+
+                    let min_toi = (rb.ccd.ccd_thickness
                         * 0.15
-                        * crate::utils::inv(ccd.max_point_velocity(vels)))
+                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.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;
-                    });
+                    let new_pos = rb
+                        .vels
+                        .integrate(toi.max(min_toi), &rb.pos.position, &local_com);
+                    rb.pos.next_position = new_pos;
                 }
             }
             _ => {}
@@ -98,17 +87,16 @@ impl CCDSolver {
 
         // println!("Checking CCD activation");
         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;
-                });
-
+            let rb = bodies.index_mut_internal(*handle);
+
+            if rb.ccd.ccd_enabled {
+                let forces = if include_forces {
+                    Some(&rb.forces)
+                } else {
+                    None
+                };
+                let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces);
+                rb.ccd.ccd_active = moving_fast;
                 ccd_active = ccd_active || moving_fast;
             }
         }
@@ -137,36 +125,31 @@ impl CCDSolver {
         let mut min_toi = dt;
 
         for handle in islands.active_dynamic_bodies() {
-            let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
-
-            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);
-
-                let predicted_body_pos1 =
-                    rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1);
-
-                for ch1 in &rb_colliders1.0 {
-                    let co_parent1: &ColliderParent = colliders
-                        .get(ch1.0)
+            let rb1 = &bodies[*handle];
+
+            if rb1.ccd.ccd_active {
+                let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
+                    dt,
+                    &rb1.forces,
+                    &rb1.vels,
+                    &rb1.mprops,
+                );
+
+                for ch1 in &rb1.colliders.0 {
+                    let co1 = &colliders[*ch1];
+                    let co1_parent = co1
+                        .parent
+                        .as_ref()
                         .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() {
+                    if co1.is_sensor() {
                         continue; // Ignore sensors.
                     }
 
-                    let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
-                    let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
+                    let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.pos_wrt_parent;
+                    let aabb1 = co1
+                        .shape
+                        .compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
 
                     self.query_pipeline
                         .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -182,21 +165,17 @@ impl CCDSolver {
                                 )
                                 .is_none()
                             {
-                                let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
-                                let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                                let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
-                                let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
-                                let co_type1: &ColliderType = colliders.index(ch1.0);
-                                let co_type2: &ColliderType = colliders.index(ch1.0);
+                                let co1 = &colliders[*ch1];
+                                let co2 = &colliders[*ch2];
 
-                                let bh1 = co_parent1.map(|p| p.handle);
-                                let bh2 = co_parent2.map(|p| p.handle);
+                                let bh1 = co1.parent.map(|p| p.handle);
+                                let bh2 = co2.parent.map(|p| p.handle);
 
                                 // Ignore self-intersection and sensors and apply collision groups filter.
-                                if bh1 == bh2                                                 // Ignore self-intersection.
-                                    || (co_type1.is_sensor() || co_type2.is_sensor())         // Ignore sensors.
-                                    || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups.
-                                    || !c1.3.solver_groups.test(c2.3.solver_groups)
+                                if bh1 == bh2                                                       // Ignore self-intersection.
+                                    || (co1.is_sensor() || co2.is_sensor())                         // Ignore sensors.
+                                    || !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
+                                    || !co1.flags.solver_groups.test(co2.flags.solver_groups)
                                 // Apply solver groups.
                                 {
                                     return true;
@@ -208,16 +187,16 @@ impl CCDSolver {
                                     .map(|c| c.1.dist)
                                     .unwrap_or(0.0);
 
-                                let b2 = bh2.map(|h| bodies.index_bundle(h.0));
+                                let rb2 = bh2.and_then(|h| bodies.get(h));
 
                                 if let Some(toi) = TOIEntry::try_from_colliders(
                                     self.query_pipeline.query_dispatcher(),
                                     *ch1,
                                     *ch2,
-                                    (c1.0, c1.1, c1.2, c1.3, co_parent1),
-                                    (c2.0, c2.1, c2.2, c2.3, co_parent2),
-                                    Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
-                                    b2,
+                                    co1,
+                                    co2,
+                                    Some(rb1),
+                                    rb2,
                                     None,
                                     None,
                                     0.0,
@@ -271,29 +250,27 @@ impl CCDSolver {
          */
         // TODO: don't iterate through all the colliders.
         for handle in islands.active_dynamic_bodies() {
-            let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
-
-            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);
-
-                let predicted_body_pos1 =
-                    rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1);
-
-                for ch1 in &rb_colliders1.0 {
-                    let co_parent1: &ColliderParent = colliders
-                        .get(ch1.0)
+            let rb1 = &bodies[*handle];
+
+            if rb1.ccd.ccd_active {
+                let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
+                    dt,
+                    &rb1.forces,
+                    &rb1.vels,
+                    &rb1.mprops,
+                );
+
+                for ch1 in &rb1.colliders.0 {
+                    let co1 = &colliders[*ch1];
+                    let co_parent1 = co1
+                        .parent
+                        .as_ref()
                         .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);
+                    let aabb1 = co1
+                        .shape
+                        .compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
 
                     self.query_pipeline
                         .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -309,16 +286,15 @@ impl CCDSolver {
                                 )
                                 .is_none()
                             {
-                                let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
-                                let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                                let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
-                                let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
+                                let co1 = &colliders[*ch1];
+                                let co2 = &colliders[*ch2];
 
-                                let bh1 = co_parent1.map(|p| p.handle);
-                                let bh2 = co_parent2.map(|p| p.handle);
+                                let bh1 = co1.parent.map(|p| p.handle);
+                                let bh2 = co2.parent.map(|p| p.handle);
 
                                 // Ignore self-intersections and apply groups filter.
-                                if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups)
+                                if bh1 == bh2
+                                    || !co1.flags.collision_groups.test(co2.flags.collision_groups)
                                 {
                                     return true;
                                 }
@@ -329,17 +305,17 @@ impl CCDSolver {
                                     .map(|c| c.1.dist)
                                     .unwrap_or(0.0);
 
-                                let b1 = bh1.map(|h| bodies.index_bundle(h.0));
-                                let b2 = bh2.map(|h| bodies.index_bundle(h.0));
+                                let rb1 = bh1.map(|h| &bodies[h]);
+                                let rb2 = bh2.map(|h| &bodies[h]);
 
                                 if let Some(toi) = TOIEntry::try_from_colliders(
                                     self.query_pipeline.query_dispatcher(),
                                     *ch1,
                                     *ch2,
-                                    (c1.0, c1.1, c1.2, c1.3, co_parent1),
-                                    (c2.0, c2.1, c2.2, c2.3, co_parent2),
-                                    b1,
-                                    b2,
+                                    co1,
+                                    co2,
+                                    rb1,
+                                    rb2,
                                     None,
                                     None,
                                     0.0,
@@ -381,17 +357,15 @@ impl CCDSolver {
         while let Some(toi) = all_toi.pop() {
             assert!(toi.toi <= dt);
 
-            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 rb1 = toi.b1.and_then(|b| bodies.get(b));
+            let rb2 = toi.b2.and_then(|b| bodies.get(b));
 
             let mut colliders_to_check = Vec::new();
             let should_freeze1 = rb1.is_some()
-                && rb1.unwrap().0.ccd_active
+                && rb1.unwrap().ccd.ccd_active
                 && !frozen.contains_key(&toi.b1.unwrap());
             let should_freeze2 = rb2.is_some()
-                && rb2.unwrap().0.ccd_active
+                && rb2.unwrap().ccd.ccd_active
                 && !frozen.contains_key(&toi.b2.unwrap());
 
             if !should_freeze1 && !should_freeze2 {
@@ -413,12 +387,12 @@ impl CCDSolver {
 
             if should_freeze1 {
                 let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
-                colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0);
+                colliders_to_check.extend_from_slice(&rb1.unwrap().colliders.0);
             }
 
             if should_freeze2 {
                 let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
-                colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0);
+                colliders_to_check.extend_from_slice(&rb2.unwrap().colliders.0);
             }
 
             let start_time = toi.toi;
@@ -426,39 +400,36 @@ impl CCDSolver {
             // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the
             //       ones we used above.
             for ch1 in &colliders_to_check {
-                let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap();
-                let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
-                    colliders.index_bundle(ch1.0);
+                let co1 = &colliders[*ch1];
+                let co1_parent = co1.parent.as_ref().unwrap();
+                let rb1 = &bodies[co1_parent.handle];
 
-                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);
+                let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent;
+                let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1);
 
                 self.query_pipeline
                     .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
-                        let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
-                        let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                        let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
-                        let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
+                        let co2 = &colliders[*ch2];
 
-                        let bh1 = co_parent1.map(|p| p.handle);
-                        let bh2 = co_parent2.map(|p| p.handle);
+                        let bh1 = co1.parent.map(|p| p.handle);
+                        let bh2 = co2.parent.map(|p| p.handle);
 
                         // Ignore self-intersection and apply groups filter.
-                        if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) {
+                        if bh1 == bh2
+                            || !co1.flags.collision_groups.test(co2.flags.collision_groups)
+                        {
                             return true;
                         }
 
                         let frozen1 = bh1.and_then(|h| frozen.get(&h));
                         let frozen2 = bh2.and_then(|h| frozen.get(&h));
 
-                        let b1: Option<(_, _, _, &RigidBodyCcd)> =
-                            bh1.map(|h| bodies.index_bundle(h.0));
-                        let b2: Option<(_, _, _, &RigidBodyCcd)> =
-                            bh2.map(|h| bodies.index_bundle(h.0));
+                        let rb1 = bh1.and_then(|h| bodies.get(h));
+                        let rb2 = bh2.and_then(|h| bodies.get(h));
 
-                        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))
+                        if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false))
+                            && (frozen2.is_some()
+                                || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
                         {
                             // We already did a resweep.
                             return true;
@@ -474,10 +445,10 @@ impl CCDSolver {
                             self.query_pipeline.query_dispatcher(),
                             *ch1,
                             *ch2,
-                            (c1.0, c1.1, c1.2, c1.3, co_parent1),
-                            (c2.0, c2.1, c2.2, c2.3, co_parent2),
-                            b1,
-                            b2,
+                            co1,
+                            co2,
+                            rb1,
+                            rb2,
                             frozen1.copied(),
                             frozen2.copied(),
                             start_time,
@@ -500,20 +471,10 @@ 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 (co_type1, co_pos1, co_shape1, co_flags1): (
-                &ColliderType,
-                &ColliderPosition,
-                &ColliderShape,
-                &ColliderFlags,
-            ) = colliders.index_bundle(toi.c1.0);
-            let (co_type2, co_pos2, co_shape2, co_flags2): (
-                &ColliderType,
-                &ColliderPosition,
-                &ColliderShape,
-                &ColliderFlags,
-            ) = colliders.index_bundle(toi.c2.0);
-
-            if !co_type1.is_sensor() && !co_type2.is_sensor() {
+            let co1 = &colliders[toi.c1];
+            let co2 = &colliders[toi.c2];
+
+            if !co1.is_sensor() && !co2.is_sensor() {
                 // TODO: this happens if we found a TOI between two non-sensor
                 //       colliders with mismatching solver_flags. It is not clear
                 //       what we should do in this case: we could report a
@@ -525,56 +486,46 @@ impl CCDSolver {
             }
 
             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.local_mprops.local_com;
+                let co_parent1: &ColliderParent = co1.parent.as_ref().unwrap();
+                let rb1 = &bodies[b1];
+                let local_com1 = &rb1.mprops.local_mprops.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);
+                    .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
+                    .unwrap_or(rb1.pos.next_position);
                 pos1 * co_parent1.pos_wrt_parent
             } else {
-                co_pos1.0
+                co1.pos.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.local_mprops.local_com;
+                let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap();
+                let rb2 = &bodies[b2];
+                let local_com2 = &rb2.mprops.local_mprops.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);
+                    .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
+                    .unwrap_or(rb2.pos.next_position);
                 pos2 * co_parent2.pos_wrt_parent
             } else {
-                co_pos2.0
+                co2.pos.0
             };
 
-            let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2);
+            let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos);
             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, co_shape1.as_ref(), co_shape2.as_ref())
+                .intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
                 .unwrap_or(false);
 
             let intersect_after = query_dispatcher
-                .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
+                .intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
                 .unwrap_or(false);
 
             if !intersect_before
                 && !intersect_after
-                && (co_flags1.active_events | co_flags2.active_events)
+                && (co1.flags.active_events | co2.flags.active_events)
                     .contains(ActiveEvents::COLLISION_EVENTS)
             {
                 // Emit one intersection-started and one intersection-stopped event.
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index f937979..4591825 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -1,9 +1,5 @@
-use crate::dynamics::{
-    RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::geometry::{
-    ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
-};
+use crate::dynamics::{RigidBody, RigidBodyHandle};
+use crate::geometry::{Collider, ColliderHandle};
 use crate::math::Real;
 use parry::query::{NonlinearRigidMotion, QueryDispatcher};
 
@@ -45,32 +41,10 @@ impl TOIEntry {
         query_dispatcher: &QD,
         ch1: ColliderHandle,
         ch2: ColliderHandle,
-        c1: (
-            &ColliderType,
-            &ColliderShape,
-            &ColliderPosition,
-            &ColliderFlags,
-            Option<&ColliderParent>,
-        ),
-        c2: (
-            &ColliderType,
-            &ColliderShape,
-            &ColliderPosition,
-            &ColliderFlags,
-            Option<&ColliderParent>,
-        ),
-        b1: Option<(
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyCcd,
-        )>,
-        b2: Option<(
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyCcd,
-        )>,
+        co1: &Collider,
+        co2: &Collider,
+        rb1: Option<&RigidBody>,
+        rb2: Option<&RigidBody>,
         frozen1: Option<Real>,
         frozen2: Option<Real>,
         start_time: Real,
@@ -78,39 +52,36 @@ impl TOIEntry {
         smallest_contact_dist: Real,
     ) -> Option<Self> {
         assert!(start_time <= end_time);
-        if b1.is_none() && b2.is_none() {
+        if rb1.is_none() && rb2.is_none() {
             return None;
         }
 
-        let (co_type1, co_shape1, co_pos1, co_flags1, co_parent1) = c1;
-        let (co_type2, co_shape2, co_pos2, co_flags2, co_parent2) = c2;
-
         let linvel1 =
-            frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
+            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero());
         let linvel2 =
-            frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
+            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero());
         let angvel1 =
-            frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
+            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero());
         let angvel2 =
-            frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
+            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero());
 
         #[cfg(feature = "dim2")]
         let vel12 = (linvel2 - linvel1).norm()
-            + 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);
+            + angvel1.abs() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
+            + angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
         #[cfg(feature = "dim3")]
         let vel12 = (linvel2 - linvel1).norm()
-            + 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);
+            + angvel1.norm() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
+            + angvel2.norm() * rb2.map(|b| b.ccd.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 = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
+        let thickness = (co1.shape.0.ccd_thickness() + co2.shape.0.ccd_thickness())
             + smallest_contact_dist.max(0.0);
-        let is_pseudo_intersection_test = co_type1.is_sensor()
-            || co_type2.is_sensor()
-            || !co_flags1.solver_groups.test(co_flags2.solver_groups);
+        let is_pseudo_intersection_test = co1.is_sensor()
+            || co2.is_sensor()
+            || !co1.flags.solver_groups.test(co2.flags.solver_groups);
 
         if (end_time - start_time) * vel12 < thickness {
             return None;
@@ -118,8 +89,8 @@ impl TOIEntry {
 
         // Compute the TOI.
         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);
+        let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity);
+        let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity);
 
         if let Some(t) = frozen1 {
             motion1.freeze(t);
@@ -129,8 +100,8 @@ impl TOIEntry {
             motion2.freeze(t);
         }
 
-        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));
+        let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0));
+        let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0));
 
         // println!("start_time: {}", start_time);
 
@@ -146,9 +117,9 @@ impl TOIEntry {
         let res_toi = query_dispatcher
             .nonlinear_time_of_impact(
                 &motion_c1,
-                co_shape1.as_ref(),
+                co1.shape.as_ref(),
                 &motion_c2,
-                co_shape2.as_ref(),
+                co2.shape.as_ref(),
                 start_time,
                 end_time,
                 stop_at_penetration,
@@ -160,31 +131,24 @@ impl TOIEntry {
         Some(Self::new(
             toi.toi,
             ch1,
-            co_parent1.map(|p| p.handle),
+            co1.parent.map(|p| p.handle),
             ch2,
-            co_parent2.map(|p| p.handle),
+            co2.parent.map(|p| p.handle),
             is_pseudo_intersection_test,
             0,
         ))
     }
 
-    fn body_motion(
-        (poss, vels, mprops, ccd): (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyCcd,
-        ),
-    ) -> NonlinearRigidMotion {
-        if ccd.ccd_active {
+    fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion {
+        if rb.ccd.ccd_active {
             NonlinearRigidMotion::new(
-                poss.position,
-                mprops.local_mprops.local_com,
-                vels.linvel,
-                vels.angvel,
+                rb.pos.position,
+                rb.mprops.local_mprops.local_com,
+                rb.vels.linvel,
+                rb.vels.angvel,
             )
         } else {
-            NonlinearRigidMotion::constant_position(poss.next_position)
+            NonlinearRigidMotion::constant_position(rb.pos.next_position)
         }
     }
 }
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 0cb92e9..246b50e 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -47,12 +47,15 @@ impl IslandManager {
 
             while i < active_set.len() {
                 let handle = active_set[i];
-                if bodies.get(handle.0).is_none() {
+                if bodies.get(handle).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);
+                        // Update the active_set_id for the body that has been swapped.
+                        if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) {
+                            swapped_rb.ids.active_set_id = i;