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
use crate::math::{Isometry, Real};
use crate::query::{
self,
gjk::{GJKResult, VoronoiSimplex},
ContactManifold, TrackedContact,
};
use crate::shape::{PolygonalFeature, PolygonalFeatureMap, Shape};
use na::Unit;
pub fn contact_manifold_pfm_pfm_shapes<ManifoldData, ContactData>(
pos12: &Isometry<Real>,
shape1: &dyn Shape,
shape2: &dyn Shape,
prediction: Real,
manifold: &mut ContactManifold<ManifoldData, ContactData>,
) where
ManifoldData: Default,
ContactData: Default + Copy,
{
if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
shape1.as_polygonal_feature_map(),
shape2.as_polygonal_feature_map(),
) {
contact_manifold_pfm_pfm(
pos12,
pfm1,
border_radius1,
pfm2,
border_radius2,
prediction,
manifold,
);
}
}
pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>(
pos12: &Isometry<Real>,
pfm1: &'a S1,
border_radius1: Real,
pfm2: &'a S2,
border_radius2: Real,
prediction: Real,
manifold: &mut ContactManifold<ManifoldData, ContactData>,
) where
S1: ?Sized + PolygonalFeatureMap,
S2: ?Sized + PolygonalFeatureMap,
ManifoldData: Default,
ContactData: Default + Copy,
{
if manifold.try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6) {
return;
}
let init_dir = Unit::try_new(manifold.local_n1, crate::math::DEFAULT_EPSILON);
let total_prediction = prediction + border_radius1 + border_radius2;
let contact = query::details::contact_support_map_support_map_with_params(
&pos12,
pfm1,
pfm2,
total_prediction,
&mut VoronoiSimplex::new(),
init_dir,
);
let old_manifold_points = manifold.points.clone();
manifold.clear();
match contact {
GJKResult::ClosestPoints(p1, p2_1, dir) => {
let local_n1 = dir;
let local_n2 = pos12.inverse_transform_unit_vector(&-dir);
let mut feature1 = PolygonalFeature::default();
let mut feature2 = PolygonalFeature::default();
pfm1.local_support_feature(&local_n1, &mut feature1);
pfm2.local_support_feature(&local_n2, &mut feature2);
PolygonalFeature::contacts(
pos12,
&pos12.inverse(),
&local_n1,
&local_n2,
&feature1,
&feature2,
total_prediction,
manifold,
false,
);
if cfg!(feature = "dim3") || (cfg!(feature = "dim2") && manifold.points.is_empty()) {
let contact = TrackedContact::new(
p1,
pos12.inverse_transform_point(&p2_1),
u32::MAX,
u32::MAX,
(p2_1 - p1).dot(&dir),
);
manifold.points.push(contact);
}
if border_radius1 != 0.0 || border_radius2 != 0.0 {
for contact in &mut manifold.points {
contact.local_p1 += *local_n1 * border_radius1;
contact.local_p2 += *local_n2 * border_radius2;
contact.dist -= border_radius1 + border_radius2;
}
}
manifold.local_n1 = *local_n1;
manifold.local_n2 = *local_n2;
}
GJKResult::NoIntersection(dir) => {
manifold.local_n1 = *dir;
}
_ => {
manifold.local_n1.fill(0.0);
}
}
manifold.match_contacts(&old_manifold_points);
}