ncollide3d/query/closest_points/
closest_points_composite_shape_shape.rs

Help
1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point, Vector};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{self, ClosestPoints, PointQuery};
5use crate::shape::{CompositeShape, Shape};
6use na::{self, RealField};
7
8/// Closest points between a composite shape and any other shape.
9pub fn closest_points_composite_shape_shape<N, G1: ?Sized>(
10    m1: &Isometry<N>,
11    g1: &G1,
12    m2: &Isometry<N>,
13    g2: &dyn Shape<N>,
14    margin: N,
15) -> ClosestPoints<N>
16where
17    N: RealField + Copy,
18    G1: CompositeShape<N>,
19{
20    let mut visitor = CompositeShapeAgainstShapeClosestPointsVisitor::new(m1, g1, m2, g2, margin);
21
22    g1.bvh()
23        .best_first_search(&mut visitor)
24        .expect("The composite shape must not be empty.")
25        .1
26}
27
28/// Closest points between a shape and a composite shape.
29pub fn closest_points_shape_composite_shape<N, G2: ?Sized>(
30    m1: &Isometry<N>,
31    g1: &dyn Shape<N>,
32    m2: &Isometry<N>,
33    g2: &G2,
34    margin: N,
35) -> ClosestPoints<N>
36where
37    N: RealField + Copy,
38    G2: CompositeShape<N>,
39{
40    let mut res = closest_points_composite_shape_shape(m2, g2, m1, g1, margin);
41    res.flip();
42    res
43}
44
45struct CompositeShapeAgainstShapeClosestPointsVisitor<'a, N: 'a + RealField + Copy, G1: ?Sized + 'a>
46{
47    msum_shift: Vector<N>,
48    msum_margin: Vector<N>,
49    margin: N,
50
51    m1: &'a Isometry<N>,
52    g1: &'a G1,
53    m2: &'a Isometry<N>,
54    g2: &'a dyn Shape<N>,
55}
56
57impl<'a, N, G1: ?Sized> CompositeShapeAgainstShapeClosestPointsVisitor<'a, N, G1>
58where
59    N: RealField + Copy,
60    G1: CompositeShape<N>,
61{
62    pub fn new(
63        m1: &'a Isometry<N>,
64        g1: &'a G1,
65        m2: &'a Isometry<N>,
66        g2: &'a dyn Shape<N>,
67        margin: N,
68    ) -> CompositeShapeAgainstShapeClosestPointsVisitor<'a, N, G1> {
69        let ls_m2 = m1.inverse() * m2.clone();
70        let ls_aabb2 = g2.aabb(&ls_m2);
71
72        CompositeShapeAgainstShapeClosestPointsVisitor {
73            msum_shift: -ls_aabb2.center().coords,
74            msum_margin: ls_aabb2.half_extents(),
75            margin: margin,
76            m1: m1,
77            g1: g1,
78            m2: m2,
79            g2: g2,
80        }
81    }
82}
83
84impl<'a, N, G1: ?Sized> BestFirstVisitor<N, usize, AABB<N>>
85    for CompositeShapeAgainstShapeClosestPointsVisitor<'a, N, G1>
86where
87    N: RealField + Copy,
88    G1: CompositeShape<N>,
89{
90    type Result = ClosestPoints<N>;
91
92    fn visit(
93        &mut self,
94        best: N,
95        bv: &AABB<N>,
96        data: Option<&usize>,
97    ) -> BestFirstVisitStatus<N, Self::Result> {
98        // Compute the minkowski sum of the two AABBs.
99        let msum = AABB::new(
100            bv.mins + self.msum_shift + (-self.msum_margin),
101            bv.maxs + self.msum_shift + self.msum_margin,
102        );
103
104        let dist = msum.distance_to_point(&Isometry::identity(), &Point::origin(), true);
105
106        let mut res = BestFirstVisitStatus::Continue {
107            cost: dist,
108            result: None,
109        };
110
111        if let Some(b) = data {
112            if dist < best {
113                self.g1.map_part_at(*b, self.m1, &mut |m1, g1| {
114                    let pts = query::closest_points(m1, g1, self.m2, self.g2, self.margin);
115                    match pts {
116                        ClosestPoints::WithinMargin(ref p1, ref p2) => {
117                            res = BestFirstVisitStatus::Continue {
118                                cost: na::distance(p1, p2),
119                                result: Some(pts),
120                            }
121                        }
122                        ClosestPoints::Intersecting => {
123                            res = BestFirstVisitStatus::ExitEarly(Some(pts))
124                        }
125                        ClosestPoints::Disjoint => {}
126                    };
127                });
128            }
129        }
130
131        res
132    }
133}
ncollide3d/query/closest_points/closest_points_composite_shape_shape.rs

ncollide3d/query/closest_points/
closest_points_composite_shape_shape.rs
