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

CHANGELOG.md

@@ -1,11 +1,42 @@
 ## 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.
+
 ### Added
 
 - `CompoundFlags::FIX_INTERNAL_EDGES` makes a `Compound` treat the edges (2D) or faces (3D) its parts share as
   interior to the union, so a body sliding across the cut between two parts of a convex
   decomposition no longer catches on it. `Compound::PartNormalConstraints` is now
   `CompoundPseudoNormals`, matching what `TriMesh` and `Polyline` already provide.
+- `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,
+    VoxelType,
 };
 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();
@@ -55,7 +59,7 @@ pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>(
     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() {
+            match vox1.voxel_type() {
                 #[cfg(feature = "dim2")]
                 VoxelType::Vertex | VoxelType::Face => { /* Ok */ }
                 #[cfg(feature = "dim3")]
@@ -65,9 +69,9 @@ pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>(
 
             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, VoxelType,
+};
 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,7 +94,7 @@ 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();
+            let vox_type1 = vox1.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 {
@@ -135,7 +140,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 {
@@ -237,7 +242,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.");
@@ -254,9 +260,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, VoxelType};
 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> =
@@ -148,7 +151,7 @@ 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();
+            let vox_type1 = vox1.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 {
@@ -185,7 +188,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 +285,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 +301,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 +337,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 +350,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 +387,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)));
             }
         }