diff options
Diffstat (limited to 'src/pipeline')
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 17 | ||||
| -rw-r--r-- | src/pipeline/event_handler.rs | 20 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 16 | ||||
| -rw-r--r-- | src/pipeline/query_pipeline.rs | 425 |
4 files changed, 390 insertions, 88 deletions
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 188be27..866c3a5 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -2,9 +2,10 @@ use crate::dynamics::{JointSet, RigidBodySet}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase, - ProximityPairFilter, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, + IntersectionPairFilter, NarrowPhase, }; +use crate::math::Real; use crate::pipeline::EventHandler; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -38,13 +39,13 @@ impl CollisionPipeline { /// Executes one step of the collision detection. pub fn step( &mut self, - prediction_distance: f32, + prediction_distance: Real, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, contact_pair_filter: Option<&dyn ContactPairFilter>, - proximity_pair_filter: Option<&dyn ProximityPairFilter>, + proximity_pair_filter: Option<&dyn IntersectionPairFilter>, events: &dyn EventHandler, ) { bodies.maintain(colliders); @@ -64,13 +65,7 @@ impl CollisionPipeline { contact_pair_filter, events, ); - narrow_phase.compute_proximities( - prediction_distance, - bodies, - colliders, - proximity_pair_filter, - events, - ); + narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); bodies.update_active_set_with_contacts( colliders, diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs index 67e4a78..9d7b17a 100644 --- a/src/pipeline/event_handler.rs +++ b/src/pipeline/event_handler.rs @@ -1,14 +1,14 @@ -use crate::geometry::{ContactEvent, ProximityEvent}; +use crate::geometry::{ContactEvent, IntersectionEvent}; use crossbeam::channel::Sender; /// Trait implemented by structures responsible for handling events generated by the physics engine. /// /// Implementors of this trait will typically collect these events for future processing. pub trait EventHandler: Send + Sync { - /// Handle a proximity event. + /// Handle an intersection event. /// - /// A proximity event is emitted when the state of proximity between two colliders changes. - fn handle_proximity_event(&self, event: ProximityEvent); + /// A intersection event is emitted when the state of intersection between two colliders changes. + fn handle_intersection_event(&self, event: IntersectionEvent); /// Handle a contact event. /// /// A contact event is emitted when two collider start or stop touching, independently from the @@ -17,32 +17,32 @@ pub trait EventHandler: Send + Sync { } impl EventHandler for () { - fn handle_proximity_event(&self, _event: ProximityEvent) {} + fn handle_intersection_event(&self, _event: IntersectionEvent) {} fn handle_contact_event(&self, _event: ContactEvent) {} } /// A physics event handler that collects events into a crossbeam channel. pub struct ChannelEventCollector { - proximity_event_sender: Sender<ProximityEvent>, + intersection_event_sender: Sender<IntersectionEvent>, contact_event_sender: Sender<ContactEvent>, } impl ChannelEventCollector { /// Initialize a new physics event handler from crossbeam channel senders. pub fn new( - proximity_event_sender: Sender<ProximityEvent>, + intersection_event_sender: Sender<IntersectionEvent>, contact_event_sender: Sender<ContactEvent>, ) -> Self { Self { - proximity_event_sender, + intersection_event_sender, contact_event_sender, } } } impl EventHandler for ChannelEventCollector { - fn handle_proximity_event(&self, event: ProximityEvent) { - let _ = self.proximity_event_sender.send(event); + fn handle_intersection_event(&self, event: IntersectionEvent) { + let _ = self.intersection_event_sender.send(event); } fn handle_contact_event(&self, event: ContactEvent) { diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 1c8666f..b5f123d 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -8,9 +8,9 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, - ContactPairFilter, NarrowPhase, ProximityPairFilter, + ContactPairFilter, IntersectionPairFilter, NarrowPhase, }; -use crate::math::Vector; +use crate::math::{Real, Vector}; use crate::pipeline::EventHandler; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -62,7 +62,7 @@ impl PhysicsPipeline { /// Executes one timestep of the physics simulation. pub fn step( &mut self, - gravity: &Vector<f32>, + gravity: &Vector<Real>, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, @@ -70,7 +70,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, joints: &mut JointSet, contact_pair_filter: Option<&dyn ContactPairFilter>, - proximity_pair_filter: Option<&dyn ProximityPairFilter>, + proximity_pair_filter: Option<&dyn IntersectionPairFilter>, events: &dyn EventHandler, ) { self.counters.step_started(); @@ -118,13 +118,7 @@ impl PhysicsPipeline { contact_pair_filter, events, ); - narrow_phase.compute_proximities( - integration_parameters.prediction_distance, - bodies, - colliders, - proximity_pair_filter, - events, - ); + narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); // println!("Compute contact time: {}", instant::now() - t); self.counters.stages.island_construction_time.start(); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 665fee8..8cc6a60 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,15 +1,71 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ - Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree, + Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray, + RayIntersection, SimdQuadTree, }; +use crate::math::{Isometry, Point, Real, Vector}; +use crate::parry::motion::RigidMotion; +use parry::query::details::{ + IntersectionCompositeShapeShapeBestFirstVisitor, + NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, + PointCompositeShapeProjWithFeatureBestFirstVisitor, + RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor, + TOICompositeShapeShapeBestFirstVisitor, +}; +use parry::query::visitors::{ + BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor, +}; +use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI}; +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 { - quadtree: WQuadtree<ColliderHandle>, + #[cfg_attr( + feature = "serde-serialize", + serde(skip, default = "crate::geometry::default_query_dispatcher") + )] + query_dispatcher: Arc<dyn QueryDispatcher>, + quadtree: SimdQuadTree<ColliderHandle>, tree_built: bool, - dilation_factor: f32, + dilation_factor: Real, +} + +struct QueryPipelineAsCompositeShape<'a> { + query_pipeline: &'a QueryPipeline, + colliders: &'a ColliderSet, + groups: InteractionGroups, +} + +impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { + type PartShape = dyn Shape; + type PartId = ColliderHandle; + + fn map_typed_part_at( + &self, + shape_id: Self::PartId, + mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape), + ) { + if let Some(collider) = self.colliders.get(shape_id) { + if collider.collision_groups.test(self.groups) { + f(Some(collider.position()), collider.shape()) + } + } + } + + fn map_untyped_part_at( + &self, + shape_id: Self::PartId, + f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape), + ) { + self.map_typed_part_at(shape_id, f); + } + + fn typed_quadtree(&self) -> &SimdQuadTree<ColliderHandle> { + &self.query_pipeline.quadtree + } } impl Default for QueryPipeline { @@ -21,8 +77,32 @@ impl Default for QueryPipeline { impl QueryPipeline { /// Initializes an empty query pipeline. pub fn new() -> Self { + Self::with_query_dispatcher(DefaultQueryDispatcher) + } + + fn as_composite_shape<'a>( + &'a self, + colliders: &'a ColliderSet, + groups: InteractionGroups, + ) -> QueryPipelineAsCompositeShape<'a> { + QueryPipelineAsCompositeShape { + query_pipeline: self, + colliders, + groups, + } + } + + /// Initializes an empty query pipeline with a custom `QueryDispatcher`. + /// + /// Use this constructor in order to use a custom `QueryDispatcher` that is + /// awary of your own user-defined shapes. + pub fn with_query_dispatcher<D>(d: D) -> Self + where + D: 'static + QueryDispatcher, + { Self { - quadtree: WQuadtree::new(), + query_dispatcher: Arc::new(d), + quadtree: SimdQuadTree::new(), tree_built: false, dilation_factor: 0.01, } @@ -47,7 +127,10 @@ impl QueryPipeline { } } - self.quadtree.update(colliders, self.dilation_factor); + self.quadtree.update( + |handle| colliders[*handle].compute_aabb(), + self.dilation_factor, + ); } /// Find the closest intersection between a ray and a set of collider. @@ -56,40 +139,46 @@ impl QueryPipeline { /// - `position`: the position of this shape. /// - `ray`: the ray to cast. /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. - pub fn cast_ray<'a>( + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. + pub fn cast_ray( &self, - colliders: &'a ColliderSet, + colliders: &ColliderSet, ray: &Ray, - max_toi: f32, + max_toi: Real, + solid: bool, groups: InteractionGroups, - ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { - // TODO: avoid allocation? - let mut inter = Vec::new(); - self.quadtree.cast_ray(ray, max_toi, &mut inter); - - let mut best = f32::MAX; - let mut result = None; - - for handle in inter { - if let Some(collider) = colliders.get(handle) { - if collider.collision_groups.test(groups) { - if let Some(inter) = collider.shape().toi_and_normal_with_ray( - collider.position(), - ray, - max_toi, - true, - ) { - if inter.toi < best { - best = inter.toi; - result = Some((handle, collider, inter)); - } - } - } - } - } + ) -> Option<(ColliderHandle, Real)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = + RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); - result + self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. + pub fn cast_ray_and_get_normal( + &self, + colliders: &ColliderSet, + ray: &Ray, + max_toi: Real, + solid: bool, + groups: InteractionGroups, + ) -> Option<(ColliderHandle, RayIntersection)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( + &pipeline_shape, + ray, + max_toi, + solid, + ); + + self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) } /// Find the all intersections between a ray and a set of collider and passes them to a callback. @@ -98,37 +187,261 @@ impl QueryPipeline { /// - `position`: the position of this shape. /// - `ray`: the ray to cast. /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. /// - `callback`: function executed on each collider for which a ray intersection has been found. /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, - /// this method will exit early, ignory any further raycast. - pub fn interferences_with_ray<'a>( + /// this method will exit early, ignore any further raycast. + pub fn intersections_with_ray<'a>( &self, colliders: &'a ColliderSet, ray: &Ray, - max_toi: f32, + max_toi: Real, + solid: bool, groups: InteractionGroups, mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, ) { - // TODO: avoid allocation? - let mut inter = Vec::new(); - self.quadtree.cast_ray(ray, max_toi, &mut inter); - - for handle in inter { - let collider = &colliders[handle]; - - if collider.collision_groups.test(groups) { - if let Some(inter) = collider.shape().toi_and_normal_with_ray( - collider.position(), - ray, - max_toi, - true, - ) { - if !callback(handle, collider, inter) { - return; + let mut leaf_callback = &mut |handle: &ColliderHandle| { + if let Some(coll) = colliders.get(*handle) { + if coll.collision_groups.test(groups) { + if let Some(hit) = + coll.shape() + .cast_ray_and_get_normal(coll.position(), ray, max_toi, solid) + { + return callback(*handle, coll, hit); } } } - } + + true + }; + + let mut visitor = RayIntersectionsVisitor::new(ray, max_toi, &mut leaf_callback); + self.quadtree.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. + /// * `groups` - The bit groups and filter associated to the ray, in order to only + /// hit the colliders with collision groups compatible with the ray's group. + pub fn intersection_with_shape( + &self, + colliders: &ColliderSet, + shape_pos: &Isometry<Real>, + shape: &dyn Shape, + groups: InteractionGroups, + ) -> Option<ColliderHandle> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new( + &*self.query_dispatcher, + shape_pos, + &pipeline_shape, + shape, + ); + + self.quadtree + .traverse_best_first(&mut visitor) + .map(|h| (h.1 .0)) + } + + /// 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). + /// * `groups` - The bit groups and filter associated to the point to project, in order to only + /// project on colliders with collision groups compatible with the ray's group. + pub fn project_point( + &self, + colliders: &ColliderSet, + point: &Point<Real>, + solid: bool, + groups: InteractionGroups, + ) -> Option<(ColliderHandle, PointProjection)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = + PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); + + self.quadtree + .traverse_best_first(&mut visitor) + .map(|h| (h.1 .1, h.1 .0)) + } + + /// 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. + /// * `groups` - The bit groups and filter associated to the point to test, in order to only + /// test on colliders with collision groups compatible with the ray's group. + /// * `callback` - A function called with each collider with a shape + /// containing the `point`. + pub fn intersections_with_point<'a>( + &self, + colliders: &'a ColliderSet, + point: &Point<Real>, + groups: InteractionGroups, + mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, + ) { + let mut leaf_callback = &mut |handle: &ColliderHandle| { + if let Some(coll) = colliders.get(*handle) { + if coll.collision_groups.test(groups) + && coll.shape().contains_point(coll.position(), point) + { + return callback(*handle, coll); + } + } + + true + }; + + let mut visitor = PointIntersectionsVisitor::new(point, &mut leaf_callback); + + self.quadtree.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). + /// * `groups` - The bit groups and filter associated to the point to project, in order to only + /// project on colliders with collision groups compatible with the ray's group. + pub fn project_point_and_get_feature( + &self, + colliders: &ColliderSet, + point: &Point<Real>, + groups: InteractionGroups, + ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = + PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); + self.quadtree + .traverse_best_first(&mut visitor) + .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1)) + } + + /// 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). + /// + /// # 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`. + /// * `groups` - The bit groups and filter associated to the shape to cast, in order to only + /// test on colliders with collision groups compatible with this group. + pub fn cast_shape<'a>( + &self, + colliders: &'a ColliderSet, + shape_pos: &Isometry<Real>, + shape_vel: &Vector<Real>, + shape: &dyn Shape, + max_toi: Real, + target_distance: Real, + groups: InteractionGroups, + ) -> Option<(ColliderHandle, TOI)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( + &*self.query_dispatcher, + shape_pos, + shape_vel, + &pipeline_shape, + shape, + max_toi, + target_distance, + ); + self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) + } + + /// Casts a shape with an arbitrary continuous motion and retrieve the first collider it hits. + /// + /// # Parameters + /// * `colliders` - The set of colliders taking part in this pipeline. + /// * `shape_motion` - The motion of the shape. + /// * `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`. + /// * `groups` - The bit groups and filter associated to the shape to cast, in order to only + /// test on colliders with collision groups compatible with this group. + pub fn nonlinear_cast_shape( + &self, + colliders: &ColliderSet, + shape_motion: &dyn RigidMotion, + shape: &dyn Shape, + max_toi: Real, + target_distance: Real, + groups: InteractionGroups, + ) -> Option<(ColliderHandle, TOI)> { + let pipeline_shape = self.as_composite_shape(colliders, groups); + let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( + &*self.query_dispatcher, + shape_motion, + &pipeline_shape, + shape, + max_toi, + target_distance, + ); + self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) + } + + /// 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. + /// * `groups` - The bit groups and filter associated to the shape to test, in order to only + /// test on colliders with collision groups compatible with this group. + /// * `callback` - A function called with the handles of each collider intersecting the `shape`. + pub fn intersections_with_shape<'a>( + &self, + colliders: &'a ColliderSet, + shape_pos: &Isometry<Real>, + shape: &dyn Shape, + groups: InteractionGroups, + mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, + ) { + 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(groups) { + let pos12 = inv_shape_pos * coll.position(); + + if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) { + return callback(*handle, coll); + } + } + } + + true + }; + + let shape_aabb = shape.compute_aabb(shape_pos); + let mut visitor = BoundingVolumeIntersectionsVisitor::new(&shape_aabb, &mut leaf_callback); + + self.quadtree.traverse_depth_first(&mut visitor); } } |