ncollide3d/query/closest_points/
closest_points_composite_shape_shape.rs
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
8pub 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
28pub 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 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}