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

3 files changed, 79 insertions, 31 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 6255d60..5df3f6a 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -69,21 +69,18 @@ impl CollisionPipeline {
 
         // // Update kinematic bodies velocities.
         // bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-        //     body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
+        //     body.compute_velocity_from_next_position(integration_parameters.inv_dt());
         // });
 
         // Update colliders positions and kinematic bodies positions.
         bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
-            } else {
-                rb.update_predicted_position(0.0);
-            }
+            rb.position = rb.next_position;
+            rb.update_colliders_positions(colliders);
 
             for handle in &rb.colliders {
                 let collider = &mut colliders[*handle];
+                collider.prev_position = collider.position;
                 collider.position = rb.position * collider.delta;
-                collider.predicted_position = rb.predicted_position * collider.delta;
             }
         });
 
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 1908fad..7abe634 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -3,7 +3,7 @@
 use crate::counters::Counters;
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
@@ -68,6 +68,7 @@ impl PhysicsPipeline {
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
+        ccd_solver: Option<&mut CCDSolver>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -81,7 +82,7 @@ impl PhysicsPipeline {
         // there to determine if this kinematic body should wake-up dynamic
         // bodies it is touching.
         bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
+            body.compute_velocity_from_next_position(integration_parameters.inv_dt());
         });
 
         self.counters.stages.collision_detection_time.start();
@@ -218,23 +219,33 @@ impl PhysicsPipeline {
             });
         }
 
-        // Update colliders positions and kinematic bodies positions.
-        // FIXME: do this in the solver?
+        // Handle CCD
+        if let Some(ccd_solver) = ccd_solver {
+            let impacts = ccd_solver.predict_next_impacts(
+                integration_parameters,
+                bodies,
+                colliders,
+                integration_parameters.dt,
+                events,
+            );
+            ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
+        }
+
+        // Set the rigid-bodies and kinematic bodies to their final position.
         bodies.foreach_active_body_mut_internal(|_, rb| {
             if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
                 rb.linvel = na::zero();
                 rb.angvel = na::zero();
-            } else {
-                rb.update_predicted_position(integration_parameters.dt);
             }
 
+            rb.position = rb.next_position;
             rb.update_colliders_positions(colliders);
         });
 
         self.counters.stages.solver_time.pause();
 
         bodies.modified_inactive_set.clear();
+
         self.counters.step_completed();
     }
 }
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 8cc6a60..e89a03e 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,10 +1,9 @@
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
-    RayIntersection, SimdQuadTree,
+    RayIntersection, SimdQuadTree, AABB,
 };
 use crate::math::{Isometry, Point, Real, Vector};
-use crate::parry::motion::RigidMotion;
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -15,7 +14,7 @@ use parry::query::details::{
 use parry::query::visitors::{
     BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
 };
-use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
+use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
 use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
 use std::sync::Arc;
 
@@ -95,7 +94,7 @@ impl QueryPipeline {
     /// Initializes an empty query pipeline with a custom `QueryDispatcher`.
     ///
     /// Use this constructor in order to use a custom `QueryDispatcher` that is
-    /// awary of your own user-defined shapes.
+    /// aware of your own user-defined shapes.
     pub fn with_query_dispatcher<D>(d: D) -> Self
     where
         D: 'static + QueryDispatcher,
@@ -108,11 +107,26 @@ impl QueryPipeline {
         }
     }
 
+    /// The query dispatcher used by this query pipeline for running scene queries.
+    pub fn query_dispatcher(&self) -> &dyn QueryDispatcher {
+        &*self.query_dispatcher
+    }
+
     /// Update the acceleration structure on the query pipeline.
-    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
         if !self.tree_built {
-            let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
-            self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            if !use_swept_aabb {
+                let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
+                self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            } else {
+                let data = colliders.iter().map(|(h, co)| {
+                    let next_position =
+                        bodies[co.parent()].next_position * co.position_wrt_parent();
+                    (h, co.compute_swept_aabb(&next_position))
+                });
+                self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            }
+
             // FIXME: uncomment this once we handle insertion/removals properly.
             // self.tree_built = true;
             return;
@@ -127,10 +141,22 @@ impl QueryPipeline {
             }
         }
 
-        self.quadtree.update(
-            |handle| colliders[*handle].compute_aabb(),
-            self.dilation_factor,
-        );
+        if !use_swept_aabb {
+            self.quadtree.update(
+                |handle| colliders[*handle].compute_aabb(),
+                self.dilation_factor,
+            );
+        } else {
+            self.quadtree.update(
+                |handle| {
+                    let co = &colliders[*handle];
+                    let next_position =
+                        bodies[co.parent()].next_position * co.position_wrt_parent();
+                    co.compute_swept_aabb(&next_position)
+                },
+                self.dilation_factor,
+            );
+        }
     }
 
     /// Find the closest intersection between a ray and a set of collider.
@@ -336,6 +362,16 @@ impl QueryPipeline {
             .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
     }
 
+    /// Finds all handles of all the colliders with an AABB intersecting the given AABB.
+    pub fn colliders_with_aabb_intersecting_aabb(
+        &self,
+        aabb: &AABB,
+        mut callback: impl FnMut(&ColliderHandle) -> bool,
+    ) {
+        let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback);
+        self.quadtree.traverse_depth_first(&mut visitor);
+    }
+
     /// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
     ///
     /// This is similar to ray-casting except that we are casting a whole shape instead of
@@ -386,20 +422,24 @@ impl QueryPipeline {
     pub fn nonlinear_cast_shape(
         &self,
         colliders: &ColliderSet,
-        shape_motion: &dyn RigidMotion,
+        shape_motion: &NonlinearRigidMotion,
         shape: &dyn Shape,
-        max_toi: Real,
-        target_distance: Real,
+        start_time: Real,
+        end_time: Real,
+        stop_at_penetration: bool,
         groups: InteractionGroups,
     ) -> Option<(ColliderHandle, TOI)> {
         let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
-            shape_motion,
+            &pipeline_motion,
             &pipeline_shape,
+            shape_motion,
             shape,
-            max_toi,
-            target_distance,
+            start_time,
+            end_time,
+            stop_at_penetration,
         );
         self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
     }