parry2d/query/closest_points/
closest_points_composite_shape_shape.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Pose, Real};
3use crate::partitioning::BvhNode;
4use crate::query::{ClosestPoints, QueryDispatcher};
5use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape};
6use crate::utils::PoseOpt;
7
8impl<S: ?Sized + TypedCompositeShape> CompositeShapeRef<'_, S> {
9    /// Returns the closest points between `self` and the given `shape2` positioned at
10    /// `pose12` relative to `self`.
11    ///
12    /// Returns the index of the sub-shape of `self` involved in the contact as well as the closest
13    /// points information.
14    ///
15    /// Returns `ClosestPoints::Disjoint` if `self` and `shape2` are separated by a distance larger
16    /// than `margin`.
17    ///
18    /// Returns `None` if no closest point could be calculated (e.g. if the `dispatcher` doesn’t
19    /// support the involved shapes at all, or if `self` is empty).
20    pub fn closest_points_to_shape<D: ?Sized + QueryDispatcher>(
21        &self,
22        dispatcher: &D,
23        pose12: &Pose,
24        shape2: &dyn Shape,
25        margin: Real,
26    ) -> Option<(u32, ClosestPoints)> {
27        let ls_aabb2 = shape2.compute_aabb(pose12);
28        let msum_shift = -ls_aabb2.center();
29        let msum_margin = ls_aabb2.half_extents();
30
31        self.0
32            .bvh()
33            .find_best(
34                margin,
35                |node: &BvhNode, _| {
36                    // Compute the minkowski sum of the two Aabbs.
37                    let msum = Aabb {
38                        mins: node.mins() + msum_shift - msum_margin,
39                        maxs: node.maxs() + msum_shift + msum_margin,
40                    };
41                    msum.distance_to_origin()
42                },
43                |part_id, _| {
44                    self.0
45                        .map_untyped_part_at(part_id, |part_pos1, part_g1, _| {
46                            if let Ok(mut pts) = dispatcher.closest_points(
47                                &part_pos1.inv_mul(pose12),
48                                part_g1,
49                                shape2,
50                                margin,
51                            ) {
52                                let cost = match &mut pts {
53                                    ClosestPoints::WithinMargin(p1, p2) => {
54                                        *p1 = part_pos1.transform_point(*p1);
55                                        let p2_1 = pose12 * *p2;
56                                        (*p1 - p2_1).length()
57                                    }
58                                    ClosestPoints::Intersecting => -Real::MAX,
59                                    ClosestPoints::Disjoint => Real::MAX,
60                                };
61                                (cost, pts)
62                            } else {
63                                (Real::MAX, ClosestPoints::Disjoint)
64                            }
65                        })
66                },
67            )
68            .map(|(part_id, (_, pts))| (part_id, pts))
69    }
70}
71
72/// Closest points between a composite shape and any other shape.
73pub fn closest_points_composite_shape_shape<D, G1>(
74    dispatcher: &D,
75    pos12: &Pose,
76    g1: &G1,
77    g2: &dyn Shape,
78    margin: Real,
79) -> ClosestPoints
80where
81    D: ?Sized + QueryDispatcher,
82    G1: ?Sized + TypedCompositeShape,
83{
84    CompositeShapeRef(g1)
85        .closest_points_to_shape(dispatcher, pos12, g2, margin)
86        .map(|cp| cp.1)
87        .unwrap_or(ClosestPoints::Disjoint)
88}
89
90/// Closest points between a shape and a composite shape.
91pub fn closest_points_shape_composite_shape<D, G2>(
92    dispatcher: &D,
93    pos12: &Pose,
94    g1: &dyn Shape,
95    g2: &G2,
96    margin: Real,
97) -> ClosestPoints
98where
99    D: ?Sized + QueryDispatcher,
100    G2: ?Sized + TypedCompositeShape,
101{
102    closest_points_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1, margin).flipped()
103}
parry2d/query/closest_points/closest_points_composite_shape_shape.rs

parry2d/query/closest_points/
closest_points_composite_shape_shape.rs
