CCD: take angular motion and penetration depth into account in various thresholds.

2 files changed, 62 insertions, 11 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 2d39956..41b195c 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -1,6 +1,6 @@
 use super::TOIEntry;
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::{ColliderSet, IntersectionEvent};
+use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase};
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
@@ -44,11 +44,13 @@ impl CCDSolver {
     pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
         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.linvel.norm()))
-                                .min(dt);
+                        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), false);
                     }
@@ -61,11 +63,18 @@ 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(&self, bodies: &mut RigidBodySet, dt: Real) -> bool {
+    pub fn update_ccd_active_flags(
+        &self,
+        bodies: &mut RigidBodySet,
+        dt: Real,
+        include_forces: bool,
+    ) -> bool {
         let mut ccd_active = false;
 
+        // println!("Checking CCD activation");
         bodies.foreach_active_dynamic_body_mut_internal(|_, body| {
-            body.update_ccd_active_flag(dt);
+            body.update_ccd_active_flag(dt, include_forces);
+            // println!("CCD is active: {}, for {:?}", ccd_active, handle);
             ccd_active = ccd_active || body.is_ccd_active();
         });
 
@@ -78,6 +87,7 @@ impl CCDSolver {
         dt: Real,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        narrow_phase: &NarrowPhase,
     ) -> Option<Real> {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
@@ -127,6 +137,12 @@ impl CCDSolver {
                                     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 = bodies.get(bh1).unwrap();
                                 let b2 = bodies.get(bh2).unwrap();
 
@@ -142,6 +158,7 @@ impl CCDSolver {
                                     None,
                                     0.0,
                                     min_toi,
+                                    smallest_dist,
                                 ) {
                                     min_toi = min_toi.min(toi.toi);
                                 }
@@ -166,6 +183,7 @@ impl CCDSolver {
         dt: Real,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        narrow_phase: &NarrowPhase,
         events: &dyn EventHandler,
     ) -> PredictedImpacts {
         let mut frozen = HashMap::<_, Real>::default();
@@ -218,6 +236,12 @@ impl CCDSolver {
                             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,
@@ -232,6 +256,7 @@ impl CCDSolver {
                                 // 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);
@@ -331,6 +356,12 @@ impl CCDSolver {
                             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);
+
                         if let Some(toi) = TOIEntry::try_from_colliders(
                             self.query_pipeline.query_dispatcher(),
                             *ch1,
@@ -343,6 +374,7 @@ impl CCDSolver {
                             frozen2.copied(),
                             start_time,
                             dt,
+                            smallest_dist,
                         ) {
                             all_toi.push(toi);
                         }
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 3916a4b..cc6773c 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -47,16 +47,35 @@ impl TOIEntry {
         frozen2: Option<Real>,
         start_time: Real,
         end_time: Real,
+        smallest_contact_dist: Real,
     ) -> Option<Self> {
         assert!(start_time <= end_time);
 
-        let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel;
-        let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel;
-
-        let vel12 = linvel2 - linvel1;
-        let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();
+        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();
+
+        #[cfg(feature = "dim2")]
+        let vel12 = (linvel2 - linvel1).norm()
+            + angvel1.abs() * b1.ccd_max_dist
+            + angvel2.abs() * b2.ccd_max_dist;
+        #[cfg(feature = "dim3")]
+        let vel12 = (linvel2 - linvel1).norm()
+            + angvel1.norm() * b1.ccd_max_dist
+            + angvel2.norm() * b2.ccd_max_dist;
+
+        // 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())
+            + smallest_contact_dist.max(0.0);
         let is_intersection_test = c1.is_sensor() || c2.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);