Implement the ability to run multiple CCD substeps.

3 files changed, 44 insertions, 16 deletions

diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 6679a91..3916a4b 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
+use crate::dynamics::{RigidBody, RigidBodyHandle};
 use crate::geometry::{Collider, ColliderHandle};
 use crate::math::Real;
 use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -36,7 +36,6 @@ impl TOIEntry {
     }
 
     pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
-        params: &IntegrationParameters,
         query_dispatcher: &QD,
         ch1: ColliderHandle,
         ch2: ColliderHandle,
@@ -56,16 +55,11 @@ impl TOIEntry {
 
         let vel12 = linvel2 - linvel1;
         let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();
-
-        if params.dt * vel12.norm() < thickness {
-            return None;
-        }
-
         let is_intersection_test = c1.is_sensor() || c2.is_sensor();
 
         // Compute the TOI.
-        let mut motion1 = Self::body_motion(params.dt, b1);
-        let mut motion2 = Self::body_motion(params.dt, b2);
+        let mut motion1 = Self::body_motion(b1);
+        let mut motion2 = Self::body_motion(b2);
 
         if let Some(t) = frozen1 {
             motion1.freeze(t);
@@ -114,8 +108,8 @@ impl TOIEntry {
         ))
     }
 
-    fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion {
-        if body.should_resolve_ccd(dt) {
+    fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
+        if body.is_ccd_active() {
             NonlinearRigidMotion::new(
                 0.0,
                 body.position,
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 136e345..615bfee 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -6,6 +6,17 @@ use crate::math::Real;
 pub struct IntegrationParameters {
     /// The timestep length (default: `1.0 / 60.0`)
     pub dt: Real,
+    /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`)
+    ///
+    /// When CCD with multiple substeps is enabled, the timestep is subdivided
+    /// into smaller pieces. This timestep subdivision won't generate timestep
+    /// lengths smaller than `min_dt`.
+    ///
+    /// Setting this to a large value will reduce the opportunity to performing
+    /// CCD substepping, resulting in potentially more time dropped by the
+    /// motion-clamping mechanism. Setting this to an very small value may lead
+    /// to numerical instabilities.
+    pub min_ccd_dt: Real,
 
     //    /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
     //    ///
@@ -195,6 +206,7 @@ impl Default for IntegrationParameters {
     fn default() -> Self {
         Self {
             dt: 1.0 / 60.0,
+            min_ccd_dt: 1.0 / 60.0 / 100.0,
             //        multithreading_enabled:             true,
             return_after_ccd_substep: false,
             erp: 0.2,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 3c6bd65..08c5629 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -5,7 +5,7 @@ use crate::geometry::{
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
 };
-use crate::utils::{self, WCross, WDot};
+use crate::utils::{self, WAngularInertia, WCross, WDot};
 use na::ComplexField;
 use num::Zero;
 
@@ -37,6 +37,7 @@ bitflags::bitflags! {
         const ROTATION_LOCKED_Y = 1 << 2;
         const ROTATION_LOCKED_Z = 1 << 3;
         const CCD_ENABLED = 1 << 4;
+        const CCD_ACTIVE = 1 << 5;
     }
 }
 
@@ -204,12 +205,20 @@ impl RigidBody {
         self.flags.contains(RigidBodyFlags::CCD_ENABLED)
     }
 
-    pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
-        self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+    // 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 {
+        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);
+        self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
     }
 
-    pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool {
-        self.is_ccd_enabled() && self.is_moving_fast(dt)
+    pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
+        self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
     }
 
     /// Sets the rigid-body's mass properties.
@@ -373,6 +382,19 @@ impl RigidBody {
         !self.linvel.is_zero() || !self.angvel.is_zero()
     }
 
+    pub(crate) 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
+            .transform_vector(self.torque * dt);
+        let linvel = self.linvel + dlinvel;
+        let angvel = self.angvel + dangvel;
+
+        let com = self.position * self.mass_properties.local_com;
+        let shift = Translation::from(com.coords);
+        shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.position
+    }
+
     pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
         let com = self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);