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
123
124
125
126
127
128
129
130
131
132
133
134
use crate::math::{Isometry, Real};
use crate::query::{sat, Contact, PointQuery};
use crate::shape::{Cuboid, SupportMap};
use approx::AbsDiffEq;
use na::Unit;
#[inline]
pub fn contact_cuboid_cuboid(
pos12: &Isometry<Real>,
cuboid1: &Cuboid,
cuboid2: &Cuboid,
prediction: Real,
) -> Option<Contact> {
let pos21 = pos12.inverse();
let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, &pos12);
if sep1.0 > prediction {
return None;
}
let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, &pos21);
if sep2.0 > prediction {
return None;
}
#[cfg(feature = "dim2")]
let sep3 = (-Real::MAX, crate::math::Vector::<Real>::y());
#[cfg(feature = "dim3")]
let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, &pos12);
if sep3.0 > prediction {
return None;
}
if sep1.0 >= sep2.0 && sep1.0 >= sep3.0 {
let pt2_1 = cuboid2.support_point(pos12, &-sep1.1);
let proj1 = cuboid1.project_local_point(&pt2_1, false);
let separation = (pt2_1 - proj1.point).dot(&sep1.1);
let normalized_dir = Unit::try_new_and_get(pt2_1 - proj1.point, Real::default_epsilon());
let normal1;
let dist;
if separation < 0.0 || normalized_dir.is_none() {
normal1 = Unit::new_unchecked(sep1.1);
dist = separation;
} else {
let (dir, norm) = normalized_dir.unwrap();
normal1 = dir;
dist = norm;
}
if dist > prediction {
return None;
}
return Some(Contact::new(
proj1.point,
pos12.inverse_transform_point(&pt2_1),
normal1,
pos12.inverse_transform_unit_vector(&-normal1),
dist,
));
}
if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1);
let proj2 = cuboid2.project_local_point(&pt1_2, false);
let separation = (pt1_2 - proj2.point).dot(&sep2.1);
let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon());
let normal2;
let dist;
if separation < 0.0 || normalized_dir.is_none() {
normal2 = Unit::new_unchecked(sep2.1);
dist = separation;
} else {
let (dir, norm) = normalized_dir.unwrap();
normal2 = dir;
dist = norm;
}
if dist > prediction {
return None;
}
return Some(Contact::new(
pos12.transform_point(&pt1_2),
proj2.point,
pos12 * -normal2,
normal2,
dist,
));
}
#[cfg(feature = "dim3")]
if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
use crate::query::{details, ClosestPoints};
let edge1 = cuboid1.local_support_edge_segment(sep3.1);
let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1);
match details::closest_points_segment_segment(pos12, &edge1, &edge2, prediction) {
ClosestPoints::Disjoint => return None,
ClosestPoints::WithinMargin(a, b) => {
let normal1 = Unit::new_unchecked(sep3.1);
let normal2 = pos12.inverse_transform_unit_vector(&-normal1);
return Some(Contact::new(a, b, normal1, normal2, sep3.0));
}
ClosestPoints::Intersecting => unreachable!(),
}
}
unreachable!()
}