parry3d/query/contact_manifolds/
contact_manifolds_voxels_shape.rs1use crate::bounding_volume::Aabb;
2use crate::math::{Isometry, Point, Real, Translation, Vector, DIM};
3use crate::query::{
4 ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery,
5 TypedWorkspaceData, WorkspaceData,
6};
7use crate::shape::{AxisMask, Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels};
8use crate::utils::hashmap::{Entry, HashMap};
9use crate::utils::IsometryOpt;
10use alloc::{boxed::Box, vec::Vec};
11use na::{SVector, Vector2};
12
13#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
14#[cfg_attr(
15 feature = "rkyv",
16 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
17 archive(check_bytes)
18)]
19#[derive(Clone)]
20pub(crate) struct VoxelsShapeSubDetector {
21 pub manifold_id: usize,
22 pub selected_contacts: u32,
23 pub timestamp: bool,
24}
25
26#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
30#[derive(Clone, Default)]
31pub struct VoxelsShapeContactManifoldsWorkspace<const N: usize> {
32 pub(crate) timestamp: bool,
33 pub(crate) sub_detectors: HashMap<SVector<u32, N>, VoxelsShapeSubDetector>,
34}
35
36impl<const N: usize> VoxelsShapeContactManifoldsWorkspace<N> {
37 pub fn new() -> Self {
39 Self::default()
40 }
41
42 pub(crate) fn ensure_exists(workspace: &mut Option<ContactManifoldsWorkspace>)
43 where
44 Self: WorkspaceData,
45 {
46 if workspace
47 .as_ref()
48 .and_then(|w| {
49 w.0.downcast_ref::<VoxelsShapeContactManifoldsWorkspace<N>>()
50 })
51 .is_some()
52 {
53 return;
54 }
55
56 *workspace = Some(ContactManifoldsWorkspace(Box::new(
57 VoxelsShapeContactManifoldsWorkspace::new(),
58 )));
59 }
60}
61
62impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<2> {
63 fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
64 TypedWorkspaceData::VoxelsShapeContactManifoldsWorkspace(self)
65 }
66
67 fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
68 Box::new(self.clone())
69 }
70}
71
72pub fn contact_manifolds_voxels_shape_shapes<ManifoldData, ContactData>(
74 dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
75 pos12: &Isometry<Real>,
76 shape1: &dyn Shape,
77 shape2: &dyn Shape,
78 prediction: Real,
79 manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
80 workspace: &mut Option<ContactManifoldsWorkspace>,
81) where
82 ManifoldData: Default + Clone,
83 ContactData: Default + Copy,
84{
85 if let Some(voxels1) = shape1.as_voxels() {
86 contact_manifolds_voxels_shape(
87 dispatcher, pos12, voxels1, shape2, prediction, manifolds, workspace, false,
88 );
89 } else if let Some(voxels2) = shape2.as_voxels() {
90 contact_manifolds_voxels_shape(
91 dispatcher,
92 &pos12.inverse(),
93 voxels2,
94 shape1,
95 prediction,
96 manifolds,
97 workspace,
98 true,
99 );
100 }
101}
102
103pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
105 dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
106 pos12: &Isometry<Real>,
107 voxels1: &Voxels,
108 shape2: &dyn Shape,
109 prediction: Real,
110 manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
111 workspace: &mut Option<ContactManifoldsWorkspace>,
112 flipped: bool,
113) where
114 ManifoldData: Default + Clone,
115 ContactData: Default + Copy,
116{
117 VoxelsShapeContactManifoldsWorkspace::<2>::ensure_exists(workspace);
118 let workspace: &mut VoxelsShapeContactManifoldsWorkspace<2> =
119 workspace.as_mut().unwrap().0.downcast_mut().unwrap();
120 let new_timestamp = !workspace.timestamp;
121 workspace.timestamp = new_timestamp;
122
123 let mut old_manifolds = core::mem::take(manifolds);
125
126 let radius1 = voxels1.voxel_size() / 2.0;
127
128 let aabb1 = voxels1.local_aabb();
129 let aabb2_1 = shape2.compute_aabb(pos12);
130 let domain2_1 = Aabb {
131 mins: aabb2_1.mins - radius1 * 10.0,
132 maxs: aabb2_1.maxs + radius1 * 10.0,
133 };
134
135 if let Some(intersection_aabb1) = aabb1.intersection(&aabb2_1) {
136 for vox1 in voxels1.voxels_intersecting_local_aabb(&intersection_aabb1) {
137 let vox_type1 = vox1.state.voxel_type();
138
139 if vox_type1 == VoxelType::Empty || vox_type1 == VoxelType::Interior {
141 continue;
142 }
143
144 let canon1 = CanonicalVoxelShape::from_voxel(voxels1, &vox1);
145
146 let (sub_detector, manifold_updated) =
149 match workspace.sub_detectors.entry(canon1.workspace_key) {
150 Entry::Occupied(entry) => {
151 let sub_detector = entry.into_mut();
152
153 if sub_detector.timestamp != new_timestamp {
154 let manifold = old_manifolds[sub_detector.manifold_id].take();
155 *sub_detector = VoxelsShapeSubDetector {
156 manifold_id: manifolds.len(),
157 timestamp: new_timestamp,
158 selected_contacts: 0,
159 };
160
161 manifolds.push(manifold);
162 (sub_detector, false)
163 } else {
164 (sub_detector, true)
165 }
166 }
167 Entry::Vacant(entry) => {
168 let sub_detector = VoxelsShapeSubDetector {
169 manifold_id: manifolds.len(),
170 selected_contacts: 0,
171 timestamp: new_timestamp,
172 };
173
174 let vid = vox1.linear_id;
175 let (id1, id2) = if flipped { (0, vid) } else { (vid, 0) };
176 manifolds.push(ContactManifold::with_data(
177 id1,
178 id2,
179 ManifoldData::default(),
180 ));
181
182 (entry.insert(sub_detector), false)
183 }
184 };
185
186 let manifold = &mut manifolds[sub_detector.manifold_id];
190
191 if !manifold_updated {
192 let (canonical_center1, canonical_pseudo_cube1) =
193 canon1.cuboid(voxels1, &vox1, domain2_1);
194
195 let canonical_shape1 = &canonical_pseudo_cube1 as &dyn Shape;
196 let canonical_pos12 = Translation::from(-canonical_center1) * pos12;
197
198 let prev_center = if flipped {
205 manifold
206 .subshape_pos2
207 .as_ref()
208 .map(|p| p.translation.vector)
209 .unwrap_or_default()
210 } else {
211 manifold
212 .subshape_pos1
213 .as_ref()
214 .map(|p| p.translation.vector)
215 .unwrap_or_default()
216 };
217 let delta_center = canonical_center1.coords - prev_center;
218
219 if flipped {
220 for pt in &mut manifold.points {
221 pt.local_p2 -= delta_center;
222 }
223 } else {
224 for pt in &mut manifold.points {
225 pt.local_p1 -= delta_center;
226 }
227 }
228
229 if flipped {
231 manifold.subshape_pos2 = Some(Isometry::from(canonical_center1));
232 let _ = dispatcher.contact_manifold_convex_convex(
233 &canonical_pos12.inverse(),
234 shape2,
235 canonical_shape1,
236 None,
237 None,
238 prediction,
239 manifold,
240 );
241 } else {
242 manifold.subshape_pos1 = Some(Isometry::from(canonical_center1));
243 let _ = dispatcher.contact_manifold_convex_convex(
244 &canonical_pos12,
245 canonical_shape1,
246 shape2,
247 None,
248 None,
249 prediction,
250 manifold,
251 );
252 }
253 }
254
255 let test_voxel = Cuboid::new(radius1 + Vector::repeat(1.0e-2));
259 let penetration_dir1 = if flipped {
260 manifold.local_n2
261 } else {
262 manifold.local_n1
263 };
264
265 for (i, pt) in manifold.points.iter().enumerate() {
266 if pt.dist < 0.0 {
267 let cuboid1 = Cuboid::new(radius1);
271 let sp1 = cuboid1.local_support_point(&-penetration_dir1) + vox1.center.coords;
272 let sm2 = shape2
273 .as_support_map()
274 .expect("Unsupported collision pair.");
275 let sp2 = sm2.support_point(pos12, &penetration_dir1);
276 let test_dist = (sp2 - sp1).dot(&-penetration_dir1);
277 let keep = test_dist < pt.dist;
278
279 if !keep {
280 continue;
283 }
284 }
285
286 let pt_in_voxel_space = if flipped {
287 manifold.subshape_pos2.transform_point(&pt.local_p2) - vox1.center.coords
288 } else {
289 manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords
290 };
291 sub_detector.selected_contacts |=
292 (test_voxel.contains_local_point(&pt_in_voxel_space) as u32) << i;
293 }
294 }
295 }
296
297 for sub_detector in workspace.sub_detectors.values() {
299 if sub_detector.timestamp == new_timestamp {
300 let manifold = &mut manifolds[sub_detector.manifold_id];
301 let mut k = 0;
302 manifold.points.retain(|_| {
303 let keep = (sub_detector.selected_contacts & (1 << k)) != 0;
304 k += 1;
305 keep
306 });
307 }
308 }
309
310 workspace
312 .sub_detectors
313 .retain(|_, detector| detector.timestamp == new_timestamp);
314}
315
316#[derive(Copy, Clone, Debug)]
317pub(crate) struct CanonicalVoxelShape {
318 pub range: [Point<i32>; 2],
319 pub workspace_key: Vector2<u32>,
320}
321
322impl CanonicalVoxelShape {
323 pub fn from_voxel(voxels: &Voxels, vox: &VoxelData) -> Self {
324 let mut key_low = vox.grid_coords;
325 let mut key_high = key_low;
326
327 let mins = voxels.domain()[0] - Vector::repeat(1);
331 let maxs = voxels.domain()[1];
332 let mask1 = vox.state.free_faces();
333
334 let adjust_canon = |axis: AxisMask, i: usize, key: &mut Point<i32>, val: i32| {
335 if !mask1.contains(axis) {
336 key[i] = val;
337 }
338 };
339
340 adjust_canon(AxisMask::X_POS, 0, &mut key_high, maxs[0]);
341 adjust_canon(AxisMask::X_NEG, 0, &mut key_low, mins[0]);
342 adjust_canon(AxisMask::Y_POS, 1, &mut key_high, maxs[1]);
343 adjust_canon(AxisMask::Y_NEG, 1, &mut key_low, mins[1]);
344
345 #[cfg(feature = "dim3")]
346 {
347 adjust_canon(AxisMask::Z_POS, 2, &mut key_high, maxs[2]);
348 adjust_canon(AxisMask::Z_NEG, 2, &mut key_low, mins[2]);
349 }
350
351 Self {
352 range: [key_low, key_high],
353 workspace_key: Vector2::new(
354 voxels.linear_index(key_low),
355 voxels.linear_index(key_high),
356 ),
357 }
358 }
359
360 pub fn cuboid(
361 &self,
362 voxels: &Voxels,
363 vox: &VoxelData,
364 domain2_1: Aabb,
365 ) -> (Point<Real>, Cuboid) {
366 let radius = voxels.voxel_size() / 2.0;
367 let mut canonical_mins = voxels.voxel_center(self.range[0]);
368 let mut canonical_maxs = voxels.voxel_center(self.range[1]);
369
370 for k in 0..DIM {
371 if self.range[0][k] != vox.grid_coords[k] {
372 canonical_mins[k] = canonical_mins[k].max(domain2_1.mins[k]);
373 }
374
375 if self.range[1][k] != vox.grid_coords[k] {
376 canonical_maxs[k] = canonical_maxs[k].min(domain2_1.maxs[k]);
377 }
378 }
379
380 let canonical_half_extents = (canonical_maxs - canonical_mins) / 2.0 + radius;
381 let canonical_center = na::center(&canonical_mins, &canonical_maxs);
382 let canonical_cube = Cuboid::new(canonical_half_extents);
383
384 (canonical_center, canonical_cube)
385 }
386}