parry3d/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/// A dispatcher that exposes built-in queries
18#[derive(Debug, Clone)]
19pub struct DefaultQueryDispatcher;
20
21impl QueryDispatcher for DefaultQueryDispatcher {
22    fn intersection_test(
23        &self,
24        pos12: &Isometry<Real>,
25        shape1: &dyn Shape,
26        shape2: &dyn Shape,
27    ) -> Result<bool, Unsupported> {
28        if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) {
29            let p12 = Point::from(pos12.translation.vector);
30            Ok(query::details::intersection_test_ball_ball(&p12, b1, b2))
31        } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
32            Ok(query::details::intersection_test_cuboid_cuboid(
33                pos12, c1, c2,
34            ))
35        } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
36            Ok(query::details::intersection_test_triangle_cuboid(
37                pos12, t1, c2,
38            ))
39        } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
40            Ok(query::details::intersection_test_cuboid_triangle(
41                pos12, c1, t2,
42            ))
43        } else if let Some(b1) = shape1.as_ball() {
44            Ok(query::details::intersection_test_ball_point_query(
45                pos12, b1, shape2,
46            ))
47        } else if let Some(b2) = shape2.as_ball() {
48            Ok(query::details::intersection_test_point_query_ball(
49                pos12, shape1, b2,
50            ))
51        } else if let (Some(p1), Some(s2)) =
52            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
53        {
54            Ok(query::details::intersection_test_halfspace_support_map(
55                pos12, p1, s2,
56            ))
57        } else if let (Some(s1), Some(p2)) =
58            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
59        {
60            Ok(query::details::intersection_test_support_map_halfspace(
61                pos12, s1, p2,
62            ))
63        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
64            Ok(query::details::intersection_test_support_map_support_map(
65                pos12, s1, s2,
66            ))
67        } else {
68            #[cfg(feature = "alloc")]
69            if let Some(c1) = shape1.as_composite_shape() {
70                return Ok(query::details::intersection_test_composite_shape_shape(
71                    self, pos12, c1, shape2,
72                ));
73            } else if let Some(c2) = shape2.as_composite_shape() {
74                return Ok(query::details::intersection_test_shape_composite_shape(
75                    self, pos12, shape1, c2,
76                ));
77            } else if let Some(v1) = shape1.as_voxels() {
78                return Ok(query::details::intersection_test_voxels_shape(
79                    self, pos12, v1, shape2,
80                ));
81            } else if let Some(v2) = shape2.as_voxels() {
82                return Ok(query::details::intersection_test_shape_voxels(
83                    self, pos12, shape1, v2,
84                ));
85            }
86
87            Err(Unsupported)
88        }
89    }
90
91    /// Computes the minimum distance separating two shapes.
92    ///
93    /// Returns `0.0` if the objects are touching or penetrating.
94    fn distance(
95        &self,
96        pos12: &Isometry<Real>,
97        shape1: &dyn Shape,
98        shape2: &dyn Shape,
99    ) -> Result<Real, Unsupported> {
100        let ball1 = shape1.as_ball();
101        let ball2 = shape2.as_ball();
102
103        if let (Some(b1), Some(b2)) = (ball1, ball2) {
104            let p2 = Point::from(pos12.translation.vector);
105            Ok(query::details::distance_ball_ball(b1, &p2, b2))
106        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
107            Ok(query::details::distance_ball_convex_polyhedron(
108                pos12, b1, shape2,
109            ))
110        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
111            Ok(query::details::distance_convex_polyhedron_ball(
112                pos12, shape1, b2,
113            ))
114        } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
115            Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2))
116        } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) {
117            Ok(query::details::distance_segment_segment(pos12, s1, s2))
118        } else if let (Some(p1), Some(s2)) =
119            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
120        {
121            Ok(query::details::distance_halfspace_support_map(
122                pos12, p1, s2,
123            ))
124        } else if let (Some(s1), Some(p2)) =
125            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
126        {
127            Ok(query::details::distance_support_map_halfspace(
128                pos12, s1, p2,
129            ))
130        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
131            Ok(query::details::distance_support_map_support_map(
132                pos12, s1, s2,
133            ))
134        } else {
135            #[cfg(feature = "alloc")]
136            if let Some(c1) = shape1.as_composite_shape() {
137                return Ok(query::details::distance_composite_shape_shape(
138                    self, pos12, c1, shape2,
139                ));
140            } else if let Some(c2) = shape2.as_composite_shape() {
141                return Ok(query::details::distance_shape_composite_shape(
142                    self, pos12, shape1, c2,
143                ));
144            }
145
146            Err(Unsupported)
147        }
148    }
149
150    fn contact(
151        &self,
152        pos12: &Isometry<Real>,
153        shape1: &dyn Shape,
154        shape2: &dyn Shape,
155        prediction: Real,
156    ) -> Result<Option<Contact>, Unsupported> {
157        let ball1 = shape1.as_ball();
158        let ball2 = shape2.as_ball();
159
160        if let (Some(b1), Some(b2)) = (ball1, ball2) {
161            Ok(query::details::contact_ball_ball(pos12, b1, b2, prediction))
162        // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
163        //     Ok(query::details::contact_cuboid_cuboid(
164        //         pos12, c1, c2, prediction,
165        //     ))
166        } else if let (Some(p1), Some(s2)) =
167            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
168        {
169            Ok(query::details::contact_halfspace_support_map(
170                pos12, p1, s2, prediction,
171            ))
172        } else if let (Some(s1), Some(p2)) =
173            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
174        {
175            Ok(query::details::contact_support_map_halfspace(
176                pos12, s1, p2, prediction,
177            ))
178        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
179            Ok(query::details::contact_ball_convex_polyhedron(
180                pos12, b1, shape2, prediction,
181            ))
182        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
183            Ok(query::details::contact_convex_polyhedron_ball(
184                pos12, shape1, b2, prediction,
185            ))
186        } else {
187            #[cfg(feature = "alloc")]
188            if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
189                return Ok(query::details::contact_support_map_support_map(
190                    pos12, s1, s2, prediction,
191                ));
192            } else if let Some(c1) = shape1.as_composite_shape() {
193                return Ok(query::details::contact_composite_shape_shape(
194                    self, pos12, c1, shape2, prediction,
195                ));
196            } else if let Some(c2) = shape2.as_composite_shape() {
197                return Ok(query::details::contact_shape_composite_shape(
198                    self, pos12, shape1, c2, prediction,
199                ));
200            }
201
202            Err(Unsupported)
203        }
204    }
205
206    fn closest_points(
207        &self,
208        pos12: &Isometry<Real>,
209        shape1: &dyn Shape,
210        shape2: &dyn Shape,
211        max_dist: Real,
212    ) -> Result<ClosestPoints, Unsupported> {
213        let ball1 = shape1.as_ball();
214        let ball2 = shape2.as_ball();
215
216        if let (Some(b1), Some(b2)) = (ball1, ball2) {
217            Ok(query::details::closest_points_ball_ball(
218                pos12, b1, b2, max_dist,
219            ))
220        } else if let (Some(b1), true) = (ball1, shape2.is_convex()) {
221            Ok(query::details::closest_points_ball_convex_polyhedron(
222                pos12, b1, shape2, max_dist,
223            ))
224        } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) {
225            Ok(query::details::closest_points_convex_polyhedron_ball(
226                pos12, shape1, b2, max_dist,
227            ))
228        } else if let (Some(s1), Some(s2)) =
229            (shape1.as_shape::<Segment>(), shape2.as_shape::<Segment>())
230        {
231            Ok(query::details::closest_points_segment_segment(
232                pos12, s1, s2, max_dist,
233            ))
234        // } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) {
235        //     Ok(query::details::closest_points_cuboid_cuboid(
236        //         pos12, c1, c2, max_dist,
237        //     ))
238        } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) {
239            Ok(query::details::closest_points_segment_segment(
240                pos12, s1, s2, max_dist,
241            ))
242        // } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
243        //     Ok(query::details::closest_points_cuboid_triangle(
244        //         pos12, c1, t2, max_dist,
245        //     ))
246        } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
247            Ok(query::details::closest_points_triangle_cuboid(
248                pos12, t1, c2, max_dist,
249            ))
250        } else if let (Some(p1), Some(s2)) =
251            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
252        {
253            Ok(query::details::closest_points_halfspace_support_map(
254                pos12, p1, s2, max_dist,
255            ))
256        } else if let (Some(s1), Some(p2)) =
257            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
258        {
259            Ok(query::details::closest_points_support_map_halfspace(
260                pos12, s1, p2, max_dist,
261            ))
262        } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) {
263            Ok(query::details::closest_points_support_map_support_map(
264                pos12, s1, s2, max_dist,
265            ))
266        } else {
267            #[cfg(feature = "alloc")]
268            if let Some(c1) = shape1.as_composite_shape() {
269                return Ok(query::details::closest_points_composite_shape_shape(
270                    self, pos12, c1, shape2, max_dist,
271                ));
272            } else if let Some(c2) = shape2.as_composite_shape() {
273                return Ok(query::details::closest_points_shape_composite_shape(
274                    self, pos12, shape1, c2, max_dist,
275                ));
276            }
277
278            Err(Unsupported)
279        }
280    }
281
282    fn cast_shapes(
283        &self,
284        pos12: &Isometry<Real>,
285        local_vel12: &Vector<Real>,
286        shape1: &dyn Shape,
287        shape2: &dyn Shape,
288        options: ShapeCastOptions,
289    ) -> Result<Option<ShapeCastHit>, Unsupported> {
290        if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) {
291            Ok(query::details::cast_shapes_ball_ball(
292                pos12,
293                local_vel12,
294                b1,
295                b2,
296                options,
297            ))
298        } else if let (Some(p1), Some(s2)) =
299            (shape1.as_shape::<HalfSpace>(), shape2.as_support_map())
300        {
301            Ok(query::details::cast_shapes_halfspace_support_map(
302                pos12,
303                local_vel12,
304                p1,
305                s2,
306                options,
307            ))
308        } else if let (Some(s1), Some(p2)) =
309            (shape1.as_support_map(), shape2.as_shape::<HalfSpace>())
310        {
311            Ok(query::details::cast_shapes_support_map_halfspace(
312                pos12,
313                local_vel12,
314                s1,
315                p2,
316                options,
317            ))
318        } else {
319            #[cfg(feature = "alloc")]
320            if let Some(heightfield1) = shape1.as_heightfield() {
321                return query::details::cast_shapes_heightfield_shape(
322                    self,
323                    pos12,
324                    local_vel12,
325                    heightfield1,
326                    shape2,
327                    options,
328                );
329            } else if let Some(heightfield2) = shape2.as_heightfield() {
330                return query::details::cast_shapes_shape_heightfield(
331                    self,
332                    pos12,
333                    local_vel12,
334                    shape1,
335                    heightfield2,
336                    options,
337                );
338            } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map())
339            {
340                return Ok(query::details::cast_shapes_support_map_support_map(
341                    pos12,
342                    local_vel12,
343                    s1,
344                    s2,
345                    options,
346                ));
347            } else if let Some(c1) = shape1.as_composite_shape() {
348                return Ok(query::details::cast_shapes_composite_shape_shape(
349                    self,
350                    pos12,
351                    local_vel12,
352                    c1,
353                    shape2,
354                    options,
355                ));
356            } else if let Some(c2) = shape2.as_composite_shape() {
357                return Ok(query::details::cast_shapes_shape_composite_shape(
358                    self,
359                    pos12,
360                    local_vel12,
361                    shape1,
362                    c2,
363                    options,
364                ));
365            } else if let Some(v1) = shape1.as_voxels() {
366                return Ok(query::details::cast_shapes_voxels_shape(
367                    self,
368                    pos12,
369                    local_vel12,
370                    v1,
371                    shape2,
372                    options,
373                ));
374            } else if let Some(v2) = shape2.as_voxels() {
375                return Ok(query::details::cast_shapes_shape_voxels(
376                    self,
377                    pos12,
378                    local_vel12,
379                    shape1,
380                    v2,
381                    options,
382                ));
383            }
384
385            Err(Unsupported)
386        }
387    }
388
389    fn cast_shapes_nonlinear(
390        &self,
391        motion1: &NonlinearRigidMotion,
392        shape1: &dyn Shape,
393        motion2: &NonlinearRigidMotion,
394        shape2: &dyn Shape,
395        start_time: Real,
396        end_time: Real,
397        stop_at_penetration: bool,
398    ) -> Result<Option<ShapeCastHit>, Unsupported> {
399        if let (Some(sm1), Some(sm2)) = (shape1.as_support_map(), shape2.as_support_map()) {
400            let mode = if stop_at_penetration {
401                NonlinearShapeCastMode::StopAtPenetration
402            } else {
403                NonlinearShapeCastMode::directional_toi(shape1, shape2)
404            };
405
406            Ok(
407                query::details::cast_shapes_nonlinear_support_map_support_map(
408                    self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode,
409                ),
410            )
411        } else {
412            #[cfg(feature = "alloc")]
413            if let Some(c1) = shape1.as_composite_shape() {
414                return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape(
415                    self,
416                    motion1,
417                    c1,
418                    motion2,
419                    shape2,
420                    start_time,
421                    end_time,
422                    stop_at_penetration,
423                ));
424            } else if let Some(c2) = shape2.as_composite_shape() {
425                return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape(
426                    self,
427                    motion1,
428                    shape1,
429                    motion2,
430                    c2,
431                    start_time,
432                    end_time,
433                    stop_at_penetration,
434                ));
435            } else if let Some(c1) = shape1.as_voxels() {
436                return Ok(query::details::cast_shapes_nonlinear_voxels_shape(
437                    self,
438                    motion1,
439                    c1,
440                    motion2,
441                    shape2,
442                    start_time,
443                    end_time,
444                    stop_at_penetration,
445                ));
446            } else if let Some(c2) = shape2.as_voxels() {
447                return Ok(query::details::cast_shapes_nonlinear_shape_voxels(
448                    self,
449                    motion1,
450                    shape1,
451                    motion2,
452                    c2,
453                    start_time,
454                    end_time,
455                    stop_at_penetration,
456                ));
457            }
458            /* } else if let (Some(p1), Some(s2)) = (shape1.as_shape::<HalfSpace>(), shape2.as_support_map()) {
459            //        query::details::cast_shapes_nonlinear_halfspace_support_map(m1, vel1, p1, m2, vel2, s2)
460                    unimplemented!()
461                } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::<HalfSpace>()) {
462            //        query::details::cast_shapes_nonlinear_support_map_halfspace(m1, vel1, s1, m2, vel2, p2)
463                    unimplemented!() */
464
465            Err(Unsupported)
466        }
467    }
468}
469
470#[cfg(feature = "alloc")]
471impl<ManifoldData, ContactData> PersistentQueryDispatcher<ManifoldData, ContactData>
472    for DefaultQueryDispatcher
473where
474    ManifoldData: Default + Clone,
475    ContactData: Default + Copy,
476{
477    fn contact_manifolds(
478        &self,
479        pos12: &Isometry<Real>,
480        shape1: &dyn Shape,
481        shape2: &dyn Shape,
482        prediction: Real,
483        manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
484        workspace: &mut Option<ContactManifoldsWorkspace>,
485    ) -> Result<(), Unsupported> {
486        use crate::query::contact_manifolds::*;
487
488        let composite1 = shape1.as_composite_shape();
489        let composite2 = shape2.as_composite_shape();
490
491        if let (Some(composite1), Some(composite2)) = (composite1, composite2) {
492            contact_manifolds_composite_shape_composite_shape(
493                self, pos12, composite1, composite2, prediction, manifolds, workspace,
494            );
495
496            return Ok(());
497        }
498
499        match (shape1.shape_type(), shape2.shape_type()) {
500            (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => {
501                contact_manifolds_trimesh_shape_shapes(
502                    self, pos12, shape1, shape2, prediction, manifolds, workspace,
503                );
504            }
505            (ShapeType::HeightField, _) => {
506                if let Some(composite2) = composite2 {
507                    contact_manifolds_heightfield_composite_shape(
508                        self,
509                        pos12,
510                        &pos12.inverse(),
511                        shape1.as_heightfield().unwrap(),
512                        composite2,
513                        prediction,
514                        manifolds,
515                        workspace,
516                        false,
517                    )
518                } else {
519                    contact_manifolds_heightfield_shape_shapes(
520                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
521                    );
522                }
523            }
524            (_, ShapeType::HeightField) => {
525                if let Some(composite1) = composite1 {
526                    contact_manifolds_heightfield_composite_shape(
527                        self,
528                        &pos12.inverse(),
529                        pos12,
530                        shape2.as_heightfield().unwrap(),
531                        composite1,
532                        prediction,
533                        manifolds,
534                        workspace,
535                        true,
536                    )
537                } else {
538                    contact_manifolds_heightfield_shape_shapes(
539                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
540                    );
541                }
542            }
543            (ShapeType::Voxels, ShapeType::Voxels) => contact_manifolds_voxels_voxels_shapes(
544                self, pos12, shape1, shape2, prediction, manifolds, workspace,
545            ),
546            (ShapeType::Voxels, ShapeType::Ball) | (ShapeType::Ball, ShapeType::Voxels) => {
547                contact_manifolds_voxels_ball_shapes(pos12, shape1, shape2, prediction, manifolds)
548            }
549            (ShapeType::Voxels, _) | (_, ShapeType::Voxels) => {
550                if composite1.is_some() || composite2.is_some() {
551                    contact_manifolds_voxels_composite_shape_shapes(
552                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
553                    )
554                } else {
555                    contact_manifolds_voxels_shape_shapes(
556                        self, pos12, shape1, shape2, prediction, manifolds, workspace,
557                    )
558                }
559            }
560            _ => {
561                if let Some(composite1) = composite1 {
562                    contact_manifolds_composite_shape_shape(
563                        self, pos12, composite1, shape2, prediction, manifolds, workspace, false,
564                    );
565                } else if let Some(composite2) = composite2 {
566                    contact_manifolds_composite_shape_shape(
567                        self,
568                        &pos12.inverse(),
569                        composite2,
570                        shape1,
571                        prediction,
572                        manifolds,
573                        workspace,
574                        true,
575                    );
576                } else {
577                    if manifolds.is_empty() {
578                        manifolds.push(ContactManifold::new());
579                    }
580
581                    return self.contact_manifold_convex_convex(
582                        pos12,
583                        shape1,
584                        shape2,
585                        None,
586                        None,
587                        prediction,
588                        &mut manifolds[0],
589                    );
590                }
591            }
592        }
593
594        Ok(())
595    }
596
597    fn contact_manifold_convex_convex(
598        &self,
599        pos12: &Isometry<Real>,
600        shape1: &dyn Shape,
601        shape2: &dyn Shape,
602        normal_constraints1: Option<&dyn NormalConstraints>,
603        normal_constraints2: Option<&dyn NormalConstraints>,
604        prediction: Real,
605        manifold: &mut ContactManifold<ManifoldData, ContactData>,
606    ) -> Result<(), Unsupported> {
607        use crate::query::contact_manifolds::*;
608
609        match (shape1.shape_type(), shape2.shape_type()) {
610            (ShapeType::Ball, ShapeType::Ball) => {
611                contact_manifold_ball_ball_shapes(pos12, shape1, shape2, prediction, manifold)
612            }
613            (ShapeType::Cuboid, ShapeType::Cuboid) =>
614                contact_manifold_cuboid_cuboid_shapes(pos12, shape1, shape2, prediction, manifold)
615            ,
616            // (ShapeType::Polygon, ShapeType::Polygon) => (
617            //     PrimitiveContactGenerator {
618            //         generate_contacts: super::generate_contacts_polygon_polygon,
619            //         ..PrimitiveContactGenerator::default()
620            //     },
621            //     None,
622            // ),
623            (ShapeType::Capsule, ShapeType::Capsule) => {
624                contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold)
625            }
626            (_, ShapeType::Ball) | (ShapeType::Ball, _) => {
627                contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold)
628            }
629            // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) =>
630            //     contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold),
631            (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
632                contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2,  prediction, manifold)
633            }
634            (ShapeType::HalfSpace, _) => {
635                if let Some((pfm2, border_radius2)) = shape2.as_polygonal_feature_map() {
636                    contact_manifold_halfspace_pfm(
637                        pos12,
638                        shape1.as_halfspace().unwrap(),
639                        pfm2,
640                        border_radius2,
641                        prediction,
642                        manifold,
643                        false
644                    )
645                } else {
646                    return Err(Unsupported)
647                }
648            }
649            (_, ShapeType::HalfSpace) => {
650                if let Some((pfm1, border_radius1)) = shape1.as_polygonal_feature_map() {
651                    contact_manifold_halfspace_pfm(
652                        &pos12.inverse(),
653                        shape2.as_halfspace().unwrap(),
654                        pfm1,
655                        border_radius1,
656                        prediction,
657                        manifold,
658                        true
659                    )
660                } else {
661                    return Err(Unsupported)
662                }
663            }
664            _ => {
665                if let (Some(pfm1), Some(pfm2)) = (
666                    shape1.as_polygonal_feature_map(),
667                    shape2.as_polygonal_feature_map(),
668                ) {
669                    contact_manifold_pfm_pfm(
670                        pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold,
671                    )
672                } else {
673                    return Err(Unsupported);
674                }
675            }
676        }
677
678        Ok(())
679    }
680}
parry3d/query/default_query_dispatcher.rs

parry3d/query/
default_query_dispatcher.rs
