Add more options for filtering colliders in scene queries.

2 files changed, 226 insertions, 50 deletions

diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index fb14e4c..bac67d2 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -4,7 +4,7 @@ pub use collision_pipeline::CollisionPipeline;
 pub use event_handler::{ActiveEvents, ChannelEventCollector, EventHandler};
 pub use physics_hooks::{ActiveHooks, ContactModificationContext, PairFilterContext, PhysicsHooks};
 pub use physics_pipeline::PhysicsPipeline;
-pub use query_pipeline::{QueryPipeline, QueryPipelineMode};
+pub use query_pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline, QueryPipelineMode};
 
 #[cfg(feature = "debug-render")]
 pub use self::debug_render_pipeline::{
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 1149a72..174a523 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,6 +1,6 @@
-use crate::dynamics::IslandManager;
+use crate::dynamics::{IslandManager, RigidBodyHandle};
 use crate::geometry::{
-    ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
+    Collider, ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
 };
 use crate::math::{Isometry, Point, Real, Vector};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
@@ -35,9 +35,194 @@ pub struct QueryPipeline {
 
 struct QueryPipelineAsCompositeShape<'a> {
     query_pipeline: &'a QueryPipeline,
+    bodies: &'a RigidBodySet,
     colliders: &'a ColliderSet,
-    query_groups: InteractionGroups,
-    filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
+    filter: QueryFilter<'a>,
+}
+
+bitflags::bitflags! {
+    #[derive(Default)]
+    /// Flags for excluding whole sets of colliders from a scene query.
+    pub struct QueryFilterFlags: u32 {
+        /// Exclude from the query any collider attached to a fixed rigid-body and colliders with no rigid-body attached.
+        const EXCLUDE_FIXED = 1 << 1;
+        /// Exclude from the query any collider attached to a dynamic rigid-body.
+        const EXCLUDE_KINEMATIC = 1 << 2;
+        /// Exclude from the query any collider attached to a kinematic rigid-body.
+        const EXCLUDE_DYNAMIC = 1 << 3;
+        /// Exclude from the query any collider that is a sensor.
+        const EXCLUDE_SENSORS = 1 << 4;
+        /// Exclude from the query any collider that is not a sensor.
+        const EXCLUDE_SOLIDS = 1 << 5;
+        /// Excludes all colliders not attached to a dynamic rigid-body.
+        const ONLY_DYNAMIC = Self::EXCLUDE_FIXED.bits | Self::EXCLUDE_KINEMATIC.bits;
+        /// Excludes all colliders not attached to a kinematic rigid-body.
+        const ONLY_KINEMATIC = Self::EXCLUDE_DYNAMIC.bits | Self::EXCLUDE_FIXED.bits;
+        /// Exclude all colliders attached to a non-fixed rigid-body
+        /// (this will not exclude colliders not attached to any rigid-body).
+        const ONLY_FIXED = Self::EXCLUDE_DYNAMIC.bits | Self::EXCLUDE_KINEMATIC.bits;
+    }
+}
+
+impl QueryFilterFlags {
+    /// Tests if the given collider should be taken into account by a scene query, based
+    /// on the flags on `self`.
+    #[inline]
+    pub fn test(&self, bodies: &RigidBodySet, collider: &Collider) -> bool {
+        if self.is_empty() {
+            // No filter.
+            return true;
+        }
+
+        if (self.contains(QueryFilterFlags::EXCLUDE_SENSORS) && collider.is_sensor())
+            || (self.contains(QueryFilterFlags::EXCLUDE_SOLIDS) && !collider.is_sensor())
+        {
+            return false;
+        }
+
+        if self.contains(QueryFilterFlags::EXCLUDE_FIXED) && collider.parent.is_none() {
+            return false;
+        }
+
+        if let Some(parent) = collider.parent.and_then(|p| bodies.get(p.handle)) {
+            let parent_type = parent.body_type();
+
+            if (self.contains(QueryFilterFlags::EXCLUDE_FIXED) && parent_type.is_fixed())
+                || (self.contains(QueryFilterFlags::EXCLUDE_KINEMATIC)
+                    && parent_type.is_kinematic())
+                || (self.contains(QueryFilterFlags::EXCLUDE_DYNAMIC) && parent_type.is_dynamic())
+            {
+                return false;
+            }
+        }
+
+        true
+    }
+}
+
+/// A filter tha describes what collider should be included or excluded from a scene query.
+#[derive(Copy, Clone, Default)]
+pub struct QueryFilter<'a> {
+    /// Flags indicating what particular type of colliders should be exclude.
+    pub flags: QueryFilterFlags,
+    /// If set, only colliders with collision groups compatible with this one will
+    /// be included in the scene query.
+    pub groups: Option<InteractionGroups>,
+    /// If set, this collider will be excluded by the query.
+    pub exclude_collider: Option<ColliderHandle>,
+    /// If set, any collider attached to this rigid-body will be exclude by the query.
+    pub exclude_rigid_body: Option<RigidBodyHandle>,
+    /// If set, any collider for which this closure returns false
+    pub predicate: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>,
+}
+
+impl<'a> QueryFilter<'a> {
+    /// Applies the filters described by `self` to a collider to determine if it has to be
+    /// included in a scene query (`true`) or not (`false`).
+    #[inline]
+    pub fn test(&self, bodies: &RigidBodySet, handle: ColliderHandle, collider: &Collider) -> bool {
+        self.exclude_collider != Some(handle)
+            && self.exclude_rigid_body != collider.parent.map(|p| p.handle)
+            && self
+                .groups
+                .map(|grps| collider.flags.collision_groups.test(grps))
+                .unwrap_or(true)
+            && self.flags.test(bodies, collider)
+            && self.predicate.map(|f| f(handle, collider)).unwrap_or(true)
+    }
+}
+
+impl<'a> From<QueryFilterFlags> for QueryFilter<'a> {
+    fn from(flags: QueryFilterFlags) -> Self {
+        Self {
+            flags,
+            ..QueryFilter::default()
+        }
+    }
+}
+
+impl<'a> From<InteractionGroups> for QueryFilter<'a> {
+    fn from(groups: InteractionGroups) -> Self {
+        Self {
+            groups: Some(groups),
+            ..QueryFilter::default()
+        }
+    }
+}
+
+impl<'a> QueryFilter<'a> {
+    /// A query filter that doesn’t exclude any collider.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Exclude from the query any collider attached to a fixed rigid-body and colliders with no rigid-body attached.
+    pub fn exclude_fixed() -> Self {
+        QueryFilterFlags::EXCLUDE_FIXED.into()
+    }
+
+    /// Exclude from the query any collider attached to a dynamic rigid-body.
+    pub fn exclude_kinematic() -> Self {
+        QueryFilterFlags::EXCLUDE_KINEMATIC.into()
+    }
+
+    /// Exclude from the query any collider attached to a kinematic rigid-body.
+    pub fn exclude_dynamic(self) -> Self {
+        QueryFilterFlags::EXCLUDE_DYNAMIC.into()
+    }
+
+    /// Excludes all colliders not attached to a dynamic rigid-body.
+    pub fn only_dynamic() -> Self {
+        QueryFilterFlags::ONLY_DYNAMIC.into()
+    }
+
+    /// Excludes all colliders not attached to a kinematic rigid-body.
+    pub fn only_kinematic() -> Self {
+        QueryFilterFlags::ONLY_KINEMATIC.into()
+    }
+
+    /// Exclude all colliders attached to a non-fixed rigid-body
+    /// (this will not exclude colliders not attached to any rigid-body).
+    pub fn only_fixed() -> Self {
+        QueryFilterFlags::ONLY_FIXED.into()
+    }
+
+    /// Exclude from the query any collider that is a sensor.
+    pub fn exclude_sensors(mut self) -> Self {
+        self.flags |= QueryFilterFlags::EXCLUDE_SENSORS;
+        self
+    }
+
+    /// Exclude from the query any collider that is not a sensor.
+    pub fn exclude_solids(mut self) -> Self {
+        self.flags |= QueryFilterFlags::EXCLUDE_SOLIDS;
+        self
+    }
+
+    /// Only colliders with collision groups compatible with this one will
+    /// be included in the scene query.
+    pub fn groups(mut self, groups: InteractionGroups) -> Self {
+        self.groups = Some(groups);
+        self
+    }
+
+    /// Set the collider that will be excluded from the scene query.
+    pub fn exclude_collider(mut self, collider: ColliderHandle) -> Self {
+        self.exclude_collider = Some(collider);
+        self
+    }
+
+    /// Set the rigid-body that will be excluded from the scene query.
+    pub fn exclude_rigid_body(mut self, rigid_body: RigidBodyHandle) -> Self {
+        self.exclude_rigid_body = Some(rigid_body);
+        self
+    }
+
+    /// Set the predicate to apply a custom collider filtering during the scene query.
+    pub fn predicate(mut self, predicate: &'a impl Fn(ColliderHandle, &Collider) -> bool) -> Self {
+        self.predicate = Some(predicate);
+        self
+    }
 }
 
 /// Indicates how the colliders position should be taken into account when
@@ -66,9 +251,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
         mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
     ) {
         if let Some(co) = self.colliders.get(shape_id) {
-            if co.flags.collision_groups.test(self.query_groups)
-                && self.filter.map(|f| f(shape_id)).unwrap_or(true)
-            {
+            if self.filter.test(self.bodies, shape_id, co) {
                 f(Some(&co.pos), &*co.shape)
             }
         }
@@ -101,14 +284,14 @@ impl QueryPipeline {
 
     fn as_composite_shape<'a>(
         &'a self,
+        bodies: &'a RigidBodySet,
         colliders: &'a ColliderSet,
-        query_groups: InteractionGroups,
-        filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter<'a>,
     ) -> QueryPipelineAsCompositeShape<'a> {
         QueryPipelineAsCompositeShape {
             query_pipeline: self,
+            bodies,
             colliders,
-            query_groups,
             filter,
         }
     }
@@ -290,14 +473,14 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn cast_ray(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, Real)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor =
             RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
 
@@ -321,14 +504,14 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn cast_ray_and_get_normal(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, RayIntersection)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
             &pipeline_shape,
             ray,
@@ -359,19 +542,17 @@ impl QueryPipeline {
     ///               this method will exit early, ignore any further raycast.
     pub fn intersections_with_ray<'a>(
         &self,
+        bodies: &'a RigidBodySet,
         colliders: &'a ColliderSet,
         ray: &Ray,
         max_toi: Real,
         solid: bool,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
         mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
     ) {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
             if let Some(co) = colliders.get(*handle) {
-                if co.flags.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle)).unwrap_or(true)
-                {
+                if filter.test(bodies, *handle, co) {
                     if let Some(hit) = co
                         .shape
                         .cast_ray_and_get_normal(&co.pos, ray, max_toi, solid)
@@ -401,13 +582,13 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn intersection_with_shape(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         shape_pos: &Isometry<Real>,
         shape: &dyn Shape,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<ColliderHandle> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
             shape_pos,
@@ -437,13 +618,13 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn project_point(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         point: &Point<Real>,
         solid: bool,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, PointProjection)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor =
             PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
 
@@ -464,20 +645,17 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     /// * `callback` - A function called with each collider with a shape
     ///                containing the `point`.
-    pub fn intersections_with_point<'a>(
+    pub fn intersections_with_point(
         &self,
-        colliders: &'a ColliderSet,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         point: &Point<Real>,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
         mut callback: impl FnMut(ColliderHandle) -> bool,
     ) {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
             if let Some(co) = colliders.get(*handle) {
-                if co.flags.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle)).unwrap_or(true)
-                    && co.shape.contains_point(&co.pos, point)
-                {
+                if filter.test(bodies, *handle, co) && co.shape.contains_point(&co.pos, point) {
                     return callback(*handle);
                 }
             }
@@ -509,12 +687,12 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn project_point_and_get_feature(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         point: &Point<Real>,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor =
             PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
         self.qbvh
@@ -552,15 +730,15 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn cast_shape<'a>(
         &self,
+        bodies: &RigidBodySet,
         colliders: &'a ColliderSet,
         shape_pos: &Isometry<Real>,
         shape_vel: &Vector<Real>,
         shape: &dyn Shape,
         max_toi: Real,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, TOI)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
             shape_pos,
@@ -597,16 +775,16 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     pub fn nonlinear_cast_shape(
         &self,
+        bodies: &RigidBodySet,
         colliders: &ColliderSet,
         shape_motion: &NonlinearRigidMotion,
         shape: &dyn Shape,
         start_time: Real,
         end_time: Real,
         stop_at_penetration: bool,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
     ) -> Option<(ColliderHandle, TOI)> {
-        let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
+        let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
@@ -636,11 +814,11 @@ impl QueryPipeline {
     /// * `callback` - A function called with the handles of each collider intersecting the `shape`.
     pub fn intersections_with_shape<'a>(
         &self,
+        bodies: &RigidBodySet,
         colliders: &'a ColliderSet,
         shape_pos: &Isometry<Real>,
         shape: &dyn Shape,
-        query_groups: InteractionGroups,
-        filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+        filter: QueryFilter,
         mut callback: impl FnMut(ColliderHandle) -> bool,
     ) {
         let dispatcher = &*self.query_dispatcher;
@@ -648,9 +826,7 @@ impl QueryPipeline {
 
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
             if let Some(co) = colliders.get(*handle) {
-                if co.flags.collision_groups.test(query_groups)
-                    && filter.map(|f| f(*handle)).unwrap_or(true)
-                {
+                if filter.test(bodies, *handle, co) {
                     let pos12 = inv_shape_pos * co.pos.as_ref();
 
                     if dispatcher.intersection_test(&pos12, shape, &*co.shape) == Ok(true) {