Make voxel collision queries generic over the voxel storage with a VoxelQuery trait

CHANGELOG.md

@@ -1,5 +1,20 @@
 ## Unreleased
 
+### Breaking changes
+
+- The voxel query functions are now generic over the voxel storage instead of taking `&Voxels`:
+  `contact_manifolds_voxels_shape`, `contact_manifolds_voxels_ball`,
+  `contact_manifolds_voxels_composite_shape`, `contact_manifolds_voxels_voxels`,
+  `intersection_test_voxels_shape`, `intersection_test_shape_voxels`, `cast_shapes_voxels_shape`,
+  `cast_shapes_shape_voxels`, `cast_shapes_nonlinear_voxels_shape`,
+  `cast_shapes_nonlinear_shape_voxels`. Calls that pass a `&Voxels` keep working unchanged;
+  callers naming the functions with explicit turbofish generics gain one extra type parameter.
+- Reading a voxel's type, state, center, or grid coordinates from the item yielded by
+  `Voxels::voxels`, `Voxels::voxels_in_range`, and `Voxels::voxels_intersecting_local_aabb` should
+  go through the new `QueriedVoxel` trait methods (`voxel_type()`, `voxel_state()`, `center()`,
+  `grid_coords()`, `linear_id()`) for code that must also work with custom storages. The public
+  fields of `VoxelData` remain available.
+
 ### Modified
 
 - Deserializing a `ContactManifoldsWorkspace` that was serialized by a custom contact-manifolds
@@ -68,6 +83,22 @@
   discarding them ([#447](https://github.com/dimforge/parry/pull/447)).
 - `Polyline::pseudo_normals` exposes the per-vertex outward pseudo-normals (2D)
   ([#447](https://github.com/dimforge/parry/pull/447)).
+- `VoxelQuery` trait, an abstraction over the storage of a shape made of axis-aligned, uniformly
+  sized voxels. Implementing it for a custom sparse structure (chunked grid, octree, VDB-like tree)
+  lets Parry's voxel collision algorithms (contact manifolds, intersection tests, linear and
+  nonlinear shape-casting) run directly on that structure without copying it into a `Voxels` shape. Implementors provide `voxel_size`, `domain`, and `voxels_in_range`; grid helpers
+  such as `voxel_at_point`, `voxel_center`, `voxel_aabb`, `voxel_range_intersecting_local_aabb`,
+  `align_aabb_to_grid`, and `local_aabb` have default implementations.
+- `QueriedVoxel` trait describing the per-voxel view handed out by a `VoxelQuery` storage. Views
+  may borrow from their storage so that `voxel_state()` can be computed lazily from local context,
+  while `voxel_type()` stays cheap for bulk iteration.
+- `Voxels` implements `VoxelQuery` with `VoxelData` as its voxel view, and `VoxelData` implements
+  `QueriedVoxel`.
+- `VoxelState::with_filled_neighbors(AxisMask)` builds the state of a non-empty voxel from the set
+  of its filled axis-aligned neighbors, for custom storages that only track occupancy.
+- `Default` implementations for `VoxelType` (`Empty`) and `VoxelState` (`EMPTY`).
+- `contact_manifolds_voxels_ball` is now re-exported from `parry::query`, alongside the other
+  voxel contact-manifold functions.
 
 ## 0.30.2
 

src/mass_properties/mass_properties_voxels.rs

@@ -2,7 +2,7 @@ use crate::mass_properties::MassProperties;
 #[cfg(feature = "dim3")]
 use crate::math::Matrix;
 use crate::math::{Real, Vector};
-use crate::shape::Voxels;
+use crate::shape::{QueriedVoxel, VoxelType, Voxels};
 
 impl MassProperties {
     /// Computes the mass properties of a voxel grid.
@@ -163,7 +163,9 @@ impl MassProperties {
     ///
     /// - Only non-empty voxels contribute to mass
     /// - Empty voxels are ignored (zero mass, no inertia)
-    /// - The voxel state is checked using `vox.state.is_empty()`
+    /// - A voxel is considered empty if its
+    ///   [`QueriedVoxel::voxel_type`] is
+    ///   [`VoxelType::Empty`]
     ///
     /// # See Also
     ///
@@ -181,18 +183,18 @@ impl MassProperties {
         let block_ref_mprops = MassProperties::from_cuboid(density, voxels.voxel_size() / 2.0);
 
         for vox in voxels.voxels() {
-            if !vox.state.is_empty() {
-                com += vox.center;
+            if vox.voxel_type() != VoxelType::Empty {
+                com += vox.center();
                 num_not_empty += 1;
             }
         }
 
         com /= num_not_empty as Real;
 
         for vox in voxels.voxels() {
-            if !vox.state.is_empty() {
+            if vox.voxel_type() != VoxelType::Empty {
                 angular_inertia +=
-                    block_ref_mprops.construct_shifted_inertia_matrix(vox.center - com);
+                    block_ref_mprops.construct_shifted_inertia_matrix(vox.center() - com);
             }
         }
 

src/query/contact_manifolds/contact_manifolds_voxels_ball.rs

@@ -2,7 +2,8 @@ use crate::bounding_volume::BoundingVolume;
 use crate::math::{Pose, Real, Vector, VectorExt};
 use crate::query::{ContactManifold, PointQuery, TrackedContact};
 use crate::shape::{
-    Ball, Cuboid, OctantPattern, PackedFeatureId, Shape, VoxelState, VoxelType, Voxels,
+    Ball, Cuboid, OctantPattern, PackedFeatureId, QueriedVoxel, Shape, VoxelQuery, VoxelState,
+    VoxelTypes,
 };
 use alloc::vec::Vec;
 
@@ -31,17 +32,20 @@ pub fn contact_manifolds_voxels_ball_shapes<ManifoldData, ContactData>(
     }
 }
 
-/// Computes the contact manifold between a convex shape and a ball.
-pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>(
+/// Computes the contact manifold between a voxels shape and a ball.
+///
+/// The voxels shape can be any voxel storage implementing [`VoxelQuery`].
+pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData, V>(
     pos12: &Pose,
-    voxels1: &'a Voxels,
+    voxels1: &'a V,
     ball2: &'a Ball,
     prediction: Real,
     manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
     flipped: bool,
 ) where
     ManifoldData: Default,
     ContactData: Default + Copy,
+    V: ?Sized + VoxelQuery,
 {
     // TODO: don’t generate one manifold per voxel.
     manifolds.clear();
@@ -54,20 +58,14 @@ pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>(
     let aabb1 = voxels1.local_aabb().loosened(prediction / 2.0);
     let aabb2 = ball2.aabb(pos12).loosened(prediction / 2.0);
     if let Some(aabb_intersection) = aabb1.intersection(&aabb2) {
-        for vox1 in voxels1.voxels_intersecting_local_aabb(&aabb_intersection) {
-            match vox1.state.voxel_type() {
-                #[cfg(feature = "dim2")]
-                VoxelType::Vertex | VoxelType::Face => { /* Ok */ }
-                #[cfg(feature = "dim3")]
-                VoxelType::Vertex | VoxelType::Edge | VoxelType::Face => { /* Ok */ }
-                _ => continue,
-            }
-
+        for vox1 in
+            voxels1.voxels_intersecting_local_aabb_of_types(&aabb_intersection, VoxelTypes::SURFACE)
+        {
             detect_hit_voxel_ball(
                 *pos12,
-                vox1.center,
+                vox1.center(),
                 radius1,
-                vox1.state,
+                vox1.voxel_state(),
                 center2,
                 radius2,
                 prediction,

src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs

@@ -7,7 +7,9 @@ use crate::query::{
     ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery,
     TypedWorkspaceData, WorkspaceData,
 };
-use crate::shape::{CompositeShape, Cuboid, Shape, SupportMap, VoxelType, Voxels};
+use crate::shape::{
+    CompositeShape, Cuboid, QueriedVoxel, Shape, SupportMap, VoxelQuery, VoxelTypes,
+};
 use crate::utils::hashmap::Entry;
 use crate::utils::PoseOpt;
 use alloc::{boxed::Box, vec::Vec};
@@ -55,10 +57,12 @@ pub fn contact_manifolds_voxels_composite_shape_shapes<ManifoldData, ContactData
 }
 
 /// Computes the contact manifold between voxels and a composite shape.
-pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
+///
+/// The voxels shape can be any voxel storage implementing [`VoxelQuery`].
+pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData, V>(
     dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
     pos12: &Pose,
-    voxels1: &Voxels,
+    voxels1: &V,
     shape2: &dyn CompositeShape,
     prediction: Real,
     manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
@@ -67,6 +71,7 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
 ) where
     ManifoldData: Default + Clone,
     ContactData: Default + Copy,
+    V: ?Sized + VoxelQuery,
 {
     VoxelsShapeContactManifoldsWorkspace::<3>::ensure_exists(workspace);
     let workspace: &mut VoxelsShapeContactManifoldsWorkspace<3> =
@@ -89,14 +94,10 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
     };
 
     if let Some(intersection_aabb1) = aabb1.intersection(&aabb2_1) {
-        for vox1 in voxels1.voxels_intersecting_local_aabb(&intersection_aabb1) {
-            let vox_type1 = vox1.state.voxel_type();
-
-            // TODO: would be nice to have a strategy to handle interior voxels for depenetration.
-            if vox_type1 == VoxelType::Empty || vox_type1 == VoxelType::Interior {
-                continue;
-            }
-
+        // TODO: would be nice to have a strategy to handle interior voxels for depenetration.
+        for vox1 in voxels1
+            .voxels_intersecting_local_aabb_of_types(&intersection_aabb1, VoxelTypes::SURFACE)
+        {
             // PERF: could we avoid repeated BVH traversals involving the same canonical shape?
             //       The issue is that we need to figure out what contact manifolds are associated
             //       to that canonical shape so we can update the included contact bitmask (and
@@ -136,7 +137,7 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
                                 timestamp: new_timestamp,
                             };
 
-                            let vox_id = vox1.linear_id.flat_id() as u32;
+                            let vox_id = vox1.linear_id();
                             let (id1, id2) = if flipped {
                                 (leaf2, vox_id)
                             } else {
@@ -242,7 +243,8 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
                             // interior of the infinitely expanded canonical shape by checking if
                             // the opposite normal had led to a better vector.
                             let cuboid1 = Cuboid::new(radius1);
-                            let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center;
+                            let sp1 =
+                                cuboid1.local_support_point(-penetration_dir1) + vox1.center();
                             let sm2 = part_shape2
                                 .as_support_map()
                                 .expect("Unsupported collision pair.");
@@ -259,9 +261,9 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
                         }
 
                         let pt_in_voxel_space = if flipped {
-                            manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center
+                            manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center()
                         } else {
-                            manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center
+                            manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center()
                         };
                         sub_detector.selected_contacts |=
                             (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i;

src/query/contact_manifolds/contact_manifolds_voxels_shape.rs

@@ -4,7 +4,7 @@ use crate::query::{
     ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery,
     TypedWorkspaceData, WorkspaceData,
 };
-use crate::shape::{AxisMask, Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels};
+use crate::shape::{AxisMask, Cuboid, QueriedVoxel, Shape, SupportMap, VoxelQuery, VoxelTypes};
 use crate::utils::hashmap::{Entry, HashMap};
 use crate::utils::PoseOpt;
 use alloc::{boxed::Box, vec::Vec};
@@ -113,10 +113,12 @@ pub fn contact_manifolds_voxels_shape_shapes<ManifoldData, ContactData>(
 }
 
 /// Computes the contact manifold between a convex shape and a voxels shape.
-pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
+///
+/// The voxels shape can be any voxel storage implementing [`VoxelQuery`].
+pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData, V>(
     dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
     pos12: &Pose,
-    voxels1: &Voxels,
+    voxels1: &V,
     shape2: &dyn Shape,
     prediction: Real,
     manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
@@ -125,6 +127,7 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
 ) where
     ManifoldData: Default + Clone,
     ContactData: Default + Copy,
+    V: ?Sized + VoxelQuery,
 {
     VoxelsShapeContactManifoldsWorkspace::<2>::ensure_exists(workspace);
     let workspace: &mut VoxelsShapeContactManifoldsWorkspace<2> =
@@ -147,14 +150,10 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
     };
 
     if let Some(intersection_aabb1) = aabb1.intersection(&aabb2_1) {
-        for vox1 in voxels1.voxels_intersecting_local_aabb(&intersection_aabb1) {
-            let vox_type1 = vox1.state.voxel_type();
-
-            // TODO: would be nice to have a strategy to handle interior voxels for depenetration.
-            if vox_type1 == VoxelType::Empty || vox_type1 == VoxelType::Interior {
-                continue;
-            }
-
+        // TODO: would be nice to have a strategy to handle interior voxels for depenetration.
+        for vox1 in voxels1
+            .voxels_intersecting_local_aabb_of_types(&intersection_aabb1, VoxelTypes::SURFACE)
+        {
             let canon1 = CanonicalVoxelShape::from_voxel(voxels1, &vox1);
 
             // TODO: could we refactor the workspace system between Voxels, HeightField, and CompoundShape?
@@ -185,7 +184,7 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
                             timestamp: new_timestamp,
                         };
 
-                        let vid = vox1.linear_id.flat_id() as u32;
+                        let vid = vox1.linear_id();
                         let (id1, id2) = if flipped { (0, vid) } else { (vid, 0) };
                         manifolds.push(ContactManifold::with_data(
                             id1,
@@ -282,7 +281,7 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
                     // interior of the infinitely expanded canonical shape by checking if
                     // the opposite normal had led to a better vector.
                     let cuboid1 = Cuboid::new(radius1);
-                    let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center;
+                    let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center();
                     let sm2 = shape2
                         .as_support_map()
                         .expect("Unsupported collision pair.");
@@ -298,9 +297,9 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
                 }
 
                 let pt_in_voxel_space = if flipped {
-                    manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center
+                    manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center()
                 } else {
-                    manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center
+                    manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center()
                 };
                 sub_detector.selected_contacts |=
                     (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i;
@@ -334,8 +333,11 @@ pub(crate) struct CanonicalVoxelShape {
 }
 
 impl CanonicalVoxelShape {
-    pub fn from_voxel(voxels: &Voxels, vox: &VoxelData) -> Self {
-        let mut key_low = vox.grid_coords;
+    pub fn from_voxel<'a, 'b, V: ?Sized + VoxelQuery>(
+        voxels: &V,
+        vox: &'a impl QueriedVoxel<'b>,
+    ) -> Self {
+        let mut key_low = vox.grid_coords();
         let mut key_high = key_low;
 
         // NOTE: the mins/maxs here are offset by 1 so we can expand past the last voxel if it
@@ -344,7 +346,7 @@ impl CanonicalVoxelShape {
         let mins = voxels.domain()[0] - IVector::splat(1);
         let maxs = voxels.domain()[1];
         let counts = maxs - mins;
-        let mask1 = vox.state.free_faces();
+        let mask1 = vox.voxel_state().free_faces();
 
         let adjust_canon = |axis: AxisMask, i: usize, key: &mut IVector, val: Int| {
             if !mask1.contains(axis) {
@@ -381,17 +383,22 @@ impl CanonicalVoxelShape {
         }
     }
 
-    pub fn cuboid(&self, voxels: &Voxels, vox: &VoxelData, domain2_1: Aabb) -> (Vector, Cuboid) {
+    pub fn cuboid<'a, 'b, V: ?Sized + VoxelQuery>(
+        &self,
+        voxels: &V,
+        vox: &'a impl QueriedVoxel<'b>,
+        domain2_1: Aabb,
+    ) -> (Vector, Cuboid) {
         let radius = voxels.voxel_size() / 2.0;
         let mut canonical_mins = voxels.voxel_center(self.range[0]);
         let mut canonical_maxs = voxels.voxel_center(self.range[1]);
 
         for k in 0..DIM {
-            if self.range[0].ivget(k) != vox.grid_coords.ivget(k) {
+            if self.range[0].ivget(k) != vox.grid_coords().ivget(k) {
                 canonical_mins.vset(k, canonical_mins.vget(k).max(domain2_1.mins.vget(k)));
             }
 
-            if self.range[1].ivget(k) != vox.grid_coords.ivget(k) {
+            if self.range[1].ivget(k) != vox.grid_coords().ivget(k) {
                 canonical_maxs.vset(k, canonical_maxs.vget(k).min(domain2_1.maxs.vget(k)));
             }
         }