parry3d/query/
default_query_dispatcher.rs

1use crate::math::{Pose, Real, Vector};
2use crate::query::details::ShapeCastOptions;
3use crate::query::{
4    self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion,
5    QueryDispatcher, ShapeCastHit, ShapeDistance, ShapeIntersection, Unsupported,
6};
7#[cfg(feature = "alloc")]
8use crate::query::{
9    contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints},
10    query_dispatcher::PersistentQueryDispatcher,
11    ContactManifold,
12};
13use crate::shape::{HalfSpace, Segment, Shape};
14#[cfg(feature = "alloc")]
15use alloc::vec::Vec;
16
17#[cfg(any(feature = "std", feature = "alloc"))]
18use crate::shape::ShapeType;
19
20/// The default query dispatcher implementation provided by Parry.
21///
22/// This dispatcher handles all the built-in shape types and automatically selects the most
23/// appropriate algorithm for each shape pair combination. It is used internally by all the
24/// free functions in the [`crate::query`] module.
25///
26/// # What It Does
27///
28/// `DefaultQueryDispatcher` implements efficient query dispatch logic that:
29///
30/// 1. **Examines shape types** using runtime type checking (`as_ball()`, `as_cuboid()`, etc.)
31/// 2. **Selects specialized algorithms** for specific shape pairs (e.g., ball-ball, cuboid-cuboid)
32/// 3. **Falls back to general algorithms** when specialized versions aren't available (e.g., GJK/EPA for support map shapes)
33/// 4. **Handles composite shapes** by decomposing them and performing multiple sub-queries
34///
35/// # Supported Shape Combinations
36///
37/// The dispatcher provides optimized implementations for many shape pairs, including:
38///
39/// ## Basic Shapes
40/// - **Ball-Ball**: Analytical formulas (fastest)
41/// - **Ball-Convex**: Specialized algorithms
42/// - **Cuboid-Cuboid**: SAT-based algorithms
43/// - **Segment-Segment**: Direct geometric calculations
44///
45/// ## Support Map Shapes
46/// For shapes implementing the `SupportMap` trait (most convex shapes):
47/// - Uses **GJK algorithm** for distance and intersection queries
48/// - Uses **EPA algorithm** for penetration depth when shapes overlap
49///
50/// ## Composite Shapes
51/// Handles complex shapes by decomposing them:
52/// - **TriMesh**: Queries individual triangles using BVH acceleration
53/// - **Compound**: Queries component shapes
54/// - **HeightField**: Efficiently queries relevant cells
55/// - **Voxels**: Queries occupied voxels
56///
57/// ## Special Cases
58/// - **HalfSpace**: Infinite planes with specialized handling
59/// - **Rounded shapes**: Automatically accounts for border radius
60///
61/// # When to Use
62///
63/// You typically don't need to create `DefaultQueryDispatcher` explicitly. The free functions
64/// in [`crate::query`] use it automatically:
65///
66/// ```
67/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
68/// use parry3d::query;
69/// use parry3d::shape::Ball;
70/// use parry3d::math::Pose;
71///
72/// let ball1 = Ball::new(1.0);
73/// let ball2 = Ball::new(1.0);
74/// let pos1 = Pose::identity();
75/// let pos2 = Pose::translation(5.0, 0.0, 0.0);
76///
77/// // This uses DefaultQueryDispatcher internally
78/// let distance = query::distance(&pos1, &ball1, &pos2, &ball2);
79/// # }
80/// ```
81///
82/// However, you might use it explicitly when:
83///
84/// - Creating a dispatcher chain with custom dispatchers
85/// - Implementing custom query logic that needs to delegate to default behavior
86/// - Building a custom collision detection pipeline
87///
88/// # Example: Direct Usage
89///
90/// ```
91/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
92/// use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher};
93/// use parry3d::shape::{Ball, Cuboid};
94/// use parry3d::math::{Pose, Vector};
95///
96/// let dispatcher = DefaultQueryDispatcher;
97///
98/// let ball = Ball::new(1.0);
99/// let cuboid = Cuboid::new(Vector::splat(1.0));
100///
101/// let pos1 = Pose::identity();
102/// let pos2 = Pose::translation(3.0, 0.0, 0.0);
103/// let pos12 = pos1.inv_mul(&pos2);
104///
105/// // Query intersection
106/// let intersects = dispatcher.intersection_test(&pos12, &ball, &cuboid)
107///     .expect("This shape pair is supported")
108///     .intersecting;
109///
110/// // Query distance
111/// let dist = dispatcher.distance(&pos12, &ball, &cuboid)
112///     .expect("This shape pair is supported")
113///     .distance;
114///
115/// println!("Distance: {}, Intersecting: {}", dist, intersects);
116/// # }
117/// ```
118///
119/// # Example: Chaining with Custom Dispatcher
120///
121/// ```ignore
122/// # {
123/// use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher};
124///
125/// struct MyCustomDispatcher;
126/// // ... implement QueryDispatcher for MyCustomDispatcher ...
127///
128/// // Try custom dispatcher first, fall back to default
129/// let dispatcher = MyCustomDispatcher.chain(DefaultQueryDispatcher);
130///
131/// // Now queries will use your custom logic when applicable,
132/// // and Parry's default logic otherwise
133/// let dist = dispatcher.distance(pos12, shape1, shape2)?;
134/// # }
135/// ```
136///
137/// # Algorithm Selection Strategy
138///
139/// The dispatcher follows this priority order when selecting algorithms:
140///
141/// 1. **Exact shape type matching**: Ball-Ball, Cuboid-Cuboid, etc.
142/// 2. **Specialized asymmetric pairs**: Ball-ConvexShape, HalfSpace-SupportMap, etc.
143/// 3. **Support map fallback**: Any SupportMap-SupportMap pair uses GJK/EPA
144/// 4. **Composite shape decomposition**: TriMesh, Compound, HeightField, Voxels
145/// 5. **Unsupported**: Returns `Err(Unsupported)` if no algorithm exists
146///
147/// # Performance Characteristics
148///
149/// - **Type checking overhead**: Minimal - uses efficient trait object downcasting
150/// - **Specialized algorithms**: O(1) for ball-ball, O(log n) to O(n) for composite shapes
151/// - **GJK/EPA**: Iterative algorithms that typically converge in 5-20 iterations
152/// - **Composite shapes**: Use BVH for O(log n) acceleration of sub-queries
153///
154/// # Thread Safety
155///
156/// `DefaultQueryDispatcher` is `Send + Sync` and has no internal state, making it safe to
157/// share across threads. You can use a single instance for all queries in a parallel
158/// collision detection system.
159///
160/// # Limitations
161///
162/// Some shape pairs are not supported and will return `Err(Unsupported)`:
163///
164/// - Custom shapes not implementing required traits (e.g., not convex, no support map)
165/// - Some asymmetric pairs that lack specialized implementations
166/// - Certain combinations involving custom user shapes
167///
168/// When encountering `Unsupported`, you can implement a custom dispatcher to handle these cases.
169///
170/// # See Also
171///
172/// - [`QueryDispatcher`]: The trait this struct implements
173/// - [`crate::query`]: High-level query functions that use this dispatcher
174/// - [`PersistentQueryDispatcher`]: Extended trait for contact manifold queries
175#[derive(Debug, Clone)]
176pub struct DefaultQueryDispatcher;
177
178impl QueryDispatcher for DefaultQueryDispatcher {
179    fn intersection_test(
180        &self,
181        pos12: &Pose,
182        shape1: &dyn Shape,
183        shape2: &dyn Shape,
184    ) -> Result<ShapeIntersection, Unsupported> {
185        if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) {
186            let p12 = pos12.translation;
187            Ok(query::details::intersection_test_ball_ball(p12, b1, b2).into())
188        } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
189            Ok(query::details::intersection_test_cuboid_cuboid(pos12, c1, c2).into())
190        } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
191            Ok(query::details::intersection_test_triangle_cuboid(pos12, t1, c2).into())
192        } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
193            Ok(query::details::intersection_test_cuboid_triangle(pos12, c1, t2).into())
194        } else if let Some(b1) = shape1.as_ball() {
195            Ok(query::details::intersection_test_ball_point_query(
196                pos12, b1, shape2,
197            ))
198        } else if let Some(b2) = shape2.as_ball() {
199            Ok(query::details::intersection_test_point_query_ball(
200                pos12, shape1, b2,
201            ))
202        } else if let (Some(p1), Some(s2)) =
203            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
204        {
205            Ok(query::details::intersection_test_halfspace_support_map(pos12, p1, s2).into())
206        } else if let (Some(s1), Some(p2)) =
207            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
208        {
209            Ok(query::details::intersection_test_support_map_halfspace(pos12, s1, p2).into())
210        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
211            Ok(query::details::intersection_test_support_map_support_map(pos12, s1, s2).into())
212        } else {
213            #[cfg(feature = "alloc")]
214            if let Some(c1) = shape1.as_composite_shape() {
215                return Ok(query::details::intersection_test_composite_shape_shape(
216                    self, pos12, c1, shape2,
217                ));
218            } else if let Some(c2) = shape2.as_composite_shape() {
219                return Ok(query::details::intersection_test_shape_composite_shape(
220                    self, pos12, shape1, c2,
221                ));
222            } else if let Some(v1) = shape1.as_voxels() {
223                return Ok(query::details::intersection_test_voxels_shape(
224                    self, pos12, v1, shape2,
225                ));
226            } else if let Some(v2) = shape2.as_voxels() {
227                return Ok(query::details::intersection_test_shape_voxels(
228                    self, pos12, shape1, v2,
229                ));
230            }
231
232            Err(Unsupported)
233        }
234    }
235
236    /// Computes the minimum distance separating two shapes.
237    ///
238    /// Returns `0.0` if the objects are touching or penetrating.
239    fn distance(
240        &self,
241        pos12: &Pose,
242        shape1: &dyn Shape,
243        shape2: &dyn Shape,
244    ) -> Result<ShapeDistance, Unsupported> {
245        let ball1 = shape1.as_ball();
246        let ball2 = shape2.as_ball();
247
248        if let (Some(b1), Some(b2)) = (ball1, ball2) {
249            let p2 = pos12.translation;
250            Ok(query::details::distance_ball_ball(b1, p2, b2).into())
251        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
252            Ok(query::details::distance_ball_convex_polyhedron(pos12, b1, shape2).into())
253        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
254            Ok(query::details::distance_convex_polyhedron_ball(pos12, shape1, b2).into())
255        } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
256            Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2).into())
257        } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) {
258            Ok(query::details::distance_segment_segment(pos12, s1, s2).into())
259        } else if let (Some(p1), Some(s2)) =
260            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
261        {
262            Ok(query::details::distance_halfspace_support_map(pos12, p1, s2).into())
263        } else if let (Some(s1), Some(p2)) =
264            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
265        {
266            Ok(query::details::distance_support_map_halfspace(pos12, s1, p2).into())
267        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
268            Ok(query::details::distance_support_map_support_map(pos12, s1, s2).into())
269        } else {
270            #[cfg(feature = "alloc")]
271            if let Some(c1) = shape1.as_composite_shape() {
272                return Ok(query::details::distance_composite_shape_shape(
273                    self, pos12, c1, shape2,
274                ));
275            } else if let Some(c2) = shape2.as_composite_shape() {
276                return Ok(query::details::distance_shape_composite_shape(
277                    self, pos12, shape1, c2,
278                ));
279            }
280
281            Err(Unsupported)
282        }
283    }
284
285    fn contact(
286        &self,
287        pos12: &Pose,
288        shape1: &dyn Shape,
289        shape2: &dyn Shape,
290        prediction: Real,
291    ) -> Result<Option<Contact>, Unsupported> {
292        let ball1 = shape1.as_ball();
293        let ball2 = shape2.as_ball();
294
295        if let (Some(b1), Some(b2)) = (ball1, ball2) {
296            Ok(query::details::contact_ball_ball(pos12, b1, b2, prediction))
297        // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
298        //     Ok(query::details::contact_cuboid_cuboid(
299        //         pos12, c1, c2, prediction,
300        //     ))
301        } else if let (Some(p1), Some(s2)) =
302            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
303        {
304            Ok(query::details::contact_halfspace_support_map(
305                pos12, p1, s2, prediction,
306            ))
307        } else if let (Some(s1), Some(p2)) =
308            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
309        {
310            Ok(query::details::contact_support_map_halfspace(
311                pos12, s1, p2, prediction,
312            ))
313        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
314            Ok(query::details::contact_ball_convex_polyhedron(
315                pos12, b1, shape2, prediction,
316            ))
317        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
318            Ok(query::details::contact_convex_polyhedron_ball(
319                pos12, shape1, b2, prediction,
320            ))
321        } else {
322            #[cfg(feature = "alloc")]
323            if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
324                return Ok(query::details::contact_support_map_support_map(
325                    pos12, s1, s2, prediction,
326                ));
327            } else if let Some(hf1) = shape1.as_heightfield() {
328                return Ok(query::details::contact_heightfield_shape(
329                    self, pos12, hf1, shape2, prediction,
330                ));
331            } else if let Some(hf2) = shape2.as_heightfield() {
332                return Ok(query::details::contact_shape_heightfield(
333                    self, pos12, shape1, hf2, prediction,
334                ));
335            } else if let Some(c1) = shape1.as_composite_shape() {
336                return Ok(query::details::contact_composite_shape_shape(
337                    self, pos12, c1, shape2, prediction,
338                ));
339            } else if let Some(c2) = shape2.as_composite_shape() {
340                return Ok(query::details::contact_shape_composite_shape(
341                    self, pos12, shape1, c2, prediction,
342                ));
343            }
344
345            Err(Unsupported)
346        }
347    }
348
349    fn closest_points(
350        &self,
351        pos12: &Pose,
352        shape1: &dyn Shape,
353        shape2: &dyn Shape,
354        max_dist: Real,
355    ) -> Result<ClosestPoints, Unsupported> {
356        let ball1 = shape1.as_ball();
357        let ball2 = shape2.as_ball();
358
359        if let (Some(b1), Some(b2)) = (ball1, ball2) {
360            Ok(query::details::closest_points_ball_ball(
361                pos12, b1, b2, max_dist,
362            ))
363        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
364            Ok(query::details::closest_points_ball_convex_polyhedron(
365                pos12, b1, shape2, max_dist,
366            ))
367        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
368            Ok(query::details::closest_points_convex_polyhedron_ball(
369                pos12, shape1, b2, max_dist,
370            ))
371        } else if let (Some(s1), Some(s2)) =
372            (shape1.as_shape::<Segment>(), shape2.as_shape::<Segment>())
373        {
374            Ok(query::details::closest_points_segment_segment(
375                pos12, s1, s2, max_dist,
376            ))
377        // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
378        //     Ok(query::details::closest_points_cuboid_cuboid(
379        //         pos12, c1, c2, max_dist,
380        //     ))
381        } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) {
382            Ok(query::details::closest_points_segment_segment(
383                pos12, s1, s2, max_dist,
384            ))
385        // } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
386        //     Ok(query::details::closest_points_cuboid_triangle(
387        //         pos12, c1, t2, max_dist,
388        //     ))
389        } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
390            Ok(query::details::closest_points_triangle_cuboid(
391                pos12, t1, c2, max_dist,
392            ))
393        } else if let (Some(p1), Some(s2)) =
394            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
395        {
396            Ok(query::details::closest_points_halfspace_support_map(
397                pos12, p1, s2, max_dist,
398            ))
399        } else if let (Some(s1), Some(p2)) =
400            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
401        {
402            Ok(query::details::closest_points_support_map_halfspace(
403                pos12, s1, p2, max_dist,
404            ))
405        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
406            Ok(query::details::closest_points_support_map_support_map(
407                pos12, s1, s2, max_dist,
408            ))
409        } else {
410            #[cfg(feature = "alloc")]
411            if let Some(c1) = shape1.as_composite_shape() {
412                return Ok(query::details::closest_points_composite_shape_shape(
413                    self, pos12, c1, shape2, max_dist,
414                ));
415            } else if let Some(c2) = shape2.as_composite_shape() {
416                return Ok(query::details::closest_points_shape_composite_shape(
417                    self, pos12, shape1, c2, max_dist,
418                ));
419            }
420
421            Err(Unsupported)
422        }
423    }
424
425    fn cast_shapes(
426        &self,
427        pos12: &Pose,
428        local_vel12: Vector,
429        shape1: &dyn Shape,
430        shape2: &dyn Shape,
431        options: ShapeCastOptions,
432    ) -> Result<Option<ShapeCastHit>, Unsupported> {
433        if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) {
434            Ok(query::details::cast_shapes_ball_ball(
435                pos12,
436                local_vel12,
437                b1,
438                b2,
439                options,
440            ))
441        } else if let (Some(p1), Some(s2)) =
442            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
443        {
444            Ok(query::details::cast_shapes_halfspace_support_map(
445                pos12,
446                local_vel12,
447                p1,
448                s2,
449                options,
450            ))
451        } else if let (Some(s1), Some(p2)) =
452            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
453        {
454            Ok(query::details::cast_shapes_support_map_halfspace(
455                pos12,
456                local_vel12,
457                s1,
458                p2,
459                options,
460            ))
461        } else {
462            #[cfg(feature = "alloc")]
463            if let Some(heightfield1) = shape1.as_heightfield() {
464                return query::details::cast_shapes_heightfield_shape(
465                    self,
466                    pos12,
467                    local_vel12,
468                    heightfield1,
469                    shape2,
470                    options,
471                );
472            } else if let Some(heightfield2) = shape2.as_heightfield() {
473                return query::details::cast_shapes_shape_heightfield(
474                    self,
475                    pos12,
476                    local_vel12,
477                    shape1,
478                    heightfield2,
479                    options,
480                );
481            } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map())
482            {
483                return Ok(query::details::cast_shapes_support_map_support_map(
484                    pos12,
485                    local_vel12,
486                    s1,
487                    s2,
488                    options,
489                ));
490            } else if let Some(c1) = shape1.as_composite_shape() {
491                return Ok(query::details::cast_shapes_composite_shape_shape(
492                    self,
493                    pos12,
494                    local_vel12,
495                    c1,
496                    shape2,
497                    options,
498                ));
499            } else if let Some(c2) = shape2.as_composite_shape() {
500                return Ok(query::details::cast_shapes_shape_composite_shape(
501                    self,
502                    pos12,
503                    local_vel12,
504                    shape1,
505                    c2,
506                    options,
507                ));
508            } else if let Some(v1) = shape1.as_voxels() {
509                return Ok(query::details::cast_shapes_voxels_shape(
510                    self,
511                    pos12,
512                    local_vel12,
513                    v1,
514                    shape2,
515                    options,
516                ));
517            } else if let Some(v2) = shape2.as_voxels() {
518                return Ok(query::details::cast_shapes_shape_voxels(
519                    self,
520                    pos12,
521                    local_vel12,
522                    shape1,
523                    v2,
524                    options,
525                ));
526            }
527
528            Err(Unsupported)
529        }
530    }
531
532    fn cast_shapes_nonlinear(
533        &self,
534        motion1: &NonlinearRigidMotion,
535        shape1: &dyn Shape,
536        motion2: &NonlinearRigidMotion,
537        shape2: &dyn Shape,
538        start_time: Real,
539        end_time: Real,
540        stop_at_penetration: bool,
541    ) -> Result<Option<ShapeCastHit>, Unsupported> {
542        if let (Some(sm1), Some(sm2)) = (shape1.as_support_map(), shape2.as_support_map()) {
543            let mode = if stop_at_penetration {
544                NonlinearShapeCastMode::StopAtPenetration
545            } else {
546                NonlinearShapeCastMode::directional_toi(shape1, shape2)
547            };
548
549            Ok(
550                query::details::cast_shapes_nonlinear_support_map_support_map(
551                    self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode,
552                ),
553            )
554        } else {
555            #[cfg(feature = "alloc")]
556            if let Some(c1) = shape1.as_composite_shape() {
557                return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape(
558                    self,
559                    motion1,
560                    c1,
561                    motion2,
562                    shape2,
563                    start_time,
564                    end_time,
565                    stop_at_penetration,
566                ));
567            } else if let Some(c2) = shape2.as_composite_shape() {
568                return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape(
569                    self,
570                    motion1,
571                    shape1,
572                    motion2,
573                    c2,
574                    start_time,
575                    end_time,
576                    stop_at_penetration,
577                ));
578            } else if let Some(c1) = shape1.as_voxels() {
579                return Ok(query::details::cast_shapes_nonlinear_voxels_shape(
580                    self,
581                    motion1,
582                    c1,
583                    motion2,
584                    shape2,
585                    start_time,
586                    end_time,
587                    stop_at_penetration,
588                ));
589            } else if let Some(c2) = shape2.as_voxels() {
590                return Ok(query::details::cast_shapes_nonlinear_shape_voxels(
591                    self,
592                    motion1,
593                    shape1,
594                    motion2,
595                    c2,
596                    start_time,
597                    end_time,
598                    stop_at_penetration,
599                ));
600            }
601            /* } else if let (Some(p1), Some(s2)) = (shape1.as_shape::<HalfSpace>(), shape2.as_support_map()) {
602            //        query::details::cast_shapes_nonlinear_halfspace_support_map(m1, vel1, p1, m2, vel2, s2)
603                    unimplemented!()
604                } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::<HalfSpace>()) {
605            //        query::details::cast_shapes_nonlinear_support_map_halfspace(m1, vel1, s1, m2, vel2, p2)
606                    unimplemented!() */
607
608            Err(Unsupported)
609        }
610    }
611}
612
613#[cfg(feature = "alloc")]
614impl<ManifoldData, ContactData> PersistentQueryDispatcher<ManifoldData, ContactData>
615    for DefaultQueryDispatcher
616where
617    ManifoldData: Default + Clone + Send + Sync,
618    ContactData: Default + Copy + Send + Sync,
619{
620    fn contact_manifolds(
621        &self,
622        pos12: &Pose,
623        shape1: &dyn Shape,
624        shape2: &dyn Shape,
625        prediction: Real,
626        manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
627        workspace: &mut Option<ContactManifoldsWorkspace>,
628    ) -> Result<(), Unsupported> {
629        use crate::query::contact_manifolds::*;
630
631        let composite1 = shape1.as_composite_shape();
632        let composite2 = shape2.as_composite_shape();
633
634        if let (Some(composite1), Some(composite2)) = (composite1, composite2) {
635            contact_manifolds_composite_shape_composite_shape(
636                self, pos12, composite1, composite2, prediction, manifolds, workspace,
637            );
638
639            return Ok(());
640        }
641
642        match (shape1.shape_type(), shape2.shape_type()) {
643            (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => {
644                contact_manifolds_trimesh_shape_shapes(
645                    self, pos12, shape1, shape2, prediction, manifolds, workspace,
646                );
647            }
648            (ShapeType::HeightField, _) => {
649                if let Some(composite2) = composite2 {
650                    contact_manifolds_heightfield_composite_shape(
651                        self,
652                        pos12,
653                        &pos12.inverse(),
654                        shape1.as_heightfield().unwrap(),
655                        composite2,
656                        prediction,
657                        manifolds,
658                        workspace,
659                        false,
660                    )
661                } else {
662                    contact_manifolds_heightfield_shape_shapes(
663                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
664                    );
665                }
666            }
667            (_, ShapeType::HeightField) => {
668                if let Some(composite1) = composite1 {
669                    contact_manifolds_heightfield_composite_shape(
670                        self,
671                        &pos12.inverse(),
672                        pos12,
673                        shape2.as_heightfield().unwrap(),
674                        composite1,
675                        prediction,
676                        manifolds,
677                        workspace,
678                        true,
679                    )
680                } else {
681                    contact_manifolds_heightfield_shape_shapes(
682                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
683                    );
684                }
685            }
686            (ShapeType::Voxels, ShapeType::Voxels) => contact_manifolds_voxels_voxels_shapes(
687                self, pos12, shape1, shape2, prediction, manifolds, workspace,
688            ),
689            (ShapeType::Voxels, ShapeType::Ball) | (ShapeType::Ball, ShapeType::Voxels) => {
690                contact_manifolds_voxels_ball_shapes(pos12, shape1, shape2, prediction, manifolds)
691            }
692            (ShapeType::Voxels, _) | (_, ShapeType::Voxels) => {
693                if composite1.is_some() || composite2.is_some() {
694                    contact_manifolds_voxels_composite_shape_shapes(
695                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
696                    )
697                } else {
698                    contact_manifolds_voxels_shape_shapes(
699                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
700                    )
701                }
702            }
703            _ => {
704                if let Some(composite1) = composite1 {
705                    contact_manifolds_composite_shape_shape(
706                        self, pos12, composite1, shape2, prediction, manifolds, workspace, false,
707                    );
708                } else if let Some(composite2) = composite2 {
709                    contact_manifolds_composite_shape_shape(
710                        self,
711                        &pos12.inverse(),
712                        composite2,
713                        shape1,
714                        prediction,
715                        manifolds,
716                        workspace,
717                        true,
718                    );
719                } else {
720                    if manifolds.is_empty() {
721                        manifolds.push(ContactManifold::new());
722                    }
723
724                    return self.contact_manifold_convex_convex(
725                        pos12,
726                        shape1,
727                        shape2,
728                        None,
729                        None,
730                        prediction,
731                        &mut manifolds[0],
732                    );
733                }
734            }
735        }
736
737        Ok(())
738    }
739
740    fn contact_manifold_convex_convex(
741        &self,
742        pos12: &Pose,
743        shape1: &dyn Shape,
744        shape2: &dyn Shape,
745        normal_constraints1: Option<&dyn NormalConstraints>,
746        normal_constraints2: Option<&dyn NormalConstraints>,
747        prediction: Real,
748        manifold: &mut ContactManifold<ManifoldData, ContactData>,
749    ) -> Result<(), Unsupported> {
750        use crate::query::contact_manifolds::*;
751
752        match (shape1.shape_type(), shape2.shape_type()) {
753            (ShapeType::Ball, ShapeType::Ball) => {
754                contact_manifold_ball_ball_shapes(pos12, shape1, shape2, prediction, manifold)
755            }
756            (ShapeType::Cuboid, ShapeType::Cuboid) =>
757                contact_manifold_cuboid_cuboid_shapes(pos12, shape1, shape2, prediction, manifold)
758            ,
759            // (ShapeType::Polygon, ShapeType::Polygon) => (
760            //     PrimitiveContactGenerator {
761            //         generate_contacts: super::generate_contacts_polygon_polygon,
762            //         ..PrimitiveContactGenerator::default()
763            //     },
764            //     None,
765            // ),
766            (ShapeType::Capsule, ShapeType::Capsule) => {
767                contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold)
768            }
769            (_, ShapeType::Ball) | (ShapeType::Ball, _) => {
770                contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold)
771            }
772            // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) =>
773            //     contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold),
774            (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
775                contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2,  prediction, manifold)
776            }
777            (ShapeType::HalfSpace, _) => {
778                if let Some((pfm2, border_radius2)) = shape2.as_polygonal_feature_map() {
779                    contact_manifold_halfspace_pfm(
780                        pos12,
781                        shape1.as_halfspace().unwrap(),
782                        pfm2,
783                        border_radius2,
784                        prediction,
785                        manifold,
786                        false
787                    )
788                } else {
789                    return Err(Unsupported)
790                }
791            }
792            (_, ShapeType::HalfSpace) => {
793                if let Some((pfm1, border_radius1)) = shape1.as_polygonal_feature_map() {
794                    contact_manifold_halfspace_pfm(
795                        &pos12.inverse(),
796                        shape2.as_halfspace().unwrap(),
797                        pfm1,
798                        border_radius1,
799                        prediction,
800                        manifold,
801                        true
802                    )
803                } else {
804                    return Err(Unsupported)
805                }
806            }
807            _ => {
808                if let (Some(pfm1), Some(pfm2)) = (
809                    shape1.as_polygonal_feature_map(),
810                    shape2.as_polygonal_feature_map(),
811                ) {
812                    contact_manifold_pfm_pfm(
813                        pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold,
814                    )
815                } else {
816                    return Err(Unsupported);
817                }
818            }
819        }
820
821        Ok(())
822    }
823}
parry3d/query/default_query_dispatcher.rs

parry3d/query/
default_query_dispatcher.rs
