feat: make the voxels shape use a sparse internal storage

crates/parry2d-f64/Cargo.toml

@@ -36,6 +36,7 @@ dim2 = []
 f64 = []
 serde-serialize = [
     "serde",
+    "serde_arrays",
     "nalgebra/serde-serialize",
     "arrayvec/serde",
     "bitflags/serde",
@@ -73,9 +74,10 @@ num-traits = { version = "0.2", default-features = false }
 slab = { version = "0.4", optional = true }
 arrayvec = { version = "0.7", default-features = false }
 simba = { version = "0.9", default-features = false }
-nalgebra = { version = "0.34", default-features = false, features = ["libm"] }
+nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] }
 approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive"] }
+serde_arrays = { version = "0.2", optional = true }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
 indexmap = { version = "2", features = ["serde"], optional = true }

crates/parry2d/Cargo.toml

@@ -36,6 +36,7 @@ dim2 = []
 f32 = []
 serde-serialize = [
     "serde",
+    "serde_arrays",
     "nalgebra/serde-serialize",
     "arrayvec/serde",
     "bitflags/serde",
@@ -73,9 +74,10 @@ num-traits = { version = "0.2", default-features = false }
 slab = { version = "0.4", optional = true }
 arrayvec = { version = "0.7", default-features = false }
 simba = { version = "0.9", default-features = false }
-nalgebra = { version = "0.34", default-features = false, features = ["libm"] }
+nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] }
 approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive"] }
+serde_arrays = { version = "0.2", optional = true }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
 indexmap = { version = "2", features = ["serde"], optional = true }

crates/parry3d-f64/Cargo.toml

@@ -35,6 +35,7 @@ dim3 = []
 f64 = []
 serde-serialize = [
     "serde",
+    "serde_arrays",
     "nalgebra/serde-serialize",
     "bitflags/serde",
     "hashbrown?/serde",
@@ -73,9 +74,10 @@ num-traits = { version = "0.2", default-features = false }
 slab = { version = "0.4", optional = true }
 arrayvec = { version = "0.7", default-features = false }
 simba = { version = "0.9", default-features = false }
-nalgebra = { version = "0.34", default-features = false, features = ["libm"] }
+nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] }
 approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive", "rc"] }
+serde_arrays = { version = "0.2", optional = true }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
 indexmap = { version = "2", features = ["serde"], optional = true }

crates/parry3d/Cargo.toml

@@ -35,6 +35,7 @@ dim3 = []
 f32 = []
 serde-serialize = [
     "serde",
+    "serde_arrays",
     "nalgebra/serde-serialize",
     "bitflags/serde",
     "hashbrown?/serde",
@@ -74,9 +75,10 @@ num-traits = { version = "0.2", default-features = false }
 slab = { version = "0.4", optional = true }
 arrayvec = { version = "0.7", default-features = false }
 simba = { version = "0.9", default-features = false }
-nalgebra = { version = "0.34", default-features = false, features = ["libm"] }
+nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] }
 approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive", "rc"] }
+serde_arrays = { version = "0.2", optional = true }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
 indexmap = { version = "2", features = ["serde"], optional = true }

src/bounding_volume/aabb_voxels.rs

@@ -1,20 +1,17 @@
 use crate::bounding_volume::Aabb;
-use crate::math::{Isometry, Real, Translation};
-use crate::shape::{Cuboid, Voxels};
+use crate::math::{Isometry, Real};
+use crate::shape::Voxels;
 
 impl Voxels {
     /// Computes the world-space Aabb of this set of voxels, transformed by `pos`.
     #[inline]
     pub fn aabb(&self, pos: &Isometry<Real>) -> Aabb {
-        let shift = Translation::from(self.domain_center());
-        Cuboid::new(self.extents() / 2.0).aabb(&(pos * shift))
+        self.chunk_bvh().root_aabb().transform_by(pos)
     }
 
     /// Computes the local-space Aabb of this set of voxels.
     #[inline]
     pub fn local_aabb(&self) -> Aabb {
-        Cuboid::new(self.extents() / 2.0)
-            .local_aabb()
-            .translated(&self.domain_center().coords)
+        self.chunk_bvh().root_aabb()
     }
 }

src/bounding_volume/bounding_sphere_voxels.rs

@@ -1,20 +1,17 @@
 use crate::bounding_volume::BoundingSphere;
-use crate::math::{Isometry, Real, Translation};
-use crate::shape::{Cuboid, Voxels};
+use crate::math::{Isometry, Real};
+use crate::shape::Voxels;
 
 impl Voxels {
     /// Computes the world-space bounding sphere of this set of voxels, transformed by `pos`.
     #[inline]
     pub fn bounding_sphere(&self, pos: &Isometry<Real>) -> BoundingSphere {
-        let shift = Translation::from(self.domain_center().coords);
-        Cuboid::new(self.extents() / 2.0).bounding_sphere(&(pos * shift))
+        self.local_aabb().bounding_sphere().transform_by(pos)
     }
 
     /// Computes the local-space bounding sphere of this set of voxels.
     #[inline]
     pub fn local_bounding_sphere(&self) -> BoundingSphere {
-        Cuboid::new(self.extents() / 2.0)
-            .local_bounding_sphere()
-            .translated(&(self.domain_center().coords))
+        self.local_aabb().bounding_sphere()
     }
 }

src/partitioning/bvh/bvh_queries.rs

@@ -4,6 +4,7 @@ use crate::math::Point;
 use crate::math::Real;
 use crate::query::PointProjection;
 use crate::query::{PointQuery, Ray};
+use crate::shape::FeatureId;
 
 #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))]
 pub(super) struct SimdInvRay {
@@ -59,6 +60,30 @@ impl Bvh {
         )
     }
 
+    /// Projects a point on this BVH using the provided leaf projection function.
+    ///
+    /// Also returns the feature the point was projected on.
+    ///
+    /// The `primitive_check` delegates the point-projection task to an external function that
+    /// is assumed to map a leaf index to an actual geometry to project on. The `Real` argument
+    /// given to that closure is the distance to the closest point found so far (or is equal to
+    /// `max_distance` if no projection was found so far).
+    pub fn project_point_and_get_feature(
+        &self,
+        point: &Point<Real>,
+        max_distance: Real,
+        primitive_check: impl Fn(u32, Real) -> Option<(PointProjection, FeatureId)>,
+    ) -> Option<(u32, (Real, (PointProjection, FeatureId)))> {
+        self.find_best(
+            max_distance,
+            |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true),
+            |primitive, _| {
+                let proj = primitive_check(primitive, max_distance)?;
+                Some((na::distance(&proj.0.point, point), proj))
+            },
+        )
+    }
+
     /// Casts a ray on this BVH using the provided leaf ray-cast function.
     ///
     /// The `primitive_check` delegates the ray-casting task to an external function that

src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs

@@ -135,7 +135,7 @@ pub fn contact_manifolds_voxels_composite_shape<ManifoldData, ContactData>(
                             timestamp: new_timestamp,
                         };
 
-                        let vox_id = vox1.linear_id;
+                        let vox_id = vox1.linear_id.flat_id() as u32;
                         let (id1, id2) = if flipped {
                             (leaf2, vox_id)
                         } else {

src/query/contact_manifolds/contact_manifolds_voxels_shape.rs

@@ -171,7 +171,7 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
                             timestamp: new_timestamp,
                         };
 
-                        let vid = vox1.linear_id;
+                        let vid = vox1.linear_id.flat_id() as u32;
                         let (id1, id2) = if flipped { (0, vid) } else { (vid, 0) };
                         manifolds.push(ContactManifold::with_data(
                             id1,
@@ -329,6 +329,7 @@ impl CanonicalVoxelShape {
         //       detection).
         let mins = voxels.domain()[0] - Vector::repeat(1);
         let maxs = voxels.domain()[1];
+        let counts = maxs - mins;
         let mask1 = vox.state.free_faces();
 
         let adjust_canon = |axis: AxisMask, i: usize, key: &mut Point<i32>, val: i32| {
@@ -348,12 +349,21 @@ impl CanonicalVoxelShape {
             adjust_canon(AxisMask::Z_NEG, 2, &mut key_low, mins[2]);
         }
 
+        #[cfg(feature = "dim2")]
+        let workspace_id = |vox: Point<i32>| {
+            let local = vox - mins;
+            (local.x + local.y * counts.x) as u32
+        };
+
+        #[cfg(feature = "dim3")]
+        let workspace_id = |vox: Point<i32>| {
+            let local = vox - mins;
+            (local.x + local.y * counts.x + local.z * counts.x * counts.y) as u32
+        };
+
         Self {
             range: [key_low, key_high],
-            workspace_key: Vector2::new(
-                voxels.linear_index(key_low),
-                voxels.linear_index(key_high),
-            ),
+            workspace_key: Vector2::new(workspace_id(key_low), workspace_id(key_high)),
         }
     }
 

src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs

@@ -132,8 +132,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>(
                         };
 
                         manifolds.push(ContactManifold::with_data(
-                            vox1.linear_id,
-                            vox2.linear_id,
+                            vox1.linear_id.flat_id() as u32,
+                            vox2.linear_id.flat_id() as u32,
                             ManifoldData::default(),
                         ));
 

src/query/point/point_voxels.rs

@@ -1,38 +1,64 @@
 use crate::math::{Point, Real};
 use crate::query::{PointProjection, PointQuery};
-use crate::shape::{Cuboid, FeatureId, VoxelType, Voxels};
+use crate::shape::{Cuboid, FeatureId, Voxels, VoxelsChunkRef};
 
 impl PointQuery for Voxels {
     #[inline]
     fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
-        // TODO: optimize this very naive implementation.
-        let base_cuboid = Cuboid::new(self.voxel_size() / 2.0);
+        self.chunk_bvh()
+            .project_point(pt, Real::MAX, |chunk_id, _| {
+                let chunk = self.chunk_ref(chunk_id);
+                Some(chunk.project_local_point_and_get_vox_id(pt, solid).0)
+            })
+            .unwrap()
+            .1
+             .1
+    }
+
+    #[inline]
+    fn project_local_point_and_get_feature(
+        &self,
+        pt: &Point<Real>,
+    ) -> (PointProjection, FeatureId) {
+        self.chunk_bvh()
+            .project_point_and_get_feature(pt, Real::MAX, |chunk_id, _| {
+                let chunk = self.chunk_ref(chunk_id);
+                let (proj, vox) = chunk.project_local_point_and_get_vox_id(pt, false);
+                // TODO: we need a way to return both the voxel id, and the feature on the voxel.
+                Some((proj, FeatureId::Face(vox)))
+            })
+            .unwrap()
+            .1
+             .1
+    }
+}
+
+impl<'a> VoxelsChunkRef<'a> {
+    #[inline]
+    fn project_local_point_and_get_vox_id(
+        &self,
+        pt: &Point<Real>,
+        solid: bool,
+    ) -> (PointProjection, u32) {
+        // TODO: optimize this naive implementation that just iterates on all the voxels
+        //       from this chunk.
+        let base_cuboid = Cuboid::new(self.parent.voxel_size() / 2.0);
         let mut smallest_dist = Real::MAX;
         let mut result = PointProjection::new(false, *pt);
+        let mut result_vox_id = 0;
 
         for vox in self.voxels() {
-            if vox.state.voxel_type() != VoxelType::Empty {
-                let mut candidate =
-                    base_cuboid.project_local_point(&(pt - vox.center.coords), solid);
-                candidate.point += vox.center.coords;
+            let mut candidate = base_cuboid.project_local_point(&(pt - vox.center.coords), solid);
+            candidate.point += vox.center.coords;
 
-                let candidate_dist = (candidate.point - pt).norm();
-                if candidate_dist < smallest_dist {
-                    result = candidate;
-                    smallest_dist = candidate_dist;
-                }
+            let candidate_dist = (candidate.point - pt).norm();
+            if candidate_dist < smallest_dist {
+                result = candidate;
+                result_vox_id = vox.linear_id.flat_id();
+                smallest_dist = candidate_dist;
             }
         }
 
-        result
-    }
-
-    #[inline]
-    fn project_local_point_and_get_feature(
-        &self,
-        pt: &Point<Real>,
-    ) -> (PointProjection, FeatureId) {
-        // TODO: get the actual feature.
-        (self.project_local_point(pt, false), FeatureId::Unknown)
+        (result, result_vox_id as u32)
     }
 }

src/query/ray/ray_voxels.rs

@@ -1,8 +1,30 @@
 use crate::math::{Real, Vector};
+use crate::partitioning::BvhNode;
 use crate::query::{Ray, RayCast, RayIntersection};
-use crate::shape::{FeatureId, Voxels};
+use crate::shape::{FeatureId, Voxels, VoxelsChunkRef};
 
 impl RayCast for Voxels {
+    #[inline]
+    fn cast_local_ray_and_get_normal(
+        &self,
+        ray: &Ray,
+        max_time_of_impact: Real,
+        solid: bool,
+    ) -> Option<RayIntersection> {
+        self.chunk_bvh()
+            .find_best(
+                max_time_of_impact,
+                |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far),
+                |primitive, best_so_far| {
+                    let chunk = self.chunk_ref(primitive);
+                    chunk.cast_local_ray_and_get_normal(ray, best_so_far, solid)
+                },
+            )
+            .map(|(_chunk_id, hit)| hit)
+    }
+}
+
+impl<'a> RayCast for VoxelsChunkRef<'a> {
     #[inline]
     fn cast_local_ray_and_get_normal(
         &self,
@@ -31,19 +53,22 @@ impl RayCast for Voxels {
         let [domain_mins, domain_maxs] = self.domain();
 
         loop {
-            let voxel = self.voxel_state(voxel_key);
-            let aabb = self.voxel_aabb(voxel_key);
+            let aabb = self.voxel_aabb_unchecked(voxel_key);
 
-            if !voxel.is_empty() {
-                // We hit a voxel!
-                // TODO: if `solid` is false, and we started hitting from the first iteration,
-                //       then we should continue the ray propagation until we reach empty space again.
-                let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid);
+            if let Some(voxel) = self.voxel_state(voxel_key) {
+                if !voxel.is_empty() {
+                    // We hit a voxel!
+                    // TODO: if `solid` is false, and we started hitting from the first iteration,
+                    //       then we should continue the ray propagation until we reach empty space again.
+                    let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid);
 
-                if let Some(mut hit) = hit {
-                    // TODO: have the feature id be based on the voxel type?
-                    hit.feature = FeatureId::Face(self.linear_index(voxel_key));
-                    return Some(hit);
+                    if let Some(mut hit) = hit {
+                        // TODO: have the feature id be based on the voxel type?
+                        hit.feature = FeatureId::Face(
+                            self.flat_id(voxel_key).unwrap_or_else(|| unreachable!()),
+                        );
+                        return Some(hit);
+                    }
                 }
             }
 

src/shape/mod.rs

@@ -22,7 +22,7 @@ pub use self::{
     compound::Compound,
     polyline::Polyline,
     shared_shape::SharedShape,
-    voxels::{AxisMask, OctantPattern, VoxelData, VoxelState, VoxelType, Voxels},
+    voxels::{AxisMask, OctantPattern, VoxelData, VoxelState, VoxelType, Voxels, VoxelsChunkRef},
 };
 
 #[cfg(feature = "dim2")]