parry2d/query/
default_query_dispatcher.rs

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

parry2d/query/
default_query_dispatcher.rs
