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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
use crate::math::Point;
use crate::query::ContactPreprocessor;
use crate::query::{Contact, ContactKinematic, TrackedContact};
use crate::shape::FeatureId;
use na::{self, RealField};
use slab::Slab;
use std::collections::{hash_map::Entry, HashMap};
/// The technique used for contact tracking.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum ContactTrackingMode<N: RealField + Copy> {
/// Contact tracking using features.
/// Two contacts are considered the same if they are on the same features.
FeatureBased,
/// Contact tracking using distances.
/// Two contacts are considered the same if they are closer than the given threshold.
DistanceBased(N),
}
#[derive(Clone, Debug)]
enum ContactCache<N: RealField + Copy> {
FeatureBased(HashMap<(FeatureId, FeatureId), usize>),
DistanceBased(Vec<(Point<N>, usize)>, N),
}
/// A contact manifold.
///
/// A contact manifold is a set of contacts between two shapes.
/// If the shapes are convex, then the convex hull of those contacts are often interpreted as surface.
/// This structure is responsible for matching new contacts with old ones in order to perform an
/// approximate tracking of the contact points.
#[derive(Clone, Debug)]
pub struct ContactManifold<N: RealField + Copy> {
ncontacts: usize,
persistence: usize,
deepest: usize,
contacts: Slab<(TrackedContact<N>, usize)>,
cache: ContactCache<N>,
}
impl<N: RealField + Copy> ContactManifold<N> {
/// Initializes a contact manifold without any contact.
///
/// The default contact tracking mode is set to `ContactTrackingMode::DistanceBased(0.02)`.
pub fn new() -> Self {
ContactManifold {
ncontacts: 0,
deepest: 0,
persistence: 1,
contacts: Slab::new(),
cache: ContactCache::DistanceBased(Vec::new(), na::convert(0.02)),
}
}
/// The number of contacts contained by this manifold.
pub fn len(&self) -> usize {
self.ncontacts
}
/// All the contact tracked by this manifold.
pub fn contacts(&self) -> impl Iterator<Item = &TrackedContact<N>> {
let persistence = self.persistence;
self.contacts
.iter()
.filter_map(move |(_, c)| if c.1 == persistence { Some(&c.0) } else { None })
}
/// Mutable reference to all the contact tracked by this manifold.
pub fn contacts_mut(&mut self) -> impl Iterator<Item = &mut TrackedContact<N>> {
let persistence = self.persistence;
self.contacts.iter_mut().filter_map(move |(_, c)| {
if c.1 == persistence {
Some(&mut c.0)
} else {
None
}
})
}
/// The contact of this manifold with the deepest penetration depth.
pub fn deepest_contact(&self) -> Option<&TrackedContact<N>> {
if self.len() != 0 {
Some(&self.contacts[self.deepest].0)
} else {
None
}
}
/// Empty the manifold as well as its cache.
pub fn clear(&mut self) {
match &mut self.cache {
ContactCache::FeatureBased(h) => h.clear(),
ContactCache::DistanceBased(v, _) => v.clear(),
}
self.contacts.clear();
self.ncontacts = 0;
}
/// Gets the technique currently used for tracking contacts.
pub fn tracking_mode(&self) -> ContactTrackingMode<N> {
match self.cache {
ContactCache::FeatureBased(_) => ContactTrackingMode::FeatureBased,
ContactCache::DistanceBased(_, threshold) => {
ContactTrackingMode::DistanceBased(threshold)
}
}
}
/// Sets the technique used for tracking contacts.
///
/// If the selected method is different from the current one,
/// the current contact cache is cleared.
pub fn set_tracking_mode(&mut self, mode: ContactTrackingMode<N>) {
match mode {
ContactTrackingMode::FeatureBased => {
if let ContactCache::FeatureBased(_) = self.cache {
// Nothing to do.
} else {
self.cache = ContactCache::FeatureBased(HashMap::new())
}
}
ContactTrackingMode::DistanceBased(new_threshold) => {
if let ContactCache::DistanceBased(_, threshold) = &mut self.cache {
*threshold = new_threshold;
return;
}
self.cache = ContactCache::DistanceBased(Vec::new(), new_threshold)
}
}
}
/// Save the contacts to a cache and empty the manifold.
pub fn save_cache_and_clear(&mut self) {
match &mut self.cache {
ContactCache::DistanceBased(cache, _) => {
let ctcts = &self.contacts;
cache.retain(|c| ctcts[c.1].1 != 0);
}
ContactCache::FeatureBased(cache) => {
let ctcts = &self.contacts;
cache.retain(|_k, v| ctcts[*v].1 != 0);
}
}
self.deepest = 0;
self.ncontacts = 0;
self.contacts.retain(|_i, c| {
if c.1 == 0 {
false
} else {
c.1 -= 1;
true
}
});
}
// FIXME: the method taking a preprocessor should be different?
/// Add a new contact to the manifold.
///
/// The manifold will attempt to match this contact with another one
/// previously added and added to the cache by the last call to
/// `save_cache_and_clear`. The matching is done by spacial proximity, i.e.,
/// two contacts that are sufficiently close will be given the same identifier.
pub fn push(
&mut self,
mut contact: Contact<N>,
mut kinematic: ContactKinematic<N>,
tracking_pt: Point<N>,
preprocessor1: Option<&dyn ContactPreprocessor<N>>,
preprocessor2: Option<&dyn ContactPreprocessor<N>>,
) -> bool {
if let Some(pp) = preprocessor1 {
if !pp.process_contact(&mut contact, &mut kinematic, true) {
return false;
}
}
if let Some(pp) = preprocessor2 {
if !pp.process_contact(&mut contact, &mut kinematic, false) {
return false;
}
}
let is_deepest =
self.ncontacts == 0 || contact.depth > self.contacts[self.deepest].0.contact.depth;
match &mut self.cache {
ContactCache::DistanceBased(cache, threshold) => {
let mut closest = cache.len();
let mut closest_dist: N = *threshold * *threshold;
for (i, cached) in cache.iter().enumerate() {
let dist = na::distance_squared(&tracking_pt, &cached.0);
if dist < closest_dist {
closest_dist = dist;
closest = i;
}
}
if closest == cache.len() {
let tracked = TrackedContact::new(contact, kinematic);
let i = self.contacts.insert((tracked, self.persistence));
cache.push((tracking_pt, i));
self.ncontacts += 1;
if is_deepest {
self.deepest = i;
}
false
} else {
let contact_i = cache[closest].1;
if is_deepest {
self.deepest = contact_i;
}
let c = &mut self.contacts[contact_i];
if c.1 == self.persistence {
if contact.depth <= c.0.contact.depth {
// Keep the contact already in cache because it is deeper.
return true;
}
} else {
self.ncontacts += 1;
c.1 = self.persistence;
}
c.0.contact = contact;
c.0.kinematic = kinematic;
cache[closest].0 = tracking_pt;
true
}
}
ContactCache::FeatureBased(cache) => {
match cache.entry((kinematic.feature1(), kinematic.feature2())) {
Entry::Vacant(e) => {
let tracked = TrackedContact::new(contact, kinematic);
let i = self.contacts.insert((tracked, self.persistence));
let _ = e.insert(i);
self.ncontacts += 1;
if is_deepest {
self.deepest = i;
}
false
}
Entry::Occupied(e) => {
if is_deepest {
self.deepest = *e.get();
}
let c = &mut self.contacts[*e.get()];
if c.1 == self.persistence {
if contact.depth <= c.0.contact.depth {
// Keep the contact already in cache because it is deeper.
return true;
}
} else {
self.ncontacts += 1;
c.1 = self.persistence;
}
c.0.contact = contact;
c.0.kinematic = kinematic;
true
}
}
}
}
}
}