parry3d/query/contact/
contact_composite_shape_shape.rs

1use crate::bounding_volume::BoundingVolume;
2use crate::math::{Isometry, Real};
3use crate::query::{Contact, QueryDispatcher};
4use crate::shape::{CompositeShape, CompositeShapeRef, Shape};
5use crate::utils::IsometryOpt;
6
7impl<S: ?Sized + CompositeShape> CompositeShapeRef<'_, S> {
8    /// Returns the closest/deepest contact between `self` and the given `shape2` positioned at
9    /// `pose12` relative to `self`.
10    ///
11    /// Returns `None` if `self` and `shape2` are separated by a distance larger than
12    /// `prediction`. Otherwise, returns the index of the sub-shape of `self` involved in the contact
13    /// as well as the contact information.
14    pub fn contact_with_shape<D: ?Sized + QueryDispatcher>(
15        &self,
16        dispatcher: &D,
17        pose12: &Isometry<Real>,
18        shape2: &dyn Shape,
19        prediction: Real,
20    ) -> Option<(u32, Contact)> {
21        let ls_aabb2 = shape2.compute_aabb(pose12).loosened(prediction);
22        let mut result = None::<(u32, Contact)>;
23
24        for part_id in self.0.bvh().intersect_aabb(&ls_aabb2) {
25            self.0.map_part_at(part_id, &mut |part_pos1, part1, _| {
26                if let Ok(Some(mut c)) =
27                    dispatcher.contact(&part_pos1.inv_mul(pose12), part1, shape2, prediction)
28                {
29                    let replace = result.is_none_or(|(_, cbest)| c.dist < cbest.dist);
30
31                    if replace {
32                        if let Some(part_pos1) = part_pos1 {
33                            c.transform1_by_mut(part_pos1);
34                        }
35                        result = Some((part_id, c))
36                    }
37                }
38            });
39        }
40
41        result
42    }
43}
44
45/// Best contact between a composite shape (`Mesh`, `Compound`) and any other shape.
46pub fn contact_composite_shape_shape<D, G1>(
47    dispatcher: &D,
48    pose12: &Isometry<Real>,
49    g1: &G1,
50    g2: &dyn Shape,
51    prediction: Real,
52) -> Option<Contact>
53where
54    D: ?Sized + QueryDispatcher,
55    G1: ?Sized + CompositeShape,
56{
57    CompositeShapeRef(g1)
58        .contact_with_shape(dispatcher, &pose12.inverse(), g2, prediction)
59        .map(|c| c.1)
60}
61
62/// Best contact between a shape and a composite (`Mesh`, `Compound`) shape.
63pub fn contact_shape_composite_shape<D, G2>(
64    dispatcher: &D,
65    pose12: &Isometry<Real>,
66    g1: &dyn Shape,
67    g2: &G2,
68    prediction: Real,
69) -> Option<Contact>
70where
71    D: ?Sized + QueryDispatcher,
72    G2: ?Sized + CompositeShape,
73{
74    contact_composite_shape_shape(dispatcher, &pose12.inverse(), g2, g1, prediction)
75        .map(|c| c.flipped())
76}
parry3d/query/contact/contact_composite_shape_shape.rs

parry3d/query/contact/
contact_composite_shape_shape.rs
