Finalize refactoring

3 files changed, 132 insertions, 185 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 7afc671..2c8c5e3 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -451,7 +451,7 @@ impl BroadPhase {
         for handle in modified_colliders {
             // NOTE: we use `get` because the collider may no longer
             //       exist if it has been removed.
-            if let Some(co) = colliders.get(*handle) {
+            if let Some(co) = colliders.get_mut_internal(*handle) {
                 if !co.changes.needs_broad_phase_update() {
                     continue;
                 }
@@ -471,12 +471,9 @@ impl BroadPhase {
 
                     // 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,
-                        },
-                    );
+                    co.bf_data = ColliderBroadPhaseData {
+                        proxy_index: new_proxy_id,
+                    };
                 }
             }
         }
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index de182e7..b9f4ae3 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -73,7 +73,7 @@ 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.co_parent = None;
+        coll.parent = None;
         let handle = ColliderHandle(self.colliders.insert(coll));
         self.modified_colliders.push(handle);
         handle
@@ -91,12 +91,12 @@ impl ColliderSet {
         // if this collider was obtained by cloning another one.
         coll.reset_internal_references();
 
-        if let Some(prev_parent) = &mut coll.co_parent {
+        if let Some(prev_parent) = &mut coll.parent {
             prev_parent.handle = parent_handle;
         } else {
-            coll.co_parent = Some(ColliderParent {
+            coll.parent = Some(ColliderParent {
                 handle: parent_handle,
-                pos_wrt_parent: coll.co_pos.0,
+                pos_wrt_parent: coll.pos.0,
             });
         }
 
@@ -111,10 +111,10 @@ impl ColliderSet {
         let coll = self.colliders.get_mut(handle.0).unwrap();
         parent.add_collider(
             handle,
-            coll.co_parent.as_mut().unwrap(),
-            &mut coll.co_pos,
-            &coll.co_shape,
-            &coll.co_mprops,
+            coll.parent.as_mut().unwrap(),
+            &mut coll.pos,
+            &coll.shape,
+            &coll.mprops,
         );
         handle
     }
@@ -128,12 +128,12 @@ impl ColliderSet {
         bodies: &mut RigidBodySet,
     ) {
         if let Some(collider) = self.get_mut(handle) {
-            let curr_parent = collider.co_parent.map(|p| p.handle);
+            let curr_parent = collider.parent.map(|p| p.handle);
             if new_parent_handle == curr_parent {
                 return; // Nothing to do, this is the same parent.
             }
 
-            collider.co_changes |= ColliderChanges::PARENT;
+            collider.changes |= ColliderChanges::PARENT;
 
             if let Some(parent_handle) = curr_parent {
                 if let Some(rb) = bodies.get_mut(parent_handle) {
@@ -143,10 +143,10 @@ impl ColliderSet {
 
             match new_parent_handle {
                 Some(new_parent_handle) => {
-                    if let Some(co_parent) = &mut collider.co_parent {
-                        co_parent.handle = new_parent_handle;
+                    if let Some(parent) = &mut collider.parent {
+                        parent.handle = new_parent_handle;
                     } else {
-                        collider.co_parent = Some(ColliderParent {
+                        collider.parent = Some(ColliderParent {
                             handle: new_parent_handle,
                             pos_wrt_parent: Isometry::identity(),
                         })
@@ -155,14 +155,14 @@ impl ColliderSet {
                     if let Some(rb) = bodies.get_mut(new_parent_handle) {
                         rb.add_collider(
                             handle,
-                            collider.co_parent.as_ref().unwrap(),
-                            &mut collider.co_pos,
-                            &collider.co_shape,
-                            &collider.co_mprops,
+                            collider.parent.as_ref().unwrap(),
+                            &mut collider.pos,
+                            &collider.shape,
+                            &collider.mprops,
                         );
                     }
                 }
-                None => collider.co_parent = None,
+                None => collider.parent = None,
             }
         }
     }
@@ -185,14 +185,14 @@ impl ColliderSet {
          */
         // 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(co_parent) = &collider.co_parent {
-            if let Some(parent) =
-                bodies.get_mut_internal_with_modification_tracking(co_parent.handle)
+        if let Some(parent) = &collider.parent {
+            if let Some(parent_rb) =
+                bodies.get_mut_internal_with_modification_tracking(parent.handle)
             {
-                parent.remove_collider_internal(handle, &collider);
+                parent_rb.remove_collider_internal(handle, &collider);
 
                 if wake_up {
-                    islands.wake_up(bodies, co_parent.handle, true);
+                    islands.wake_up(bodies, parent.handle, true);
                 }
             }
         }
@@ -247,8 +247,8 @@ impl ColliderSet {
         collider: &mut Collider,
         modified_colliders: &mut Vec<ColliderHandle>,
     ) {
-        if !collider.co_changes.contains(ColliderChanges::MODIFIED) {
-            collider.co_changes = ColliderChanges::MODIFIED;
+        if !collider.changes.contains(ColliderChanges::MODIFIED) {
+            collider.changes = ColliderChanges::MODIFIED;
             modified_colliders.push(handle);
         }
     }
@@ -261,6 +261,10 @@ impl ColliderSet {
         Some(result)
     }
 
+    pub(crate) fn index_mut_internal(&mut self, handle: ColliderHandle) -> &mut Collider {
+        &mut self.colliders[handle.0]
+    }
+
     pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         self.colliders.get_mut(handle.0)
     }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 26bddf1..f4e8c58 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -3,13 +3,11 @@ use rayon::prelude::*;
 
 use crate::data::Coarena;
 use crate::dynamics::{
-    CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds,
-    RigidBodySet, RigidBodyType,
+    CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
 };
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial,
-    ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType,
-    CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
+    BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
+    ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
     InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
 };
 use crate::math::{Real, Vector};
@@ -17,7 +15,6 @@ use crate::pipeline::{
     ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
     PhysicsHooks,
 };
-use crate::prelude::ColliderFlags;
 use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 use parry::utils::IsometryOpt;
 use std::collections::HashMap;
@@ -311,12 +308,12 @@ impl NarrowPhase {
         // Wake up every body in contact with the deleted collider and generate Stopped collision events.
         if let Some(islands) = islands.as_deref_mut() {
             for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
-                if let Some(parent) = colliders.get(a.0).map(|c| c.handle) {
-                    islands.wake_up(bodies, parent, true)
+                if let Some(parent) = colliders.get(a).and_then(|c| c.parent.as_ref()) {
+                    islands.wake_up(bodies, parent.handle, true)
                 }
 
-                if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
-                    islands.wake_up(bodies, parent, true)
+                if let Some(parent) = colliders.get(b).and_then(|c| c.parent.as_ref()) {
+                    islands.wake_up(bodies, parent.handle, true)
                 }
 
                 if pair.start_event_emited {
@@ -379,10 +376,8 @@ impl NarrowPhase {
         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.needs_narrow_phase_update() {
+            if let Some(co) = colliders.get(*handle) {
+                if co.changes.needs_narrow_phase_update() {
                     // No flag relevant to the narrow-phase is enabled for this collider.
                     continue;
                 }
@@ -392,13 +387,8 @@ impl NarrowPhase {
                     // 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 fixed 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(islands) = islands.as_deref_mut() {
-                        if let Some(co_parent) = co_parent {
+                        if let Some(co_parent) = &co.parent {
                             islands.wake_up(bodies, co_parent.handle, true);
                         }
 
@@ -407,8 +397,9 @@ impl NarrowPhase {
                             .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);
+                            let other_parent = colliders
+                                .get(other_handle)
+                                .and_then(|co| co.parent.as_ref());
 
                             if let Some(other_parent) = other_parent {
                                 islands.wake_up(bodies, other_parent.handle, true);
@@ -420,8 +411,8 @@ impl NarrowPhase {
                     // 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() {
+                    if co.changes.contains(ColliderChanges::TYPE) {
+                        if co.is_sensor() {
                             // Find the contact pairs for this collider and
                             // push them to `pairs_to_remove`.
                             for inter in self
@@ -441,9 +432,7 @@ impl NarrowPhase {
                                 .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()
+                                    !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
                                 })
                             {
                                 pairs_to_remove.push((
@@ -484,10 +473,9 @@ impl NarrowPhase {
         events: &dyn EventHandler,
         mode: PairRemovalMode,
     ) {
-        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) {
+        if let (Some(co1), Some(co2)) =
+            (colliders.get(pair.collider1), colliders.get(pair.collider2))
+        {
             // 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)) = (
@@ -495,8 +483,7 @@ impl NarrowPhase {
                 self.graph_indices.get(pair.collider2.0),
             ) {
                 if mode == PairRemovalMode::FromIntersectionGraph
-                    || (mode == PairRemovalMode::Auto
-                        && (co_type1.is_sensor() || co_type2.is_sensor()))
+                    || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
                 {
                     let intersection = self
                         .intersection_graph
@@ -505,10 +492,7 @@ impl NarrowPhase {
                     // Emit an intersection lost event if we had an intersection before removing the edge.
                     if let Some(mut intersection) = intersection {
                         if intersection.intersecting {
-                            let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
-                            let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
-
-                            if (co_flag1.active_events | co_flag2.active_events)
+                            if (co1.flags.active_events | co2.flags.active_events)
                                 .contains(ActiveEvents::COLLISION_EVENTS)
                             {
                                 intersection.emit_stop_event(pair.collider1, pair.collider2, events)
@@ -524,25 +508,17 @@ impl NarrowPhase {
                     // Also wake up the dynamic bodies that were in contact.
                     if let Some(mut ctct) = contact_pair {
                         if ctct.has_any_active_contact {
-                            let co_parent1: Option<&ColliderParent> =
-                                colliders.get(pair.collider1.0);
-                            let co_parent2: Option<&ColliderParent> =
-                                colliders.get(pair.collider2.0);
-
                             if let Some(islands) = islands {
-                                if let Some(co_parent1) = co_parent1 {
+                                if let Some(co_parent1) = &co1.parent {
                                     islands.wake_up(bodies, co_parent1.handle, true);
                                 }
 
-                                if let Some(co_parent2) = co_parent2 {
+                                if let Some(co_parent2) = co2.parent {
                                     islands.wake_up(bodies, co_parent2.handle, true);
                                 }
                             }
 
-                            let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
-                            let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
-
-                            if (co_flag1.active_events | co_flag2.active_events)
+                            if (co1.flags.active_events | co2.flags.active_events)
                                 .contains(ActiveEvents::COLLISION_EVENTS)
                             {
                                 ctct.emit_stop_event(events);
@@ -555,15 +531,11 @@ impl NarrowPhase {
     }
 
     fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) {
-        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) {
-                if co_parent1.is_some() {
+        if let (Some(co1), Some(co2)) =
+            (colliders.get(pair.collider1), colliders.get(pair.collider2))
+        {
+            if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) {
+                if co1.parent.is_some() {
                     // Same parents. Ignore collisions.
                     return;
                 }
@@ -577,7 +549,7 @@ impl NarrowPhase {
                 ColliderGraphIndices::invalid(),
             );
 
-            if co_type1.is_sensor() || co_type2.is_sensor() {
+            if co1.is_sensor() || co2.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)
@@ -682,27 +654,13 @@ impl NarrowPhase {
             let handle1 = nodes[edge.source().index()].weight;
             let handle2 = nodes[edge.target().index()].weight;
             let had_intersection = edge.weight.intersecting;
+            let co1 = &colliders[handle1];
+            let co2 = &colliders[handle2];
 
             // TODO: remove the `loop` once labels on blocks is stabilized.
             'emit_events: loop {
-                let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
-                let (co_changes1, co_shape1, co_pos1, co_flags1): (
-                    &ColliderChanges,
-                    &ColliderShape,
-                    &ColliderPosition,
-                    &ColliderFlags,
-                ) = colliders.index_bundle(handle1.0);
-
-                let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
-                let (co_changes2, co_shape2, co_pos2, co_flags2): (
-                    &ColliderChanges,
-                    &ColliderShape,
-                    &ColliderPosition,
-                    &ColliderFlags,
-                ) = colliders.index_bundle(handle2.0);
-
-                if !co_changes1.needs_narrow_phase_update()
-                    && !co_changes2.needs_narrow_phase_update()
+                if !co1.changes.needs_narrow_phase_update()
+                    && !co2.changes.needs_narrow_phase_update()
                 {
                     // No update needed for these colliders.
                     return;
@@ -712,36 +670,36 @@ impl NarrowPhase {
                 let mut rb_type1 = RigidBodyType::Fixed;
                 let mut rb_type2 = RigidBodyType::Fixed;
 
-                if let Some(co_parent1) = co_parent1 {
-                    rb_type1 = *bodies.index(co_parent1.handle.0);
+                if let Some(co_parent1) = &co1.parent {
+                    rb_type1 = bodies[co_parent1.handle].body_type;
                 }
 
-                if let Some(co_parent2) = co_parent2 {
-                    rb_type2 = *bodies.index(co_parent2.handle.0);
+                if let Some(co_parent2) = &co2.parent {
+                    rb_type2 = bodies[co_parent2.handle].body_type;
                 }
 
                 // Filter based on the rigid-body types.
-                if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
-                    && !co_flags2.active_collision_types.test(rb_type1, rb_type2)
+                if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
+                    && !co2.flags.active_collision_types.test(rb_type1, rb_type2)
                 {
                     edge.weight.intersecting = false;
                     break 'emit_events;
                 }
 
                 // Filter based on collision groups.
-                if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
+                if !co1.flags.collision_groups.test(co2.flags.collision_groups) {
                     edge.weight.intersecting = false;
                     break 'emit_events;
                 }
 
-                let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
+                let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks;
 
                 if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) {
                     let context = PairFilterContext {
                         bodies,
                         colliders,
-                        rigid_body1: co_parent1.map(|p| p.handle),
-                        rigid_body2: co_parent2.map(|p| p.handle),
+                        rigid_body1: co1.parent.map(|p| p.handle),
+                        rigid_body2: co2.parent.map(|p| p.handle),
                         collider1: handle1,
                         collider2: handle2,
                     };
@@ -753,16 +711,14 @@ impl NarrowPhase {
                     }
                 }
 
-                let pos12 = co_pos1.inv_mul(co_pos2);
+                let pos12 = co1.pos.inv_mul(&co2.pos);
                 edge.weight.intersecting = query_dispatcher
-                    .intersection_test(&pos12, &**co_shape1, &**co_shape2)
+                    .intersection_test(&pos12, &*co1.shape, &*co2.shape)
                     .unwrap_or(false);
                 break 'emit_events;
             }
 
-            let co_flags1: &ColliderFlags = colliders.index(handle1.0);
-            let co_flags2: &ColliderFlags = colliders.index(handle2.0);
-            let active_events = co_flags1.active_events | co_flags2.active_events;
+            let active_events = co1.flags.active_events | co2.flags.active_events;
 
             if active_events.contains(ActiveEvents::COLLISION_EVENTS)
                 && had_intersection != edge.weight.intersecting
@@ -795,29 +751,13 @@ impl NarrowPhase {
         par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
             let pair = &mut edge.weight;
             let had_any_active_contact = pair.has_any_active_contact;
+            let co1 = &colliders[pair.collider1];
+            let co2 = &colliders[pair.collider2];
 
             // TODO: remove the `loop` once labels on blocks are supported.
             'emit_events: loop {
-                let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
-                let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): (
-                    &ColliderChanges,
-                    &ColliderShape,
-                    &ColliderPosition,
-                    &ColliderMaterial,
-                    &ColliderFlags,
-                ) = colliders.index_bundle(pair.collider1.0);
-
-                let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
-                let (co_changes2, co_shape2, co_pos2, co_material2, co_flags2): (
-                    &ColliderChanges,
-                    &ColliderShape,
-                    &ColliderPosition,
-                    &ColliderMaterial,
-                    &ColliderFlags,
-                ) = colliders.index_bundle(pair.collider2.0);
-
-                if !co_changes1.needs_narrow_phase_update()
-                    && !co_changes2.needs_narrow_phase_update()
+                if !co1.changes.needs_narrow_phase_update()
+                    && !co2.changes.needs_narrow_phase_update()
                 {
                     // No update needed for these colliders.
                     return;
@@ -827,36 +767,36 @@ impl NarrowPhase {
                 let mut rb_type1 = RigidBodyType::Fixed;
                 let mut rb_type2 = RigidBodyType::Fixed;
 
-                if let Some(co_parent1) = co_parent1 {
-                    rb_type1 = *bodies.index(co_parent1.handle.0);
+                if let Some(co_parent1) = &co1.parent {
+                    rb_type1 = bodies[co_parent1.handle].body_type;
                 }
 
-                if let Some(co_parent2) = co_parent2 {
-                    rb_type2 = *bodies.index(co_parent2.handle.0);
+                if let Some(co_parent2) = &co2.parent {
+                    rb_type2 = bodies[co_parent2.handle].body_type;
                 }
 
                 // Filter based on the rigid-body types.
-                if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
-                    && !co_flags2.active_collision_types.test(rb_type1, rb_type2)
+                if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
+                    && !co2.flags.active_collision_types.test(rb_type1, rb_type2)
                 {
                     pair.clear();
                     break 'emit_events;
                 }
 
                 // Filter based on collision groups.
-                if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
+                if !co1.flags.collision_groups.test(co2.flags.collision_groups) {
                     pair.clear();
                     break 'emit_events;
                 }
 
-                let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
+                let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks;
 
                 let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
                     let context = PairFilterContext {
                         bodies,
                         colliders,
-                        rigid_body1: co_parent1.map(|p| p.handle),
-                        rigid_body2: co_parent2.map(|p| p.handle),
+                        rigid_body1: co1.parent.map(|p| p.handle),
+                        rigid_body2: co2.parent.map(|p| p.handle),
                         collider1: pair.collider1,
                         collider2: pair.collider2,
                     };
@@ -872,53 +812,55 @@ impl NarrowPhase {
                     SolverFlags::default()
                 };
 
-                if !co_flags1.solver_groups.test(co_flags2.solver_groups) {
+                if !co1.flags.solver_groups.test(co2.flags.solver_groups) {
                     solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
                 }
 
-                if co_changes1.contains(ColliderChanges::SHAPE)
-                    || co_changes2.contains(ColliderChanges::SHAPE)
+                if co1.changes.contains(ColliderChanges::SHAPE)
+                    || co2.changes.contains(ColliderChanges::SHAPE)
                 {
                     // The shape changed so the workspace is no longer valid.
                     pair.workspace = None;
                 }
 
-                let pos12 = co_pos1.inv_mul(co_pos2);
+                let pos12 = co1.pos.inv_mul(&co2.pos);
                 let _ = query_dispatcher.contact_manifolds(
                     &pos12,
-                    &**co_shape1,
-                    &**co_shape2,
+                    &*co1.shape,
+                    &*co2.shape,
                     prediction_distance,
                     &mut pair.manifolds,
                     &mut pair.workspace,
                 );
 
                 let friction = CoefficientCombineRule::combine(
-                    co_material1.friction,
-                    co_material2.friction,
-                    co_material1.friction_combine_rule as u8,
-                    co_material2.friction_combine_rule as u8,
+                    co1.material.friction,
+                    co2.material.friction,
+                    co1.material.friction_combine_rule as u8,
+                    co2.material.friction_combine_rule as u8,
                 );
                 let restitution = CoefficientCombineRule::combine(
-                    co_material1.restitution,
-                    co_material2.restitution,
-                    co_material1.restitution_combine_rule as u8,
-                    co_material2.restitution_combine_rule as u8,
+                    co1.material.restitution,
+                    co2.material.restitution,
+                    co1.material.restitution_combine_rule as u8,
+                    co2.material.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))
+                let dominance1 = co1
+                    .parent
+                    .map(|p1| bodies[p1.handle].dominance)
                     .unwrap_or(zero);
-                let dominance2 = co_parent2
-                    .map(|p2| *bodies.index(p2.handle.0))
+                let dominance2 = co2
+                    .parent
+                    .map(|p2| bodies[p2.handle].dominance)
                     .unwrap_or(zero);
 
                 for manifold in &mut pair.manifolds {
-                    let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
+                    let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos);
                     manifold.data.solver_contacts.clear();
-                    manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
-                    manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
+                    manifold.data.rigid_body1 = co1.parent.map(|p| p.handle);
+                    manifold.data.rigid_body2 = co2.parent.map(|p| p.handle);
                     manifold.data.solver_flags = solver_flags;
                     manifold.data.relative_dominance = dominance1.effective_group(&rb_type1)
                         - dominance2.effective_group(&rb_type2);
@@ -960,8 +902,8 @@ impl NarrowPhase {
                         let mut context = ContactModificationContext {
                             bodies,
                             colliders,
-                            rigid_body1: co_parent1.map(|p| p.handle),
-                            rigid_body2: co_parent2.map(|p| p.handle),
+                            rigid_body1: co1.parent.map(|p| p.handle),
+                            rigid_body2: co2.parent.map(|p| p.handle),
                             collider1: pair.collider1,
                             collider2: pair.collider2,
                             manifold,
@@ -981,9 +923,7 @@ impl NarrowPhase {
                 break 'emit_events;
             }
 
-            let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0);
-            let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0);
-            let active_events = co_flags1.active_events | co_flags2.active_events;
+            let active_events = co1.flags.active_events | co2.flags.active_events;
 
             if pair.has_any_active_contact != had_any_active_contact {
                 if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
@@ -1021,18 +961,24 @@ impl NarrowPhase {
                 {
                     let (active_island_id1, rb_type1, 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)
+                            let rb1 = &bodies[handle1];
+                            (
+                                rb1.ids.active_island_id,
+                                rb1.body_type,
+                                rb1.activation.sleeping,
+                            )
                         } else {
                             (0, RigidBodyType::Fixed, true)
                         };
 
                     let (active_island_id2, rb_type2, 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)
+                            let rb2 = &bodies[handle2];
+                            (
+                                rb2.ids.active_island_id,
+                                rb2.body_type,
+                                rb2.activation.sleeping,
+                            )
                         } else {
                             (0, RigidBodyType::Fixed, true)
                         };