feat: add configurable distance cap to soft-ccd

2 files changed, 67 insertions, 27 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index e1075e6..d4043c2 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -448,17 +448,26 @@ impl RigidBody {
         self.ccd.ccd_enabled
     }
 
-    /// Enables of disable soft CCD (soft Continuous Collision-Detection) for this rigid-body.
+    /// Sets the maximum prediction distance Soft Continuous Collision-Detection.
     ///
-    /// Soft-CCD helps prevent tunneling, but may still let tunnelling happen depending on solver
-    /// convergence. This is cheaper than the full ccd enabled by [`RigidBody::enable_ccd`].
-    pub fn enable_soft_ccd(&mut self, enabled: bool) {
-        self.ccd.soft_ccd_enabled = enabled;
+    /// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
+    ///
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBody::enable_ccd`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub fn set_soft_ccd_prediction(&mut self, prediction_distance: Real) {
+        self.ccd.soft_ccd_prediction = prediction_distance;
     }
 
-    /// Is Soft-CCD (Soft Continous Collision-Detection) enabled for this rigid-body?
-    pub fn is_soft_ccd_enabled(&self) -> bool {
-        self.ccd.soft_ccd_enabled
+    /// The soft-CCD prediction distance for this rigid-body.
+    ///
+    /// See the documentation of [`RigidBody::set_soft_ccd_prediction`] for additional details on
+    /// soft-CCD.
+    pub fn soft_ccd_prediction(&self) -> Real {
+        self.ccd.soft_ccd_prediction
     }
 
     // This is different from `is_ccd_enabled`. This checks that CCD
@@ -880,6 +889,25 @@ impl RigidBody {
 
     /// Predicts the next position of this rigid-body, by integrating its velocity and forces
     /// by a time of `dt`.
+    pub(crate) fn predict_position_using_velocity_and_forces_with_max_dist(
+        &self,
+        dt: Real,
+        max_dist: Real,
+    ) -> Isometry<Real> {
+        let new_vels = self.forces.integrate(dt, &self.vels, &self.mprops);
+        // Compute the clamped dt such that the body doesn’t travel more than `max_dist`.
+        let linvel_norm = new_vels.linvel.norm();
+        let clamped_linvel = linvel_norm.min(max_dist * crate::utils::inv(dt));
+        let clamped_dt = dt * clamped_linvel * crate::utils::inv(linvel_norm);
+        new_vels.integrate(
+            clamped_dt,
+            &self.pos.position,
+            &self.mprops.local_mprops.local_com,
+        )
+    }
+
+    /// Predicts the next position of this rigid-body, by integrating its velocity and forces
+    /// by a time of `dt`.
     pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
         self.pos
             .integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops)
@@ -1110,19 +1138,25 @@ pub struct RigidBodyBuilder {
     mprops_flags: LockedAxes,
     /// The additional mass-properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
     additional_mass_properties: RigidBodyAdditionalMassProps,
-    /// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    /// Whether the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
     pub can_sleep: bool,
-    /// Whether or not the rigid-body is to be created asleep.
+    /// Whether the rigid-body is to be created asleep.
     pub sleeping: bool,
     /// Whether Continuous Collision-Detection is enabled for the rigid-body to be built.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub ccd_enabled: bool,
-    /// Whether Soft Continuous Collision-Detection is enabled for the rigid-body to be built.
+    /// The maximum prediction distance Soft Continuous Collision-Detection.
     ///
-    /// Soft-CCD helps prevent tunneling, but may still let tunnelling happen depending on solver
-    /// convergence. This is cheaper than the full ccd enabled by [`RigidBodyBuilder::ccd_enabled`].
-    pub soft_ccd_enabled: bool,
+    /// When set to 0, soft CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
+    ///
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub soft_ccd_prediction: Real,
     /// The dominance group of the rigid-body to be built.
     pub dominance_group: i8,
     /// Will the rigid-body being built be enabled?
@@ -1152,7 +1186,7 @@ impl RigidBodyBuilder {
             can_sleep: true,
             sleeping: false,
             ccd_enabled: false,
-            soft_ccd_enabled: false,
+            soft_ccd_prediction: 0.0,
             dominance_group: 0,
             enabled: true,
             user_data: 0,
@@ -1391,13 +1425,13 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    /// Sets whether the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
     pub fn can_sleep(mut self, can_sleep: bool) -> Self {
         self.can_sleep = can_sleep;
         self
     }
 
-    /// Sets whether or not Continuous Collision-Detection is enabled for this rigid-body.
+    /// Sets whether Continuous Collision-Detection is enabled for this rigid-body.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub fn ccd_enabled(mut self, enabled: bool) -> Self {
@@ -1405,16 +1439,22 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets whether or not Soft Continuous Collision-Detection is enabled for this rigid-body.
+    /// Sets the maximum prediction distance Soft Continuous Collision-Detection.
+    ///
+    /// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
     ///
-    /// Soft-CCD helps prevent tunneling, but may still let tunnelling happen depending on solver
-    /// convergence. This is cheaper than the full ccd enabled by [`RigidBodyBuilder::ccd_enabled`].
-    pub fn soft_ccd_enabled(mut self, enabled: bool) -> Self {
-        self.soft_ccd_enabled = enabled;
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub fn soft_ccd_prediction(mut self, prediction_distance: Real) -> Self {
+        self.soft_ccd_prediction = prediction_distance;
         self
     }
 
-    /// Sets whether or not the rigid-body is to be created asleep.
+    /// Sets whether the rigid-body is to be created asleep.
     pub fn sleeping(mut self, sleeping: bool) -> Self {
         self.sleeping = sleeping;
         self
@@ -1451,7 +1491,7 @@ impl RigidBodyBuilder {
         rb.dominance = RigidBodyDominance(self.dominance_group);
         rb.enabled = self.enabled;
         rb.enable_ccd(self.ccd_enabled);
-        rb.enable_soft_ccd(self.soft_ccd_enabled);
+        rb.set_soft_ccd_prediction(self.soft_ccd_prediction);
 
         if self.can_sleep && self.sleeping {
             rb.sleep();
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index 3eea73c..998253c 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -821,8 +821,8 @@ pub struct RigidBodyCcd {
     pub ccd_active: bool,
     /// Is CCD enabled for this rigid-body?
     pub ccd_enabled: bool,
-    /// Is soft-CCD enabled for this rigid-body?
-    pub soft_ccd_enabled: bool,
+    /// The soft-CCD prediction distance for this rigid-body.
+    pub soft_ccd_prediction: Real,
 }
 
 impl Default for RigidBodyCcd {
@@ -832,7 +832,7 @@ impl Default for RigidBodyCcd {
             ccd_max_dist: 0.0,
             ccd_active: false,
             ccd_enabled: false,
-            soft_ccd_enabled: false,
+            soft_ccd_prediction: 0.0,
         }
     }
 }