diff --git a/crates/parry3d/examples/custom_dispatcher.rs b/crates/parry3d/examples/custom_dispatcher.rs index 5179cdd2..a56975b1 100644 --- a/crates/parry3d/examples/custom_dispatcher.rs +++ b/crates/parry3d/examples/custom_dispatcher.rs @@ -12,6 +12,8 @@ use parry3d::{ shape::{Ball, Cuboid, FeatureId, Shape, ShapeType, TypedShape}, }; +pub struct CustomBall(pub Ball); + fn main() { let cube = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = CustomBall(Ball::new(1.0)); @@ -24,8 +26,6 @@ fn main() { dbg!(contact); } -pub struct CustomBall(pub Ball); - impl PointQuery for CustomBall { fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { self.0.project_local_point(pt, solid) @@ -95,8 +95,31 @@ impl QueryDispatcher for CustomBallDispatcher { g1: &dyn Shape, g2: &dyn Shape, ) -> Result { - let dispatcher = DefaultQueryDispatcher; - dispatcher.intersection_test(pos12, g1, g2) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + let p12 = Point::from(pos12.translation.vector); + return Ok(parry3d::query::details::intersection_test_ball_ball( + &p12, &ball1.0, &ball2.0, + )); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.intersection_test(pos12, &ball1.0, g2) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.intersection_test(pos12, g1, &ball2.0) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.intersection_test(pos12, g1, g2) + } + } } fn distance( @@ -105,8 +128,31 @@ impl QueryDispatcher for CustomBallDispatcher { g1: &dyn Shape, g2: &dyn Shape, ) -> Result { - let dispatcher = DefaultQueryDispatcher; - dispatcher.distance(pos12, g1, g2) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + let p2 = Point::from(pos12.translation.vector); + return Ok(parry3d::query::details::distance_ball_ball( + &ball1.0, &p2, &ball2.0, + )); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.distance(pos12, &ball1.0, g2) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.distance(pos12, g1, &ball2.0) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.distance(pos12, g1, g2) + } + } } fn contact( @@ -116,8 +162,30 @@ impl QueryDispatcher for CustomBallDispatcher { g2: &dyn Shape, prediction: f32, ) -> Result, Unsupported> { - let dispatcher = DefaultQueryDispatcher; - dispatcher.contact(pos12, g1, g2, prediction) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + return Ok(parry3d::query::details::contact_ball_ball( + pos12, &ball1.0, &ball2.0, prediction, + )); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.contact(pos12, &ball1.0, g2, prediction) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.contact(pos12, g1, &ball2.0, prediction) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.contact(pos12, g1, g2, prediction) + } + } } fn closest_points( @@ -127,8 +195,30 @@ impl QueryDispatcher for CustomBallDispatcher { g2: &dyn Shape, max_dist: f32, ) -> Result { - let dispatcher = DefaultQueryDispatcher; - dispatcher.closest_points(pos12, g1, g2, max_dist) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + return Ok(parry3d::query::details::closest_points_ball_ball( + pos12, &ball1.0, &ball2.0, max_dist, + )); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.closest_points(pos12, &ball1.0, g2, max_dist) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.closest_points(pos12, g1, &ball2.0, max_dist) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.closest_points(pos12, g1, g2, max_dist) + } + } } fn cast_shapes( @@ -139,8 +229,34 @@ impl QueryDispatcher for CustomBallDispatcher { g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { - let dispatcher = DefaultQueryDispatcher; - dispatcher.cast_shapes(pos12, local_vel12, g1, g2, options) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + return Ok(parry3d::query::details::cast_shapes_ball_ball( + pos12, + local_vel12, + &ball1.0, + &ball2.0, + options, + )); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes(pos12, local_vel12, &ball1.0, g2, options) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes(pos12, local_vel12, g1, &ball2.0, options) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes(pos12, local_vel12, g1, g2, options) + } + } } fn cast_shapes_nonlinear( @@ -153,15 +269,59 @@ impl QueryDispatcher for CustomBallDispatcher { end_time: f32, stop_at_penetration: bool, ) -> Result, Unsupported> { - let dispatcher = DefaultQueryDispatcher; - dispatcher.cast_shapes_nonlinear( - motion1, - g1, - motion2, - g2, - start_time, - end_time, - stop_at_penetration, - ) + let (ball1, ball2) = ( + g1.downcast_ref::(), + g2.downcast_ref::(), + ); + + match (ball1, ball2) { + (Some(ball1), Some(ball2)) => { + return parry3d::query::details::cast_shapes_nonlinear( + motion1, + &ball1.0, + motion2, + &ball2.0, + start_time, + end_time, + stop_at_penetration, + ); + } + (Some(ball1), None) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes_nonlinear( + motion1, + &ball1.0, + motion2, + g2, + start_time, + end_time, + stop_at_penetration, + ) + } + (None, Some(ball2)) => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes_nonlinear( + motion1, + g1, + motion2, + &ball2.0, + start_time, + end_time, + stop_at_penetration, + ) + } + _ => { + let dispatcher = DefaultQueryDispatcher; + dispatcher.cast_shapes_nonlinear( + motion1, + g1, + motion2, + g2, + start_time, + end_time, + stop_at_penetration, + ) + } + } } }