parry3d/query/contact_manifolds/
contact_manifolds_voxels_shape.rs

1use 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// NOTE: this is using a similar kind of cache as compound shape and height-field.
27//       It is different from the trimesh cash though. Which one is better?
28/// A workspace for collision-detection against voxels shape.
29#[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    /// A new empty workspace for collision-detection against voxels shape.
38    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
72/// Computes the contact manifold between a convex shape and a voxels shape, both represented as a `Shape` trait-object.
73pub 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
103/// Computes the contact manifold between a convex shape and a voxels shape.
104pub 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    // TODO: avoid reallocating the new `manifolds` vec at each step.
124    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            // TODO: would be nice to have a strategy to handle interior voxels for depenetration.
140            if vox_type1 == VoxelType::Empty || vox_type1 == VoxelType::Interior {
141                continue;
142            }
143
144            let canon1 = CanonicalVoxelShape::from_voxel(voxels1, &vox1);
145
146            // TODO: could we refactor the workspace system between Voxels, HeightField, and CompoundShape?
147            //       (and maybe TriMesh too but it’s using a different approach).
148            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            /*
187             * Update the contact manifold if needed.
188             */
189            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                // If we already computed contacts in the previous simulation step, their
199                // local points are relative to the previously calculated canonical shape
200                // which might not have the same local center as the one computed in this
201                // step (because it’s based on the position of shape2 relative to voxels1).
202                // So we need to adjust the local points to account for the position difference
203                // and keep the point at the same "canonica-shape-space" location as in the previous frame.
204                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                // Update contacts.
230                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            /*
256             * Filter-out points that don’t belong to this block.
257             */
258            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                    // If this is a penetration, double-check that we are not hitting the
268                    // interior of the infinitely expanded canonical shape by checking if
269                    // the opposite normal had led to a better vector.
270                    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                        // We don’t want to keep this point as it is an incorrect penetration
281                        // caused by the canonical shape expansion.
282                        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    // Remove contacts marked as ignored.
298    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    // Remove detectors which no longer index a valid manifold.
311    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        // NOTE: the mins/maxs here are offset by 1 so we can expand past the last voxel if it
328        //       happens to also be infinite along the same axis (due to cross-voxels internal edges
329        //       detection).
330        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}