From 404e0324334acb2f7d2cb3b21b5da2e362926dd2 Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <sebcrozet@dimforge.com>
Date: Sun, 14 Apr 2024 15:54:39 +0200
Subject: feat: add soft (solver-based) ccd implementation

---
 src/geometry/broad_phase.rs                        |  3 +++
 .../broad_phase_multi_sap/broad_phase_multi_sap.rs | 22 +++++++++++++--
 src/geometry/narrow_phase.rs                       | 31 +++++++++++++++++++---
 3 files changed, 50 insertions(+), 6 deletions(-)

(limited to 'src/geometry')

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,
                 );
-- 
cgit