From 9367198282f9a30c23cc7ddd3a56db56b7506aae Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Sun, 9 Jun 2024 14:16:03 +0200 Subject: feat: rework QueryPipeline update API to take less parameters (#647) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * chore: rework QueryPipeline API to take a generic qbvh updater This allows to pass less parameters depending on the updating mode. * chore: rework struct and functions names, and docs --------- Co-authored-by: Sébastien Crozet --- src/dynamics/ccd/ccd_solver.rs | 20 +- src/pipeline/mod.rs | 4 +- src/pipeline/query_pipeline.rs | 782 ------------------------------ src/pipeline/query_pipeline/generators.rs | 109 +++++ src/pipeline/query_pipeline/mod.rs | 714 +++++++++++++++++++++++++++ 5 files changed, 836 insertions(+), 793 deletions(-) delete mode 100644 src/pipeline/query_pipeline.rs create mode 100644 src/pipeline/query_pipeline/generators.rs create mode 100644 src/pipeline/query_pipeline/mod.rs (limited to 'src') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 79c4495..eaabf77 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -3,8 +3,8 @@ use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase}; use crate::math::Real; use crate::parry::utils::SortedPair; -use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; -use crate::prelude::{ActiveEvents, CollisionEventFlags}; +use crate::pipeline::{EventHandler, QueryPipeline}; +use crate::prelude::{query_pipeline_generators, ActiveEvents, CollisionEventFlags}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -117,10 +117,12 @@ impl CCDSolver { narrow_phase: &NarrowPhase, ) -> Option { // Update the query pipeline. - self.query_pipeline.update_with_mode( - bodies, - colliders, - QueryPipelineMode::SweepTestWithPredictedPosition { dt }, + self.query_pipeline.update_with_generator( + query_pipeline_generators::SweptAabbWithPredictedPosition { + bodies, + colliders, + dt, + }, ); let mut pairs_seen = HashMap::default(); @@ -238,10 +240,8 @@ impl CCDSolver { let mut min_overstep = dt; // Update the query pipeline. - self.query_pipeline.update_with_mode( - bodies, - colliders, - QueryPipelineMode::SweepTestWithNextPosition, + self.query_pipeline.update_with_generator( + query_pipeline_generators::SweptAabbWithNextPosition { bodies, colliders }, ); /* diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index bac67d2..d32d342 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -4,7 +4,9 @@ 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::{QueryFilter, QueryFilterFlags, QueryPipeline, QueryPipelineMode}; +pub use query_pipeline::{ + generators as query_pipeline_generators, QueryFilter, QueryFilterFlags, QueryPipeline, +}; #[cfg(feature = "debug-render")] pub use self::debug_render_pipeline::{ diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs deleted file mode 100644 index f202107..0000000 --- a/src/pipeline/query_pipeline.rs +++ /dev/null @@ -1,782 +0,0 @@ -use crate::dynamics::RigidBodyHandle; -use crate::geometry::{ - Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection, -}; -use crate::math::{Isometry, Point, Real, Vector}; -use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; -use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace}; -use parry::query::details::{ - NonlinearTOICompositeShapeShapeBestFirstVisitor, NormalConstraints, - PointCompositeShapeProjBestFirstVisitor, PointCompositeShapeProjWithFeatureBestFirstVisitor, - RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor, - ShapeCastOptions, TOICompositeShapeShapeBestFirstVisitor, -}; -use parry::query::visitors::{ - BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor, -}; -use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit}; -use parry::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, - qbvh: Qbvh, - dilation_factor: Real, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - workspace: QbvhUpdateWorkspace, -} - -struct QueryPipelineAsCompositeShape<'a> { - query_pipeline: &'a QueryPipeline, - bodies: &'a RigidBodySet, - colliders: &'a ColliderSet, - 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 kinematic rigid-body. - const EXCLUDE_KINEMATIC = 1 << 2; - /// Exclude from the query any collider attached to a dynamic 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 that 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 excluded from the scene query. - pub flags: QueryFilterFlags, - /// If set, only colliders with collision groups compatible with this one will - /// be included in the scene query. - pub groups: Option, - /// If set, this collider will be excluded from the scene query. - pub exclude_collider: Option, - /// If set, any collider attached to this rigid-body will be excluded from the scene query. - pub exclude_rigid_body: Option, - /// If set, any collider for which this closure returns false will be excluded from the scene query. - #[allow(clippy::type_complexity)] // Type doesn’t look really complex? - 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.is_none() // NOTE: deal with the `None` case separately otherwise the next test is incorrect if the collider’s parent is `None` too. - || 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 for QueryFilter<'a> { - fn from(flags: QueryFilterFlags) -> Self { - Self { - flags, - ..QueryFilter::default() - } - } -} - -impl<'a> From 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 kinematic rigid-body. - pub fn exclude_kinematic() -> Self { - QueryFilterFlags::EXCLUDE_KINEMATIC.into() - } - - /// Exclude from the query any collider attached to a dynamic rigid-body. - pub fn exclude_dynamic() -> 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 -/// updating the query pipeline. -pub enum QueryPipelineMode { - /// The `Collider::position` is taken into account. - CurrentPosition, - /// The `RigidBody::next_position * Collider::position_wrt_parent` is taken into account for - /// the colliders positions. - SweepTestWithNextPosition, - /// The `RigidBody::predict_position_using_velocity_and_forces * Collider::position_wrt_parent` - /// is taken into account for the colliders position. - SweepTestWithPredictedPosition { - /// The time used to integrate the rigid-body's velocity and acceleration. - dt: Real, - }, -} - -impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { - type PartShape = dyn Shape; - type PartNormalConstraints = dyn NormalConstraints; - type PartId = ColliderHandle; - - fn map_typed_part_at( - &self, - shape_id: Self::PartId, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ), - ) { - if let Some(co) = self.colliders.get(shape_id) { - if self.filter.test(self.bodies, shape_id, co) { - f(Some(&co.pos), &*co.shape, None) - } - } - } - - fn map_untyped_part_at( - &self, - shape_id: Self::PartId, - f: impl FnMut(Option<&Isometry>, &Self::PartShape, Option<&dyn NormalConstraints>), - ) { - self.map_typed_part_at(shape_id, f); - } - - fn typed_qbvh(&self) -> &Qbvh { - &self.query_pipeline.qbvh - } -} - -impl Default for QueryPipeline { - fn default() -> Self { - Self::new() - } -} - -impl QueryPipeline { - /// Initializes an empty query pipeline. - pub fn new() -> Self { - Self::with_query_dispatcher(DefaultQueryDispatcher) - } - - fn as_composite_shape<'a>( - &'a self, - bodies: &'a RigidBodySet, - colliders: &'a ColliderSet, - filter: QueryFilter<'a>, - ) -> QueryPipelineAsCompositeShape<'a> { - QueryPipelineAsCompositeShape { - query_pipeline: self, - bodies, - colliders, - filter, - } - } - - /// Initializes an empty query pipeline with a custom `QueryDispatcher`. - /// - /// Use this constructor in order to use a custom `QueryDispatcher` that is - /// aware of your own user-defined shapes. - pub fn with_query_dispatcher(d: D) -> Self - where - D: 'static + QueryDispatcher, - { - Self { - query_dispatcher: Arc::new(d), - qbvh: Qbvh::new(), - dilation_factor: 0.01, - workspace: QbvhUpdateWorkspace::default(), - } - } - - /// The query dispatcher used by this query pipeline for running scene queries. - pub fn query_dispatcher(&self) -> &dyn QueryDispatcher { - &*self.query_dispatcher - } - - /// Update the query pipeline incrementally, avoiding a complete rebuild of its - /// internal data-structure. - pub fn update_incremental( - &mut self, - colliders: &ColliderSet, - modified_colliders: &[ColliderHandle], - removed_colliders: &[ColliderHandle], - refit_and_rebalance: bool, - ) { - // We remove first. This is needed to avoid the ABA problem: if a collider was removed - // and another added right after with the same handle index, we can remove first, and - // then update the new one (but only if its actually exists, to address the case where - // a collider was added/modified and then removed during the same frame). - for removed in removed_colliders { - self.qbvh.remove(*removed); - } - - for modified in modified_colliders { - // Check that the collider still exists as it may have been removed. - if colliders.contains(*modified) { - self.qbvh.pre_update_or_insert(*modified); - } - } - - if refit_and_rebalance { - let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { - colliders[*handle].compute_aabb() - }); - self.qbvh.rebalance(0.0, &mut self.workspace); - } - } - - /// Update the acceleration structure on the query pipeline. - pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { - self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition) - } - - /// Update the acceleration structure on the query pipeline. - pub fn update_with_mode( - &mut self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - mode: QueryPipelineMode, - ) { - struct DataGenerator<'a> { - bodies: &'a RigidBodySet, - colliders: &'a ColliderSet, - mode: QueryPipelineMode, - } - - impl<'a> QbvhDataGenerator for DataGenerator<'a> { - fn size_hint(&self) -> usize { - self.colliders.len() - } - - #[inline(always)] - fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { - match self.mode { - QueryPipelineMode::CurrentPosition => { - for (h, co) in self.colliders.iter_enabled() { - f(h, co.shape.compute_aabb(&co.pos)) - } - } - QueryPipelineMode::SweepTestWithNextPosition => { - for (h, co) in self.colliders.iter_enabled() { - if let Some(co_parent) = co.parent { - let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; - let next_position = rb_next_pos * co_parent.pos_wrt_parent; - f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) - } else { - f(h, co.shape.compute_aabb(&co.pos)) - } - } - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - for (h, co) in self.colliders.iter_enabled() { - if let Some(co_parent) = co.parent { - let rb = &self.bodies[co_parent.handle]; - let predicted_pos = rb.pos.integrate_forces_and_velocities( - dt, &rb.forces, &rb.vels, &rb.mprops, - ); - - let next_position = predicted_pos * co_parent.pos_wrt_parent; - f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) - } else { - f(h, co.shape.compute_aabb(&co.pos)) - } - } - } - } - } - } - - let generator = DataGenerator { - bodies, - colliders, - mode, - }; - self.qbvh.clear_and_rebuild(generator, self.dilation_factor); - } - - /// Find the closest intersection between a ray and a set of collider. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `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. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn cast_ray( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - ray: &Ray, - max_toi: Real, - solid: bool, - filter: QueryFilter, - ) -> Option<(ColliderHandle, Real)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let mut visitor = - RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); - - self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1) - } - - /// Find the closest intersection between a ray and a set of collider. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `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. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn cast_ray_and_get_normal( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - ray: &Ray, - max_toi: Real, - solid: bool, - filter: QueryFilter, - ) -> Option<(ColliderHandle, RayIntersection)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( - &pipeline_shape, - ray, - max_toi, - solid, - ); - - self.qbvh.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. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `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. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `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, ignore any further raycast. - pub fn intersections_with_ray<'a>( - &self, - bodies: &'a RigidBodySet, - colliders: &'a ColliderSet, - ray: &Ray, - max_toi: Real, - solid: 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 filter.test(bodies, *handle, co) { - if let Some(hit) = co - .shape - .cast_ray_and_get_normal(&co.pos, ray, max_toi, solid) - { - return callback(*handle, hit); - } - } - } - - true - }; - - let mut visitor = RayIntersectionsVisitor::new(ray, max_toi, &mut leaf_callback); - self.qbvh.traverse_depth_first(&mut visitor); - } - - /// Gets the handle of up to one collider intersecting the given shape. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `shape_pos` - The position of the shape used for the intersection test. - /// * `shape` - The shape used for the intersection test. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn intersection_with_shape( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - shape_pos: &Isometry, - shape: &dyn Shape, - filter: QueryFilter, - ) -> Option { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - #[allow(deprecated)] - // TODO: replace this with IntersectionCompositeShapeShapeVisitor when it - // can return the shape part id. - let mut visitor = - parry::query::details::IntersectionCompositeShapeShapeBestFirstVisitor::new( - &*self.query_dispatcher, - shape_pos, - &pipeline_shape, - shape, - ); - - self.qbvh - .traverse_best_first(&mut visitor) - .map(|h| (h.1 .0)) - } - - /// Find the projection of a point on the closest collider. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `point` - The point to project. - /// * `solid` - If this is set to `true` then the collider shapes are considered to - /// be plain (if the point is located inside of a plain shape, its projection is the point - /// itself). If it is set to `false` the collider shapes are considered to be hollow - /// (if the point is located inside of an hollow shape, it is projected on the shape's - /// boundary). - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn project_point( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - point: &Point, - solid: bool, - filter: QueryFilter, - ) -> Option<(ColliderHandle, PointProjection)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let mut visitor = - PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); - - self.qbvh - .traverse_best_first(&mut visitor) - .map(|h| (h.1 .1, h.1 .0)) - } - - /// Find all the colliders containing the given point. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `point` - The point used for the containment test. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `callback` - A function called with each collider with a shape - /// containing the `point`. - pub fn intersections_with_point( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - point: &Point, - filter: QueryFilter, - mut callback: impl FnMut(ColliderHandle) -> bool, - ) { - let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(co) = colliders.get(*handle) { - if filter.test(bodies, *handle, co) && co.shape.contains_point(&co.pos, point) { - return callback(*handle); - } - } - - true - }; - - let mut visitor = PointIntersectionsVisitor::new(point, &mut leaf_callback); - - self.qbvh.traverse_depth_first(&mut visitor); - } - - /// Find the projection of a point on the closest collider. - /// - /// The results include the ID of the feature hit by the point. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `point` - The point to project. - /// * `solid` - If this is set to `true` then the collider shapes are considered to - /// be plain (if the point is located inside of a plain shape, its projection is the point - /// itself). If it is set to `false` the collider shapes are considered to be hollow - /// (if the point is located inside of an hollow shape, it is projected on the shape's - /// boundary). - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn project_point_and_get_feature( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - point: &Point, - filter: QueryFilter, - ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let mut visitor = - PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); - self.qbvh - .traverse_best_first(&mut visitor) - .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.qbvh.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 just a - /// point (the ray origin). In the resulting `TOI`, witness and normal 1 refer to the world - /// collider, and are in world space. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `shape_pos` - The initial position of the shape to cast. - /// * `shape_vel` - The constant velocity of the shape to cast (i.e. the cast direction). - /// * `shape` - The shape to cast. - /// * `max_toi` - The maximum time-of-impact that can be reported by this cast. This effectively - /// limits the distance traveled by the shape to `shapeVel.norm() * maxToi`. - /// * `stop_at_penetration` - If set to `false`, the linear shape-cast won’t immediately stop if - /// the shape is penetrating another shape at its starting point **and** its trajectory is such - /// that it’s on a path to exist that penetration state. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn cast_shape( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - shape_pos: &Isometry, - shape_vel: &Vector, - shape: &dyn Shape, - options: ShapeCastOptions, - filter: QueryFilter, - ) -> Option<(ColliderHandle, ShapeCastHit)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( - &*self.query_dispatcher, - shape_pos, - shape_vel, - &pipeline_shape, - shape, - options, - ); - self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1) - } - - /// Casts a shape with an arbitrary continuous motion and retrieve the first collider it hits. - /// - /// In the resulting `TOI`, witness and normal 1 refer to the world collider, and are in world - /// space. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `shape_motion` - The motion of the shape. - /// * `shape` - The shape to cast. - /// * `start_time` - The starting time of the interval where the motion takes place. - /// * `end_time` - The end time of the interval where the motion takes place. - /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any - /// collider, two results are possible. If `stop_at_penetration` is `true` then, the - /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` - /// then the nonlinear shape-casting will see if further motion with respect to the penetration normal - /// would result in tunnelling. If it does not (i.e. we have a separating velocity along - /// that normal) then the nonlinear shape-casting will attempt to find another impact, - /// at a time `> start_time` that could result in tunnelling. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - 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, - filter: QueryFilter, - ) -> Option<(ColliderHandle, ShapeCastHit)> { - let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); - let pipeline_motion = NonlinearRigidMotion::identity(); - let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( - &*self.query_dispatcher, - &pipeline_motion, - &pipeline_shape, - shape_motion, - shape, - start_time, - end_time, - stop_at_penetration, - ); - self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1) - } - - /// Retrieve all the colliders intersecting the given shape. - /// - /// # Parameters - /// * `colliders` - The set of colliders taking part in this pipeline. - /// * `shapePos` - The position of the shape to test. - /// * `shapeRot` - The orientation of the shape to test. - /// * `shape` - The shape to test. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `callback` - A function called with the handles of each collider intersecting the `shape`. - pub fn intersections_with_shape( - &self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - shape_pos: &Isometry, - shape: &dyn Shape, - filter: QueryFilter, - mut callback: impl FnMut(ColliderHandle) -> bool, - ) { - let dispatcher = &*self.query_dispatcher; - let inv_shape_pos = shape_pos.inverse(); - - let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(co) = colliders.get(*handle) { - 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) { - return callback(*handle); - } - } - } - - true - }; - - let shape_aabb = shape.compute_aabb(shape_pos); - let mut visitor = BoundingVolumeIntersectionsVisitor::new(&shape_aabb, &mut leaf_callback); - - self.qbvh.traverse_depth_first(&mut visitor); - } -} diff --git a/src/pipeline/query_pipeline/generators.rs b/src/pipeline/query_pipeline/generators.rs new file mode 100644 index 0000000..5b86d6a --- /dev/null +++ b/src/pipeline/query_pipeline/generators.rs @@ -0,0 +1,109 @@ +//! Structs implementing [`QbvhDataGenerator`] to be used with [`QueryPipeline::update_with_generator`]. + +use parry::partitioning::QbvhDataGenerator; + +use crate::math::Real; +use crate::prelude::{Aabb, ColliderHandle, ColliderSet, RigidBodySet}; + +#[cfg(doc)] +use crate::{ + dynamics::{IntegrationParameters, RigidBodyPosition}, + pipeline::QueryPipeline, +}; + +/// Generates collider AABBs based on the union of their current AABB and the AABB predicted +/// from the velocity and forces of their parent rigid-body. +/// +/// The main purpose of this struct is to be passed as a parameters to +/// [`QueryPipeline::update_with_generator`] to update the [`QueryPipeline`]. +/// +/// The predicted position is calculated as +/// `RigidBody::predict_position_using_velocity_and_forces * Collider::position_wrt_parent`. +pub struct SweptAabbWithPredictedPosition<'a> { + /// The rigid bodies of your simulation. + pub bodies: &'a RigidBodySet, + /// The colliders of your simulation. + pub colliders: &'a ColliderSet, + /// The delta time to compute predicted position. + /// + /// You probably want to set it to [`IntegrationParameter::dt`]. + pub dt: Real, +} +impl<'a> QbvhDataGenerator for SweptAabbWithPredictedPosition<'a> { + fn size_hint(&self) -> usize { + self.colliders.len() + } + + #[inline(always)] + fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { + for (h, co) in self.colliders.iter_enabled() { + if let Some(co_parent) = co.parent { + let rb = &self.bodies[co_parent.handle]; + let predicted_pos = rb + .pos + .integrate_forces_and_velocities(self.dt, &rb.forces, &rb.vels, &rb.mprops); + + let next_position = predicted_pos * co_parent.pos_wrt_parent; + f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) + } else { + f(h, co.shape.compute_aabb(&co.pos)) + } + } + } +} + +/// Generates collider AABBs based on the union of their AABB at their current [`Collider::position`] +/// and the AABB predicted from their parent’s [`RigidBody::next_position`]. +/// +/// The main purpose of this struct is to be passed as a parameters to +/// [`QueryPipeline::update_with_generator`] to update the [`QueryPipeline`]. +/// +/// The predicted position is calculated as +/// `RigidBody::next_position * Collider::position_wrt_parent`. +pub struct SweptAabbWithNextPosition<'a> { + /// The rigid bodies of your simulation. + pub bodies: &'a RigidBodySet, + /// The colliders of your simulation. + pub colliders: &'a ColliderSet, +} + +impl<'a> QbvhDataGenerator for SweptAabbWithNextPosition<'a> { + fn size_hint(&self) -> usize { + self.colliders.len() + } + + #[inline(always)] + fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { + for (h, co) in self.colliders.iter_enabled() { + if let Some(co_parent) = co.parent { + let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; + let next_position = rb_next_pos * co_parent.pos_wrt_parent; + f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) + } else { + f(h, co.shape.compute_aabb(&co.pos)) + } + } + } +} + +/// Generates collider AABBs based on the AABB at their current [`Collider::position`]. +/// +/// The main purpose of this struct is to be passed as a parameters to +/// [`QueryPipeline::update_with_generator`] to update the [`QueryPipeline`]. +pub struct CurrentAabb<'a> { + /// The colliders of your simulation. + pub colliders: &'a ColliderSet, +} + +impl<'a> QbvhDataGenerator for CurrentAabb<'a> { + fn size_hint(&self) -> usize { + self.colliders.len() + } + + #[inline(always)] + fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { + for (h, co) in self.colliders.iter_enabled() { + f(h, co.shape.compute_aabb(&co.pos)) + } + } +} diff --git a/src/pipeline/query_pipeline/mod.rs b/src/pipeline/query_pipeline/mod.rs new file mode 100644 index 0000000..829499f --- /dev/null +++ b/src/pipeline/query_pipeline/mod.rs @@ -0,0 +1,714 @@ +pub mod generators; + +use crate::dynamics::RigidBodyHandle; +use crate::geometry::{ + Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection, +}; +use crate::math::{Isometry, Point, Real, Vector}; +use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; +use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace}; +use parry::query::details::{ + NonlinearTOICompositeShapeShapeBestFirstVisitor, NormalConstraints, + PointCompositeShapeProjBestFirstVisitor, PointCompositeShapeProjWithFeatureBestFirstVisitor, + RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor, + ShapeCastOptions, TOICompositeShapeShapeBestFirstVisitor, +}; +use parry::query::visitors::{ + BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor, +}; +use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit}; +use parry::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, + qbvh: Qbvh, + dilation_factor: Real, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, +} + +struct QueryPipelineAsCompositeShape<'a> { + query_pipeline: &'a QueryPipeline, + bodies: &'a RigidBodySet, + colliders: &'a ColliderSet, + 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 kinematic rigid-body. + const EXCLUDE_KINEMATIC = 1 << 2; + /// Exclude from the query any collider attached to a dynamic 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 that 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 excluded from the scene query. + pub flags: QueryFilterFlags, + /// If set, only colliders with collision groups compatible with this one will + /// be included in the scene query. + pub groups: Option, + /// If set, this collider will be excluded from the scene query. + pub exclude_collider: Option, + /// If set, any collider attached to this rigid-body will be excluded from the scene query. + pub exclude_rigid_body: Option, + /// If set, any collider for which this closure returns false will be excluded from the scene query. + #[allow(clippy::type_complexity)] // Type doesn’t look really complex? + 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.is_none() // NOTE: deal with the `None` case separately otherwise the next test is incorrect if the collider’s parent is `None` too. + || 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 for QueryFilter<'a> { + fn from(flags: QueryFilterFlags) -> Self { + Self { + flags, + ..QueryFilter::default() + } + } +} + +impl<'a> From 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 kinematic rigid-body. + pub fn exclude_kinematic() -> Self { + QueryFilterFlags::EXCLUDE_KINEMATIC.into() + } + + /// Exclude from the query any collider attached to a dynamic rigid-body. + pub fn exclude_dynamic() -> 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 + } +} + +impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { + type PartShape = dyn Shape; + type PartNormalConstraints = dyn NormalConstraints; + type PartId = ColliderHandle; + + fn map_typed_part_at( + &self, + shape_id: Self::PartId, + mut f: impl FnMut( + Option<&Isometry>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ), + ) { + if let Some(co) = self.colliders.get(shape_id) { + if self.filter.test(self.bodies, shape_id, co) { + f(Some(&co.pos), &*co.shape, None) + } + } + } + + fn map_untyped_part_at( + &self, + shape_id: Self::PartId, + f: impl FnMut(Option<&Isometry>, &Self::PartShape, Option<&dyn NormalConstraints>), + ) { + self.map_typed_part_at(shape_id, f); + } + + fn typed_qbvh(&self) -> &Qbvh { + &self.query_pipeline.qbvh + } +} + +impl Default for QueryPipeline { + fn default() -> Self { + Self::new() + } +} + +impl QueryPipeline { + /// Initializes an empty query pipeline. + pub fn new() -> Self { + Self::with_query_dispatcher(DefaultQueryDispatcher) + } + + fn as_composite_shape<'a>( + &'a self, + bodies: &'a RigidBodySet, + colliders: &'a ColliderSet, + filter: QueryFilter<'a>, + ) -> QueryPipelineAsCompositeShape<'a> { + QueryPipelineAsCompositeShape { + query_pipeline: self, + bodies, + colliders, + filter, + } + } + + /// Initializes an empty query pipeline with a custom `QueryDispatcher`. + /// + /// Use this constructor in order to use a custom `QueryDispatcher` that is + /// aware of your own user-defined shapes. + pub fn with_query_dispatcher(d: D) -> Self + where + D: 'static + QueryDispatcher, + { + Self { + query_dispatcher: Arc::new(d), + qbvh: Qbvh::new(), + dilation_factor: 0.01, + workspace: QbvhUpdateWorkspace::default(), + } + } + + /// The query dispatcher used by this query pipeline for running scene queries. + pub fn query_dispatcher(&self) -> &dyn QueryDispatcher { + &*self.query_dispatcher + } + + /// Update the query pipeline incrementally, avoiding a complete rebuild of its + /// internal data-structure. + pub fn update_incremental( + &mut self, + colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + refit_and_rebalance: bool, + ) { + // We remove first. This is needed to avoid the ABA problem: if a collider was removed + // and another added right after with the same handle index, we can remove first, and + // then update the new one (but only if its actually exists, to address the case where + // a collider was added/modified and then removed during the same frame). + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + for modified in modified_colliders { + // Check that the collider still exists as it may have been removed. + if colliders.contains(*modified) { + self.qbvh.pre_update_or_insert(*modified); + } + } + + if refit_and_rebalance { + let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { + colliders[*handle].compute_aabb() + }); + self.qbvh.rebalance(0.0, &mut self.workspace); + } + } + + /// Update the acceleration structure on the query pipeline. + /// + /// Uses [`generators::CurrentAabb`] to update. + pub fn update(&mut self, colliders: &ColliderSet) { + self.update_with_generator(generators::CurrentAabb { colliders }) + } + + /// Update the acceleration structure on the query pipeline using a custom collider bounding + /// volume generator. + /// + /// See [`generators`] for available generators. + pub fn update_with_generator(&mut self, mode: impl QbvhDataGenerator) { + self.qbvh.clear_and_rebuild(mode, self.dilation_factor); + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `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. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn cast_ray( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ray: &Ray, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(ColliderHandle, Real)> { + let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); + let mut visitor = + RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); + + self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1) + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `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. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn cast_ray_and_get_normal( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ray: &Ray, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(ColliderHandle, RayIntersection)> { + let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); + let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( + &pipeline_shape, + ray, + max_toi, + solid, + ); + + self.qbvh.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. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `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. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + /// * `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, ignore any further raycast. + pub fn intersections_with_ray<'a>( + &self, + bodies: &'a RigidBodySet, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: Real, + solid: 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 filter.test(bodies, *handle, co) { + if let Some(hit) = co + .shape + .cast_ray_and_get_normal(&co.pos, ray, max_toi, solid) + { + return callback(*handle, hit); + } + } + } + + true + }; + + let mut visitor = RayIntersectionsVisitor::new(ray, max_toi, &mut leaf_callback); + self.qbvh.traverse_depth_first(&mut visitor); + } + + /// Gets the handle of up to one collider intersecting the given shape. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `shape_pos` - The position of the shape used for the intersection test. + /// * `shape` - The shape used for the intersection test. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn intersection_with_shape( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + shape_pos: &Isometry, + shape: &dyn Shape, + filter: QueryFilter, + ) -> Option { + let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); + #[allow(deprecated)] + // TODO: replace this with IntersectionCompositeShapeShapeVisitor when it + // can return the shape part id. + let mut visitor = + parry::query::details::IntersectionCompositeShapeShapeBestFirstVisitor::new( + &*self.query_dispatcher, + shape_pos, + &pipeline_shape, + shape, + ); + + self.qbvh + .traverse_best_first(&mut visitor) + .map(|h| (h.1 .0)) + } + + /// Find the projection of a point on the closest collider. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `point` - The point to project. + /// * `solid` - If this is set to `true` then the collider shapes are considered to + /// be plain (if the point is located inside of a plain shape, its projection is the point + /// itself). If it is set to `false` the collider shapes are considered to be hollow + /// (if the point is located inside of an hollow shape, it is projected on the shape's + /// boundary). + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn project_point( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + point: &Point, + solid: bool, + filter: QueryFilter, + ) -> Option<(ColliderHandle, PointProjection)> { + let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); + let mut visitor = + PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); + + self.qbvh + .traverse_best_first(&mut visitor) + .map(|h| (h.1 .1, h.1 .0)) + } + + /// Find all the colliders containing the given point. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `point` - The point used for the containment test. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + /// * `callback` - A function called with each collider with a shape + /// containing the `point`. + pub fn intersections_with_point( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + point: &Point, + filter: QueryFilter, + mut callback: impl FnMut(ColliderHandle) -> bool, + ) { + let mut leaf_callback = &mut |handle: &ColliderHandle| { + if let Some(co) = colliders.get(*handle) { + if filter.test(bodies, *handle, co) && co.shape.contains_point(&co.pos, point) { + return callback(*handle); + } + } + + true + }; + + let mut visitor = PointIntersectionsVisitor::new(point, &mut leaf_callback); + + self.qbvh.traverse_depth_first(&mut visitor); + } + + /// Find the projection of a point on the closest collider. + /// + /// The results include the ID of the feature hit by the point. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `point` - The point to project. + /// * `solid` - If this is set to `true` then the collider shapes are considered to + /// be plain (if the point is located inside of a plain shape, its projection is the point + /// itself). If it is set to `false` the collider shapes are considered to be hollow + /// (if the point is located inside of an hollow shape, it is projected on the shape's + /// boundary). + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn project_point_and_get_feature( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + point: &Point, + filter: QueryFilter, + ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { + let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); + let mut visitor = + PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); + self.qbvh + .traverse_best_first(&mut visitor) + .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