Merge pull request #183 from dimforge/bundles

Make Rapier accept any kind of data storage instead of RigidBodySet/ColliderSet

47 files changed, 4025 insertions, 2662 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index ff463c7..b348283 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,33 @@ 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 +86,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 +152,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_forces_and_velocities(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 +198,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 +219,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 +253,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 +281,7 @@ impl CCDSolver {
 
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
+            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithNextPosition,
@@ -207,71 +293,95 @@ impl CCDSolver {
          *
          */
         // TODO: don't iterate through all the colliders.
-        for (ch1, co1) in colliders.iter() {
-            let rb1 = &bodies[co1.parent()];
-            if rb1.is_ccd_active() {
-                let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent()));
+        for handle in islands.active_dynamic_bodies() {
+            let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
+
+            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)
+                        .expect("Could not find the ColliderParent component.");
+                    let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
+                        colliders.index_bundle(ch1.0);
+
+                    let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
+                    let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
 
-                self.query_pipeline
-                    .colliders_with_aabb_intersecting_aabb(&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();
-
-                            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 b1 = bh1.map(|h| bodies.index_bundle(h.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),
+                                    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);
+                                    }
                                 }
                             }
-                        }
 
-                        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,53 @@ 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;
 
+            // 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 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(&