First working version of non-linear CCD based on single-substep motion-clamping.

3 files changed, 18 insertions, 9 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 236fd5a..43f8294 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -2,7 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
 use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
-use parry::bounding_volume::AABB;
+use parry::bounding_volume::{BoundingVolume, AABB};
 use parry::shape::Shape;
 
 bitflags::bitflags! {
@@ -62,7 +62,7 @@ pub struct Collider {
     pub(crate) parent: RigidBodyHandle,
     pub(crate) delta: Isometry<Real>,
     pub(crate) position: Isometry<Real>,
-    pub(crate) predicted_position: Isometry<Real>,
+    pub(crate) prev_position: Isometry<Real>,
     /// The friction coefficient of this collider.
     pub friction: Real,
     /// The restitution coefficient of this collider.
@@ -139,11 +139,12 @@ impl Collider {
         self.shape.compute_aabb(&self.position)
     }
 
-    // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB {
-    //     let aabb1 = self.shape.compute_aabb(&self.position);
-    //     let aabb2 = self.shape.compute_aabb(&self.predicted_position);
-    //     aabb1.merged(&aabb2)
-    // }
+    /// Compute the axis-aligned bounding box of this collider.
+    pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
+        let aabb1 = self.shape.compute_aabb(&self.position);
+        let aabb2 = self.shape.compute_aabb(next_position);
+        aabb1.merged(&aabb2)
+    }
 
     /// Compute the local-space mass properties of this collider.
     pub fn mass_properties(&self) -> MassProperties {
@@ -595,8 +596,8 @@ impl ColliderBuilder {
             flags,
             solver_flags,
             parent: RigidBodyHandle::invalid(),
+            prev_position: Isometry::identity(),
             position: Isometry::identity(),
-            predicted_position: Isometry::identity(),
             proxy_index: crate::INVALID_U32,
             collision_groups: self.collision_groups,
             solver_groups: self.solver_groups,
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index 76b3f6d..d84639c 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -108,8 +108,8 @@ impl ColliderSet {
         let parent = bodies
             .get_mut(parent_handle)
             .expect("Parent rigid body not found.");
+        coll.prev_position = parent.position * coll.delta;
         coll.position = parent.position * coll.delta;
-        coll.predicted_position = parent.predicted_position * coll.delta;
         let handle = ColliderHandle(self.colliders.insert(coll));
         let coll = self.colliders.get(handle.0).unwrap();
         parent.add_collider(handle, &coll);
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 9c635dc..92cf57d 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -71,6 +71,14 @@ impl NarrowPhase {
         }
     }
 
+    /// The query dispatcher used by this narrow-phase to select the right collision-detection
+    /// algorithms depending of the shape types.
+    pub fn query_dispatcher(
+        &self,
+    ) -> &dyn PersistentQueryDispatcher<ContactManifoldData, ContactData> {
+        &*self.query_dispatcher
+    }
+
     /// The contact graph containing all contact pairs and their contact information.
     pub fn contact_graph(&self) -> &InteractionGraph<ColliderHandle, ContactPair> {
         &self.contact_graph