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

8 files changed, 183 insertions, 30 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);
diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs
index 1bef022..9b3b9ee 100644
--- a/src/dynamics/coefficient_combine_rule.rs
+++ b/src/dynamics/coefficient_combine_rule.rs
@@ -21,7 +21,7 @@ pub enum CoefficientCombineRule {
 }
 
 impl CoefficientCombineRule {
-    pub fn from_value(val: u8) -> Self {
+    pub(crate) fn from_value(val: u8) -> Self {
         match val {
             0 => CoefficientCombineRule::Average,
             1 => CoefficientCombineRule::Min,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 08c5629..15ddce3 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -113,6 +113,7 @@ pub struct RigidBody {
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
     pub(crate) ccd_thickness: Real,
+    pub(crate) ccd_max_dist: Real,
 }
 
 impl RigidBody {
@@ -146,6 +147,7 @@ impl RigidBody {
             dominance_group: 0,
             user_data: 0,
             ccd_thickness: Real::MAX,
+            ccd_max_dist: 0.0,
         }
     }
 
@@ -172,8 +174,6 @@ impl RigidBody {
 
         self.linvel += linear_acc * dt;
         self.angvel += angular_acc * dt;
-        self.force = na::zero();
-        self.torque = na::zero();
     }
 
     /// The mass properties of this rigid-body.
@@ -208,17 +208,56 @@ impl RigidBody {
     // This is different from `is_ccd_enabled`. This checks that CCD
     // is active for this rigid-body, i.e., if it was seen to move fast
     // enough to justify a CCD run.
-    pub(crate) fn is_ccd_active(&self) -> bool {
+    /// Is CCD active for this rigid-body?
+    ///
+    /// The CCD is considered active if the rigid-body is moving at
+    /// a velocity greater than an automatically-computed threshold.
+    ///
+    /// This is not the same as `self.is_ccd_enabled` which only
+    /// checks if CCD is allowed to run for this rigid-body or if
+    /// it is completely disabled (independently from its velocity).
+    pub fn is_ccd_active(&self) -> bool {
         self.flags.contains(RigidBodyFlags::CCD_ACTIVE)
     }
 
-    pub(crate) fn update_ccd_active_flag(&mut self, dt: Real) {
-        let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt);
+    pub(crate) fn update_ccd_active_flag(&mut self, dt: Real, include_forces: bool) {
+        let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt, include_forces);
         self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
     }
 
-    pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
-        self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+    pub(crate) fn is_moving_fast(&self, dt: Real, include_forces: bool) -> bool {
+        if self.is_dynamic() {
+            // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
+            //       should use `self.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
+            //       is the deepest contact (the contact with the largest penetration depth, i.e., the
+            //       negative `dist` with the largest absolute value.
+            //       However, getting this penetration depth assumes querying the contact graph from
+            //       the narrow-phase, which can be pretty expensive. So we use the CCD thickness
+            //       divided by 10 right now. We will see in practice if this value is OK or if we
+            //       should use a smaller (to be less conservative) or larger divisor (to be more conservative).
+            let threshold = self.ccd_thickness / 10.0;
+
+            if include_forces {
+                let linear_part = (self.linvel + self.force * dt).norm();
+                #[cfg(feature = "dim2")]
+                let angular_part = (self.angvel + self.torque * dt).abs() * self.ccd_max_dist;
+                #[cfg(feature = "dim3")]
+                let angular_part = (self.angvel + self.torque * dt).norm() * self.ccd_max_dist;
+                let vel_with_forces = linear_part + angular_part;
+                vel_with_forces > threshold
+            } else {
+                self.max_point_velocity() * dt > threshold
+            }
+        } else {
+            false
+        }
+    }
+
+    pub(crate) fn max_point_velocity(&self) -> Real {
+        #[cfg(feature = "dim2")]
+        return self.linvel.norm() + self.angvel.abs() * self.ccd_max_dist;
+        #[cfg(feature = "dim3")]
+        return self.linvel.norm() + self.angvel.norm() * self.ccd_max_dist;
     }
 
     /// Sets the rigid-body's mass properties.
@@ -301,6 +340,13 @@ impl RigidBody {
 
         self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
 
+        let shape_bsphere = coll
+            .shape()
+            .compute_bounding_sphere(coll.position_wrt_parent());
+        self.ccd_max_dist = self
+            .ccd_max_dist
+            .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
+
         let mass_properties = coll
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
@@ -311,7 +357,7 @@ impl RigidBody {
 
     pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
         for handle in &self.colliders {
-            // NOTE: we don't use `get_mut_internal` here because we want to
+            // NOTE: we use `get_mut_internal_with_modification_tracking` here because we want to
             //       benefit from the modification tracking to know the colliders
             //       we need to update the broad-phase and narrow-phase for.
             let collider = colliders
@@ -382,7 +428,9 @@ impl RigidBody {
         !self.linvel.is_zero() || !self.angvel.is_zero()
     }
 
-    pub(crate) fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
+    /// Computes the predict position of this rigid-body after `dt` seconds, taking
+    /// into account its velocities and external forces applied to it.
+    pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
         let dlinvel = self.force * (self.effective_inv_mass * dt);
         let dangvel = self
             .effective_world_inv_inertia_sqrt
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 0f01798..ff4ceed 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -157,6 +157,13 @@ impl InteractionGroups {
         }
     }
 
+    pub fn clear(&mut self) {
+        self.buckets.clear();
+        self.body_masks.clear();
+        self.grouped_interactions.clear();
+        self.nongrouped_interactions.clear();
+    }
+
     // FIXME: there is a lot of duplicated code with group_manifolds here.
     // But we don't refactor just now because we may end up with distinct
     // grouping strategies in the future.
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index e35603e..11fe5ee 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -660,6 +660,14 @@ impl ColliderBuilder {
 
     /// Sets the initial position (translation and orientation) of the collider to be created,
     /// relative to the rigid-body it is attached to.
+    pub fn position_wrt_parent(mut self, pos: Isometry<Real>) -> Self {
+        self.delta = pos;
+        self
+    }
+
+    /// Sets the initial position (translation and orientation) of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    #[deprecated(note = "Use `.position_wrt_parent` instead.")]
     pub fn position(mut self, pos: Isometry<Real>) -> Self {
         self.delta = pos;
         self
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index ffd5d7f..d4e8ac4 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{BodyPair, RigidBodyHandle};
-use crate::geometry::{ColliderPair, ContactManifold};
+use crate::geometry::{ColliderPair, Contact, ContactManifold};
 use crate::math::{Point, Real, Vector};
 use parry::query::ContactManifoldsWorkspace;
 
@@ -76,6 +76,35 @@ impl ContactPair {
             workspace: None,
         }
     }
+
+    /// Finds the contact with the smallest signed distance.
+    ///
+    /// If the colliders involved in this contact pair are penetrating, then
+    /// this returns the contact with the largest penetration depth.
+    ///
+    /// Returns a reference to the contact, as well as the contact manifold
+    /// it is part of.
+    pub fn find_deepest_contact(&self) -> Option<(&ContactManifold, &Contact)> {
+        let mut deepest = None;
+
+        for m2 in &self.manifolds {
+            let deepest_candidate = m2.find_deepest_contact();
+
+            deepest = match (deepest, deepest_candidate) {
+                (_, None) => deepest,
+                (None, Some(c2)) => Some((m2, c2)),
+                (Some((m1, c1)), Some(c2)) => {
+                    if c1.dist <= c2.dist {
+                        Some((m1, c1))
+                    } else {
+                        Some((m2, c2))
+                    }
+                }
+            }
+        }
+
+        deepest
+    }
 }
 
 #[derive(Clone, Debug)]
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 5b2cc88..5451cfa 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -38,10 +38,20 @@ 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,
-    SweepTestWithPredictedPosition { dt: Real },
+    /// 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> {
@@ -137,19 +147,19 @@ impl QueryPipeline {
                     self.quadtree.clear_and_rebuild(data, self.dilation_factor);
                 }
                 QueryPipelineMode::SweepTestWithNextPosition => {
-                    let data = colliders.iter().map(|(h, co)| {
+                    let data = colliders.iter().map(|(h, c)| {
                         let next_position =
-                            bodies[co.parent()].next_position * co.position_wrt_parent();
-                        (h, co.compute_swept_aabb(&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, co)| {
-                        let next_position = bodies[co.parent()]
+                    let data = colliders.iter().map(|(h, c)| {
+                        let next_position = bodies[c.parent()]
                             .predict_position_using_velocity_and_forces(dt)
-                            * co.position_wrt_parent();
-                        (h, co.compute_swept_aabb(&next_position))
+                            * c.position_wrt_parent();
+                        (h, c.compute_swept_aabb(&next_position))
                     });
                     self.quadtree.clear_and_rebuild(data, self.dilation_factor);
                 }