feat: add soft (solver-based) ccd implementation

7 files changed, 89 insertions, 9 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 5af4131..e1075e6 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -436,7 +436,7 @@ impl RigidBody {
         ]
     }
 
-    /// Enables of disable CCD (continuous collision-detection) for this rigid-body.
+    /// Enables of disable CCD (Continuous Collision-Detection) for this rigid-body.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub fn enable_ccd(&mut self, enabled: bool) {
@@ -448,6 +448,19 @@ impl RigidBody {
         self.ccd.ccd_enabled
     }
 
+    /// Enables of disable soft CCD (soft Continuous Collision-Detection) for this rigid-body.
+    ///
+    /// 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;
+    }
+
+    /// 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
+    }
+
     // 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.
@@ -1101,10 +1114,15 @@ pub struct RigidBodyBuilder {
     pub can_sleep: bool,
     /// Whether or not the rigid-body is to be created asleep.
     pub sleeping: bool,
-    /// Whether continuous collision-detection is enabled for the rigid-body to be built.
+    /// 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.
+    ///
+    /// 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,
     /// The dominance group of the rigid-body to be built.
     pub dominance_group: i8,
     /// Will the rigid-body being built be enabled?
@@ -1134,6 +1152,7 @@ impl RigidBodyBuilder {
             can_sleep: true,
             sleeping: false,
             ccd_enabled: false,
+            soft_ccd_enabled: false,
             dominance_group: 0,
             enabled: true,
             user_data: 0,
@@ -1378,7 +1397,7 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets whether or not continuous collision-detection is enabled for this rigid-body.
+    /// Sets whether or not 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 {
@@ -1386,6 +1405,15 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Sets whether or not Soft Continuous Collision-Detection is enabled for this rigid-body.
+    ///
+    /// 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;
+        self
+    }
+
     /// Sets whether or not the rigid-body is to be created asleep.
     pub fn sleeping(mut self, sleeping: bool) -> Self {
         self.sleeping = sleeping;
@@ -1423,6 +1451,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);
 
         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 2291742..3eea73c 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -821,6 +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,
 }
 
 impl Default for RigidBodyCcd {
@@ -830,6 +832,7 @@ impl Default for RigidBodyCcd {
             ccd_max_dist: 0.0,
             ccd_active: false,
             ccd_enabled: false,
+            soft_ccd_enabled: false,
         }
     }
 }
diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs
index e51c351..ea7c774 100644
--- a/src/geometry/broad_phase.rs
+++ b/src/geometry/broad_phase.rs
@@ -1,3 +1,4 @@
+use crate::dynamics::RigidBodySet;
 use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
 use parry::math::Real;
 
@@ -38,8 +39,10 @@ pub trait BroadPhase {
     ///   are still touching or closer than `prediction_distance`.
     fn update(
         &mut self,
+        dt: Real,
         prediction_distance: Real,
         colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
         modified_colliders: &[ColliderHandle],
         removed_colliders: &[ColliderHandle],
         events: &mut Vec<BroadPhasePairEvent>,
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
index 1d026ce..bc09b57 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
@@ -5,8 +5,8 @@ use crate::geometry::{
     BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
     ColliderPosition, ColliderSet, ColliderShape,
 };
-use crate::math::Real;
-use crate::prelude::BroadPhase;
+use crate::math::{Isometry, Real};
+use crate::prelude::{BroadPhase, RigidBodySet};
 use crate::utils::IndexMut2;
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::HashMap;
@@ -354,6 +354,7 @@ impl BroadPhaseMultiSap {
         handle: ColliderHandle,
         proxy_index: &mut u32,
         collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
+        next_position: Option<&Isometry<Real>>,
     ) -> bool {
         let (co_pos, co_shape, co_changes) = collider;
 
@@ -361,6 +362,13 @@ impl BroadPhaseMultiSap {
             .compute_aabb(co_pos)
             .loosened(prediction_distance / 2.0);
 
+        if let Some(next_position) = next_position {
+            let next_aabb = co_shape
+                .compute_aabb(next_position)
+                .loosened(prediction_distance / 2.0);
+            aabb.merge(&next_aabb);
+        }
+
         if aabb.mins.coords.iter().any(|e| !e.is_finite())
             || aabb.maxs.coords.iter().any(|e| !e.is_finite())
         {
@@ -563,8 +571,10 @@ impl BroadPhase for BroadPhaseMultiSap {
     /// Updates the broad-phase, taking into account the new collider positions.
     fn update(
         &mut self,
+        dt: Real,
         prediction_distance: Real,
         colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
         modified_colliders: &[ColliderHandle],
         removed_colliders: &[ColliderHandle],
         events: &mut Vec<BroadPhasePairEvent>,
@@ -585,11 +595,19 @@ impl BroadPhase for BroadPhaseMultiSap {
 
                 let mut new_proxy_id = co.bf_data.proxy_index;
 
+                let next_pos = co.parent.and_then(|p| {
+                    let parent = bodies.get(p.handle)?;
+                    parent.is_soft_ccd_enabled().then(|| {
+                        parent.predict_position_using_velocity_and_forces(dt) * p.pos_wrt_parent
+                    })
+                });
+
                 if self.handle_modified_collider(
                     prediction_distance,
                     *handle,
                     &mut new_proxy_id,
                     (&co.pos, &co.shape, &co.changes),
+                    next_pos.as_ref(),
                 ) {
                     need_region_propagation = true;
                 }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 1452864..f754808 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -8,9 +8,10 @@ use crate::dynamics::{
     RigidBodyType,
 };
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
-    ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
-    InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex,
+    BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle,
+    ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData,
+    ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
+    TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{
@@ -896,11 +897,33 @@ impl NarrowPhase {
                 }
 
                 let pos12 = co1.pos.inv_mul(&co2.pos);
+
+                let effective_prediction_distance = if rb1.map(|rb| rb.is_soft_ccd_enabled()) == Some(true) ||
+                    rb2.map(|rb| rb.is_soft_ccd_enabled()) == Some(true) {
+
+                    let aabb1 = co1.compute_aabb();
+                    let aabb2 = co2.compute_aabb();
+
+                    let linvel1 = rb1.map(|rb| *rb.linvel()).unwrap_or_default();
+                    let linvel2 = rb2.map(|rb| *rb.linvel()).unwrap_or_default();
+
+                    if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) {
+                        pair.clear();
+                        break 'emit_events;
+                    }
+
+
+                    prediction_distance.max(
+                        dt * (linvel1 - linvel2).norm())
+                } else {
+                    prediction_distance
+                };
+
                 let _ = query_dispatcher.contact_manifolds(
                     &pos12,
                     &*co1.shape,
                     &*co2.shape,
-                    prediction_distance,
+                    effective_prediction_distance,
                     &mut pair.manifolds,
                     &mut pair.workspace,
                 );
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index f8bee80..2ce7fed 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -58,8 +58,10 @@ impl CollisionPipeline {
         self.broadphase_collider_pairs.clear();
 
         broad_phase.update(
+            0.0,
             prediction_distance,
             colliders,
+            bodies,
             modified_colliders,
             removed_colliders,
             &mut self.broad_phase_events,
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index a268cda..0c1685b 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -112,8 +112,10 @@ impl PhysicsPipeline {
         self.broad_phase_events.clear();
         self.broadphase_collider_pairs.clear();
         broad_phase.update(
+            integration_parameters.dt,
             integration_parameters.prediction_distance,
             colliders,
+            bodies,
             modified_colliders,
             removed_colliders,
             &mut self.broad_phase_events,