From 870d9e8f1cd266247f474611ed0e1e949f26fbd9 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 18 Feb 2025 17:50:52 +0100 Subject: [PATCH 1/7] empty dispatcher ; seems complicated to implement. --- src/query/composable_dispatcher.rs | 93 ++++++++++++++++++++++++++++++ src/query/mod.rs | 1 + 2 files changed, 94 insertions(+) create mode 100644 src/query/composable_dispatcher.rs diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs new file mode 100644 index 00000000..3ca67036 --- /dev/null +++ b/src/query/composable_dispatcher.rs @@ -0,0 +1,93 @@ +//! Logic to dispatch queries to the appropriate handler. +//! +//! This can be customized to add support to your own types. + +use crate::{ + math::{Isometry, Real}, + query::QueryDispatcher, + shape::{Shape, ShapeType}, +}; + +use super::{ContactManifold, PersistentQueryDispatcher}; + +pub struct DispatchDefinition { + pub type_arg1: ShapeType, + pub type_arg2: ShapeType, + pub func: Box< + dyn Fn( + &Isometry, + &dyn Shape, + &dyn Shape, + Real, + &mut ContactManifold, + ), + >, +} + +pub struct ComposableQueryDispatcher { + dispatchers: Vec<((ShapeType, ShapeType), Vec>)>, +} + +impl QueryDispatcher for ComposableQueryDispatcher { + fn intersection_test( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + todo!() + } + + fn distance( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + todo!() + } + + fn contact( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + ) -> Result, super::Unsupported> { + todo!() + } + + fn closest_points( + &self, + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, + max_dist: Real, + ) -> Result { + todo!() + } + + fn cast_shapes( + &self, + pos12: &Isometry, + local_vel12: &crate::math::Vector, + g1: &dyn Shape, + g2: &dyn Shape, + options: super::ShapeCastOptions, + ) -> Result, super::Unsupported> { + todo!() + } + + fn cast_shapes_nonlinear( + &self, + motion1: &super::NonlinearRigidMotion, + g1: &dyn Shape, + motion2: &super::NonlinearRigidMotion, + g2: &dyn Shape, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + ) -> Result, super::Unsupported> { + todo!() + } +} diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..d31be8b0 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -46,6 +46,7 @@ pub use self::split::{IntersectResult, SplitResult}; mod clip; pub mod closest_points; +pub mod composable_dispatcher; pub mod contact; #[cfg(feature = "std")] mod contact_manifolds; From 08856a9775b3cd18bfc04eb3b7f8186fa465c7e3 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 24 Feb 2025 16:56:14 +0100 Subject: [PATCH 2/7] POC dispatcher functions on intersection test ; it seems convoluted and I'm not sure about perfs --- src/query/composable_dispatcher.rs | 673 ++++++++++++++++++++++++++--- 1 file changed, 622 insertions(+), 51 deletions(-) diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs index 3ca67036..32cc0972 100644 --- a/src/query/composable_dispatcher.rs +++ b/src/query/composable_dispatcher.rs @@ -2,92 +2,663 @@ //! //! This can be customized to add support to your own types. -use crate::{ - math::{Isometry, Real}, - query::QueryDispatcher, - shape::{Shape, ShapeType}, +use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::details::ShapeCastOptions; +use crate::query::{ + self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, + QueryDispatcher, ShapeCastHit, Unsupported, }; +#[cfg(feature = "std")] +use crate::query::{ + contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, + query_dispatcher::PersistentQueryDispatcher, + ContactManifold, +}; +use crate::shape::{Ball, HalfSpace, Segment, Shape, ShapeType}; -use super::{ContactManifold, PersistentQueryDispatcher}; - -pub struct DispatchDefinition { - pub type_arg1: ShapeType, - pub type_arg2: ShapeType, - pub func: Box< - dyn Fn( - &Isometry, - &dyn Shape, - &dyn Shape, - Real, - &mut ContactManifold, - ), - >, -} - -pub struct ComposableQueryDispatcher { - dispatchers: Vec<((ShapeType, ShapeType), Vec>)>, -} +/// A dispatcher that exposes built-in queries +#[derive(Debug, Clone)] +pub struct ComposableQueryDispatcher; impl QueryDispatcher for ComposableQueryDispatcher { fn intersection_test( &self, pos12: &Isometry, - g1: &dyn Shape, - g2: &dyn Shape, - ) -> Result { - todo!() + shape1: &dyn Shape, + shape2: &dyn Shape, + ) -> Result { + /// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. + // TODO: this would be great to have as const, or a macro. + fn to_as_shape<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( + inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, + ) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> + { + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + to_custom( + |s: &'a dyn Shape| s.as_shape::(), + |s: &'b dyn Shape| s.as_shape::(), + inner, + )(pose, s1, s2) + }, + ) + } + /// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. + // TODO: this would be great to have as const, or a macro. + fn to_custom<'c, 'a: 'c, 'b: 'c, ShapeIn1: 'a, ShapeIn2: 'b>( + shape_1: impl Fn(&'a dyn Shape) -> Option + 'a, + shape_2: impl Fn(&'b dyn Shape) -> Option + 'b, + inner: impl Fn(&Isometry, ShapeIn1, ShapeIn2) -> bool + 'c, + ) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> + { + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = shape_1(s1).ok_or(())?; + let shape2 = shape_2(s2).ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + ) + } + + let functions = vec![ + Box::new(|pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + Ok(query::details::intersection_test_ball_ball( + &pose.translation.vector.into(), + s1.as_ball().ok_or(())?, + s2.as_ball().ok_or(())?, + )) + }), + to_as_shape(query::details::intersection_test_cuboid_cuboid), + to_as_shape(query::details::intersection_test_cuboid_triangle), + to_as_shape(query::details::intersection_test_triangle_cuboid), + to_custom( + |s: &dyn Shape| s.as_shape::(), + |s: &dyn Shape| Some(s), + query::details::intersection_test_ball_point_query, + ), + to_custom( + |s: &dyn Shape| Some(s), + |s: &dyn Shape| s.as_shape::(), + query::details::intersection_test_point_query_ball, + ), + to_custom( + |s: &dyn Shape| s.as_shape::(), + |s: &dyn Shape| s.as_support_map(), + query::details::intersection_test_halfspace_support_map, + ), + to_custom( + |s: &dyn Shape| s.as_support_map(), + |s: &dyn Shape| s.as_shape::(), + query::details::intersection_test_support_map_halfspace, + ), + ]; + + /* + // TODO: These composite shapes are harder to implement due to taking dispatcher as argument. + #[cfg(feature = "std")] + { + functions.push({ + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = (|s: &dyn Shape| s.as_composite_shape())(s1).ok_or(())?; + let shape2 = (|s: &dyn Shape| Some(s))(s2).ok_or(())?; + Ok(query::details::intersection_test_composite_shape_shape( + self, pose, shape1, shape2, + )) + }, + ) + }); + functions.push({ + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = (|s: &dyn Shape| Some(s))(s2).ok_or(())?; + let shape2 = (|s: &dyn Shape| s.as_composite_shape())(s1).ok_or(())?; + Ok(query::details::intersection_test_shape_composite_shape( + self, pose, shape1, shape2, + )) + }, + ) + }); + }*/ + + for f in functions { + if let Ok(intersects) = f(pos12, shape1, shape2) { + return Ok(intersects); + } + } + Err(Unsupported) } + /// Computes the minimum distance separating two shapes. + /// + /// Returns `0.0` if the objects are touching or penetrating. fn distance( &self, pos12: &Isometry, - g1: &dyn Shape, - g2: &dyn Shape, - ) -> Result { - todo!() + shape1: &dyn Shape, + shape2: &dyn Shape, + ) -> Result { + let ball1 = shape1.as_ball(); + let ball2 = shape2.as_ball(); + + if let (Some(b1), Some(b2)) = (ball1, ball2) { + let p2 = Point::from(pos12.translation.vector); + Ok(query::details::distance_ball_ball(b1, &p2, b2)) + } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { + Ok(query::details::distance_ball_convex_polyhedron( + pos12, b1, shape2, + )) + } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { + Ok(query::details::distance_convex_polyhedron_ball( + pos12, shape1, b2, + )) + } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { + Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2)) + } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) { + Ok(query::details::distance_segment_segment(pos12, s1, s2)) + } else if let (Some(p1), Some(s2)) = + (shape1.as_shape::(), shape2.as_support_map()) + { + Ok(query::details::distance_halfspace_support_map( + pos12, p1, s2, + )) + } else if let (Some(s1), Some(p2)) = + (shape1.as_support_map(), shape2.as_shape::()) + { + Ok(query::details::distance_support_map_halfspace( + pos12, s1, p2, + )) + } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { + Ok(query::details::distance_support_map_support_map( + pos12, s1, s2, + )) + } else { + #[cfg(feature = "std")] + if let Some(c1) = shape1.as_composite_shape() { + return Ok(query::details::distance_composite_shape_shape( + self, pos12, c1, shape2, + )); + } else if let Some(c2) = shape2.as_composite_shape() { + return Ok(query::details::distance_shape_composite_shape( + self, pos12, shape1, c2, + )); + } + + Err(Unsupported) + } } fn contact( &self, pos12: &Isometry, - g1: &dyn Shape, - g2: &dyn Shape, + shape1: &dyn Shape, + shape2: &dyn Shape, prediction: Real, - ) -> Result, super::Unsupported> { - todo!() + ) -> Result, Unsupported> { + let ball1 = shape1.as_ball(); + let ball2 = shape2.as_ball(); + + if let (Some(b1), Some(b2)) = (ball1, ball2) { + Ok(query::details::contact_ball_ball(pos12, b1, b2, prediction)) + // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { + // Ok(query::details::contact_cuboid_cuboid( + // pos12, c1, c2, prediction, + // )) + } else if let (Some(p1), Some(s2)) = + (shape1.as_shape::(), shape2.as_support_map()) + { + Ok(query::details::contact_halfspace_support_map( + pos12, p1, s2, prediction, + )) + } else if let (Some(s1), Some(p2)) = + (shape1.as_support_map(), shape2.as_shape::()) + { + Ok(query::details::contact_support_map_halfspace( + pos12, s1, p2, prediction, + )) + } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { + Ok(query::details::contact_ball_convex_polyhedron( + pos12, b1, shape2, prediction, + )) + } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { + Ok(query::details::contact_convex_polyhedron_ball( + pos12, shape1, b2, prediction, + )) + } else { + #[cfg(feature = "std")] + if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { + return Ok(query::details::contact_support_map_support_map( + pos12, s1, s2, prediction, + )); + } else if let Some(c1) = shape1.as_composite_shape() { + return Ok(query::details::contact_composite_shape_shape( + self, 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, + )); + } + + Err(Unsupported) + } } fn closest_points( &self, pos12: &Isometry, - g1: &dyn Shape, - g2: &dyn Shape, + shape1: &dyn Shape, + shape2: &dyn Shape, max_dist: Real, - ) -> Result { - todo!() + ) -> Result { + let ball1 = shape1.as_ball(); + let ball2 = shape2.as_ball(); + + if let (Some(b1), Some(b2)) = (ball1, ball2) { + Ok(query::details::closest_points_ball_ball( + pos12, b1, b2, max_dist, + )) + } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { + Ok(query::details::closest_points_ball_convex_polyhedron( + pos12, b1, shape2, max_dist, + )) + } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { + Ok(query::details::closest_points_convex_polyhedron_ball( + pos12, shape1, b2, max_dist, + )) + } else if let (Some(s1), Some(s2)) = + (shape1.as_shape::(), shape2.as_shape::()) + { + Ok(query::details::closest_points_segment_segment( + pos12, s1, s2, max_dist, + )) + // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { + // Ok(query::details::closest_points_cuboid_cuboid( + // pos12, c1, c2, max_dist, + // )) + } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) { + Ok(query::details::closest_points_segment_segment( + pos12, s1, s2, max_dist, + )) + // } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) { + // Ok(query::details::closest_points_cuboid_triangle( + // pos12, c1, t2, max_dist, + // )) + } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) { + Ok(query::details::closest_points_triangle_cuboid( + pos12, t1, c2, max_dist, + )) + } else if let (Some(p1), Some(s2)) = + (shape1.as_shape::(), shape2.as_support_map()) + { + Ok(query::details::closest_points_halfspace_support_map( + pos12, p1, s2, max_dist, + )) + } else if let (Some(s1), Some(p2)) = + (shape1.as_support_map(), shape2.as_shape::()) + { + Ok(query::details::closest_points_support_map_halfspace( + pos12, s1, p2, max_dist, + )) + } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { + Ok(query::details::closest_points_support_map_support_map( + pos12, s1, s2, max_dist, + )) + } else { + #[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, + )); + } else if let Some(c2) = shape2.as_composite_shape() { + return Ok(query::details::closest_points_shape_composite_shape( + self, pos12, shape1, c2, max_dist, + )); + } + + Err(Unsupported) + } } fn cast_shapes( &self, pos12: &Isometry, - local_vel12: &crate::math::Vector, - g1: &dyn Shape, - g2: &dyn Shape, - options: super::ShapeCastOptions, - ) -> Result, super::Unsupported> { - todo!() + local_vel12: &Vector, + shape1: &dyn Shape, + shape2: &dyn Shape, + options: ShapeCastOptions, + ) -> Result, Unsupported> { + if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) { + Ok(query::details::cast_shapes_ball_ball( + pos12, + local_vel12, + b1, + b2, + options, + )) + } else if let (Some(p1), Some(s2)) = + (shape1.as_shape::(), shape2.as_support_map()) + { + Ok(query::details::cast_shapes_halfspace_support_map( + pos12, + local_vel12, + p1, + s2, + options, + )) + } else if let (Some(s1), Some(p2)) = + (shape1.as_support_map(), shape2.as_shape::()) + { + Ok(query::details::cast_shapes_support_map_halfspace( + pos12, + local_vel12, + s1, + p2, + options, + )) + } else { + #[cfg(feature = "std")] + if let Some(heightfield1) = shape1.as_heightfield() { + return query::details::cast_shapes_heightfield_shape( + self, + pos12, + local_vel12, + heightfield1, + shape2, + options, + ); + } else if let Some(heightfield2) = shape2.as_heightfield() { + return query::details::cast_shapes_shape_heightfield( + self, + pos12, + local_vel12, + shape1, + heightfield2, + options, + ); + } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) + { + return Ok(query::details::cast_shapes_support_map_support_map( + pos12, + local_vel12, + s1, + s2, + options, + )); + } else if let Some(c1) = shape1.as_composite_shape() { + return Ok(query::details::cast_shapes_composite_shape_shape( + self, + pos12, + local_vel12, + c1, + shape2, + options, + )); + } else if let Some(c2) = shape2.as_composite_shape() { + return Ok(query::details::cast_shapes_shape_composite_shape( + self, + pos12, + local_vel12, + shape1, + c2, + options, + )); + } + + Err(Unsupported) + } } fn cast_shapes_nonlinear( &self, - motion1: &super::NonlinearRigidMotion, - g1: &dyn Shape, - motion2: &super::NonlinearRigidMotion, - g2: &dyn Shape, + motion1: &NonlinearRigidMotion, + shape1: &dyn Shape, + motion2: &NonlinearRigidMotion, + shape2: &dyn Shape, start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Result, super::Unsupported> { - todo!() + ) -> Result, Unsupported> { + if let (Some(sm1), Some(sm2)) = (shape1.as_support_map(), shape2.as_support_map()) { + let mode = if stop_at_penetration { + NonlinearShapeCastMode::StopAtPenetration + } else { + NonlinearShapeCastMode::directional_toi(shape1, shape2) + }; + + Ok( + query::details::cast_shapes_nonlinear_support_map_support_map( + self, 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, + motion1, + c1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + )); + } else if let Some(c2) = shape2.as_composite_shape() { + return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape( + self, + motion1, + shape1, + motion2, + c2, + start_time, + end_time, + stop_at_penetration, + )); + } + /* } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) { + // query::details::cast_shapes_nonlinear_halfspace_support_map(m1, vel1, p1, m2, vel2, s2) + unimplemented!() + } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) { + // query::details::cast_shapes_nonlinear_support_map_halfspace(m1, vel1, s1, m2, vel2, p2) + unimplemented!() */ + + Err(Unsupported) + } + } +} + +#[cfg(feature = "std")] +impl PersistentQueryDispatcher + for ComposableQueryDispatcher +where + ManifoldData: Default + Clone, + ContactData: Default + Copy, +{ + fn contact_manifolds( + &self, + pos12: &Isometry, + shape1: &dyn Shape, + shape2: &dyn Shape, + prediction: Real, + manifolds: &mut Vec>, + workspace: &mut Option, + ) -> Result<(), Unsupported> { + use crate::query::contact_manifolds::*; + + let composite1 = shape1.as_composite_shape(); + let composite2 = shape2.as_composite_shape(); + + if let (Some(composite1), Some(composite2)) = (composite1, composite2) { + contact_manifolds_composite_shape_composite_shape( + self, pos12, composite1, composite2, prediction, manifolds, workspace, + ); + + return Ok(()); + } + + match (shape1.shape_type(), shape2.shape_type()) { + (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => { + contact_manifolds_trimesh_shape_shapes( + self, pos12, shape1, shape2, prediction, manifolds, workspace, + ); + } + (ShapeType::HeightField, _) => { + if let Some(composite2) = composite2 { + contact_manifolds_heightfield_composite_shape( + self, + pos12, + &pos12.inverse(), + shape1.as_heightfield().unwrap(), + composite2, + prediction, + manifolds, + workspace, + false, + ) + } else { + contact_manifolds_heightfield_shape_shapes( + self, pos12, shape1, shape2, prediction, manifolds, workspace, + ); + } + } + (_, ShapeType::HeightField) => { + if let Some(composite1) = composite1 { + contact_manifolds_heightfield_composite_shape( + self, + &pos12.inverse(), + pos12, + shape2.as_heightfield().unwrap(), + composite1, + prediction, + manifolds, + workspace, + true, + ) + } else { + contact_manifolds_heightfield_shape_shapes( + self, pos12, shape1, shape2, prediction, manifolds, workspace, + ); + } + } + _ => { + if let Some(composite1) = composite1 { + contact_manifolds_composite_shape_shape( + self, pos12, composite1, shape2, prediction, manifolds, workspace, false, + ); + } else if let Some(composite2) = composite2 { + contact_manifolds_composite_shape_shape( + self, + &pos12.inverse(), + composite2, + shape1, + prediction, + manifolds, + workspace, + true, + ); + } else { + if manifolds.is_empty() { + manifolds.push(ContactManifold::new()); + } + + return self.contact_manifold_convex_convex( + pos12, + shape1, + shape2, + None, + None, + prediction, + &mut manifolds[0], + ); + } + } + } + + Ok(()) + } + + fn contact_manifold_convex_convex( + &self, + pos12: &Isometry, + shape1: &dyn Shape, + shape2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, + prediction: Real, + manifold: &mut ContactManifold, + ) -> Result<(), Unsupported> { + use crate::query::contact_manifolds::*; + + match (shape1.shape_type(), shape2.shape_type()) { + (ShapeType::Ball, ShapeType::Ball) => { + contact_manifold_ball_ball_shapes(pos12, shape1, shape2, prediction, manifold) + } + (ShapeType::Cuboid, ShapeType::Cuboid) => + contact_manifold_cuboid_cuboid_shapes(pos12, shape1, shape2, prediction, manifold) + , + // (ShapeType::Polygon, ShapeType::Polygon) => ( + // PrimitiveContactGenerator { + // generate_contacts: super::generate_contacts_polygon_polygon, + // ..PrimitiveContactGenerator::default() + // }, + // None, + // ), + (ShapeType::Capsule, ShapeType::Capsule) => { + contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold) + } + (_, ShapeType::Ball) | (ShapeType::Ball, _) => { + contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold) + } + // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => + // contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold), + (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { + contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold) + } + (ShapeType::HalfSpace, _) => { + if let Some((pfm2, border_radius2)) = shape2.as_polygonal_feature_map() { + contact_manifold_halfspace_pfm( + pos12, + shape1.as_halfspace().unwrap(), + pfm2, + border_radius2, + prediction, + manifold, + false + ) + } else { + return Err(Unsupported) + } + } + (_, ShapeType::HalfSpace) => { + if let Some((pfm1, border_radius1)) = shape1.as_polygonal_feature_map() { + contact_manifold_halfspace_pfm( + &pos12.inverse(), + shape2.as_halfspace().unwrap(), + pfm1, + border_radius1, + prediction, + manifold, + true + ) + } else { + return Err(Unsupported) + } + } + _ => { + if let (Some(pfm1), Some(pfm2)) = ( + shape1.as_polygonal_feature_map(), + shape2.as_polygonal_feature_map(), + ) { + contact_manifold_pfm_pfm( + pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold, + ) + } else { + return Err(Unsupported); + } + } + } + + Ok(()) } } From d47a532553b757be7e2e17321e90a131c8383ac4 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 6 Mar 2025 09:59:57 +0100 Subject: [PATCH 3/7] attempt to use type_id --- src/query/composable_dispatcher.rs | 147 +++++++++++++----- .../function_dispatch.rs | 144 +++++++++++++++++ .../composable_dispatcher/intersection.rs | 38 +++++ src/query/composable_dispatcher/tests.rs | 29 ++++ src/query/mod.rs | 1 + 5 files changed, 322 insertions(+), 37 deletions(-) create mode 100644 src/query/composable_dispatcher/function_dispatch.rs create mode 100644 src/query/composable_dispatcher/intersection.rs create mode 100644 src/query/composable_dispatcher/tests.rs diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs index 32cc0972..9998fe6c 100644 --- a/src/query/composable_dispatcher.rs +++ b/src/query/composable_dispatcher.rs @@ -2,6 +2,15 @@ //! //! This can be customized to add support to your own types. +mod function_dispatch; +mod intersection; +#[cfg(test)] +mod tests; + +use core::any::TypeId; + +use function_dispatch::DispatcherTypeKey; + use crate::math::{Isometry, Point, Real, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{ @@ -14,7 +23,7 @@ use crate::query::{ query_dispatcher::PersistentQueryDispatcher, ContactManifold, }; -use crate::shape::{Ball, HalfSpace, Segment, Shape, ShapeType}; +use crate::shape::*; /// A dispatcher that exposes built-in queries #[derive(Debug, Clone)] @@ -27,6 +36,20 @@ impl QueryDispatcher for ComposableQueryDispatcher { shape1: &dyn Shape, shape2: &dyn Shape, ) -> Result { + /// Transform a function that takes two concrete shapes to a hashmap key value pair. + // TODO: this would be great to have as const, or a macro. + fn to_kv<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( + inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, + ) -> ( + DispatcherTypeKey, + Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>, + ) { + ( + DispatcherTypeKey(TypeId::of::(), TypeId::of::()), + to_as_shape(inner), + ) + } + /// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. // TODO: this would be great to have as const, or a macro. fn to_as_shape<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( @@ -60,39 +83,89 @@ impl QueryDispatcher for ComposableQueryDispatcher { ) } - let functions = vec![ - Box::new(|pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - Ok(query::details::intersection_test_ball_ball( - &pose.translation.vector.into(), - s1.as_ball().ok_or(())?, - s2.as_ball().ok_or(())?, - )) - }), - to_as_shape(query::details::intersection_test_cuboid_cuboid), - to_as_shape(query::details::intersection_test_cuboid_triangle), - to_as_shape(query::details::intersection_test_triangle_cuboid), - to_custom( - |s: &dyn Shape| s.as_shape::(), - |s: &dyn Shape| Some(s), - query::details::intersection_test_ball_point_query, - ), - to_custom( - |s: &dyn Shape| Some(s), - |s: &dyn Shape| s.as_shape::(), - query::details::intersection_test_point_query_ball, + let mut intersection_test_impls: std::collections::HashMap = + std::collections::HashMap::new(); + for (kv, function) in vec![ + to_kv(query::details::intersection_test_cuboid_cuboid), + ( + DispatcherTypeKey(TypeId::of::(), TypeId::of::()), + (Box::new(|pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + Ok(query::details::intersection_test_ball_ball( + &pose.translation.vector.into(), + s1.as_ball().ok_or(())?, + s2.as_ball().ok_or(())?, + )) + })), ), - to_custom( - |s: &dyn Shape| s.as_shape::(), - |s: &dyn Shape| s.as_support_map(), - query::details::intersection_test_halfspace_support_map, - ), - to_custom( - |s: &dyn Shape| s.as_support_map(), - |s: &dyn Shape| s.as_shape::(), - query::details::intersection_test_support_map_halfspace, + to_kv(query::details::intersection_test_cuboid_triangle), + to_kv(query::details::intersection_test_triangle_cuboid), + ( + DispatcherTypeKey(TypeId::of::(), TypeId::of::()), + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_ball().ok_or(())?; + Ok(query::details::intersection_test_ball_point_query( + pose, shape1, s2, + )) + }, + ), ), - ]; + ] { + assert!(intersection_test_impls.insert(kv, function).is_none()); + } + let types = vec![ + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + #[cfg(feature = "dim2")] + #[cfg(feature = "std")] + TypeId::of::(), + #[cfg(feature = "dim3")] + #[cfg(feature = "std")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + #[cfg(feature = "dim2")] + #[cfg(feature = "std")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + #[cfg(feature = "std")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + TypeId::of::>(), + // TODO: DilatedShapes... (and dilatedShpaed of roundshapes) (.. and I guess roundshapes of dilatedshapes ; and then probably recursively...) + ]; + for &type1 in types.iter() { + for &type2 in types.iter() { + let key = DispatcherTypeKey(type1, type2); + if intersection_test_impls.get(&key).is_none() { + println!("key {key:?} not found!"); + } + } + } /* // TODO: These composite shapes are harder to implement due to taking dispatcher as argument. #[cfg(feature = "std")] @@ -121,12 +194,12 @@ impl QueryDispatcher for ComposableQueryDispatcher { }); }*/ - for f in functions { - if let Ok(intersects) = f(pos12, shape1, shape2) { - return Ok(intersects); - } - } - Err(Unsupported) + let param_types = DispatcherTypeKey(shape1.type_id(), shape2.type_id()); + let Some(intersection_test_impl) = intersection_test_impls.get(¶m_types) else { + dbg!("key not found!"); + return Err(Unsupported); + }; + intersection_test_impl(pos12, shape1, shape2).map_err(|_| Unsupported) } /// Computes the minimum distance separating two shapes. diff --git a/src/query/composable_dispatcher/function_dispatch.rs b/src/query/composable_dispatcher/function_dispatch.rs new file mode 100644 index 00000000..cb871cc7 --- /dev/null +++ b/src/query/composable_dispatcher/function_dispatch.rs @@ -0,0 +1,144 @@ +use core::any::TypeId; +use std::collections::HashMap; + +use crate::{ + math::{Isometry, Real}, + shape::Shape, +}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub struct DispatcherTypeKey(pub TypeId, pub TypeId); + +pub struct FunctionDispatchFistParam<'c, 'a: 'c, 'b: 'c> { + /// Map of functions to call when we know how to handle the first parameter. + pub map_first_param: HashMap>, + /// Functions to attempt to call when we don't know how to handle the first parameter. + pub dispatch_unknown_first_param: FunctionDispatchSecondParam<'c, 'a, 'b>, +} + +pub struct FunctionDispatchSecondParam<'c, 'a: 'c, 'b: 'c> { + /// Map of functions to call when we know how to handle the second parameter. + pub map_second_param: HashMap< + TypeId, + Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>, + >, + /// Functions to attempt to call when we don't know how to handle the second parameter. + pub dispatch_unknown_second_param: + Vec, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>>, +} + +/// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. +// TODO: this would be great to have as const, or a macro. +fn to_as_shape<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( + inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, +) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> { + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + to_custom( + |s: &'a dyn Shape| s.as_shape::(), + |s: &'b dyn Shape| s.as_shape::(), + inner, + )(pose, s1, s2) + }, + ) +} +/// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. +// TODO: this would be great to have as const, or a macro. +fn to_custom<'c, 'a: 'c, 'b: 'c, ShapeIn1: 'a, ShapeIn2: 'b>( + shape_1: impl Fn(&'a dyn Shape) -> Option + 'a, + shape_2: impl Fn(&'b dyn Shape) -> Option + 'b, + inner: impl Fn(&Isometry, ShapeIn1, ShapeIn2) -> bool + 'c, +) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> { + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = shape_1(s1).ok_or(())?; + let shape2 = shape_2(s2).ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + ) +} + +impl<'c, 'a: 'c, 'b: 'c> FunctionDispatchFistParam<'c, 'a, 'b> { + pub fn new() -> Self { + Self { + map_first_param: HashMap::new(), + dispatch_unknown_first_param: FunctionDispatchSecondParam { + map_second_param: HashMap::new(), + dispatch_unknown_second_param: Vec::new(), + }, + } + } + pub fn add_function_known_12( + &mut self, + inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, + ) { + let dispatch_second = self + .map_first_param + .entry(TypeId::of::()) + .or_insert_with(|| FunctionDispatchSecondParam { + map_second_param: HashMap::new(), + dispatch_unknown_second_param: Vec::new(), + }); + _ = dispatch_second + .map_second_param + .insert(TypeId::of::(), to_as_shape(inner)); + } + pub fn add_function_known_1( + &mut self, + inner: impl Fn(&Isometry, &'a S1, &'b dyn Shape) -> bool + 'c + Copy, + ) { + let dispatch_second = self + .map_first_param + .entry(TypeId::of::()) + .or_insert_with(|| FunctionDispatchSecondParam { + map_second_param: HashMap::new(), + dispatch_unknown_second_param: Vec::new(), + }); + dispatch_second.dispatch_unknown_second_param.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + to_custom( + |s: &'a dyn Shape| s.as_shape::(), + |s: &'b dyn Shape| Some(s), + inner, + )(pose, s1, s2) + }, + )); + } + + pub fn add_function_known_2( + &mut self, + inner: impl Fn(&Isometry, &'a dyn Shape, &'b S2) -> bool + 'c + Copy, + ) { + _ = self + .dispatch_unknown_first_param + .map_second_param + .entry(TypeId::of::()) + .or_insert_with(|| { + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + to_custom( + |s: &'a dyn Shape| Some(s), + |s: &'b dyn Shape| s.as_shape::(), + inner, + )(pose, s1, s2) + }, + ) + }); + } + pub fn add_function_all_unknown( + &mut self, + inner: impl Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> bool + 'c + Copy, + ) { + self.dispatch_unknown_first_param + .dispatch_unknown_second_param + .push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + to_custom( + |s: &'a dyn Shape| Some(s), + |s: &'b dyn Shape| Some(s), + inner, + )(pose, s1, s2) + }, + )); + } +} diff --git a/src/query/composable_dispatcher/intersection.rs b/src/query/composable_dispatcher/intersection.rs new file mode 100644 index 00000000..5af323a4 --- /dev/null +++ b/src/query/composable_dispatcher/intersection.rs @@ -0,0 +1,38 @@ +use crate::{math::Point, query}; + +use super::function_dispatch::FunctionDispatchFistParam; + +pub fn create_intersection_dispatcher() -> FunctionDispatchFistParam<'static, 'static, 'static> { + let mut dispatcher = FunctionDispatchFistParam::new(); + + // Register intersection functions here. + + dispatcher.add_function_known_12(query::details::intersection_test_cuboid_cuboid); + dispatcher.add_function_known_12(query::details::intersection_test_cuboid_segment); + dispatcher.add_function_known_12(query::details::intersection_test_segment_cuboid); + dispatcher.add_function_known_12(query::details::intersection_test_cuboid_triangle); + dispatcher.add_function_known_12(query::details::intersection_test_triangle_cuboid); + dispatcher.add_function_known_12(|pos12, b1, b2| { + let p12 = Point::from(pos12.translation.vector); + query::details::intersection_test_ball_ball(&p12, b1, b2) + }); + dispatcher.add_function_known_12(query::details::intersection_test_segment_cuboid); + dispatcher.add_function_known_1(query::details::intersection_test_ball_point_query); + dispatcher.add_function_known_2(query::details::intersection_test_point_query_ball); + + /* + // Those can't work because SupportMap is not Shape... + // So we'd need to rely on Any... + // but then `Any`` doesn't support casting to traits easily: + // - we'd need to box the shapes + // - or use a trait to cast to the right trait? + dispatcher.add_function_known_1(query::details::intersection_test_halfspace_support_map); + dispatcher.add_function_known_2(query::details::intersection_test_support_map_halfspace); + + dispatcher.add_function_all_unknown(query::details::intersection_test_support_map_support_map); + dispatcher.add_function_known_2(query::details::intersection_test_support_map_support_map); + */ + dispatcher +} + +fn cast_params() {} diff --git a/src/query/composable_dispatcher/tests.rs b/src/query/composable_dispatcher/tests.rs new file mode 100644 index 00000000..536f393a --- /dev/null +++ b/src/query/composable_dispatcher/tests.rs @@ -0,0 +1,29 @@ +extern crate nalgebra as na; + +#[cfg(feature = "dim2")] +mod tests2d { + use crate::query::composable_dispatcher::ComposableQueryDispatcher; + use crate::query::QueryDispatcher; + use crate::shape::{Ball, Cuboid}; + use na::{Isometry2, Vector2}; + + #[test] + fn intersection_test() { + let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let ball = Ball::new(1.0); + + let cuboid_pos = Isometry2::identity(); + let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); + let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); + + let pos12 = ball_pos_intersecting.inv_mul(&cuboid_pos); + assert!(ComposableQueryDispatcher + .intersection_test(&pos12, &ball, &cuboid) + .unwrap()); + + let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); + assert!(!ComposableQueryDispatcher + .intersection_test(&pos12, &ball, &cuboid) + .unwrap()); + } +} diff --git a/src/query/mod.rs b/src/query/mod.rs index d31be8b0..8fd14edd 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -46,6 +46,7 @@ pub use self::split::{IntersectResult, SplitResult}; mod clip; pub mod closest_points; +#[cfg(feature = "std")] pub mod composable_dispatcher; pub mod contact; #[cfg(feature = "std")] From ef1bae12672379f519dd3b9a7303486e99260f7d Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 6 Mar 2025 15:45:36 +0100 Subject: [PATCH 4/7] dispatcher progress --- src/query/composable_dispatcher.rs | 178 +--------- .../function_dispatch.rs | 328 ++++++++++++------ .../composable_dispatcher/intersection.rs | 40 ++- 3 files changed, 262 insertions(+), 284 deletions(-) diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs index 9998fe6c..680d24c2 100644 --- a/src/query/composable_dispatcher.rs +++ b/src/query/composable_dispatcher.rs @@ -9,7 +9,7 @@ mod tests; use core::any::TypeId; -use function_dispatch::DispatcherTypeKey; +use function_dispatch::{DispatcherTypeKey, FunctionDispatch}; use crate::math::{Isometry, Point, Real, Vector}; use crate::query::details::ShapeCastOptions; @@ -26,180 +26,22 @@ use crate::query::{ use crate::shape::*; /// A dispatcher that exposes built-in queries -#[derive(Debug, Clone)] -pub struct ComposableQueryDispatcher; +#[derive(Debug)] +pub struct ComposableQueryDispatcher<'c, 'a: 'c, 'b: 'c> { + intersection_functions: FunctionDispatch<'c, 'a, 'b>, +} -impl QueryDispatcher for ComposableQueryDispatcher { +impl<'c, 'a: 'c, 'b: 'c> QueryDispatcher for ComposableQueryDispatcher<'c, 'a, 'b> { fn intersection_test( &self, pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, ) -> Result { - /// Transform a function that takes two concrete shapes to a hashmap key value pair. - // TODO: this would be great to have as const, or a macro. - fn to_kv<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( - inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, - ) -> ( - DispatcherTypeKey, - Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>, - ) { - ( - DispatcherTypeKey(TypeId::of::(), TypeId::of::()), - to_as_shape(inner), - ) - } - - /// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. - // TODO: this would be great to have as const, or a macro. - fn to_as_shape<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( - inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, - ) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> - { - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - to_custom( - |s: &'a dyn Shape| s.as_shape::(), - |s: &'b dyn Shape| s.as_shape::(), - inner, - )(pose, s1, s2) - }, - ) - } - /// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. - // TODO: this would be great to have as const, or a macro. - fn to_custom<'c, 'a: 'c, 'b: 'c, ShapeIn1: 'a, ShapeIn2: 'b>( - shape_1: impl Fn(&'a dyn Shape) -> Option + 'a, - shape_2: impl Fn(&'b dyn Shape) -> Option + 'b, - inner: impl Fn(&Isometry, ShapeIn1, ShapeIn2) -> bool + 'c, - ) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> - { - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = shape_1(s1).ok_or(())?; - let shape2 = shape_2(s2).ok_or(())?; - Ok(inner(pose, shape1, shape2)) - }, - ) - } - - let mut intersection_test_impls: std::collections::HashMap = - std::collections::HashMap::new(); - for (kv, function) in vec![ - to_kv(query::details::intersection_test_cuboid_cuboid), - ( - DispatcherTypeKey(TypeId::of::(), TypeId::of::()), - (Box::new(|pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - Ok(query::details::intersection_test_ball_ball( - &pose.translation.vector.into(), - s1.as_ball().ok_or(())?, - s2.as_ball().ok_or(())?, - )) - })), - ), - to_kv(query::details::intersection_test_cuboid_triangle), - to_kv(query::details::intersection_test_triangle_cuboid), - ( - DispatcherTypeKey(TypeId::of::(), TypeId::of::()), - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1.as_ball().ok_or(())?; - Ok(query::details::intersection_test_ball_point_query( - pose, shape1, s2, - )) - }, - ), - ), - ] { - assert!(intersection_test_impls.insert(kv, function).is_none()); - } - - let types = vec![ - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - TypeId::of::(), - #[cfg(feature = "dim2")] - #[cfg(feature = "std")] - TypeId::of::(), - #[cfg(feature = "dim3")] - #[cfg(feature = "std")] - TypeId::of::(), - #[cfg(feature = "dim3")] - TypeId::of::(), - #[cfg(feature = "dim3")] - TypeId::of::(), - #[cfg(feature = "dim3")] - TypeId::of::(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - TypeId::of::>(), - #[cfg(feature = "dim2")] - #[cfg(feature = "std")] - TypeId::of::>(), - #[cfg(feature = "dim3")] - #[cfg(feature = "std")] - TypeId::of::>(), - #[cfg(feature = "dim3")] - TypeId::of::>(), - #[cfg(feature = "dim3")] - TypeId::of::>(), - TypeId::of::>(), - // TODO: DilatedShapes... (and dilatedShpaed of roundshapes) (.. and I guess roundshapes of dilatedshapes ; and then probably recursively...) - ]; - for &type1 in types.iter() { - for &type2 in types.iter() { - let key = DispatcherTypeKey(type1, type2); - if intersection_test_impls.get(&key).is_none() { - println!("key {key:?} not found!"); - } - } - } - /* - // TODO: These composite shapes are harder to implement due to taking dispatcher as argument. - #[cfg(feature = "std")] - { - functions.push({ - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = (|s: &dyn Shape| s.as_composite_shape())(s1).ok_or(())?; - let shape2 = (|s: &dyn Shape| Some(s))(s2).ok_or(())?; - Ok(query::details::intersection_test_composite_shape_shape( - self, pose, shape1, shape2, - )) - }, - ) - }); - functions.push({ - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = (|s: &dyn Shape| Some(s))(s2).ok_or(())?; - let shape2 = (|s: &dyn Shape| s.as_composite_shape())(s1).ok_or(())?; - Ok(query::details::intersection_test_shape_composite_shape( - self, pose, shape1, shape2, - )) - }, - ) - }); - }*/ - - let param_types = DispatcherTypeKey(shape1.type_id(), shape2.type_id()); - let Some(intersection_test_impl) = intersection_test_impls.get(¶m_types) else { - dbg!("key not found!"); - return Err(Unsupported); - }; - intersection_test_impl(pos12, shape1, shape2).map_err(|_| Unsupported) + return self + .intersection_functions + .dispatch(pos12, shape1, shape2) + .map_err(|_| Unsupported); } /// Computes the minimum distance separating two shapes. diff --git a/src/query/composable_dispatcher/function_dispatch.rs b/src/query/composable_dispatcher/function_dispatch.rs index cb871cc7..8a27e1e3 100644 --- a/src/query/composable_dispatcher/function_dispatch.rs +++ b/src/query/composable_dispatcher/function_dispatch.rs @@ -1,144 +1,266 @@ use core::any::TypeId; use std::collections::HashMap; +use log::warn; + use crate::{ math::{Isometry, Real}, - shape::Shape, + query::PointQuery, + shape::{Shape, SimdCompositeShape, SupportMap, TypedSimdCompositeShape}, }; #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] pub struct DispatcherTypeKey(pub TypeId, pub TypeId); -pub struct FunctionDispatchFistParam<'c, 'a: 'c, 'b: 'c> { - /// Map of functions to call when we know how to handle the first parameter. - pub map_first_param: HashMap>, - /// Functions to attempt to call when we don't know how to handle the first parameter. - pub dispatch_unknown_first_param: FunctionDispatchSecondParam<'c, 'a, 'b>, -} - -pub struct FunctionDispatchSecondParam<'c, 'a: 'c, 'b: 'c> { - /// Map of functions to call when we know how to handle the second parameter. - pub map_second_param: HashMap< +pub struct FunctionDispatch<'c, 'a: 'c, 'b: 'c> { + /// Map for the function to call when both parameters are known types. + pub known_both: HashMap< + DispatcherTypeKey, + Box< + dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result + + Send + + Sync + + 'c, + >, + >, + /// Map for the functions to try when first parameters is unknown. + pub known_first: HashMap< TypeId, - Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>, + Vec< + Box< + dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result + + Send + + Sync + + 'c, + >, + >, + >, + /// Map for the functions to try when second parameters is unknown. + pub known_second: HashMap< + TypeId, + Vec< + Box< + dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result + + Send + + Sync + + 'c, + >, + >, + >, + /// Map for the functions to try when both parameters are unknown. + pub known_none: Vec< + Box< + dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result + + Send + + Sync + + 'c, + >, >, - /// Functions to attempt to call when we don't know how to handle the second parameter. - pub dispatch_unknown_second_param: - Vec, &'a dyn Shape, &'b dyn Shape) -> Result + 'c>>, } -/// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. -// TODO: this would be great to have as const, or a macro. -fn to_as_shape<'c, 'a, 'b, S1: Shape + 'a, S2: Shape + 'b>( - inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, -) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> { - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - to_custom( - |s: &'a dyn Shape| s.as_shape::(), - |s: &'b dyn Shape| s.as_shape::(), - inner, - )(pose, s1, s2) - }, - ) -} -/// Transform a function that takes two concrete shapes to a function that takes two dynamic shapes. -// TODO: this would be great to have as const, or a macro. -fn to_custom<'c, 'a: 'c, 'b: 'c, ShapeIn1: 'a, ShapeIn2: 'b>( - shape_1: impl Fn(&'a dyn Shape) -> Option + 'a, - shape_2: impl Fn(&'b dyn Shape) -> Option + 'b, - inner: impl Fn(&Isometry, ShapeIn1, ShapeIn2) -> bool + 'c, -) -> Box, &'a dyn Shape, &'b dyn Shape) -> Result + 'c> { - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = shape_1(s1).ok_or(())?; - let shape2 = shape_2(s2).ok_or(())?; - Ok(inner(pose, shape1, shape2)) - }, - ) +impl<'c, 'a: 'c, 'b: 'c> core::fmt::Debug for FunctionDispatch<'c, 'a, 'b> { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + f.debug_struct("FunctionDispatch") + .field("known_both", &self.known_both.keys()) + .field("known_first", &self.known_first.keys()) + .field("known_second", &self.known_second.keys()) + .field("known_none", &self.known_none.len()) + .finish() + } } -impl<'c, 'a: 'c, 'b: 'c> FunctionDispatchFistParam<'c, 'a, 'b> { +impl<'c, 'a: 'c, 'b: 'c> FunctionDispatch<'c, 'a, 'b> { pub fn new() -> Self { Self { - map_first_param: HashMap::new(), - dispatch_unknown_first_param: FunctionDispatchSecondParam { - map_second_param: HashMap::new(), - dispatch_unknown_second_param: Vec::new(), - }, + known_both: HashMap::new(), + known_first: HashMap::new(), + known_second: HashMap::new(), + known_none: Vec::new(), } } + pub fn add_function_known_12( &mut self, - inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Copy, + inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Send + Sync + Copy, ) { - let dispatch_second = self - .map_first_param - .entry(TypeId::of::()) - .or_insert_with(|| FunctionDispatchSecondParam { - map_second_param: HashMap::new(), - dispatch_unknown_second_param: Vec::new(), - }); - _ = dispatch_second - .map_second_param - .insert(TypeId::of::(), to_as_shape(inner)); + if self + .known_both + .insert( + DispatcherTypeKey(TypeId::of::(), TypeId::of::()), + Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_shape::().ok_or(())?; + let shape2 = s2.as_shape::().ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + ), + ) + .is_some() + { + warn!( + "Overwriting function for types {:?} and {:?}", + TypeId::of::(), + TypeId::of::() + ); + } } + pub fn add_function_known_1( &mut self, - inner: impl Fn(&Isometry, &'a S1, &'b dyn Shape) -> bool + 'c + Copy, + inner: impl Fn(&Isometry, &'a S1, &'b dyn Shape) -> bool + 'c + Send + Sync + Copy, ) { - let dispatch_second = self - .map_first_param - .entry(TypeId::of::()) - .or_insert_with(|| FunctionDispatchSecondParam { - map_second_param: HashMap::new(), - dispatch_unknown_second_param: Vec::new(), - }); - dispatch_second.dispatch_unknown_second_param.push(Box::new( + let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); + dispatch_second.push(Box::new( move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - to_custom( - |s: &'a dyn Shape| s.as_shape::(), - |s: &'b dyn Shape| Some(s), - inner, - )(pose, s1, s2) + let shape1 = s1.as_shape::().ok_or(())?; + let shape2 = s2; + Ok(inner(pose, shape1, shape2)) }, )); } pub fn add_function_known_2( &mut self, - inner: impl Fn(&Isometry, &'a dyn Shape, &'b S2) -> bool + 'c + Copy, + inner: impl Fn(&Isometry, &'a dyn Shape, &'b S2) -> bool + 'c + Send + Sync + Copy, ) { - _ = self - .dispatch_unknown_first_param - .map_second_param - .entry(TypeId::of::()) - .or_insert_with(|| { - Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - to_custom( - |s: &'a dyn Shape| Some(s), - |s: &'b dyn Shape| s.as_shape::(), - inner, - )(pose, s1, s2) - }, - ) - }); + let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); + dispatch_second.push(Box::new( + move |pose: &Isometry, s1: &'a dyn Shape, s2: &'b dyn Shape| { + let shape1 = s1; + let shape2 = s2.as_shape::().ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + )); + } + + pub fn add_function_known_1_and_support_map( + &mut self, + inner: impl Fn(&Isometry, &'a S1, &'b dyn SupportMap) -> bool + 'c + Send + Sync + Copy, + ) { + let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); + dispatch_second.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_shape::().ok_or(())?; + let shape2 = s2.as_support_map().ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + )); + } + + pub fn add_function_known_2_and_support_map( + &mut self, + inner: impl Fn(&Isometry, &'a dyn SupportMap, &'b S2) -> bool + 'c + Send + Sync + Copy, + ) { + let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); + dispatch_second.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_support_map().ok_or(())?; + let shape2 = s2.as_shape::().ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + )); } + pub fn add_function_all_unknown( &mut self, - inner: impl Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> bool + 'c + Copy, + inner: impl Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result + + 'c + + Send + + Sync + + Copy, + ) { + self.known_none.push(Box::new(inner)); + } + + pub fn add_function_all_unknown_support_map( + &mut self, + inner: impl Fn(&Isometry, &'a dyn SupportMap, &'b dyn SupportMap) -> bool + + 'c + + Send + + Sync + + Copy, ) { - self.dispatch_unknown_first_param - .dispatch_unknown_second_param - .push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - to_custom( - |s: &'a dyn Shape| Some(s), - |s: &'b dyn Shape| Some(s), - inner, - )(pose, s1, s2) - }, - )); + self.known_none.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_support_map().ok_or(())?; + let shape2 = s2.as_support_map().ok_or(())?; + Ok(inner(pose, shape1, shape2)) + }, + )); + } + + pub fn add_function_composite_shape_1<'d: 'c, D>( + &mut self, + dispatcher: &'d D, + inner: impl Fn(&D, &Isometry, &'a dyn SimdCompositeShape, &'b dyn Shape) -> bool + + 'c + + Send + + Sync + + Copy, + ) where + D: ?Sized + crate::query::QueryDispatcher, + { + self.known_none.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1.as_composite_shape().ok_or(())?; + let shape2 = s2; + Ok(inner(dispatcher, pose, shape1, shape2)) + }, + )); + } + pub fn add_function_composite_shape_2<'d: 'c, D>( + &mut self, + dispatcher: &'d D, + inner: impl Fn(&D, &Isometry, &'a dyn Shape, &'b dyn SimdCompositeShape) -> bool + + 'c + + Send + + Sync + + Copy, + ) where + D: ?Sized + crate::query::QueryDispatcher, + { + self.known_none.push(Box::new( + move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + let shape1 = s1; + let shape2 = s2.as_composite_shape().ok_or(())?; + Ok(inner(dispatcher, pose, shape1, shape2)) + }, + )); + } + + pub fn dispatch( + &self, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result { + let key = DispatcherTypeKey(s1.type_id(), s2.type_id()); + if let Some(func) = self.known_both.get(&key) { + return func(pose, s1, s2); + } + + if let Some(funcs) = self.known_first.get(&s1.type_id()) { + for func in funcs { + if let Ok(res) = func(pose, s1, s2) { + return Ok(res); + } + } + } + + if let Some(funcs) = self.known_second.get(&s2.type_id()) { + for func in funcs { + if let Ok(res) = func(pose, s1, s2) { + return Ok(res); + } + } + } + + for func in &self.known_none { + if let Ok(res) = func(pose, s1, s2) { + return Ok(res); + } + } + + Err(()) } } diff --git a/src/query/composable_dispatcher/intersection.rs b/src/query/composable_dispatcher/intersection.rs index 5af323a4..5f55ca0c 100644 --- a/src/query/composable_dispatcher/intersection.rs +++ b/src/query/composable_dispatcher/intersection.rs @@ -1,9 +1,17 @@ -use crate::{math::Point, query}; +use crate::{ + math::Point, + query::{self, QueryDispatcher}, +}; -use super::function_dispatch::FunctionDispatchFistParam; +use super::function_dispatch::FunctionDispatch; -pub fn create_intersection_dispatcher() -> FunctionDispatchFistParam<'static, 'static, 'static> { - let mut dispatcher = FunctionDispatchFistParam::new(); +pub fn create_intersection_dispatcher<'d: 'a, 'a, 'c, 'b, D>( + d: &'d D, +) -> FunctionDispatch<'a, 'c, 'b> +where + D: ?Sized + QueryDispatcher, +{ + let mut dispatcher = FunctionDispatch::new(); // Register intersection functions here. @@ -16,23 +24,29 @@ pub fn create_intersection_dispatcher() -> FunctionDispatchFistParam<'static, 's let p12 = Point::from(pos12.translation.vector); query::details::intersection_test_ball_ball(&p12, b1, b2) }); - dispatcher.add_function_known_12(query::details::intersection_test_segment_cuboid); dispatcher.add_function_known_1(query::details::intersection_test_ball_point_query); dispatcher.add_function_known_2(query::details::intersection_test_point_query_ball); - /* // Those can't work because SupportMap is not Shape... // So we'd need to rely on Any... // but then `Any`` doesn't support casting to traits easily: // - we'd need to box the shapes // - or use a trait to cast to the right trait? - dispatcher.add_function_known_1(query::details::intersection_test_halfspace_support_map); - dispatcher.add_function_known_2(query::details::intersection_test_support_map_halfspace); + dispatcher.add_function_known_1_and_support_map( + query::details::intersection_test_halfspace_support_map, + ); + dispatcher.add_function_known_2_and_support_map( + query::details::intersection_test_support_map_halfspace, + ); + + dispatcher.add_function_all_unknown_support_map( + query::details::intersection_test_support_map_support_map, + ); + // TODO: add composite shapes - dispatcher.add_function_all_unknown(query::details::intersection_test_support_map_support_map); - dispatcher.add_function_known_2(query::details::intersection_test_support_map_support_map); - */ + dispatcher + .add_function_composite_shape_1(d, query::details::intersection_test_composite_shape_shape); + dispatcher + .add_function_composite_shape_2(d, query::details::intersection_test_shape_composite_shape); dispatcher } - -fn cast_params() {} From c17f765d820152c0ba3065665a2e5b3f3e973529 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 10 Mar 2025 18:35:10 +0100 Subject: [PATCH 5/7] compiling typeid/typeid functions ; lots of cleanup to do --- src/query/composable_dispatcher.rs | 12 +- .../function_dispatch.rs | 359 +++++++++--------- .../composable_dispatcher/intersection.rs | 183 +++++++-- src/query/composable_dispatcher/tests.rs | 36 +- 4 files changed, 366 insertions(+), 224 deletions(-) diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs index 680d24c2..fb72864a 100644 --- a/src/query/composable_dispatcher.rs +++ b/src/query/composable_dispatcher.rs @@ -8,6 +8,7 @@ mod intersection; mod tests; use core::any::TypeId; +use std::collections::HashMap; use function_dispatch::{DispatcherTypeKey, FunctionDispatch}; @@ -26,12 +27,13 @@ use crate::query::{ use crate::shape::*; /// A dispatcher that exposes built-in queries + #[derive(Debug)] -pub struct ComposableQueryDispatcher<'c, 'a: 'c, 'b: 'c> { - intersection_functions: FunctionDispatch<'c, 'a, 'b>, +pub struct ComposableQueryDispatcher { + pub intersections: FunctionDispatch, } -impl<'c, 'a: 'c, 'b: 'c> QueryDispatcher for ComposableQueryDispatcher<'c, 'a, 'b> { +impl QueryDispatcher for ComposableQueryDispatcher { fn intersection_test( &self, pos12: &Isometry, @@ -39,8 +41,8 @@ impl<'c, 'a: 'c, 'b: 'c> QueryDispatcher for ComposableQueryDispatcher<'c, 'a, ' shape2: &dyn Shape, ) -> Result { return self - .intersection_functions - .dispatch(pos12, shape1, shape2) + .intersections + .dispatch(self, pos12, shape1, shape2) .map_err(|_| Unsupported); } diff --git a/src/query/composable_dispatcher/function_dispatch.rs b/src/query/composable_dispatcher/function_dispatch.rs index 8a27e1e3..796d2643 100644 --- a/src/query/composable_dispatcher/function_dispatch.rs +++ b/src/query/composable_dispatcher/function_dispatch.rs @@ -5,90 +5,111 @@ use log::warn; use crate::{ math::{Isometry, Real}, - query::PointQuery, - shape::{Shape, SimdCompositeShape, SupportMap, TypedSimdCompositeShape}, + query::QueryDispatcher, + shape::{RoundShape, Shape}, }; +use super::ComposableQueryDispatcher; + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] pub struct DispatcherTypeKey(pub TypeId, pub TypeId); -pub struct FunctionDispatch<'c, 'a: 'c, 'b: 'c> { +pub trait IntersectionWithDispatcher: Send + Sync { + fn intersection_with_dispatcher( + &self, + dispatcher: &ComposableQueryDispatcher, + pos12: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result; +} +pub struct BoxedIntersectionWithDispatcher(Box); +impl IntersectionWithDispatcher for BoxedIntersectionWithDispatcher { + fn intersection_with_dispatcher( + &self, + dispatcher: &ComposableQueryDispatcher, + pos12: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result { + self.0 + .intersection_with_dispatcher(dispatcher, pos12, s1, s2) + } +} +impl IntersectionWithDispatcher for fn(&Isometry, &dyn Shape, &dyn Shape) -> bool { + fn intersection_with_dispatcher( + &self, + _dispatcher: &ComposableQueryDispatcher, + pos12: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result { + Ok(self(pos12, s1, s2)) + } +} +/* +impl IntersectionWithDispatcher + for fn(&ComposableQueryDispatcher, &Isometry, &dyn Shape, &dyn Shape) -> bool +{ + fn intersection_with_dispatcher( + &self, + dispatcher: &ComposableQueryDispatcher, + pos12: &Isometry, + other: &dyn Shape, + ) -> bool { + self(dispatcher, pos12, other, other) + } +}*/ + +impl IntersectionWithDispatcher for F +where + F: Fn(&ComposableQueryDispatcher, &Isometry, &dyn Shape, &dyn Shape) -> Result + + Send + + Sync, +{ + fn intersection_with_dispatcher( + &self, + dispatcher: &ComposableQueryDispatcher, + pos12: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result { + self(dispatcher, pos12, s1, s2) + } +} + +pub struct FunctionDispatch { /// Map for the function to call when both parameters are known types. - pub known_both: HashMap< - DispatcherTypeKey, - Box< - dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result - + Send - + Sync - + 'c, - >, - >, - /// Map for the functions to try when first parameters is unknown. - pub known_first: HashMap< - TypeId, - Vec< - Box< - dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result - + Send - + Sync - + 'c, - >, - >, - >, - /// Map for the functions to try when second parameters is unknown. - pub known_second: HashMap< - TypeId, - Vec< - Box< - dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result - + Send - + Sync - + 'c, - >, - >, - >, - /// Map for the functions to try when both parameters are unknown. - pub known_none: Vec< - Box< - dyn Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result - + Send - + Sync - + 'c, - >, - >, + pub functions: HashMap>, } -impl<'c, 'a: 'c, 'b: 'c> core::fmt::Debug for FunctionDispatch<'c, 'a, 'b> { +impl core::fmt::Debug for FunctionDispatch { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { f.debug_struct("FunctionDispatch") - .field("known_both", &self.known_both.keys()) - .field("known_first", &self.known_first.keys()) - .field("known_second", &self.known_second.keys()) - .field("known_none", &self.known_none.len()) + .field("known_both", &self.functions.keys()) .finish() } } -impl<'c, 'a: 'c, 'b: 'c> FunctionDispatch<'c, 'a, 'b> { +impl FunctionDispatch { pub fn new() -> Self { Self { - known_both: HashMap::new(), - known_first: HashMap::new(), - known_second: HashMap::new(), - known_none: Vec::new(), + functions: HashMap::new(), } } - - pub fn add_function_known_12( + pub fn add_function_known_12( &mut self, - inner: impl Fn(&Isometry, &'a S1, &'b S2) -> bool + 'c + Send + Sync + Copy, + inner: impl Fn(&Isometry, &S1, &S2) -> bool + 'static + Send + Sync + Copy, ) { if self - .known_both + .functions .insert( DispatcherTypeKey(TypeId::of::(), TypeId::of::()), Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + move |_dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { let shape1 = s1.as_shape::().ok_or(())?; let shape2 = s2.as_shape::().ok_or(())?; Ok(inner(pose, shape1, shape2)) @@ -105,160 +126,140 @@ impl<'c, 'a: 'c, 'b: 'c> FunctionDispatch<'c, 'a, 'b> { } } - pub fn add_function_known_1( + pub fn add_function_known_1x( &mut self, - inner: impl Fn(&Isometry, &'a S1, &'b dyn Shape) -> bool + 'c + Send + Sync + Copy, + inner: impl Fn(&Isometry, &S1, &dyn Shape) -> bool + 'static + Send + Sync + Copy, + tids: Vec, ) { - let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); - dispatch_second.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1.as_shape::().ok_or(())?; - let shape2 = s2; - Ok(inner(pose, shape1, shape2)) - }, - )); + for type_s2 in tids { + self.add_both_combinations(inner, TypeId::of::(), type_s2); + self.add_both_combinations(inner, TypeId::of::>(), type_s2); + } } - pub fn add_function_known_2( + pub fn add_function_dyn_dispatcher( &mut self, - inner: impl Fn(&Isometry, &'a dyn Shape, &'b S2) -> bool + 'c + Send + Sync + Copy, + inner: impl Fn(&ComposableQueryDispatcher, &Isometry, &dyn Shape, &dyn Shape) -> bool + + 'static + + Send + + Sync + + Copy, + type_s1: TypeId, + type_s2: TypeId, ) { - let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); - dispatch_second.push(Box::new( - move |pose: &Isometry, s1: &'a dyn Shape, s2: &'b dyn Shape| { - let shape1 = s1; - let shape2 = s2.as_shape::().ok_or(())?; - Ok(inner(pose, shape1, shape2)) - }, - )); + self.add_raw_function( + type_s1, + type_s2, + move |dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { Ok(inner(dispatcher, pose, s1, s2)) }, + ); + + self.add_raw_function( + type_s2, + type_s1, + move |dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { Ok(inner(dispatcher, &pose.inverse(), s2, s1)) }, + ); } - pub fn add_function_known_1_and_support_map( + fn add_both_combinations( &mut self, - inner: impl Fn(&Isometry, &'a S1, &'b dyn SupportMap) -> bool + 'c + Send + Sync + Copy, + inner: impl Fn(&Isometry, &S1, &dyn Shape) -> bool + 'static + Send + Sync + Copy, + type_s1: TypeId, + type_s2: TypeId, ) { - let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); - dispatch_second.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { + self.add_raw_function( + type_s1, + type_s2, + move |_dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { let shape1 = s1.as_shape::().ok_or(())?; - let shape2 = s2.as_support_map().ok_or(())?; - Ok(inner(pose, shape1, shape2)) + Ok(inner(pose, shape1, s2)) }, - )); - } - - pub fn add_function_known_2_and_support_map( - &mut self, - inner: impl Fn(&Isometry, &'a dyn SupportMap, &'b S2) -> bool + 'c + Send + Sync + Copy, - ) { - let dispatch_second = self.known_first.entry(TypeId::of::()).or_insert(vec![]); - dispatch_second.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1.as_support_map().ok_or(())?; - let shape2 = s2.as_shape::().ok_or(())?; - Ok(inner(pose, shape1, shape2)) + ); + // add the inverse function + self.add_raw_function( + type_s2, + type_s1, + move |_dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { + let shape2 = s2.as_shape::().ok_or(())?; + Ok(inner(&pose.inverse(), shape2, s1)) }, - )); + ); } - pub fn add_function_all_unknown( + pub fn add_raw_function( &mut self, - inner: impl Fn(&Isometry, &'a dyn Shape, &'b dyn Shape) -> Result - + 'c - + Send - + Sync - + Copy, + type_s1: TypeId, + type_s2: TypeId, + function: impl IntersectionWithDispatcher + 'static + Send + Sync + Copy, ) { - self.known_none.push(Box::new(inner)); + if self + .functions + .insert(DispatcherTypeKey(type_s1, type_s2), Box::new(function)) + .is_some() + { + warn!( + "Overwriting function for types {:?} and {:?}", + type_s1, type_s2 + ); + } } - pub fn add_function_all_unknown_support_map( + fn add_both_combinations_dispatcher( &mut self, - inner: impl Fn(&Isometry, &'a dyn SupportMap, &'b dyn SupportMap) -> bool - + 'c + inner: impl Fn(&ComposableQueryDispatcher, &Isometry, &S1, &dyn Shape) -> bool + + 'static + Send + Sync + Copy, + type_s1: TypeId, + type_s2: TypeId, ) { - self.known_none.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1.as_support_map().ok_or(())?; - let shape2 = s2.as_support_map().ok_or(())?; - Ok(inner(pose, shape1, shape2)) - }, - )); - } - - pub fn add_function_composite_shape_1<'d: 'c, D>( - &mut self, - dispatcher: &'d D, - inner: impl Fn(&D, &Isometry, &'a dyn SimdCompositeShape, &'b dyn Shape) -> bool - + 'c - + Send - + Sync - + Copy, - ) where - D: ?Sized + crate::query::QueryDispatcher, - { - self.known_none.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1.as_composite_shape().ok_or(())?; - let shape2 = s2; - Ok(inner(dispatcher, pose, shape1, shape2)) + self.add_raw_function( + type_s1, + type_s2, + move |dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { + let shape1 = s1.as_shape::().ok_or(())?; + Ok(inner(dispatcher, pose, shape1, s2)) }, - )); - } - pub fn add_function_composite_shape_2<'d: 'c, D>( - &mut self, - dispatcher: &'d D, - inner: impl Fn(&D, &Isometry, &'a dyn Shape, &'b dyn SimdCompositeShape) -> bool - + 'c - + Send - + Sync - + Copy, - ) where - D: ?Sized + crate::query::QueryDispatcher, - { - self.known_none.push(Box::new( - move |pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape| { - let shape1 = s1; - let shape2 = s2.as_composite_shape().ok_or(())?; - Ok(inner(dispatcher, pose, shape1, shape2)) + ); + // add the inverse function + self.add_raw_function( + type_s2, + type_s1, + move |dispatcher: &ComposableQueryDispatcher, + pose: &Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { + let shape2 = s2.as_shape::().ok_or(())?; + Ok(inner(dispatcher, &pose.inverse(), shape2, s1)) }, - )); + ); } pub fn dispatch( &self, + dispatcher: &ComposableQueryDispatcher, pose: &Isometry, s1: &dyn Shape, s2: &dyn Shape, ) -> Result { let key = DispatcherTypeKey(s1.type_id(), s2.type_id()); - if let Some(func) = self.known_both.get(&key) { - return func(pose, s1, s2); - } - - if let Some(funcs) = self.known_first.get(&s1.type_id()) { - for func in funcs { - if let Ok(res) = func(pose, s1, s2) { - return Ok(res); - } - } - } - - if let Some(funcs) = self.known_second.get(&s2.type_id()) { - for func in funcs { - if let Ok(res) = func(pose, s1, s2) { - return Ok(res); - } - } - } - - for func in &self.known_none { - if let Ok(res) = func(pose, s1, s2) { - return Ok(res); - } + if let Some(func) = self.functions.get(&key) { + return func.intersection_with_dispatcher(dispatcher, pose, s1, s2); } Err(()) diff --git a/src/query/composable_dispatcher/intersection.rs b/src/query/composable_dispatcher/intersection.rs index 5f55ca0c..8892b681 100644 --- a/src/query/composable_dispatcher/intersection.rs +++ b/src/query/composable_dispatcher/intersection.rs @@ -1,16 +1,14 @@ +use core::any::TypeId; + use crate::{ math::Point, query::{self, QueryDispatcher}, + shape::*, }; use super::function_dispatch::FunctionDispatch; -pub fn create_intersection_dispatcher<'d: 'a, 'a, 'c, 'b, D>( - d: &'d D, -) -> FunctionDispatch<'a, 'c, 'b> -where - D: ?Sized + QueryDispatcher, -{ +pub fn create_intersection_dispatcher() -> FunctionDispatch { let mut dispatcher = FunctionDispatch::new(); // Register intersection functions here. @@ -24,29 +22,164 @@ where let p12 = Point::from(pos12.translation.vector); query::details::intersection_test_ball_ball(&p12, b1, b2) }); - dispatcher.add_function_known_1(query::details::intersection_test_ball_point_query); - dispatcher.add_function_known_2(query::details::intersection_test_point_query_ball); - - // Those can't work because SupportMap is not Shape... - // So we'd need to rely on Any... - // but then `Any`` doesn't support casting to traits easily: - // - we'd need to box the shapes - // - or use a trait to cast to the right trait? - dispatcher.add_function_known_1_and_support_map( - query::details::intersection_test_halfspace_support_map, + dispatcher.add_function_known_1x( + query::details::intersection_test_ball_point_query, + vec![ + // TODO: add Tetrahedron once shape is implemented. + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + #[cfg(feature = "dim2")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + TypeId::of::(), + // RoundShapes + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + #[cfg(feature = "dim2")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + TypeId::of::>(), + ], ); - dispatcher.add_function_known_2_and_support_map( - query::details::intersection_test_support_map_halfspace, + dispatcher.add_function_known_1x( + |pos12, halfspace: &HalfSpace, other| { + query::details::intersection_test_halfspace_support_map( + pos12, + halfspace, + other.as_support_map().expect("calling `as_support_map` on a non-support-map shape, make sure your type mapping is correct."), + ) + }, + vec![ + TypeId::of::(), + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + #[cfg(feature = "dim2")] + TypeId::of::(), + TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + // RoundShapes + TypeId::of::>(), + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + #[cfg(feature = "dim2")] + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + TypeId::of::>(), + #[cfg(feature = "dim3")] + TypeId::of::>(), + TypeId::of::>(), + TypeId::of::>(), + ], ); - dispatcher.add_function_all_unknown_support_map( - query::details::intersection_test_support_map_support_map, - ); // TODO: add composite shapes + macro_rules! shape_impls { + ($($prefix:tt $inner:ty),*) => { + { + use std::collections::HashMap; + let mut types = HashMap::new(); + $( + register_shape!(types, $prefix $inner); + )* + types + } + }; + } + macro_rules! register_shape { + ($types:expr, any $shape:ty) => { + _ = $types.insert(TypeId::of::<$shape>(), stringify!($shape)); + }; + ($types:tt, $f:tt $shape:ty) => { + #[cfg(feature = $f)] + assert!($types + .insert(TypeId::of::<$shape>(), stringify!($shape)) + .is_none()); + }; + } - dispatcher - .add_function_composite_shape_1(d, query::details::intersection_test_composite_shape_shape); - dispatcher - .add_function_composite_shape_2(d, query::details::intersection_test_shape_composite_shape); + let shape_impls = shape_impls!( + any Ball, + any Cuboid, + any Capsule, + any Triangle, + any Segment, + any Compound, + any Polyline, + any TriMesh, + any HeightField, + "dim2" ConvexPolygon, + "dim3" ConvexPolyhedron, + "dim3" Cylinder, + "dim3" Cone, + any HalfSpace, + // Round shapes + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + any RoundShape::, + "dim2" RoundShape::, + "dim3" RoundShape::, + "dim3" RoundShape::, + "dim3" RoundShape::, + any RoundShape:: + ); + + for composite_type in vec![ + TypeId::of::(), + TypeId::of::(), + TypeId::of::(), + ] { + for shape_type in shape_impls.iter() { + dispatcher.add_function_dyn_dispatcher( + |dispatcher, pos12, shape1, shape2| { + query::details::intersection_test_composite_shape_shape(dispatcher, + pos12, + shape1.as_composite_shape().expect("calling `as_composite_shape` on a non-support-map shape, make sure your type mapping is correct."), + shape2 + ) + }, + composite_type, *shape_type.0 + ); + } + } + + for k in dispatcher.functions.keys() { + println!( + "({} {})", + shape_impls.get(&k.0).unwrap_or(&"not found"), + shape_impls.get(&k.1).unwrap_or(&"not found") + ); + } dispatcher } diff --git a/src/query/composable_dispatcher/tests.rs b/src/query/composable_dispatcher/tests.rs index 536f393a..86208c47 100644 --- a/src/query/composable_dispatcher/tests.rs +++ b/src/query/composable_dispatcher/tests.rs @@ -2,28 +2,34 @@ extern crate nalgebra as na; #[cfg(feature = "dim2")] mod tests2d { + use crate::query::composable_dispatcher::intersection::create_intersection_dispatcher; use crate::query::composable_dispatcher::ComposableQueryDispatcher; use crate::query::QueryDispatcher; use crate::shape::{Ball, Cuboid}; use na::{Isometry2, Vector2}; #[test] - fn intersection_test() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); - let ball = Ball::new(1.0); + fn intersection_test_dispatcher_configuration() { + let dispatcher = create_intersection_dispatcher(); + } - let cuboid_pos = Isometry2::identity(); - let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); - let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); + // #[test] + // fn intersection_test() { + // let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + // let ball = Ball::new(1.0); - let pos12 = ball_pos_intersecting.inv_mul(&cuboid_pos); - assert!(ComposableQueryDispatcher - .intersection_test(&pos12, &ball, &cuboid) - .unwrap()); + // let cuboid_pos = Isometry2::identity(); + // let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); + // let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); - let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); - assert!(!ComposableQueryDispatcher - .intersection_test(&pos12, &ball, &cuboid) - .unwrap()); - } + // let pos12 = ball_pos_intersecting.inv_mul(&cuboid_pos); + // assert!(ComposableQueryDispatcher + // .intersection_test(&pos12, &ball, &cuboid) + // .unwrap()); + + // let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); + // assert!(!ComposableQueryDispatcher + // .intersection_test(&pos12, &ball, &cuboid) + // .unwrap()); + // } } From 04f59bf9aec146539de3d54e3dbf9c0dc47156a1 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 19 Mar 2025 11:13:22 +0100 Subject: [PATCH 6/7] benches + fix convexpolyhedron self intersection TODO: fix support map support map in all cases --- crates/parry3d/benches/common/dispatcher.rs | 96 +++++++++++++++++++ crates/parry3d/benches/common/mod.rs | 1 + src/query/composable_dispatcher.rs | 2 + .../composable_dispatcher/intersection.rs | 30 ++++++ src/query/composable_dispatcher/tests.rs | 34 +++---- 5 files changed, 147 insertions(+), 16 deletions(-) create mode 100644 crates/parry3d/benches/common/dispatcher.rs diff --git a/crates/parry3d/benches/common/dispatcher.rs b/crates/parry3d/benches/common/dispatcher.rs new file mode 100644 index 00000000..3cb3ce42 --- /dev/null +++ b/crates/parry3d/benches/common/dispatcher.rs @@ -0,0 +1,96 @@ +use na::Isometry3; +use parry3d::math::Isometry; +use parry3d::query::composable_dispatcher::ComposableQueryDispatcher; +use parry3d::query::{self, Unsupported}; +use parry3d::query::{ + composable_dispatcher::create_intersection_dispatcher, DefaultQueryDispatcher, QueryDispatcher, +}; +use parry3d::shape::{Ball, ConvexPolyhedron, Shape}; +use rand::SeedableRng; +use rand_isaac::IsaacRng; +use test::Bencher; + +use crate::common::{generate, unref}; + +#[path = "../common/macros.rs"] +#[macro_use] +mod macros; + +#[bench] +fn bench_dispatcher_creation(bh: &mut Bencher) { + let mut i = 0; + bh.iter(|| unsafe { test::black_box(create_intersection_dispatcher()) }) +} + +pub fn intersection_default( + pos12: &Isometry, + g1: &dyn Shape, + g2: &dyn Shape, +) -> Result { + DefaultQueryDispatcher.intersection_test(pos12, g1, g2) +} + +bench_free_fn!( + bench_intersection_ball_ball, + intersection_default, + pos12: Isometry3, + b1: Ball, + b2: Ball +); + +#[bench] +fn bench_intersection_ball_ball_composable(bh: &mut Bencher) { + const LEN: usize = 1 << 7; + let mut rng: IsaacRng = SeedableRng::seed_from_u64(0); + let pos12: Vec> = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let b1: Vec = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let b2: Vec = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let mut i = 0; + let dispatcher = ComposableQueryDispatcher { + intersections: create_intersection_dispatcher(), + }; + bh.iter(|| { + i = (i + 1) & (LEN - 1); + unsafe { + test::black_box(dispatcher.intersection_test( + unref(pos12.get_unchecked(i)), + unref(b1.get_unchecked(i)), + unref(b2.get_unchecked(i)), + )) + } + }); +} + +bench_free_fn!( + bench_intersection_convex_polyhedron_convex_polyhedron, + intersection_default, + pos12: Isometry3, + b1: ConvexPolyhedron, + b2: ConvexPolyhedron +); + +#[bench] +fn bench_intersection_convex_polyhedron_convex_polyhedron_composable(bh: &mut Bencher) { + const LEN: usize = 1 << 7; + let mut rng: IsaacRng = SeedableRng::seed_from_u64(0); + let pos12: Vec> = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let b1: Vec = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let b2: Vec = (0usize..LEN).map(|_| generate(&mut rng)).collect(); + let mut i = 0; + let dispatcher = ComposableQueryDispatcher { + intersections: create_intersection_dispatcher(), + }; + bh.iter(|| { + i = (i + 1) & (LEN - 1); + unsafe { + assert!(matches!( + test::black_box(dispatcher.intersection_test( + unref(pos12.get_unchecked(i)), + unref(b1.get_unchecked(i)), + unref(b2.get_unchecked(i)), + )), + Ok(_) + )) + } + }); +} diff --git a/crates/parry3d/benches/common/mod.rs b/crates/parry3d/benches/common/mod.rs index 746805fc..e17e35fe 100644 --- a/crates/parry3d/benches/common/mod.rs +++ b/crates/parry3d/benches/common/mod.rs @@ -3,5 +3,6 @@ pub use self::generators::generate_trimesh_around_origin; pub use self::unref::unref; mod default_gen; +mod dispatcher; mod generators; mod unref; diff --git a/src/query/composable_dispatcher.rs b/src/query/composable_dispatcher.rs index fb72864a..f2d522df 100644 --- a/src/query/composable_dispatcher.rs +++ b/src/query/composable_dispatcher.rs @@ -4,6 +4,8 @@ mod function_dispatch; mod intersection; + +pub use intersection::*; #[cfg(test)] mod tests; diff --git a/src/query/composable_dispatcher/intersection.rs b/src/query/composable_dispatcher/intersection.rs index 8892b681..50e7a6ad 100644 --- a/src/query/composable_dispatcher/intersection.rs +++ b/src/query/composable_dispatcher/intersection.rs @@ -76,6 +76,8 @@ pub fn create_intersection_dispatcher() -> FunctionDispatch { TypeId::of::(), #[cfg(feature = "dim2")] TypeId::of::(), + #[cfg(feature = "dim3")] + TypeId::of::(), TypeId::of::(), #[cfg(feature = "dim3")] TypeId::of::(), @@ -97,6 +99,34 @@ pub fn create_intersection_dispatcher() -> FunctionDispatch { TypeId::of::>(), ], ); + fn intersection_test_sm_sm( + _: &query::composable_dispatcher::ComposableQueryDispatcher, + pos12: &crate::math::Isometry, + s1: &dyn Shape, + s2: &dyn Shape, + ) -> Result { + Ok(query::details::intersection_test_support_map_support_map( + pos12, + s1.as_support_map().unwrap(), + s2.as_support_map().unwrap(), + )) + } + // TODO: add support map support map + #[cfg(feature = "dim3")] + dispatcher.add_raw_function( + TypeId::of::(), + TypeId::of::(), + |_: &query::composable_dispatcher::ComposableQueryDispatcher, + pos12: &crate::math::Isometry, + s1: &dyn Shape, + s2: &dyn Shape| { + Ok(query::details::intersection_test_support_map_support_map( + pos12, + s1.as_support_map().unwrap(), + s2.as_support_map().unwrap(), + )) + }, + ); // TODO: add composite shapes macro_rules! shape_impls { diff --git a/src/query/composable_dispatcher/tests.rs b/src/query/composable_dispatcher/tests.rs index 86208c47..47c1e194 100644 --- a/src/query/composable_dispatcher/tests.rs +++ b/src/query/composable_dispatcher/tests.rs @@ -13,23 +13,25 @@ mod tests2d { let dispatcher = create_intersection_dispatcher(); } - // #[test] - // fn intersection_test() { - // let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); - // let ball = Ball::new(1.0); + #[test] + fn intersection_test() { + let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let ball = Ball::new(1.0); - // let cuboid_pos = Isometry2::identity(); - // let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); - // let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); + let cuboid_pos = Isometry2::identity(); + let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); + let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); - // let pos12 = ball_pos_intersecting.inv_mul(&cuboid_pos); - // assert!(ComposableQueryDispatcher - // .intersection_test(&pos12, &ball, &cuboid) - // .unwrap()); + let pos12 = ball_pos_intersecting.inv_mul(&cuboid_pos); + assert!(ComposableQueryDispatcher { + intersections: create_intersection_dispatcher() + } + .intersection_test(&pos12, &ball, &cuboid) + .unwrap()); - // let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); - // assert!(!ComposableQueryDispatcher - // .intersection_test(&pos12, &ball, &cuboid) - // .unwrap()); - // } + let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); + assert!(!ComposableQueryDispatcher + .intersection_test(&pos12, &ball, &cuboid) + .unwrap()); + } } From bafa6d8d9fabcd264354480670ba671a85c998bf Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 26 Mar 2025 11:23:05 +0100 Subject: [PATCH 7/7] use internal hashmap, resulting in better performance --- src/query/composable_dispatcher/function_dispatch.rs | 4 ++-- src/query/composable_dispatcher/tests.rs | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/query/composable_dispatcher/function_dispatch.rs b/src/query/composable_dispatcher/function_dispatch.rs index 796d2643..a79500c4 100644 --- a/src/query/composable_dispatcher/function_dispatch.rs +++ b/src/query/composable_dispatcher/function_dispatch.rs @@ -1,5 +1,5 @@ +use crate::utils::hashmap::HashMap; use core::any::TypeId; -use std::collections::HashMap; use log::warn; @@ -94,7 +94,7 @@ impl core::fmt::Debug for FunctionDispatch { impl FunctionDispatch { pub fn new() -> Self { Self { - functions: HashMap::new(), + functions: HashMap::default(), } } pub fn add_function_known_12( diff --git a/src/query/composable_dispatcher/tests.rs b/src/query/composable_dispatcher/tests.rs index 47c1e194..e804a64b 100644 --- a/src/query/composable_dispatcher/tests.rs +++ b/src/query/composable_dispatcher/tests.rs @@ -30,8 +30,10 @@ mod tests2d { .unwrap()); let pos12 = ball_pos_disjoint.inv_mul(&cuboid_pos); - assert!(!ComposableQueryDispatcher - .intersection_test(&pos12, &ball, &cuboid) - .unwrap()); + assert!(!ComposableQueryDispatcher { + intersections: create_intersection_dispatcher() + } + .intersection_test(&pos12, &ball, &cuboid) + .unwrap()); } }