Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Composable QueryDispatcher #218

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Composable QueryDispatcher #218

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3ec441c
Composable QueryDispatcher
Neo-Zhixing Jul 2, 2024
ad528de
Deriving common traits
Neo-Zhixing Jul 2, 2024
fc99c1a
Add docs
Neo-Zhixing Jul 20, 2024
0acf2cb
More docs
Neo-Zhixing Jul 20, 2024
74d655a
fix for no-std
Neo-Zhixing Jul 20, 2024
a4e412a
fix no-std
Neo-Zhixing Jul 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
135 changes: 105 additions & 30 deletions src/query/default_query_dispatcher.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
Expand Down Expand Up @@ -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,
));
}

Expand All @@ -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<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
Expand Down Expand Up @@ -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,
));
}

Expand All @@ -139,6 +153,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {

fn contact(
&self,
root_dispatcher: &dyn QueryDispatcher,
pos12: &Isometry<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
Expand Down Expand Up @@ -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,
));
}

Expand All @@ -195,6 +218,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {

fn closest_points(
&self,
root_dispatcher: &dyn QueryDispatcher,
pos12: &Isometry<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
Expand Down Expand Up @@ -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,
));
}

Expand All @@ -271,6 +303,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {

fn cast_shapes(
&self,
root_dispatcher: &dyn QueryDispatcher,
pos12: &Isometry<Real>,
local_vel12: &Vector<Real>,
shape1: &dyn Shape,
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -360,6 +393,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {

fn cast_shapes_nonlinear(
&self,
root_dispatcher: &dyn QueryDispatcher,
motion1: &NonlinearRigidMotion,
shape1: &dyn Shape,
motion2: &NonlinearRigidMotion,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -418,14 +461,15 @@ impl QueryDispatcher for DefaultQueryDispatcher {
}

#[cfg(feature = "std")]
impl<ManifoldData, ContactData> PersistentQueryDispatcher<ManifoldData, ContactData>
impl<ManifoldData, ContactData> PersistentQueryDispatcherComposite<ManifoldData, ContactData>
for DefaultQueryDispatcher
where
ManifoldData: Default + Clone,
ContactData: Default + Copy,
{
fn contact_manifolds(
&self,
root_dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
pos12: &Isometry<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
Expand All @@ -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(());
Expand All @@ -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(),
Expand All @@ -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(),
Expand All @@ -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,
Expand All @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions src/query/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
Loading