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