diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 11af3acf..0f7c0307 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -1,24 +1,25 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{ - self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, - QueryDispatcher, ShapeCastHit, Unsupported, + self, details::NonlinearShapeCastMode, query_dispatcher::QueryDispatcherComposite, + ClosestPoints, Contact, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, Unsupported, }; #[cfg(feature = "std")] use crate::query::{ contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, - query_dispatcher::PersistentQueryDispatcher, + query_dispatcher::{PersistentQueryDispatcher, PersistentQueryDispatcherComposite}, ContactManifold, }; use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; /// A dispatcher that exposes built-in queries -#[derive(Debug, Clone)] +#[derive(Debug, Clone, Copy)] pub struct DefaultQueryDispatcher; -impl QueryDispatcher for DefaultQueryDispatcher { +impl QueryDispatcherComposite for DefaultQueryDispatcher { fn intersection_test( &self, + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, @@ -66,11 +67,17 @@ impl QueryDispatcher for DefaultQueryDispatcher { #[cfg(feature = "std")] if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::intersection_test_composite_shape_shape( - self, pos12, c1, shape2, + root_dispatcher, + pos12, + c1, + shape2, )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::intersection_test_shape_composite_shape( - self, pos12, shape1, c2, + root_dispatcher, + pos12, + shape1, + c2, )); } @@ -83,6 +90,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { /// Returns `0.0` if the objects are touching or penetrating. fn distance( &self, + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, @@ -125,11 +133,17 @@ impl QueryDispatcher for DefaultQueryDispatcher { #[cfg(feature = "std")] if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::distance_composite_shape_shape( - self, pos12, c1, shape2, + root_dispatcher, + pos12, + c1, + shape2, )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::distance_shape_composite_shape( - self, pos12, shape1, c2, + root_dispatcher, + pos12, + shape1, + c2, )); } @@ -139,6 +153,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn contact( &self, + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, @@ -181,11 +196,19 @@ impl QueryDispatcher for DefaultQueryDispatcher { )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::contact_composite_shape_shape( - self, pos12, c1, shape2, prediction, + root_dispatcher, + pos12, + c1, + shape2, + prediction, )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::contact_shape_composite_shape( - self, pos12, shape1, c2, prediction, + root_dispatcher, + pos12, + shape1, + c2, + prediction, )); } @@ -195,6 +218,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn closest_points( &self, + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, @@ -257,11 +281,19 @@ impl QueryDispatcher for DefaultQueryDispatcher { #[cfg(feature = "std")] if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::closest_points_composite_shape_shape( - self, pos12, c1, shape2, max_dist, + root_dispatcher, + pos12, + c1, + shape2, + max_dist, )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::closest_points_shape_composite_shape( - self, pos12, shape1, c2, max_dist, + root_dispatcher, + pos12, + shape1, + c2, + max_dist, )); } @@ -271,6 +303,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn cast_shapes( &self, + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, local_vel12: &Vector, shape1: &dyn Shape, @@ -309,7 +342,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { #[cfg(feature = "std")] if let Some(heightfield1) = shape1.as_heightfield() { return query::details::cast_shapes_heightfield_shape( - self, + root_dispatcher, pos12, local_vel12, heightfield1, @@ -318,7 +351,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { ); } else if let Some(heightfield2) = shape1.as_heightfield() { return query::details::cast_shapes_shape_heightfield( - self, + root_dispatcher, pos12, local_vel12, shape1, @@ -336,7 +369,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::cast_shapes_composite_shape_shape( - self, + root_dispatcher, pos12, local_vel12, c1, @@ -345,7 +378,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::cast_shapes_shape_composite_shape( - self, + root_dispatcher, pos12, local_vel12, shape1, @@ -360,6 +393,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn cast_shapes_nonlinear( &self, + root_dispatcher: &dyn QueryDispatcher, motion1: &NonlinearRigidMotion, shape1: &dyn Shape, motion2: &NonlinearRigidMotion, @@ -377,14 +411,23 @@ impl QueryDispatcher for DefaultQueryDispatcher { Ok( query::details::cast_shapes_nonlinear_support_map_support_map( - self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode, + root_dispatcher, + motion1, + sm1, + shape1, + motion2, + sm2, + shape2, + start_time, + end_time, + mode, ), ) } else { #[cfg(feature = "std")] if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape( - self, + root_dispatcher, motion1, c1, motion2, @@ -395,7 +438,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape( - self, + root_dispatcher, motion1, shape1, motion2, @@ -418,7 +461,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { } #[cfg(feature = "std")] -impl PersistentQueryDispatcher +impl PersistentQueryDispatcherComposite for DefaultQueryDispatcher where ManifoldData: Default + Clone, @@ -426,6 +469,7 @@ where { fn contact_manifolds( &self, + root_dispatcher: &dyn PersistentQueryDispatcher, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, @@ -440,7 +484,13 @@ where if let (Some(composite1), Some(composite2)) = (composite1, composite2) { contact_manifolds_composite_shape_composite_shape( - self, pos12, composite1, composite2, prediction, manifolds, workspace, + root_dispatcher, + pos12, + composite1, + composite2, + prediction, + manifolds, + workspace, ); return Ok(()); @@ -449,13 +499,19 @@ where match (shape1.shape_type(), shape2.shape_type()) { (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => { contact_manifolds_trimesh_shape_shapes( - self, pos12, shape1, shape2, prediction, manifolds, workspace, + root_dispatcher, + pos12, + shape1, + shape2, + prediction, + manifolds, + workspace, ); } (ShapeType::HeightField, _) => { if let Some(composite2) = composite2 { contact_manifolds_heightfield_composite_shape( - self, + root_dispatcher, pos12, &pos12.inverse(), shape1.as_heightfield().unwrap(), @@ -467,14 +523,20 @@ where ) } else { contact_manifolds_heightfield_shape_shapes( - self, pos12, shape1, shape2, prediction, manifolds, workspace, + root_dispatcher, + pos12, + shape1, + shape2, + prediction, + manifolds, + workspace, ); } } (_, ShapeType::HeightField) => { if let Some(composite1) = composite1 { contact_manifolds_heightfield_composite_shape( - self, + root_dispatcher, &pos12.inverse(), pos12, shape2.as_heightfield().unwrap(), @@ -486,18 +548,31 @@ where ) } else { contact_manifolds_heightfield_shape_shapes( - self, pos12, shape1, shape2, prediction, manifolds, workspace, + root_dispatcher, + pos12, + shape1, + shape2, + prediction, + manifolds, + workspace, ); } } _ => { if let Some(composite1) = composite1 { contact_manifolds_composite_shape_shape( - self, pos12, composite1, shape2, prediction, manifolds, workspace, false, + root_dispatcher, + pos12, + composite1, + shape2, + prediction, + manifolds, + workspace, + false, ); } else if let Some(composite2) = composite2 { contact_manifolds_composite_shape_shape( - self, + root_dispatcher, &pos12.inverse(), composite2, shape1, @@ -511,7 +586,7 @@ where manifolds.push(ContactManifold::new()); } - return self.contact_manifold_convex_convex( + return root_dispatcher.contact_manifold_convex_convex( pos12, shape1, shape2, diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..32f1ce4d 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -38,8 +38,8 @@ pub use self::intersection_test::intersection_test; pub use self::nonlinear_shape_cast::{cast_shapes_nonlinear, NonlinearRigidMotion}; pub use self::point::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "std")] -pub use self::query_dispatcher::PersistentQueryDispatcher; -pub use self::query_dispatcher::{QueryDispatcher, QueryDispatcherChain}; +pub use self::query_dispatcher::{PersistentQueryDispatcher, PersistentQueryDispatcherComposite}; +pub use self::query_dispatcher::{QueryDispatcher, QueryDispatcherChain, QueryDispatcherComposite}; pub use self::ray::{Ray, RayCast, RayIntersection, SimdRay}; pub use self::shape_cast::{cast_shapes, ShapeCastHit, ShapeCastOptions, ShapeCastStatus}; pub use self::split::{IntersectResult, SplitResult}; diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index 6bd55496..0aef4e77 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -10,6 +10,50 @@ use crate::shape::Shape; #[cfg(feature = "std")] /// A query dispatcher for queries relying on spatial coherence, including contact-manifold computation. +/// +/// Third-party crates adding custom shapes should implement +/// this trait to allow pair-wise queries between the added shape and other shapes. +/// +/// The `root_dispatcher` argument is a reference to the root dispatcher of the composite dispatcher. +/// This is necessary to support recursive dispatching for composite shapes. +pub trait PersistentQueryDispatcherComposite: + QueryDispatcherComposite +{ + /// Compute all the contacts between two shapes. + /// + /// The output is written into `manifolds` and `context`. Both can persist + /// between multiple calls to `contacts` by re-using the result of the previous + /// call to `contacts`. This persistence can significantly improve collision + /// detection performances by allowing the underlying algorithms to exploit + /// spatial and temporal coherence. + fn contact_manifolds( + &self, + root_dispatcher: &dyn PersistentQueryDispatcher, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + manifolds: &mut Vec>, + workspace: &mut Option, + ) -> Result<(), Unsupported>; + + /// Computes the contact-manifold between two convex shapes. + fn contact_manifold_convex_convex( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, + prediction: Real, + manifold: &mut ContactManifold, + ) -> Result<(), Unsupported>; +} + +#[cfg(feature = "std")] +/// A query dispatcher for queries relying on spatial coherence, including contact-manifold computation. +/// +/// This trait should not be implemented. Instead, implement the [`PersistentQueryDispatcherComposite`] trait. pub trait PersistentQueryDispatcher: QueryDispatcher { /// Compute all the contacts between two shapes. /// @@ -41,10 +85,150 @@ pub trait PersistentQueryDispatcher: QueryD ) -> Result<(), Unsupported>; } +#[cfg(feature = "std")] +impl PersistentQueryDispatcher for T +where + T: PersistentQueryDispatcherComposite, +{ + fn contact_manifolds( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + manifolds: &mut Vec>, + workspace: &mut Option, + ) -> Result<(), Unsupported> { + >::contact_manifolds( + self, self, pos12, g1, g2, prediction, manifolds, workspace, + ) + } + + fn contact_manifold_convex_convex( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, + prediction: Real, + manifold: &mut ContactManifold, + ) -> Result<(), Unsupported> { + >::contact_manifold_convex_convex(self, pos12, g1, g2, normal_constraints1, normal_constraints2, prediction, manifold) + } +} + /// Dispatcher for pairwise queries. /// -/// Custom implementations allow crates that support an abstract `QueryDispatcher` to handle custom -/// shapes. +/// Third-party crates adding custom shapes should implement +/// this trait to allow pair-wise queries between the added shape and other shapes. +/// +/// The `pos12` argument to most queries is the transform from the local space of `g2` to that of +/// `g1`. +/// +/// The `root_dispatcher` argument is a reference to the root dispatcher of the composite dispatcher. +/// This is necessary to support recursive dispatching for composite shapes. +pub trait QueryDispatcherComposite: Send + Sync { + /// Tests whether two shapes are intersecting. + fn intersection_test( + &self, + root_dispatcher: &dyn QueryDispatcher, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result; + + /// Computes the minimum distance separating two shapes. + /// + /// Returns `0.0` if the objects are touching or penetrating. + fn distance( + &self, + root_dispatcher: &dyn QueryDispatcher, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result; + + /// Computes one pair of contact points point between two shapes. + /// + /// Returns `None` if the objects are separated by a distance greater than `prediction`. + fn contact( + &self, + root_dispatcher: &dyn QueryDispatcher, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + ) -> Result, Unsupported>; + + /// Computes the pair of closest points between two shapes. + /// + /// Returns `ClosestPoints::Disjoint` if the objects are separated by a distance greater than `max_dist`. + fn closest_points( + &self, + root_dispatcher: &dyn QueryDispatcher, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + max_dist: Real, + ) -> Result; + + /// Computes the smallest time when two shapes under translational movement are separated by a + /// distance smaller or equal to `distance`. + /// + /// Returns `0.0` if the objects are touching or penetrating. + /// + /// # Parameters + /// - `pos12`: the position of the second shape relative to the first shape. + /// - `local_vel12`: the relative velocity between the two shapes, expressed in the local-space + /// of the first shape. In other world: `pos1.inverse() * (vel2 - vel1)`. + /// - `g1`: the first shape involved in the shape-cast. + /// - `g2`: the second shape involved in the shape-cast. + /// - `target_dist`: a hit will be returned as soon as the two shapes get closer than `target_dist`. + /// - `max_time_of_impact`: the maximum allowed travel time. This method returns `None` if the time-of-impact + /// detected is theater than this value. + fn cast_shapes( + &self, + root_dispatcher: &dyn QueryDispatcher, + pos12: &Isometry, + local_vel12: &Vector, + g1: &dyn Shape, + g2: &dyn Shape, + options: ShapeCastOptions, + ) -> Result, Unsupported>; + + /// Computes the smallest time of impact of two shapes under translational and rotational movement. + /// + /// # Parameters + /// * `motion1` - The motion of the first shape. + /// * `g1` - The first shape involved in the query. + /// * `motion2` - The motion of the second shape. + /// * `g2` - The second shape involved in the query. + /// * `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 `time_of_impact` equal to `start_time`. If `stop_at_penetration` is `false` + /// then the nonlinear shape-casting will see if further motion wrt. 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. + fn cast_shapes_nonlinear( + &self, + root_dispatcher: &dyn QueryDispatcher, + motion1: &NonlinearRigidMotion, + g1: &dyn Shape, + motion2: &NonlinearRigidMotion, + g2: &dyn Shape, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + ) -> Result, Unsupported>; +} + +/// Dispatcher for pairwise queries. +/// +/// This trait should not be implemented. Instead, implement the [`QueryDispatcherComposite`] trait. /// /// The `pos12` argument to most queries is the transform from the local space of `g2` to that of /// `g1`. @@ -113,7 +297,7 @@ pub trait QueryDispatcher: Send + Sync { ) -> Result, Unsupported>; /// Construct a `QueryDispatcher` that falls back on `other` for cases not handled by `self` - fn chain(self, other: U) -> QueryDispatcherChain + fn chain(self, other: U) -> QueryDispatcherChain where Self: Sized, { @@ -148,7 +332,90 @@ pub trait QueryDispatcher: Send + Sync { ) -> Result, Unsupported>; } +impl QueryDispatcher for T { + fn intersection_test( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + ::intersection_test(self, self, pos12, g1, g2) + } + + fn distance( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + ::distance(self, self, pos12, g1, g2) + } + + fn contact( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + ) -> Result, Unsupported> { + ::contact(self, self, pos12, g1, g2, prediction) + } + + fn closest_points( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + max_dist: Real, + ) -> Result { + ::closest_points(self, self, pos12, g1, g2, max_dist) + } + + fn cast_shapes( + &self, + pos12: &Isometry, + local_vel12: &Vector, + g1: &dyn Shape, + g2: &dyn Shape, + options: ShapeCastOptions, + ) -> Result, Unsupported> { + ::cast_shapes( + self, + self, + pos12, + local_vel12, + g1, + g2, + options, + ) + } + + fn cast_shapes_nonlinear( + &self, + motion1: &NonlinearRigidMotion, + g1: &dyn Shape, + motion2: &NonlinearRigidMotion, + g2: &dyn Shape, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + ) -> Result, Unsupported> { + ::cast_shapes_nonlinear( + self, + self, + motion1, + g1, + motion2, + g2, + start_time, + end_time, + stop_at_penetration, + ) + } +} + /// The composition of two dispatchers +#[derive(Clone, Copy, Debug)] pub struct QueryDispatcherChain(T, U); macro_rules! chain_method { @@ -161,20 +428,23 @@ macro_rules! chain_method { } } -impl QueryDispatcher for QueryDispatcherChain +impl QueryDispatcherComposite for QueryDispatcherChain where - T: QueryDispatcher, - U: QueryDispatcher, + T: QueryDispatcherComposite, + U: QueryDispatcherComposite, { chain_method!(intersection_test( + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape, ) -> bool); - chain_method!(distance(pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape,) -> Real); + chain_method!(distance( + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape,) -> Real); chain_method!(contact( + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape, @@ -182,6 +452,7 @@ where ) -> Option); chain_method!(closest_points( + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape, @@ -189,6 +460,7 @@ where ) -> ClosestPoints); chain_method!(cast_shapes( + root_dispatcher: &dyn QueryDispatcher, pos12: &Isometry, vel12: &Vector, g1: &dyn Shape, @@ -197,6 +469,7 @@ where ) -> Option); chain_method!(cast_shapes_nonlinear( + root_dispatcher: &dyn QueryDispatcher, motion1: &NonlinearRigidMotion, g1: &dyn Shape, motion2: &NonlinearRigidMotion, @@ -208,13 +481,14 @@ where } #[cfg(feature = "std")] -impl PersistentQueryDispatcher +impl PersistentQueryDispatcherComposite for QueryDispatcherChain where - T: PersistentQueryDispatcher, - U: PersistentQueryDispatcher, + T: PersistentQueryDispatcherComposite, + U: PersistentQueryDispatcherComposite, { chain_method!(contact_manifolds( + root_dispatcher: &dyn PersistentQueryDispatcher, pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape,