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

parry3d/query/
default_query_dispatcher.rs
