QueryPipeline: add shape casting, point projection, and intersection queries.

3 files changed, 213 insertions, 42 deletions

diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 2b6d14f..ac7e63b 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -86,6 +86,15 @@ pub(crate) use self::narrow_phase::ContactManifoldIndex;
 pub(crate) use cdl::partitioning::SimdQuadTree;
 pub use cdl::shape::*;
 
+pub(crate) fn default_persistent_query_dispatcher(
+) -> std::sync::Arc<dyn cdl::query::PersistentQueryDispatcher<ContactManifoldData, ContactData>> {
+    std::sync::Arc::new(cdl::query::DefaultQueryDispatcher)
+}
+
+pub(crate) fn default_query_dispatcher() -> std::sync::Arc<dyn cdl::query::QueryDispatcher> {
+    std::sync::Arc::new(cdl::query::DefaultQueryDispatcher)
+}
+
 mod broad_phase_multi_sap;
 mod collider;
 mod collider_set;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index fc6d5db..b94ce6f 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -38,7 +38,7 @@ impl ColliderGraphIndices {
 pub struct NarrowPhase {
     #[cfg_attr(
         feature = "serde-serialize",
-        serde(skip, default = "default_query_dispatcher")
+        serde(skip, default = "crate::geometry::default_persistent_query_dispatcher")
     )]
     query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>,
     contact_graph: InteractionGraph<ContactPair>,
@@ -47,11 +47,6 @@ pub struct NarrowPhase {
     removed_colliders: Option<Subscription<RemovedCollider>>,
 }
 
-fn default_query_dispatcher() -> Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>
-{
-    Arc::new(DefaultQueryDispatcher)
-}
-
 pub(crate) type ContactManifoldIndex = usize;
 
 impl NarrowPhase {
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 9943099..1c07a3b 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,15 +1,68 @@
+use crate::cdl::motion::RigidMotion;
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{
-    Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, SimdQuadTree,
+    Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
+    RayIntersection, SimdQuadTree,
 };
+use crate::math::{Isometry, Point, Real, Vector};
+use cdl::query::details::{
+    IntersectionCompositeShapeShapeBestFirstVisitor,
+    NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
+    PointCompositeShapeProjWithFeatureBestFirstVisitor,
+    RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor,
+    TOICompositeShapeShapeBestFirstVisitor,
+};
+use cdl::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
+use cdl::shape::{FeatureId, Shape, TypedSimdCompositeShape};
+use std::sync::Arc;
 
 /// A pipeline for performing queries on all the colliders of a scene.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub struct QueryPipeline {
+    #[cfg_attr(
+        feature = "serde-serialize",
+        serde(skip, default = "crate::geometry::default_query_dispatcher")
+    )]
+    query_dispatcher: Arc<dyn QueryDispatcher>,
     quadtree: SimdQuadTree<ColliderHandle>,
     tree_built: bool,
-    dilation_factor: f32,
+    dilation_factor: Real,
+}
+
+struct QueryPipelineAsCompositeShape<'a> {
+    query_pipeline: &'a QueryPipeline,
+    colliders: &'a ColliderSet,
+    groups: InteractionGroups,
+}
+
+impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
+    type PartShape = dyn Shape;
+    type PartId = ColliderHandle;
+
+    fn map_typed_part_at(
+        &self,
+        shape_id: Self::PartId,
+        mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
+    ) {
+        if let Some(collider) = self.colliders.get(shape_id) {
+            if collider.collision_groups.test(self.groups) {
+                f(Some(collider.position()), collider.shape())
+            }
+        }
+    }
+
+    fn map_untyped_part_at(
+        &self,
+        shape_id: Self::PartId,
+        mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
+    ) {
+        self.map_typed_part_at(shape_id, f);
+    }
+
+    fn typed_quadtree(&self) -> &SimdQuadTree<ColliderHandle> {
+        &self.query_pipeline.quadtree
+    }
 }
 
 impl Default for QueryPipeline {
@@ -21,7 +74,27 @@ impl Default for QueryPipeline {
 impl QueryPipeline {
     /// Initializes an empty query pipeline.
     pub fn new() -> Self {
+        Self::with_query_dispatcher(DefaultQueryDispatcher)
+    }
+
+    fn as_composite_shape<'a>(
+        &'a self,
+        colliders: &'a ColliderSet,
+        groups: InteractionGroups,
+    ) -> QueryPipelineAsCompositeShape<'a> {
+        QueryPipelineAsCompositeShape {
+            query_pipeline: self,
+            colliders,
+            groups,
+        }
+    }
+
+    pub fn with_query_dispatcher<D>(d: D) -> Self
+    where
+        D: 'static + QueryDispatcher,
+    {
         Self {
+            query_dispatcher: Arc::new(d),
             quadtree: SimdQuadTree::new(),
             tree_built: false,
             dilation_factor: 0.01,
@@ -59,40 +132,46 @@ impl QueryPipeline {
     /// - `position`: the position of this shape.
     /// - `ray`: the ray to cast.
     /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
-    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
-    pub fn cast_ray<'a>(
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
+    pub fn cast_ray(
         &self,
-        colliders: &'a ColliderSet,
+        colliders: &ColliderSet,
         ray: &Ray,
-        max_toi: f32,
+        max_toi: Real,
+        solid: bool,
         groups: InteractionGroups,
-    ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
-        // TODO: avoid allocation?
-        let mut inter = Vec::new();
-        self.quadtree.cast_ray(ray, max_toi, &mut inter);
+    ) -> Option<(ColliderHandle, Real)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor =
+            RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
 
-        let mut best = f32::MAX;
-        let mut result = None;
+        self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
+    }
 
-        for handle in inter {
-            if let Some(collider) = colliders.get(handle) {
-                if collider.collision_groups.test(groups) {
-                    if let Some(inter) = collider.shape().cast_ray_and_get_normal(
-                        collider.position(),
-                        ray,
-                        max_toi,
-                        true,
-                    ) {
-                        if inter.toi < best {
-                            best = inter.toi;
-                            result = Some((handle, collider, inter));
-                        }
-                    }
-                }
-            }
-        }
+    /// Find the closest intersection between a ray and a set of collider.
+    ///
+    /// # Parameters
+    /// - `position`: the position of this shape.
+    /// - `ray`: the ray to cast.
+    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
+    pub fn cast_ray_and_get_normal(
+        &self,
+        colliders: &ColliderSet,
+        ray: &Ray,
+        max_toi: Real,
+        solid: bool,
+        groups: InteractionGroups,
+    ) -> Option<(ColliderHandle, RayIntersection)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
+            &pipeline_shape,
+            ray,
+            max_toi,
+            solid,
+        );
 
-        result
+        self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
     }
 
     /// Find the all intersections between a ray and a set of collider and passes them to a callback.
@@ -101,7 +180,7 @@ impl QueryPipeline {
     /// - `position`: the position of this shape.
     /// - `ray`: the ray to cast.
     /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
-    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
     /// - `callback`: function executed on each collider for which a ray intersection has been found.
     ///   There is no guarantees on the order the results will be yielded. If this callback returns `false`,
     ///   this method will exit early, ignory any further raycast.
@@ -109,7 +188,7 @@ impl QueryPipeline {
         &self,
         colliders: &'a ColliderSet,
         ray: &Ray,
-        max_toi: f32,
+        max_toi: Real,
         groups: InteractionGroups,
         mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
     ) {
@@ -135,18 +214,106 @@ impl QueryPipeline {
         }
     }
 
-    /*
+    /// Find up to one collider intersecting the given shape.
+    fn intersection_with_shape(
+        &self,
+        colliders: &ColliderSet,
+        shape_pos: &Isometry<Real>,
+        shape: &dyn Shape,
+        groups: InteractionGroups,
+    ) -> Option<ColliderHandle> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
+            &*self.query_dispatcher,
+            shape_pos,
+            &pipeline_shape,
+            shape,
+        );
+
+        self.quadtree
+            .traverse_best_first(&mut visitor)
+            .map(|h| (h.1 .0))
+    }
+
+    // TODO: intersections_with_point (collect all colliders containing the point).
+
+    /// Projects a point on the scene.
+    fn project_point(
+        &self,
+        colliders: &ColliderSet,
+        point: &Point<Real>,
+        solid: bool,
+        groups: InteractionGroups,
+    ) -> Option<(ColliderHandle, PointProjection)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor =
+            PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
+
+        self.quadtree
+            .traverse_best_first(&mut visitor)
+            .map(|h| (h.1 .1, h.1 .0))
+    }
+
+    /// Projects a point on the scene and get
+    fn project_point_and_get_feature(
+        &self,
+        colliders: &ColliderSet,
+        point: &Point<Real>,
+        groups: InteractionGroups,
+    ) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor =
+            PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
+        self.quadtree
+            .traverse_best_first(&mut visitor)
+            .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
+    }
+
     pub fn cast_shape<'a>(
         &self,
         colliders: &'a ColliderSet,
         shape_pos: &Isometry<Real>,
+        shape_vel: &Vector<Real>,
         shape: &dyn Shape,
-        max_toi: f32,
+        max_toi: Real,
+        target_distance: Real,
         groups: InteractionGroups,
-    ) -> Option<(ColliderHandle, &'a Collider, TOI)> {
-        unimplemented!()
+    ) -> Option<(ColliderHandle, TOI)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
+            &*self.query_dispatcher,
+            shape_pos,
+            shape_vel,
+            &pipeline_shape,
+            shape,
+            max_toi,
+            target_distance,
+        );
+        self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
+    }
+
+    pub fn nonlinear_cast_shape(
+        &self,
+        colliders: &ColliderSet,
+        shape_motion: &dyn RigidMotion,
+        shape: &dyn Shape,
+        max_toi: Real,
+        target_distance: Real,
+        groups: InteractionGroups,
+    ) -> Option<(ColliderHandle, TOI)> {
+        let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
+            &*self.query_dispatcher,
+            shape_motion,
+            &pipeline_shape,
+            shape,
+            max_toi,
+            target_distance,
+        );
+        self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
     }
 
+    /*
     /// Gets all the colliders with a shape intersecting the given `shape`.
     pub fn intersections_with_shape<'a>(
         &self,