Split rigid-bodies and colliders into multiple components

2 files changed, 375 insertions, 181 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index ff463c7..6afd860 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/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 ch1 in &rb1.colliders {
-                    let co1 = &colliders[*ch1];
-
-                    if co1.is_sensor() {
+        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_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()));
-
-                self.query_pipeline
-                    .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
-                        if ch1 == *ch2 {
-                            // Ignore self-intersection.
-                            return true;
-                        }
-
-                        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();
+        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_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): (&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);
 
-                            if bh1 == bh2 {
+                    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();
-
-                            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);
-
-                            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 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 bh1 = co_parent1.map(|p| p.handle);
+                                let bh2 = co_parent2.map(|p| p.handle);
+
+                                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 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_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 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 {
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index f1066e0..4637940 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/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 (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.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 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)
         }
     }
 }