Merge pull request #157 from dimforge/ccd

Implement Continuous Collision Detection

4 files changed, 407 insertions, 93 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index a74a6e5..da572f3 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -44,16 +44,15 @@ impl CollisionPipeline {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
-        bodies.maintain(colliders);
+        colliders.handle_user_changes(bodies);
+        bodies.handle_user_changes(colliders);
         self.broadphase_collider_pairs.clear();
 
-        broad_phase.update_aabbs(prediction_distance, bodies, colliders);
-
         self.broad_phase_events.clear();
-        broad_phase.find_pairs(&mut self.broad_phase_events);
+        broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events);
 
+        narrow_phase.handle_user_changes(colliders, bodies, events);
         narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
-
         narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
         narrow_phase.compute_intersections(bodies, colliders, hooks, events);
 
@@ -61,26 +60,17 @@ impl CollisionPipeline {
             colliders,
             narrow_phase,
             self.empty_joints.joint_graph(),
-            0,
+            128,
         );
 
-        // // Update kinematic bodies velocities.
-        // bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-        //     body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
-        // });
-
         // Update colliders positions and kinematic bodies positions.
         bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
-            } else {
-                rb.update_predicted_position(0.0);
-            }
+            rb.position = rb.next_position;
+            rb.update_colliders_positions(colliders);
 
             for handle in &rb.colliders {
-                let collider = &mut colliders[*handle];
+                let collider = colliders.get_mut_internal(*handle).unwrap();
                 collider.position = rb.position * collider.delta;
-                collider.predicted_position = rb.predicted_position * collider.delta;
             }
         });
 
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index fd85cfa..5fad9bc 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -6,7 +6,7 @@ pub use physics_hooks::{
     ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
 };
 pub use physics_pipeline::PhysicsPipeline;
-pub use query_pipeline::QueryPipeline;
+pub use query_pipeline::{QueryPipeline, QueryPipelineMode};
 
 mod collision_pipeline;
 mod event_handler;
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 198c4be..0f7b5f6 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -3,7 +3,7 @@
 use crate::counters::Counters;
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
@@ -58,57 +58,37 @@ impl PhysicsPipeline {
         }
     }
 
-    /// Executes one timestep of the physics simulation.
-    pub fn step(
+    fn detect_collisions(
         &mut self,
-        gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
-        joints: &mut JointSet,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
+        handle_user_changes: bool,
     ) {
-        self.counters.step_started();
-        bodies.maintain(colliders);
-        broad_phase.maintain(colliders);
-        narrow_phase.maintain(colliders, bodies);
-
-        // Update kinematic bodies velocities.
-        // TODO: what is the best place for this? It should at least be
-        // located before the island computation because we test the velocity
-        // there to determine if this kinematic body should wake-up dynamic
-        // bodies it is touching.
-        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
-        });
+        self.counters.stages.collision_detection_time.resume();
+        self.counters.cd.broad_phase_time.resume();
 
-        self.counters.stages.collision_detection_time.start();
-        self.counters.cd.broad_phase_time.start();
+        // Update broad-phase.
+        self.broad_phase_events.clear();
         self.broadphase_collider_pairs.clear();
-        //        let t = instant::now();
-        broad_phase.update_aabbs(
+        broad_phase.update(
             integration_parameters.prediction_distance,
-            bodies,
             colliders,
+            &mut self.broad_phase_events,
         );
-        //        println!("Update AABBs time: {}", instant::now() - t);
 
-        //        let t = instant::now();
-        self.broad_phase_events.clear();
-        broad_phase.find_pairs(&mut self.broad_phase_events);
-        //        println!("Find pairs time: {}", instant::now() - t);
-
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
         self.counters.cd.broad_phase_time.pause();
+        self.counters.cd.narrow_phase_time.resume();
 
-        //        println!("Num contact pairs: {}", pairs.len());
-
-        self.counters.cd.narrow_phase_time.start();
-
-        //        let t = instant::now();
+        // Update narrow-phase.
+        if handle_user_changes {
+            narrow_phase.handle_user_changes(colliders, bodies, events);
+        }
+        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
         narrow_phase.compute_contacts(
             integration_parameters.prediction_distance,
             bodies,
@@ -117,9 +97,73 @@ impl PhysicsPipeline {
             events,
         );
         narrow_phase.compute_intersections(bodies, colliders, hooks, events);
-        //        println!("Compute contact time: {}", instant::now() - t);
 
-        self.counters.stages.island_construction_time.start();
+        // Clear colliders modification flags.
+        colliders.clear_modified_colliders();
+
+        self.counters.cd.narrow_phase_time.pause();
+        self.counters.stages.collision_detection_time.pause();
+    }
+
+    fn solve_position_constraints(
+        &mut self,
+        integration_parameters: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+    ) {
+        #[cfg(not(feature = "parallel"))]
+        {
+            enable_flush_to_zero!();
+
+            for island_id in 0..bodies.num_islands() {
+                self.solvers[island_id].solve_position_constraints(
+                    island_id,
+                    &mut self.counters,
+                    integration_parameters,
+                    bodies,
+                )
+            }
+        }
+
+        #[cfg(feature = "parallel")]
+        {
+            use rayon::prelude::*;
+            use std::sync::atomic::Ordering;
+
+            let num_islands = bodies.num_islands();
+            let solvers = &mut self.solvers[..num_islands];
+            let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
+
+            rayon::scope(|scope| {
+                enable_flush_to_zero!();
+
+                solvers
+                    .par_iter_mut()
+                    .enumerate()
+                    .for_each(|(island_id, solver)| {
+                        let bodies: &mut RigidBodySet =
+                            unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
+
+                        solver.solve_position_constraints(
+                            scope,
+                            island_id,
+                            integration_parameters,
+                            bodies,
+                        )
+                    });
+            });
+        }
+    }
+
+    fn build_islands_and_solve_velocity_constraints(
+        &mut self,
+        gravity: &Vector<Real>,
+        integration_parameters: &IntegrationParameters,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        joints: &mut JointSet,
+    ) {
+        self.counters.stages.island_construction_time.resume();
         bodies.update_active_set_with_contacts(
             colliders,
             narrow_phase,
@@ -139,25 +183,17 @@ impl PhysicsPipeline {
         }
 
         let mut manifolds = Vec::new();
-        narrow_phase.sort_and_select_active_contacts(
-            bodies,
-            &mut manifolds,
-            &mut self.manifold_indices,
-        );
+        narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices);
         joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
 
-        self.counters.cd.narrow_phase_time.pause();
-        self.counters.stages.collision_detection_time.pause();
-
-        self.counters.stages.update_time.start();
+        self.counters.stages.update_time.resume();
         bodies.foreach_active_dynamic_body_mut_internal(|_, b| {
             b.update_world_mass_properties();
             b.add_gravity(*gravity)
         });
         self.counters.stages.update_time.pause();
 
-        self.counters.solver.reset();
-        self.counters.stages.solver_time.start();
+        self.counters.stages.solver_time.resume();
         if self.solvers.len() < bodies.num_islands() {
             self.solvers
                 .resize_with(bodies.num_islands(), IslandSolver::new);
@@ -168,7 +204,7 @@ impl PhysicsPipeline {
             enable_flush_to_zero!();
 
             for island_id in 0..bodies.num_islands() {
-                self.solvers[island_id].solve_island(
+                self.solvers[island_id].init_constraints_and_solve_velocity_constraints(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
@@ -209,7 +245,7 @@ impl PhysicsPipeline {
                         let joints: &mut Vec<JointGraphEdge> =
                             unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
 
-                        solver.solve_island(
+                        solver.init_constraints_and_solve_velocity_constraints(
                             scope,
                             island_id,
                             integration_parameters,
@@ -222,31 +258,224 @@ impl PhysicsPipeline {
                     });
             });
         }
+        self.counters.stages.solver_time.pause();
+    }
+
+    fn run_ccd_motion_clamping(
+        &mut self,
+        integration_parameters: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        narrow_phase: &NarrowPhase,
+        ccd_solver: &mut CCDSolver,
+        events: &dyn EventHandler,
+    ) {
+        self.counters.ccd.toi_computation_time.start();
+        // Handle CCD
+        let impacts = ccd_solver.predict_impacts_at_next_positions(
+            integration_parameters.dt,
+            bodies,
+            colliders,
+            narrow_phase,
+            events,
+        );
+        ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
+        self.counters.ccd.toi_computation_time.pause();
+    }
 
-        // Update colliders positions and kinematic bodies positions.
-        // FIXME: do this in the solver?
+    fn advance_to_final_positions(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        clear_forces: bool,
+    ) {
+        // Set the rigid-bodies and kinematic bodies to their final position.
         bodies.foreach_active_body_mut_internal(|_, rb| {
             if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
                 rb.linvel = na::zero();
                 rb.angvel = na::zero();
-            } else {
-                rb.update_predicted_position(integration_parameters.dt);
             }
 
+            if clear_forces {
+                rb.force = na::zero();
+                rb.torque = na::zero();
+            }
+
+            rb.position = rb.next_position;
             rb.update_colliders_positions(colliders);
         });
+    }
 
-        self.counters.stages.solver_time.pause();
+    fn interpolate_kinematic_velocities(
+        &mut self,
+        integration_parameters: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+    ) {
+        // Update kinematic bodies velocities.
+        // TODO: what is the best place for this? It should at least be
+        // located before the island computation because we test the velocity
+        // there to determine if this kinematic body should wake-up dynamic
+        // bodies it is touching.
+        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
+            body.compute_velocity_from_next_position(integration_parameters.inv_dt());
+        });
+    }
+
+    /// Executes one timestep of the physics simulation.
+    pub fn step(
+        &mut self,
+        gravity: &Vector<Real>,
+        integration_parameters: &IntegrationParameters,
+        broad_phase: &mut BroadPhase,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        joints: &mut JointSet,
+        ccd_solver: &mut CCDSolver,
+        hooks: &dyn PhysicsHooks,
+        events: &dyn EventHandler,
+    ) {
+        self.counters.reset();
+        self.counters.step_started();
+        colliders.handle_user_changes(bodies);
+        bodies.handle_user_changes(colliders);
+
+        self.detect_collisions(
+            integration_parameters,
+            broad_phase,
+            narrow_phase,
+            bodies,
+            colliders,
+            hooks,
+            events,
+            true,
+        );
+
+        let mut remaining_time = integration_parameters.dt;
+        let mut integration_parameters = *integration_parameters;
+
+        let (ccd_is_enabled, mut remaining_substeps) =
+            if integration_parameters.max_ccd_substeps == 0 {
+                (false, 1)
+            } else {
+                (true, integration_parameters.max_ccd_substeps)
+            };
+
+        while remaining_substeps > 0 {
+            // If there are more than one CCD substep, we need to split
+            // the timestep into multiple intervals. First, estimate the
+            // size of the time slice we will integrate for this substep.
+            //
+            // Note that we must do this now, before the constrains resolution
+            // because we need to use the correct timestep length for the
+            // integration of external forces.
+            //
+            // If there is only one or zero CCD substep, there is no need
+            // to split the timetsep interval. So we can just skip this part.
+            if ccd_is_enabled && remaining_substeps > 1 {
+                // NOTE: Take forces into account when updating the bodies CCD activation flags
+                //       these forces have not been integrated to the body's velocity yet.
+                let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true);
+                let first_impact = if ccd_active {
+                    ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase)
+                } else {
+                    None
+                };
+
+                if let Some(toi) = first_impact {
+                    let original_interval = remaining_time / (remaining_substeps as Real);
+
+                    if toi < original_interval {
+                        integration_parameters.dt = original_interval;
+                    } else {
+                        integration_parameters.dt =
+                            toi + (remaining_time - toi) / (remaining_substeps as Real);
+                    }
+
+                    remaining_substeps -= 1;
+                } else {
+                    // No impact, don't do any other substep after this one.
+                    integration_parameters.dt = remaining_time;
+                    remaining_substeps = 0;
+                }
+
+                remaining_time -= integration_parameters.dt;
+
+                // Avoid substep length that are too small.
+                if remaining_time <= integration_parameters.min_ccd_dt {
+                    integration_parameters.dt += remaining_time;
+                    remaining_substeps = 0;
+                }
+            } else {
+                integration_parameters.dt = remaining_time;
+                remaining_time = 0.0;
+                remaining_substeps = 0;
+            }
+
+            self.counters.ccd.num_substeps += 1;
+
+            self.interpolate_kinematic_velocities(&integration_parameters, bodies);
+            self.build_islands_and_solve_velocity_constraints(
+                gravity,
+                &integration_parameters,
+                narrow_phase,
+                bodies,
+                colliders,
+                joints,
+            );
+
+            // If CCD is enabled, execute the CCD motion clamping.
+            if ccd_is_enabled {
+                // NOTE: don't the forces into account when updating the CCD active flags because
+                //       they have already been integrated into the velocities by the solver.
+                let ccd_active =
+                    ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false);
+                if ccd_active {
+                    self.run_ccd_motion_clamping(
+                        &integration_parameters,
+                        bodies,
+                        colliders,
+                        narrow_phase,
+                        ccd_solver,
+                        events,
+                    );
+                }
+            }
+
+            // NOTE: we need to run the position solver **after** the
+            //       CCD motion clamping because otherwise the clamping
+            //       would undo the depenetration done by the position
+            //       solver.
+            //       This happens because our CCD use the real rigid-body
+            //       velocities instead of just interpolating between
+            //       isometries.
+            self.solve_position_constraints(&integration_parameters, bodies);
+
+            let clear_forces = remaining_substeps == 0;
+            self.advance_to_final_positions(bodies, colliders, clear_forces);
+            self.detect_collisions(
+                &integration_parameters,
+                broad_phase,
+                narrow_phase,
+                bodies,
+                colliders,
+                hooks,
+                events,
+                false,
+            );
+
+            bodies.modified_inactive_set.clear();
+        }
 
-        bodies.modified_inactive_set.clear();
         self.counters.step_completed();
     }
 }
 
 #[cfg(test)]
 mod test {
-    use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet};
+    use crate::dynamics::{
+        CCDSolver, IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet,
+    };
     use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
     use crate::math::Vector;
     use crate::pipeline::PhysicsPipeline;
@@ -278,6 +507,7 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
+            &mut CCDSolver::new(),
             &(),
             &(),
         );
@@ -321,6 +551,7 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
+            &mut CCDSolver::new(),
             &(),
             &(),
         );
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 8cc6a60..5451cfa 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,10 +1,9 @@
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
-    RayIntersection, SimdQuadTree,
+    RayIntersection, SimdQuadTree, AABB,
 };
 use crate::math::{Isometry, Point, Real, Vector};
-use crate::parry::motion::RigidMotion;
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -15,7 +14,7 @@ use parry::query::details::{
 use parry::query::visitors::{
     BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
 };
-use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
+use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
 use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
 use std::sync::Arc;
 
@@ -39,6 +38,22 @@ struct QueryPipelineAsCompositeShape<'a> {
     groups: InteractionGroups,
 }
 
+/// Indicates how the colliders position should be taken into account when
+/// updating the query pipeline.
+pub enum QueryPipelineMode {
+    /// The `Collider::position` is taken into account.
+    CurrentPosition,
+    /// The `RigidBody::next_position * Collider::position_wrt_parent` is taken into account for
+    /// the colliders positions.
+    SweepTestWithNextPosition,
+    /// The `RigidBody::predict_position_using_velocity_and_forces * Collider::position_wrt_parent`
+    /// is taken into account for the colliders position.
+    SweepTestWithPredictedPosition {
+        /// The time used to integrate the rigid-body's velocity and acceleration.
+        dt: Real,
+    },
+}
+
 impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
     type PartShape = dyn Shape;
     type PartId = ColliderHandle;
@@ -95,7 +110,7 @@ impl QueryPipeline {
     /// Initializes an empty query pipeline with a custom `QueryDispatcher`.
     ///
     /// Use this constructor in order to use a custom `QueryDispatcher` that is
-    /// awary of your own user-defined shapes.
+    /// aware of your own user-defined shapes.
     pub fn with_query_dispatcher<D>(d: D) -> Self
     where
         D: 'static + QueryDispatcher,
@@ -108,11 +123,48 @@ impl QueryPipeline {
         }
     }
 
+    /// The query dispatcher used by this query pipeline for running scene queries.
+    pub fn query_dispatcher(&self) -> &dyn QueryDispatcher {
+        &*self.query_dispatcher
+    }
+
     /// Update the acceleration structure on the query pipeline.
     pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+        self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
+    }
+
+    /// Update the acceleration structure on the query pipeline.
+    pub fn update_with_mode(
+        &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        mode: QueryPipelineMode,
+    ) {
         if !self.tree_built {
-            let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
-            self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            match mode {
+                QueryPipelineMode::CurrentPosition => {
+                    let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
+                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                }
+                QueryPipelineMode::SweepTestWithNextPosition => {
+                    let data = colliders.iter().map(|(h, c)| {
+                        let next_position =
+                            bodies[c.parent()].next_position * c.position_wrt_parent();
+                        (h, c.compute_swept_aabb(&next_position))
+                    });
+                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                }
+                QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+                    let data = colliders.iter().map(|(h, c)| {
+                        let next_position = bodies[c.parent()]
+                            .predict_position_using_velocity_and_forces(dt)
+                            * c.position_wrt_parent();
+                        (h, c.compute_swept_aabb(&next_position))
+                    });
+                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                }
+            }
+
             // FIXME: uncomment this once we handle insertion/removals properly.
             // self.tree_built = true;
             return;
@@ -127,10 +179,37 @@ impl QueryPipeline {
             }
         }
 
-        self.quadtree.update(
-            |handle| colliders[*handle].compute_aabb(),
-            self.dilation_factor,
-        );
+        match mode {
+            QueryPipelineMode::CurrentPosition => {
+                self.quadtree.update(
+                    |handle| colliders[*handle].compute_aabb(),
+                    self.dilation_factor,
+                );
+            }
+            QueryPipelineMode::SweepTestWithNextPosition => {
+                self.quadtree.update(
+                    |handle| {
+                        let co = &colliders[*handle];
+                        let next_position =
+                            bodies[co.parent()].next_position * co.position_wrt_parent();
+                        co.compute_swept_aabb(&next_position)
+                    },
+                    self.dilation_factor,
+                );
+            }
+            QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+                self.quadtree.update(
+                    |handle| {
+                        let co = &colliders[*handle];
+                        let next_position = bodies[co.parent()]
+                            .predict_position_using_velocity_and_forces(dt)
+                            * co.position_wrt_parent();
+                        co.compute_swept_aabb(&next_position)
+                    },
+                    self.dilation_factor,
+                );
+            }
+        }
     }
 
     /// Find the closest intersection between a ray and a set of collider.
@@ -336,6 +415,16 @@ impl QueryPipeline {
             .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
     }
 
+    /// Finds all handles of all the colliders with an AABB intersecting the given AABB.
+    pub fn colliders_with_aabb_intersecting_aabb(
+        &self,
+        aabb: &AABB,
+        mut callback: impl FnMut(&ColliderHandle) -> bool,
+    ) {
+        let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback);
+        self.quadtree.traverse_depth_first(&mut visitor);
+    }
+
     /// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
     ///
     /// This is similar to ray-casting except that we are casting a whole shape instead of
@@ -386,20 +475,24 @@ impl QueryPipeline {
     pub fn nonlinear_cast_shape(
         &self,
         colliders: &ColliderSet,
-        shape_motion: &dyn RigidMotion,
+        shape_motion: &NonlinearRigidMotion,
         shape: &dyn Shape,
-        max_toi: Real,
-        target_distance: Real,
+        start_time: Real,
+        end_time: Real,
+        stop_at_penetration: bool,
         groups: InteractionGroups,
     ) -> Option<(ColliderHandle, TOI)> {
         let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
-            shape_motion,
+            &pipeline_motion,
             &pipeline_shape,
+            shape_motion,
             shape,
-            max_toi,
-            target_distance,
+            start_time,
+            end_time,
+            stop_at_penetration,
         );
         self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
     }