parry2d/query/distance/
distance_composite_shape_shape.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Pose, Real};
3use crate::partitioning::BvhNode;
4use crate::query::{QueryDispatcher, ShapeDistance};
5use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape};
6use crate::utils::PoseOpt;
7
8impl<S: ?Sized + TypedCompositeShape> CompositeShapeRef<'_, S> {
9    /// Calculates the closest distance between `self` and the given `shape2` positioned at
10    /// `pose12` relative to `self`.
11    ///
12    /// Returns the index of the sub-shape of `self` closest to `shape2` alongside the distance,
13    /// which is left as that sub-shape reported it (its `subshape1` is the sub-shape's own when
14    /// it is a composite too, and `subshape2` is `shape2`'s).
15    pub fn distance_to_shape<D: ?Sized + QueryDispatcher>(
16        &self,
17        dispatcher: &D,
18        pose12: &Pose,
19        shape2: &dyn Shape,
20    ) -> Option<(SubShapeId, ShapeDistance)> {
21        let ls_aabb2 = shape2.compute_aabb(pose12);
22        let msum_shift = -ls_aabb2.center();
23        let msum_margin = ls_aabb2.half_extents();
24
25        self.0.bvh().find_best(
26            Real::MAX,
27            |node: &BvhNode, _| {
28                // Compute the minkowski sum of the two Aabbs.
29                let msum = Aabb {
30                    mins: node.mins() + msum_shift - msum_margin,
31                    maxs: node.maxs() + msum_shift + msum_margin,
32                };
33                msum.distance_to_origin()
34            },
35            |part_id, _| {
36                self.0
37                    .map_untyped_part_at(part_id, |part_pos1, part_g1, _| {
38                        dispatcher.distance(&part_pos1.inv_mul(pose12), part_g1, shape2)
39                    })?
40                    .ok()
41            },
42        )
43    }
44}
45
46/// Smallest distance between a composite shape and any other shape.
47pub fn distance_composite_shape_shape<D, G1>(
48    dispatcher: &D,
49    pos12: &Pose,
50    g1: &G1,
51    g2: &dyn Shape,
52) -> ShapeDistance
53where
54    D: ?Sized + QueryDispatcher,
55    G1: ?Sized + TypedCompositeShape,
56{
57    // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it
58    // knows which of its parts answered.
59    CompositeShapeRef(g1)
60        .distance_to_shape(dispatcher, pos12, g2)
61        .map(|(part_id, mut result)| {
62            result.subshape1 = part_id;
63            result
64        })
65        .unwrap_or(ShapeDistance::new(Real::MAX))
66}
67
68/// Smallest distance between a shape and a composite shape.
69pub fn distance_shape_composite_shape<D, G2>(
70    dispatcher: &D,
71    pos12: &Pose,
72    g1: &dyn Shape,
73    g2: &G2,
74) -> ShapeDistance
75where
76    D: ?Sized + QueryDispatcher,
77    G2: ?Sized + TypedCompositeShape,
78{
79    distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1).swapped()
80}
parry2d/query/distance/distance_composite_shape_shape.rs

parry2d/query/distance/
distance_composite_shape_shape.rs
