Implement the ability to run multiple CCD substeps.

3 files changed, 169 insertions, 73 deletions

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 296a400..99e1bd9 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -68,8 +68,8 @@ impl PhysicsPipeline {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
-        self.counters.stages.collision_detection_time.start();
-        self.counters.cd.broad_phase_time.start();
+        self.counters.stages.collision_detection_time.resume();
+        self.counters.cd.broad_phase_time.resume();
 
         // Update broad-phase.
         self.broad_phase_events.clear();
@@ -81,7 +81,7 @@ impl PhysicsPipeline {
         );
 
         self.counters.cd.broad_phase_time.pause();
-        self.counters.cd.narrow_phase_time.start();
+        self.counters.cd.narrow_phase_time.resume();
 
         // Update narrow-phase.
         narrow_phase.handle_user_changes(colliders, bodies, events);
@@ -155,7 +155,7 @@ impl PhysicsPipeline {
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
     ) {
-        self.counters.stages.island_construction_time.start();
+        self.counters.stages.island_construction_time.resume();
         bodies.update_active_set_with_contacts(
             colliders,
             narrow_phase,
@@ -178,15 +178,14 @@ impl PhysicsPipeline {
         narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices);
         joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
 
-        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);
@@ -259,20 +258,17 @@ impl PhysicsPipeline {
         integration_parameters: &IntegrationParameters,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
-        ccd_solver: Option<&mut CCDSolver>,
+        ccd_solver: &mut CCDSolver,
         events: &dyn EventHandler,
     ) {
         // Handle CCD
-        if let Some(ccd_solver) = ccd_solver {
-            let impacts = ccd_solver.predict_next_impacts(
-                integration_parameters,
-                bodies,
-                colliders,
-                integration_parameters.dt,
-                events,
-            );
-            ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
-        }
+        let impacts = ccd_solver.predict_impacts_at_next_positions(
+            integration_parameters.dt,
+            bodies,
+            colliders,
+            events,
+        );
+        ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
     }
 
     fn advance_to_final_positions(
@@ -317,15 +313,15 @@ impl PhysicsPipeline {
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
-        ccd_solver: Option<&mut CCDSolver>,
+        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.interpolate_kinematic_velocities(integration_parameters, bodies);
         self.detect_collisions_after_user_modifications(
             integration_parameters,
             broad_phase,
@@ -335,33 +331,90 @@ impl PhysicsPipeline {
             hooks,
             events,
         );
-        self.build_islands_and_solve_constraints(
-            gravity,
-            integration_parameters,
-            narrow_phase,
-            bodies,
-            colliders,
-            joints,
-        );
-        self.run_ccd_motion_clamping(
-            integration_parameters,
-            bodies,
-            colliders,
-            ccd_solver,
-            events,
-        );
-        self.advance_to_final_positions(bodies, colliders);
-        self.detect_collisions_after_integration(
-            integration_parameters,
-            broad_phase,
-            narrow_phase,
-            bodies,
-            colliders,
-            hooks,
-            events,
-        );
 
-        bodies.modified_inactive_set.clear();
+        let mut remaining_time = integration_parameters.dt;
+        let mut remaining_substeps = integration_parameters.max_ccd_substeps;
+        let mut integration_parameters = *integration_parameters;
+
+        let ccd_active = ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt);
+
+        loop {
+            if ccd_active && remaining_substeps > 1 {
+                // 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.
+                //
+                // 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 let Some(toi) = ccd_solver.find_first_impact(remaining_time, bodies, colliders) {
+                    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 = 1;
+                }
+
+                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 = 1;
+                }
+            } else {
+                integration_parameters.dt = remaining_time;
+                remaining_time = 0.0;
+                remaining_substeps = 1;
+            }
+
+            self.interpolate_kinematic_velocities(&integration_parameters, bodies);
+            self.build_islands_and_solve_constraints(
+                gravity,
+                &integration_parameters,
+                narrow_phase,
+                bodies,
+                colliders,
+                joints,
+            );
+
+            // If CCD is enabled, execute the CCD motion clamping.
+            if ccd_active && remaining_substeps > 0 {
+                self.run_ccd_motion_clamping(
+                    &integration_parameters,
+                    bodies,
+                    colliders,
+                    ccd_solver,
+                    events,
+                );
+            }
+
+            self.advance_to_final_positions(bodies, colliders);
+            self.detect_collisions_after_integration(
+                &integration_parameters,
+                broad_phase,
+                narrow_phase,
+                bodies,
+                colliders,
+                hooks,
+                events,
+            );
+
+            bodies.modified_inactive_set.clear();
+
+            if !ccd_active || remaining_substeps <= 1 {
+                // We executed all the substeps.
+                break;
+            }
+        }
         self.counters.step_completed();
     }
 }
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index e89a03e..5b2cc88 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -38,6 +38,12 @@ struct QueryPipelineAsCompositeShape<'a> {
     groups: InteractionGroups,
 }
 
+pub enum QueryPipelineMode {
+    CurrentPosition,
+    SweepTestWithNextPosition,
+    SweepTestWithPredictedPosition { dt: Real },
+}
+
 impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
     type PartShape = dyn Shape;
     type PartId = ColliderHandle;
@@ -113,18 +119,40 @@ impl QueryPipeline {
     }
 
     /// Update the acceleration structure on the query pipeline.
-    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
+    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 {
-            if !use_swept_aabb {
-                let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
-                self.quadtree.clear_and_rebuild(data, self.dilation_factor);
-            } else {
-                let data = colliders.iter().map(|(h, co)| {
-                    let next_position =
-                        bodies[co.parent()].next_position * co.position_wrt_parent();
-                    (h, co.compute_swept_aabb(&next_position))
-                });
-                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, co)| {
+                        let next_position =
+                            bodies[co.parent()].next_position * co.position_wrt_parent();
+                        (h, co.compute_swept_aabb(&next_position))
+                    });
+                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                }
+                QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+                    let data = colliders.iter().map(|(h, co)| {
+                        let next_position = bodies[co.parent()]
+                            .predict_position_using_velocity_and_forces(dt)
+                            * co.position_wrt_parent();
+                        (h, co.compute_swept_aabb(&next_position))
+                    });
+                    self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                }
             }
 
             // FIXME: uncomment this once we handle insertion/removals properly.
@@ -141,21 +169,36 @@ impl QueryPipeline {
             }
         }
 
-        if !use_swept_aabb {
-            self.quadtree.update(
-                |handle| colliders[*handle].compute_aabb(),
-                self.dilation_factor,
-            );
-        } else {
-            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,
-            );
+        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,
+                );
+            }
         }
     }