parry2d/query/contact/
contact_composite_shape_shape.rs

1use crate::bounding_volume::BoundingVolume;
2use crate::math::{Pose, Real};
3use crate::query::{Contact, QueryDispatcher};
4use crate::shape::{CompositeShape, CompositeShapeRef, Shape, SubShapeId};
5use crate::utils::PoseOpt;
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` the contact is on
13    /// alongside the contact, which is left as that sub-shape reported it (its `subshape1` is the
14    /// sub-shape's own when it is a composite too, and `subshape2` is `shape2`'s).
15    pub fn contact_with_shape<D: ?Sized + QueryDispatcher>(
16        &self,
17        dispatcher: &D,
18        pose12: &Pose,
19        shape2: &dyn Shape,
20        prediction: Real,
21    ) -> Option<(SubShapeId, Contact)> {
22        let ls_aabb2 = shape2.compute_aabb(pose12).loosened(prediction);
23        let mut result = None::<(SubShapeId, Contact)>;
24
25        for part_id in self.0.bvh().intersect_aabb(&ls_aabb2) {
26            self.0.map_part_at(part_id, &mut |part_pos1, part1, _| {
27                if let Ok(Some(mut c)) =
28                    dispatcher.contact(&part_pos1.inv_mul(pose12), part1, shape2, prediction)
29                {
30                    let replace = result.is_none_or(|(_, cbest)| c.dist < cbest.dist);
31
32                    if replace {
33                        if let Some(part_pos1) = part_pos1 {
34                            c.transform1_by_mut(part_pos1);
35                        }
36                        result = Some((part_id, c))
37                    }
38                }
39            });
40        }
41
42        result
43    }
44}
45
46/// Best contact between a composite shape (`Mesh`, `Compound`) and any other shape.
47pub fn contact_composite_shape_shape<D, G1>(
48    dispatcher: &D,
49    pose12: &Pose,
50    g1: &G1,
51    g2: &dyn Shape,
52    prediction: Real,
53) -> Option<Contact>
54where
55    D: ?Sized + QueryDispatcher,
56    G1: ?Sized + CompositeShape,
57{
58    // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it
59    // knows which of its parts answered.
60    CompositeShapeRef(g1)
61        .contact_with_shape(dispatcher, pose12, g2, prediction)
62        .map(|(part_id, mut c)| {
63            c.subshape1 = part_id;
64            c
65        })
66}
67
68/// Best contact between a shape and a composite (`Mesh`, `Compound`) shape.
69pub fn contact_shape_composite_shape<D, G2>(
70    dispatcher: &D,
71    pose12: &Pose,
72    g1: &dyn Shape,
73    g2: &G2,
74    prediction: Real,
75) -> Option<Contact>
76where
77    D: ?Sized + QueryDispatcher,
78    G2: ?Sized + CompositeShape,
79{
80    contact_composite_shape_shape(dispatcher, &pose12.inverse(), g2, g1, prediction)
81        .map(|c| c.flipped())
82}
parry2d/query/contact/contact_composite_shape_shape.rs

parry2d/query/contact/
contact_composite_shape_shape.rs
