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(hf1) = shape1.as_heightfield() {
348                return Ok(query::details::contact_heightfield_shape(
349                    self, pos12, hf1, shape2, prediction,
350                ));
351            } else if let Some(hf2) = shape2.as_heightfield() {
352                return Ok(query::details::contact_shape_heightfield(
353                    self, pos12, shape1, hf2, prediction,
354                ));
355            } else if let Some(c1) = shape1.as_composite_shape() {
356                return Ok(query::details::contact_composite_shape_shape(
357                    self, pos12, c1, shape2, prediction,
358                ));
359            } else if let Some(c2) = shape2.as_composite_shape() {
360                return Ok(query::details::contact_shape_composite_shape(
361                    self, pos12, shape1, c2, prediction,
362                ));
363            }
364
365            Err(Unsupported)
366        }
367    }
368
369    fn closest_points(
370        &self,
371        pos12: &Pose,
372        shape1: &dyn Shape,
373        shape2: &dyn Shape,
374        max_dist: Real,
375    ) -> Result<ClosestPoints, Unsupported> {
376        let ball1 = shape1.as_ball();
377        let ball2 = shape2.as_ball();
378
379        if let (Some(b1), Some(b2)) = (ball1, ball2) {
380            Ok(query::details::closest_points_ball_ball(
381                pos12, b1, b2, max_dist,
382            ))
383        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
384            Ok(query::details::closest_points_ball_convex_polyhedron(
385                pos12, b1, shape2, max_dist,
386            ))
387        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
388            Ok(query::details::closest_points_convex_polyhedron_ball(
389                pos12, shape1, b2, max_dist,
390            ))
391        } else if let (Some(s1), Some(s2)) =
392            (shape1.as_shape::<Segment>(), shape2.as_shape::<Segment>())
393        {
394            Ok(query::details::closest_points_segment_segment(
395                pos12, s1, s2, max_dist,
396            ))
397        // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
398        //     Ok(query::details::closest_points_cuboid_cuboid(
399        //         pos12, c1, c2, max_dist,
400        //     ))
401        } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) {
402            Ok(query::details::closest_points_segment_segment(
403                pos12, s1, s2, max_dist,
404            ))
405        // } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
406        //     Ok(query::details::closest_points_cuboid_triangle(
407        //         pos12, c1, t2, max_dist,
408        //     ))
409        } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
410            Ok(query::details::closest_points_triangle_cuboid(
411                pos12, t1, c2, max_dist,
412            ))
413        } else if let (Some(p1), Some(s2)) =
414            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
415        {
416            Ok(query::details::closest_points_halfspace_support_map(
417                pos12, p1, s2, max_dist,
418            ))
419        } else if let (Some(s1), Some(p2)) =
420            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
421        {
422            Ok(query::details::closest_points_support_map_halfspace(
423                pos12, s1, p2, max_dist,
424            ))
425        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
426            Ok(query::details::closest_points_support_map_support_map(
427                pos12, s1, s2, max_dist,
428            ))
429        } else {
430            #[cfg(feature = "alloc")]
431            if let Some(c1) = shape1.as_composite_shape() {
432                return Ok(query::details::closest_points_composite_shape_shape(
433                    self, pos12, c1, shape2, max_dist,
434                ));
435            } else if let Some(c2) = shape2.as_composite_shape() {
436                return Ok(query::details::closest_points_shape_composite_shape(
437                    self, pos12, shape1, c2, max_dist,
438                ));
439            }
440
441            Err(Unsupported)
442        }
443    }
444
445    fn cast_shapes(
446        &self,
447        pos12: &Pose,
448        local_vel12: Vector,
449        shape1: &dyn Shape,
450        shape2: &dyn Shape,
451        options: ShapeCastOptions,
452    ) -> Result<Option<ShapeCastHit>, Unsupported> {
453        if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) {
454            Ok(query::details::cast_shapes_ball_ball(
455                pos12,
456                local_vel12,
457                b1,
458                b2,
459                options,
460            ))
461        } else if let (Some(p1), Some(s2)) =
462            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
463        {
464            Ok(query::details::cast_shapes_halfspace_support_map(
465                pos12,
466                local_vel12,
467                p1,
468                s2,
469                options,
470            ))
471        } else if let (Some(s1), Some(p2)) =
472            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
473        {
474            Ok(query::details::cast_shapes_support_map_halfspace(
475                pos12,
476                local_vel12,
477                s1,
478                p2,
479                options,
480            ))
481        } else {
482            #[cfg(feature = "alloc")]
483            if let Some(heightfield1) = shape1.as_heightfield() {
484                return query::details::cast_shapes_heightfield_shape(
485                    self,
486                    pos12,
487                    local_vel12,
488                    heightfield1,
489                    shape2,
490                    options,
491                );
492            } else if let Some(heightfield2) = shape2.as_heightfield() {
493                return query::details::cast_shapes_shape_heightfield(
494                    self,
495                    pos12,
496                    local_vel12,
497                    shape1,
498                    heightfield2,
499                    options,
500                );
501            } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map())
502            {
503                return Ok(query::details::cast_shapes_support_map_support_map(
504                    pos12,
505                    local_vel12,
506                    s1,
507                    s2,
508                    options,
509                ));
510            } else if let Some(c1) = shape1.as_composite_shape() {
511                return Ok(query::details::cast_shapes_composite_shape_shape(
512                    self,
513                    pos12,
514                    local_vel12,
515                    c1,
516                    shape2,
517                    options,
518                ));
519            } else if let Some(c2) = shape2.as_composite_shape() {
520                return Ok(query::details::cast_shapes_shape_composite_shape(
521                    self,
522                    pos12,
523                    local_vel12,
524                    shape1,
525                    c2,
526                    options,
527                ));
528            } else if let Some(v1) = shape1.as_voxels() {
529                return Ok(query::details::cast_shapes_voxels_shape(
530                    self,
531                    pos12,
532                    local_vel12,
533                    v1,
534                    shape2,
535                    options,
536                ));
537            } else if let Some(v2) = shape2.as_voxels() {
538                return Ok(query::details::cast_shapes_shape_voxels(
539                    self,
540                    pos12,
541                    local_vel12,
542                    shape1,
543                    v2,
544                    options,
545                ));
546            }
547
548            Err(Unsupported)
549        }
550    }
551
552    fn cast_shapes_nonlinear(
553        &self,
554        motion1: &NonlinearRigidMotion,
555        shape1: &dyn Shape,
556        motion2: &NonlinearRigidMotion,
557        shape2: &dyn Shape,
558        start_time: Real,
559        end_time: Real,
560        stop_at_penetration: bool,
561    ) -> Result<Option<ShapeCastHit>, Unsupported> {
562        if let (Some(sm1), Some(sm2)) = (shape1.as_support_map(), shape2.as_support_map()) {
563            let mode = if stop_at_penetration {
564                NonlinearShapeCastMode::StopAtPenetration
565            } else {
566                NonlinearShapeCastMode::directional_toi(shape1, shape2)
567            };
568
569            Ok(
570                query::details::cast_shapes_nonlinear_support_map_support_map(
571                    self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode,
572                ),
573            )
574        } else {
575            #[cfg(feature = "alloc")]
576            if let Some(c1) = shape1.as_composite_shape() {
577                return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape(
578                    self,
579                    motion1,
580                    c1,
581                    motion2,
582                    shape2,
583                    start_time,
584                    end_time,
585                    stop_at_penetration,
586                ));
587            } else if let Some(c2) = shape2.as_composite_shape() {
588                return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape(
589                    self,
590                    motion1,
591                    shape1,
592                    motion2,
593                    c2,
594                    start_time,
595                    end_time,
596                    stop_at_penetration,
597                ));
598            } else if let Some(c1) = shape1.as_voxels() {
599                return Ok(query::details::cast_shapes_nonlinear_voxels_shape(
600                    self,
601                    motion1,
602                    c1,
603                    motion2,
604                    shape2,
605                    start_time,
606                    end_time,
607                    stop_at_penetration,
608                ));
609            } else if let Some(c2) = shape2.as_voxels() {
610                return Ok(query::details::cast_shapes_nonlinear_shape_voxels(
611                    self,
612                    motion1,
613                    shape1,
614                    motion2,
615                    c2,
616                    start_time,
617                    end_time,
618                    stop_at_penetration,
619                ));
620            }
621            /* } else if let (Some(p1), Some(s2)) = (shape1.as_shape::<HalfSpace>(), shape2.as_support_map()) {
622            //        query::details::cast_shapes_nonlinear_halfspace_support_map(m1, vel1, p1, m2, vel2, s2)
623                    unimplemented!()
624                } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::<HalfSpace>()) {
625            //        query::details::cast_shapes_nonlinear_support_map_halfspace(m1, vel1, s1, m2, vel2, p2)
626                    unimplemented!() */
627
628            Err(Unsupported)
629        }
630    }
631}
632
633#[cfg(feature = "alloc")]
634impl<ManifoldData, ContactData> PersistentQueryDispatcher<ManifoldData, ContactData>
635    for DefaultQueryDispatcher
636where
637    ManifoldData: Default + Clone,
638    ContactData: Default + Copy,
639{
640    fn contact_manifolds(
641        &self,
642        pos12: &Pose,
643        shape1: &dyn Shape,
644        shape2: &dyn Shape,
645        prediction: Real,
646        manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
647        workspace: &mut Option<ContactManifoldsWorkspace>,
648    ) -> Result<(), Unsupported> {
649        use crate::query::contact_manifolds::*;
650
651        let composite1 = shape1.as_composite_shape();
652        let composite2 = shape2.as_composite_shape();
653
654        if let (Some(composite1), Some(composite2)) = (composite1, composite2) {
655            contact_manifolds_composite_shape_composite_shape(
656                self, pos12, composite1, composite2, prediction, manifolds, workspace,
657            );
658
659            return Ok(());
660        }
661
662        match (shape1.shape_type(), shape2.shape_type()) {
663            (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => {
664                contact_manifolds_trimesh_shape_shapes(
665                    self, pos12, shape1, shape2, prediction, manifolds, workspace,
666                );
667            }
668            (ShapeType::HeightField, _) => {
669                if let Some(composite2) = composite2 {
670                    contact_manifolds_heightfield_composite_shape(
671                        self,
672                        pos12,
673                        &pos12.inverse(),
674                        shape1.as_heightfield().unwrap(),
675                        composite2,
676                        prediction,
677                        manifolds,
678                        workspace,
679                        false,
680                    )
681                } else {
682                    contact_manifolds_heightfield_shape_shapes(
683                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
684                    );
685                }
686            }
687            (_, ShapeType::HeightField) => {
688                if let Some(composite1) = composite1 {
689                    contact_manifolds_heightfield_composite_shape(
690                        self,
691                        &pos12.inverse(),
692                        pos12,
693                        shape2.as_heightfield().unwrap(),
694                        composite1,
695                        prediction,
696                        manifolds,
697                        workspace,
698                        true,
699                    )
700                } else {
701                    contact_manifolds_heightfield_shape_shapes(
702                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
703                    );
704                }
705            }
706            (ShapeType::Voxels, ShapeType::Voxels) => contact_manifolds_voxels_voxels_shapes(
707                self, pos12, shape1, shape2, prediction, manifolds, workspace,
708            ),
709            (ShapeType::Voxels, ShapeType::Ball) | (ShapeType::Ball, ShapeType::Voxels) => {
710                contact_manifolds_voxels_ball_shapes(pos12, shape1, shape2, prediction, manifolds)
711            }
712            (ShapeType::Voxels, _) | (_, ShapeType::Voxels) => {
713                if composite1.is_some() || composite2.is_some() {
714                    contact_manifolds_voxels_composite_shape_shapes(
715                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
716                    )
717                } else {
718                    contact_manifolds_voxels_shape_shapes(
719                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
720                    )
721                }
722            }
723            _ => {
724                if let Some(composite1) = composite1 {
725                    contact_manifolds_composite_shape_shape(
726                        self, pos12, composite1, shape2, prediction, manifolds, workspace, false,
727                    );
728                } else if let Some(composite2) = composite2 {
729                    contact_manifolds_composite_shape_shape(
730                        self,
731                        &pos12.inverse(),
732                        composite2,
733                        shape1,
734                        prediction,
735                        manifolds,
736                        workspace,
737                        true,
738                    );
739                } else {
740                    if manifolds.is_empty() {
741                        manifolds.push(ContactManifold::new());
742                    }
743
744                    return self.contact_manifold_convex_convex(
745                        pos12,
746                        shape1,
747                        shape2,
748                        None,
749                        None,
750                        prediction,
751                        &mut manifolds[0],
752                    );
753                }
754            }
755        }
756
757        Ok(())
758    }
759
760    fn contact_manifold_convex_convex(
761        &self,
762        pos12: &Pose,
763        shape1: &dyn Shape,
764        shape2: &dyn Shape,
765        normal_constraints1: Option<&dyn NormalConstraints>,
766        normal_constraints2: Option<&dyn NormalConstraints>,
767        prediction: Real,
768        manifold: &mut ContactManifold<ManifoldData, ContactData>,
769    ) -> Result<(), Unsupported> {
770        use crate::query::contact_manifolds::*;
771
772        match (shape1.shape_type(), shape2.shape_type()) {
773            (ShapeType::Ball, ShapeType::Ball) => {
774                contact_manifold_ball_ball_shapes(pos12, shape1, shape2, prediction, manifold)
775            }
776            (ShapeType::Cuboid, ShapeType::Cuboid) =>
777                contact_manifold_cuboid_cuboid_shapes(pos12, shape1, shape2, prediction, manifold)
778            ,
779            // (ShapeType::Polygon, ShapeType::Polygon) => (
780            //     PrimitiveContactGenerator {
781            //         generate_contacts: super::generate_contacts_polygon_polygon,
782            //         ..PrimitiveContactGenerator::default()
783            //     },
784            //     None,
785            // ),
786            (ShapeType::Capsule, ShapeType::Capsule) => {
787                contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold)
788            }
789            (_, ShapeType::Ball) | (ShapeType::Ball, _) => {
790                contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold)
791            }
792            // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) =>
793            //     contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold),
794            (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
795                contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2,  prediction, manifold)
796            }
797            (ShapeType::HalfSpace, _) => {
798                if let Some((pfm2, border_radius2)) = shape2.as_polygonal_feature_map() {
799                    contact_manifold_halfspace_pfm(
800                        pos12,
801                        shape1.as_halfspace().unwrap(),
802                        pfm2,
803                        border_radius2,
804                        prediction,
805                        manifold,
806                        false
807                    )
808                } else {
809                    return Err(Unsupported)
810                }
811            }
812            (_, ShapeType::HalfSpace) => {
813                if let Some((pfm1, border_radius1)) = shape1.as_polygonal_feature_map() {
814                    contact_manifold_halfspace_pfm(
815                        &pos12.inverse(),
816                        shape2.as_halfspace().unwrap(),
817                        pfm1,
818                        border_radius1,
819                        prediction,
820                        manifold,
821                        true
822                    )
823                } else {
824                    return Err(Unsupported)
825                }
826            }
827            _ => {
828                if let (Some(pfm1), Some(pfm2)) = (
829                    shape1.as_polygonal_feature_map(),
830                    shape2.as_polygonal_feature_map(),
831                ) {
832                    contact_manifold_pfm_pfm(
833                        pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold,
834                    )
835                } else {
836                    return Err(Unsupported);
837                }
838            }
839        }
840
841        Ok(())
842    }
843}
parry3d/query/default_query_dispatcher.rs

parry3d/query/
default_query_dispatcher.rs
