parry3d/query/closest_points/
closest_points_composite_shape_shape.rs

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

parry3d/query/closest_points/
closest_points_composite_shape_shape.rs
