feat: make the voxels shape use a sparse internal storage (#380)

* feat: make the voxels shape use a sparse internal storage

* fix compilation of docs and examples

* chore: cargo fmt

Author: sebcrozet

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(),
                         ));