parry3d/query/distance/
distance_composite_shape_shape.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Isometry, Real};
3use crate::partitioning::BvhNode;
4use crate::query::QueryDispatcher;
5use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape};
6use crate::utils::IsometryOpt;
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 distance and the index of the sub-shape of `self` that is closest to `shape2`.
13    pub fn distance_to_shape<D: ?Sized + QueryDispatcher>(
14        &self,
15        dispatcher: &D,
16        pose12: &Isometry<Real>,
17        shape2: &dyn Shape,
18    ) -> Option<(u32, Real)> {
19        let ls_aabb2 = shape2.compute_aabb(pose12);
20        let msum_shift = -ls_aabb2.center().coords;
21        let msum_margin = ls_aabb2.half_extents();
22
23        self.0.bvh().find_best(
24            Real::MAX,
25            |node: &BvhNode, _| {
26                // Compute the minkowski sum of the two Aabbs.
27                let msum = Aabb {
28                    mins: node.mins() + msum_shift - msum_margin,
29                    maxs: node.maxs() + msum_shift + msum_margin,
30                };
31                msum.distance_to_origin()
32            },
33            |part_id, _| {
34                self.0
35                    .map_untyped_part_at(part_id, |part_pos1, part_g1, _| {
36                        dispatcher.distance(&part_pos1.inv_mul(pose12), part_g1, shape2)
37                    })?
38                    .ok()
39            },
40        )
41    }
42}
43
44/// Smallest distance between a composite shape and any other shape.
45pub fn distance_composite_shape_shape<D, G1>(
46    dispatcher: &D,
47    pos12: &Isometry<Real>,
48    g1: &G1,
49    g2: &dyn Shape,
50) -> Real
51where
52    D: ?Sized + QueryDispatcher,
53    G1: ?Sized + TypedCompositeShape,
54{
55    CompositeShapeRef(g1)
56        .distance_to_shape(dispatcher, pos12, g2)
57        .unwrap_or((u32::MAX, Real::MAX))
58        .1
59}
60
61/// Smallest distance between a shape and a composite shape.
62pub fn distance_shape_composite_shape<D, G2>(
63    dispatcher: &D,
64    pos12: &Isometry<Real>,
65    g1: &dyn Shape,
66    g2: &G2,
67) -> Real
68where
69    D: ?Sized + QueryDispatcher,
70    G2: ?Sized + TypedCompositeShape,
71{
72    distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1)
73}
parry3d/query/distance/distance_composite_shape_shape.rs

parry3d/query/distance/
distance_composite_shape_shape.rs
