aboutsummaryrefslogtreecommitdiff
path: root/src/pipeline/query_pipeline.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/pipeline/query_pipeline.rs')
-rw-r--r--src/pipeline/query_pipeline.rs444
1 files changed, 322 insertions, 122 deletions
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index cb56141..31bf3a4 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,9 +1,14 @@
-use crate::dynamics::RigidBodySet;
+use crate::data::{BundleSet, ComponentSet, ComponentSetOption};
+use crate::dynamics::{
+ IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
+ RigidBodyVelocity,
+};
use crate::geometry::{
- Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
- RayIntersection, SimdQuadTree, AABB,
+ ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape,
+ InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
};
use crate::math::{Isometry, Point, Real, Vector};
+use parry::partitioning::QBVHDataGenerator;
use parry::query::details::{
IntersectionCompositeShapeShapeBestFirstVisitor,
NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -18,6 +23,9 @@ use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher
use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
use std::sync::Arc;
+#[cfg(feature = "default-sets")]
+use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
+
/// A pipeline for performing queries on all the colliders of a scene.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
@@ -27,16 +35,16 @@ pub struct QueryPipeline {
serde(skip, default = "crate::geometry::default_query_dispatcher")
)]
query_dispatcher: Arc<dyn QueryDispatcher>,
- quadtree: SimdQuadTree<ColliderHandle>,
+ quadtree: QBVH<ColliderHandle>,
tree_built: bool,
dilation_factor: Real,
}
-struct QueryPipelineAsCompositeShape<'a> {
+struct QueryPipelineAsCompositeShape<'a, Colliders> {
query_pipeline: &'a QueryPipeline,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
query_groups: InteractionGroups,
- filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>,
+ filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
}
/// Indicates how the colliders position should be taken into account when
@@ -55,7 +63,12 @@ pub enum QueryPipelineMode {
},
}
-impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
+impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders>
+where
+ // TODO ECS: make everything optional but the shape?
+ Colliders:
+ ComponentSet<ColliderGroups> + ComponentSet<ColliderPosition> + ComponentSet<ColliderShape>,
+{
type PartShape = dyn Shape;
type PartId = ColliderHandle;
@@ -64,11 +77,15 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
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.query_groups)
- && self.filter.map(|f| f(shape_id, collider)).unwrap_or(true)
+ let co_groups: Option<&ColliderGroups> = self.colliders.get(shape_id.0);
+
+ if let Some(co_groups) = co_groups {
+ if co_groups.collision_groups.test(self.query_groups)
+ && self.filter.map(|f| f(shape_id)).unwrap_or(true)
{
- f(Some(collider.position()), collider.shape())
+ let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+ self.colliders.index_bundle(shape_id.0);
+ f(Some(co_pos), &**co_shape)
}
}
}
@@ -81,7 +98,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
self.map_typed_part_at(shape_id, f);
}
- fn typed_quadtree(&self) -> &SimdQuadTree<ColliderHandle> {
+ fn typed_quadtree(&self) -> &QBVH<ColliderHandle> {
&self.query_pipeline.quadtree
}
}
@@ -98,12 +115,12 @@ impl QueryPipeline {
Self::with_query_dispatcher(DefaultQueryDispatcher)
}
- fn as_composite_shape<'a>(
+ fn as_composite_shape<'a, Colliders>(
&'a self,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
query_groups: InteractionGroups,
- filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> QueryPipelineAsCompositeShape<'a> {
+ filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
+ ) -> QueryPipelineAsCompositeShape<'a, Colliders> {
QueryPipelineAsCompositeShape {
query_pipeline: self,
colliders,
@@ -122,7 +139,7 @@ impl QueryPipeline {
{
Self {
query_dispatcher: Arc::new(d),
- quadtree: SimdQuadTree::new(),
+ quadtree: QBVH::new(),
tree_built: false,
dilation_factor: 0.01,
}
@@ -133,53 +150,152 @@ impl QueryPipeline {
&*self.query_dispatcher
}
+ #[cfg(feature = "default-sets")]
/// 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)
+ pub fn update(
+ &mut self,
+ islands: &IslandManager,
+ bodies: &RigidBodySet,
+ colliders: &ColliderSet,
+ ) {
+ self.update_generic(islands, bodies, colliders);
}
/// Update the acceleration structure on the query pipeline.
- pub fn update_with_mode(
+ pub fn update_generic<Bodies, Colliders>(
&mut self,
- bodies: &RigidBodySet,
- colliders: &ColliderSet,
+ islands: &IslandManager,
+ bodies: &Bodies,
+ colliders: &Colliders,
+ ) where
+ Bodies: ComponentSet<RigidBodyPosition>
+ + ComponentSet<RigidBodyColliders>
+ + ComponentSet<RigidBodyVelocity>
+ + ComponentSet<RigidBodyMassProps>
+ + ComponentSet<RigidBodyForces>,
+ Colliders: ComponentSet<ColliderShape>
+ + ComponentSet<ColliderPosition>
+ + ComponentSetOption<ColliderParent>,
+ {
+ self.update_with_mode(
+ islands,
+ bodies,
+ colliders,
+ QueryPipelineMode::CurrentPosition,
+ )
+ }
+
+ /// Update the acceleration structure on the query pipeline.
+ pub fn update_with_mode<Bodies, Colliders>(
+ &mut self,
+ islands: &IslandManager,
+ bodies: &Bodies,
+ colliders: &Colliders,
mode: QueryPipelineMode,
- ) {
- if !self.tree_built {
- match mode {
- QueryPipelineMode::CurrentPosition => {
- let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
- self.quadtree.clear_and_rebuild(data, self.dilation_factor);
- }
- QueryPipelineMode::SweepTestWithNextPosition => {
- let data = colliders.iter().map(|(h, c)| {
- let next_position =
- bodies[c.parent()].next_position * c.position_wrt_parent();
- (h, c.compute_swept_aabb(&next_position))
- });
- self.quadtree.clear_and_rebuild(data, self.dilation_factor);
- }
- QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
- let data = colliders.iter().map(|(h, c)| {
- let next_position = bodies[c.parent()]
- .predict_position_using_velocity_and_forces(dt)
- * c.position_wrt_parent();
- (h, c.compute_swept_aabb(&next_position))
- });
- self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+ ) where
+ Bodies: ComponentSet<RigidBodyPosition>
+ + ComponentSet<RigidBodyColliders>
+ + ComponentSet<RigidBodyVelocity>
+ + ComponentSet<RigidBodyMassProps>
+ + ComponentSet<RigidBodyForces>,
+ Colliders: ComponentSet<ColliderShape>
+ + ComponentSet<ColliderPosition>
+ + ComponentSetOption<ColliderParent>,
+ {
+ struct DataGenerator<'a, Bs, Cs> {
+ bodies: &'a Bs,
+ colliders: &'a Cs,
+ mode: QueryPipelineMode,
+ }
+
+ impl<'a, Bs, Cs> QBVHDataGenerator<ColliderHandle> for DataGenerator<'a, Bs, Cs>
+ where
+ Bs: ComponentSet<RigidBodyPosition>
+ + ComponentSet<RigidBodyMassProps>
+ + ComponentSet<RigidBodyVelocity>
+ + ComponentSet<RigidBodyForces>,
+ Cs: ComponentSet<ColliderShape>
+ + ComponentSet<ColliderPosition>
+ + ComponentSetOption<ColliderParent>,
+ {
+ fn size_hint(&self) -> usize {
+ ComponentSet::<ColliderShape>::size_hint(self.colliders)
+ }
+
+ #[inline(always)]
+ fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) {
+ match self.mode {
+ QueryPipelineMode::CurrentPosition => {
+ self.colliders.for_each(|h, co_shape: &ColliderShape| {
+ let co_pos: &ColliderPosition = self.colliders.index(h);
+ f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+ })
+ }
+ QueryPipelineMode::SweepTestWithNextPosition => {
+ self.colliders.for_each(|h, co_shape: &ColliderShape| {
+ let co_parent: Option<&ColliderParent> = self.colliders.get(h);
+ let co_pos: &ColliderPosition = self.colliders.index(h);
+
+ if let Some(co_parent) = co_parent {
+ let rb_pos: &RigidBodyPosition =
+ self.bodies.index(co_parent.handle.0);
+ let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
+
+ f(
+ ColliderHandle(h),
+ co_shape.compute_swept_aabb(&co_pos, &next_position),
+ )
+ } else {
+ f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+ }
+ })
+ }
+ QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+ self.colliders.for_each(|h, co_shape: &ColliderShape| {
+ let co_parent: Option<&ColliderParent> = self.colliders.get(h);
+ let co_pos: &ColliderPosition = self.colliders.index(h);
+
+ if let Some(co_parent) = co_parent {
+ let (rb_pos, vels, forces, mprops): (
+ &RigidBodyPosition,
+ &RigidBodyVelocity,
+ &RigidBodyForces,
+ &RigidBodyMassProps,
+ ) = self.bodies.index_bundle(co_parent.handle.0);
+ let predicted_pos = rb_pos
+ .integrate_forces_and_velocities(dt, forces, vels, mprops);
+
+ let next_position = predicted_pos * co_parent.pos_wrt_parent;
+ f(
+ ColliderHandle(h),
+ co_shape.compute_swept_aabb(&co_pos, &next_position),
+ )
+ } else {
+ f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+ }
+ })
+ }
}
}
+ }
+
+ if !self.tree_built {
+ let generator = DataGenerator {
+ bodies,
+ colliders,
+ mode,
+ };
+ self.quadtree
+ .clear_and_rebuild(generator, self.dilation_factor);
// FIXME: uncomment this once we handle insertion/removals properly.
// self.tree_built = true;
return;
}
- for (_, body) in bodies
- .iter_active_dynamic()
- .chain(bodies.iter_active_kinematic())
- {
- for handle in &body.colliders {
+ for handle in islands.iter_active_bodies() {
+ let body_colliders: &RigidBodyColliders = bodies.index(handle.0);
+ for handle in &body_colliders.0 {
self.quadtree.pre_update(*handle)
}
}
@@ -187,17 +303,28 @@ impl QueryPipeline {
match mode {
QueryPipelineMode::CurrentPosition => {
self.quadtree.update(
- |handle| colliders[*handle].compute_aabb(),
+ |handle| {
+ let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+ colliders.index_bundle(handle.0);
+ co_shape.compute_aabb(&co_pos)
+ },
self.dilation_factor,
);
}
QueryPipelineMode::SweepTestWithNextPosition => {
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)
+ let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+ let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+ colliders.index_bundle(handle.0);
+
+ if let Some(co_parent) = co_parent {
+ let rb_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
+ let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
+ co_shape.compute_swept_aabb(&co_pos, &next_position)
+ } else {
+ co_shape.compute_aabb(&co_pos)
+ }
},
self.dilation_factor,
);
@@ -205,11 +332,26 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.quadtree.update(
|handle| {
- let co = &colliders[*handle];
- let next_position = bodies[co.parent()]
- .predict_position_using_velocity_and_forces(dt)
- * co.position_wrt_parent();
- co.compute_swept_aabb(&next_position)
+ let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
+ let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
+ colliders.index_bundle(handle.0);
+
+ if let Some(co_parent) = co_parent {
+ let (rb_pos, vels, forces, mprops): (
+ &RigidBodyPosition,
+ &RigidBodyVelocity,
+ &RigidBodyForces,
+ &RigidBodyMassProps,
+ ) = bodies.index_bundle(co_parent.handle.0);
+
+ let predicted_pos =
+ rb_pos.integrate_forces_and_velocities(dt, forces, vels, mprops);
+
+ let next_position = predicted_pos * co_parent.pos_wrt_parent;
+ co_shape.compute_swept_aabb(&co_pos, &next_position)
+ } else {
+ co_shape.compute_aabb(&co_pos)
+ }
},
self.dilation_factor,
);
@@ -232,15 +374,20 @@ impl QueryPipeline {
/// - `filter`: a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn cast_ray(
+ pub fn cast_ray<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, Real)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, Real)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
@@ -263,15 +410,20 @@ impl QueryPipeline {
/// - `filter`: a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn cast_ray_and_get_normal(
+ pub fn cast_ray_and_get_normal<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, RayIntersection)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, RayIntersection)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
&pipeline_shape,
@@ -301,26 +453,31 @@ impl QueryPipeline {
/// - `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>(
+ pub fn intersections_with_ray<'a, Colliders>(
&self,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
- ) {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
+ ) where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
- if let Some(coll) = colliders.get(*handle) {
- if coll.collision_groups.test(query_groups)
- && filter.map(|f| f(*handle, coll)).unwrap_or(true)
+ let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
+ if let Some(co_shape) = co_shape {
+ let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+ colliders.index_bundle(handle.0);
+ if co_groups.collision_groups.test(query_groups)
+ && filter.map(|f| f(*handle)).unwrap_or(true)
{
- if let Some(hit) =
- coll.shape()
- .cast_ray_and_get_normal(coll.position(), ray, max_toi, solid)
+ if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid)
{
- return callback(*handle, coll, hit);
+ return callback(*handle, hit);
}
}
}
@@ -343,14 +500,19 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn intersection_with_shape(
+ pub fn intersection_with_shape<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<ColliderHandle> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<ColliderHandle>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
@@ -379,14 +541,19 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn project_point(
+ pub fn project_point<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
point: &Point<Real>,
solid: bool,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, PointProjection)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, PointProjection)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
@@ -408,21 +575,30 @@ 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<'a, Colliders>(
&self,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
point: &Point<Real>,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool,
- ) {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ mut callback: impl FnMut(ColliderHandle) -> bool,
+ ) where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
- if let Some(coll) = colliders.get(*handle) {
- if coll.collision_groups.test(query_groups)
- && filter.map(|f| f(*handle, coll)).unwrap_or(true)
- && coll.shape().contains_point(coll.position(), point)
+ let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
+
+ if let Some(co_shape) = co_shape {
+ let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+ colliders.index_bundle(handle.0);
+
+ if co_groups.collision_groups.test(query_groups)
+ && filter.map(|f| f(*handle)).unwrap_or(true)
+ && co_shape.contains_point(co_pos, point)
{
- return callback(*handle, coll);
+ return callback(*handle);
}
}
@@ -451,13 +627,18 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn project_point_and_get_feature(
+ pub fn project_point_and_get_feature<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
point: &Point<Real>,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, PointProjection, FeatureId)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
@@ -493,16 +674,21 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn cast_shape<'a>(
+ pub fn cast_shape<'a, Colliders>(
&self,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
shape_pos: &Isometry<Real>,
shape_vel: &Vector<Real>,
shape: &dyn Shape,
max_toi: Real,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, TOI)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, TOI)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
@@ -535,17 +721,22 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
- pub fn nonlinear_cast_shape(
+ pub fn nonlinear_cast_shape<Colliders>(
&self,
- colliders: &ColliderSet,
+ colliders: &Colliders,
shape_motion: &NonlinearRigidMotion,
shape: &dyn Shape,
start_time: Real,
end_time: Real,
stop_at_penetration: bool,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- ) -> Option<(ColliderHandle, TOI)> {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ ) -> Option<(ColliderHandle, TOI)>
+ where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let pipeline_motion = NonlinearRigidMotion::identity();
let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
@@ -574,27 +765,36 @@ impl QueryPipeline {
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
/// * `callback` - A function called with the handles of each collider intersecting the `shape`.
- pub fn intersections_with_shape<'a>(
+ pub fn intersections_with_shape<'a, Colliders>(
&self,
- colliders: &'a ColliderSet,
+ colliders: &'a Colliders,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
query_groups: InteractionGroups,
- filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
- mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool,
- ) {
+ filter: Option<&dyn Fn(ColliderHandle) -> bool>,
+ mut callback: impl FnMut(ColliderHandle) -> bool,
+ ) where
+ Colliders: ComponentSet<ColliderGroups>
+ + ComponentSet<ColliderPosition>
+ + ComponentSet<ColliderShape>,
+ {
let dispatcher = &*self.query_dispatcher;
let inv_shape_pos = shape_pos.inverse();
let mut leaf_callback = &mut |handle: &ColliderHandle| {
- if let Some(coll) = colliders.get(*handle) {
- if coll.collision_groups.test(query_groups)
- && filter.map(|f| f(*handle, coll)).unwrap_or(true)
+ let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
+
+ if let Some(co_shape) = co_shape {
+ let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) =
+ colliders.index_bundle(handle.0);
+
+ if co_groups.collision_groups.test(query_groups)
+ && filter.map(|f| f(*handle)).unwrap_or(true)
{
- let pos12 = inv_shape_pos * coll.position();
+ let pos12 = inv_shape_pos * co_pos.as_ref();
- if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) {
- return callback(*handle, coll);
+ if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) {
+ return callback(*handle);
}
}
}
generated by cgit (git 2.34.1) at 2026-04-19 21:09:05 +0000