feat: replace the QBVH by a new BVH implementation (#361)

* feat: add AABB calculation from points iterator yielding values instead of refs

* fix: add missing exports

* feat: add implementation of a new BVH data-structure

* chore: remove the QBVH

* feat: integrate the new BVH into all composite shapes and geometric queries

* chore: cargo fmt

* feat: add nonparallel PLOC bvh builder

* chore: remove the incremental binned build optimization strategy

* feat: add more comments

* chore: add missing deps

* chore: rename SimdCompositeShape to CompositeShape

* chore: rename remaining occurrences of `qbvh` to `bvh`.

* chore: have TypedCompositeShape derive from CompositeShape

* chore: cargo fmt

* chore: fix cargo doc

* chore: fix no-std build

* fix: remove rkyv support for TriMesh and Polyline because VecMap doesn’t support it

* chore: vendor vec_map for rkyv and no-std support

* feat: re-enable rkyv support for polyline and trimesh by vendoring vec_map

* chore: misc CI fixes

* chore: more CI fixes

* fix enhanced-determinism build

* fix tests

* fix cargo doc

* fix broken simd bvh raycast perfs on x86

* feat: switch to static assertions for bvh node simd size/alignment.

* chore: update CHANGELOG

Author: sebcrozet

CHANGELOG.md

@@ -1,11 +1,33 @@
 # Change Log
 
+### Added
+
+- Add a new `Bvh` acceleration data-structure. It replaces `Qbvh` entirely. It supports:
+  - Traversals (best-first, depth-first, BVTT, leaf iterators, and leaf pairs iterator).
+  - It can be constructed either incrementally by inserting nodes, or from a set of leaves using either the
+    binned building strategy or the PLOC (without parallelism) strategy.
+  - Dynamic leaf insertion, update, removal.
+  - Incremental tree rebalancing.
+
 ### Fixed
 
 - Fix `clip_aabb_line` crashing when given incorrect inputs (zero length direction or NAN).
 - Fix `Segment::intersects_ray` returning false-positive when the segment is zero-length. ([#31](https://github.com/dimforge/parry/issues/31)).
 - Expose `utils::sort3` and `utils::sort2`.
 
+### Modified
+
+- The `local_point_cloud_aabb`, `point_cloud_aabb`, and `Aabb::from_points` now takes an iterator over point values instead
+  of an iterator to point references. Variants taking point references still exist and are named `local_point_cloud_aabb_ref`,
+  `point_cloud_aabb_ref` and `Aabb::from_points_ref`.
+- Renamed `SimdCompositeShape` and `TypedSimdCompositeShape` to `CompositeShape` and TypedCompositeShape`.
+- The `TypedCompositeShape` trait now derives from `CompositeShape`.
+- Removed every `*Visitor` structures. Instead, either call `Bvh::traverse` (or `Bvh::search_best`, or `Bvh::leaves`, or
+  `bvh::leaf_pairs`), or wrap your composite shape into `CompositeShapeRef` to access some generic implementation of
+  various geometric queries for any composite shape.
+- All composite shapes now rely on the new `Bvh` acceleration structure instead of `Qbvh`.
+- The `Qbvh` has been removed. Use `Bvh` instead.
+
 ## 0.21.1
 
 ### Added

crates/parry2d-f64/Cargo.toml

@@ -89,6 +89,7 @@ log = "0.4"
 ordered-float = { version = "5", default-features = false }
 thiserror = { version = "2", default-features = false }
 ena = { version = "0.14.3", optional = true, default-features = false }
+smallvec = "1"
 
 [dev-dependencies]
 simba = { version = "0.9", default-features = false }

crates/parry2d/Cargo.toml

@@ -89,6 +89,7 @@ ordered-float = { version = "5", default-features = false }
 log = "0.4"
 thiserror = { version = "2", default-features = false }
 ena = { version = "0.14.3", optional = true, default-features = false }
+smallvec = "1"
 
 [dev-dependencies]
 simba = { version = "0.9", default-features = false }

crates/parry3d-f64/Cargo.toml

@@ -89,6 +89,7 @@ bytemuck = { version = "1", features = ["derive"], optional = true }
 rstar = "0.12.0"
 obj = { version = "0.10.2", optional = true }
 ena = { version = "0.14.3", optional = true, default-features = false }
+smallvec = "1"
 
 log = "0.4"
 ordered-float = { version = "5", default-features = false }

crates/parry3d/Cargo.toml

@@ -59,7 +59,7 @@ improved_fixed_point_support = []
 
 # Do not enable this feature directly. It is automatically
 # enabled with the "simd-stable" or "simd-nightly" feature.
-simd-is-enabled = []
+simd-is-enabled = [ "glam" ]
 
 [lib]
 name = "parry3d"
@@ -92,6 +92,13 @@ thiserror = { version = "2", default-features = false }
 rstar = "0.12.0"
 obj = { version = "0.10.2", optional = true }
 ena = { version = "0.14.3", optional = true, default-features = false }
+smallvec = "1"
+static_assertions = "1"
+
+# NOTE: needed only for element_min for SIMD BVH ray-casting.
+#       can be removed once `wide` supports it (and allows filtering-out the
+#       fourth element).
+glam = { version = "0.30.4", optional = true }
 
 [dev-dependencies]
 oorandom = "11"

src/bounding_volume/aabb.rs

@@ -9,7 +9,8 @@ use na;
 use num::Bounded;
 
 #[cfg(all(feature = "dim3", not(feature = "std")))]
-use na::ComplexField; // for .sin_cos()
+use na::ComplexField;
+// for .sin_cos()
 
 use crate::query::{Ray, RayCast};
 #[cfg(feature = "rkyv")]
@@ -143,10 +144,18 @@ impl Aabb {
         Self::new(center - half_extents, center + half_extents)
     }
 
-    /// Creates a new `Aabb` from a set of points.
-    pub fn from_points<'a, I>(pts: I) -> Self
+    /// Creates a new `Aabb` from a set of point references.
+    pub fn from_points_ref<'a, I>(pts: I) -> Self
     where
         I: IntoIterator<Item = &'a Point<Real>>,
+    {
+        super::aabb_utils::local_point_cloud_aabb(pts.into_iter().copied())
+    }
+
+    /// Creates a new `Aabb` from a set of points.
+    pub fn from_points<I>(pts: I) -> Self
+    where
+        I: IntoIterator<Item = Point<Real>>,
     {
         super::aabb_utils::local_point_cloud_aabb(pts)
     }
@@ -174,6 +183,28 @@ impl Aabb {
         return extents.x * extents.y * extents.z;
     }
 
+    /// In 3D, returns the half-area. In 2D returns the half-perimeter of the AABB.
+    pub fn half_area_or_perimeter(&self) -> Real {
+        #[cfg(feature = "dim2")]
+        return self.half_perimeter();
+        #[cfg(feature = "dim3")]
+        return self.half_area();
+    }
+
+    /// The half perimeter of this `Aabb`.
+    #[cfg(feature = "dim2")]
+    pub fn half_perimeter(&self) -> Real {
+        let extents = self.extents();
+        extents.x + extents.y
+    }
+
+    /// The half area of this `Aabb`.
+    #[cfg(feature = "dim3")]
+    pub fn half_area(&self) -> Real {
+        let extents = self.extents();
+        extents.x * (extents.y + extents.z) + extents.y * extents.z
+    }
+
     /// The extents of this `Aabb`.
     #[inline]
     pub fn extents(&self) -> Vector<Real> {
@@ -251,6 +282,15 @@ impl Aabb {
         true
     }
 
+    /// Computes the distance between the origin and this AABB.
+    pub fn distance_to_origin(&self) -> Real {
+        self.mins
+            .coords
+            .sup(&-self.maxs.coords)
+            .sup(&Vector::zeros())
+            .norm()
+    }
+
     /// Does this AABB intersects an AABB `aabb2` moving at velocity `vel12` relative to `self`?
     #[inline]
     pub fn intersects_moving_aabb(&self, aabb2: &Self, vel12: Vector<Real>) -> bool {

src/bounding_volume/aabb_convex_polygon.rs

@@ -6,12 +6,12 @@ impl ConvexPolygon {
     /// Computes the world-space [`Aabb`] of this convex polygon, transformed by `pos`.
     #[inline]
     pub fn aabb(&self, pos: &Isometry<Real>) -> Aabb {
-        super::details::point_cloud_aabb(pos, self.points())
+        super::details::point_cloud_aabb_ref(pos, self.points())
     }
 
     /// Computes the local-space [`Aabb`] of this convex polygon.
     #[inline]
     pub fn local_aabb(&self) -> Aabb {
-        super::details::local_point_cloud_aabb(self.points())
+        super::details::local_point_cloud_aabb_ref(self.points())
     }
 }

src/bounding_volume/aabb_convex_polyhedron.rs

@@ -6,12 +6,12 @@ impl ConvexPolyhedron {
     /// Computes the world-space [`Aabb`] of this convex polyhedron, transformed by `pos`.
     #[inline]
     pub fn aabb(&self, pos: &Isometry<Real>) -> Aabb {
-        super::details::point_cloud_aabb(pos, self.points())
+        super::details::point_cloud_aabb_ref(pos, self.points())
     }
 
     /// Computes the local-space [`Aabb`] of this convex polyhedron.
     #[inline]
     pub fn local_aabb(&self) -> Aabb {
-        super::details::local_point_cloud_aabb(self.points())
+        super::details::local_point_cloud_aabb_ref(self.points())
     }
 }

src/bounding_volume/aabb_utils.rs

@@ -54,17 +54,25 @@ where
     Aabb::new(Point::from(min), Point::from(max))
 }
 
-/// Computes the [`Aabb`] of a set of points transformed by `m`.
-pub fn point_cloud_aabb<'a, I>(m: &Isometry<Real>, pts: I) -> Aabb
+/// Computes the [`Aabb`] of a set of point references transformed by `m`.
+pub fn point_cloud_aabb_ref<'a, I>(m: &Isometry<Real>, pts: I) -> Aabb
 where
     I: IntoIterator<Item = &'a Point<Real>>,
+{
+    point_cloud_aabb(m, pts.into_iter().copied())
+}
+
+/// Computes the [`Aabb`] of a set of points transformed by `m`.
+pub fn point_cloud_aabb<I>(m: &Isometry<Real>, pts: I) -> Aabb
+where
+    I: IntoIterator<Item = Point<Real>>,
 {
     let mut it = pts.into_iter();
 
     let p0 = it.next().expect(
         "Point cloud Aabb construction: the input iterator should yield at least one point.",
     );
-    let wp0 = m.transform_point(p0);
+    let wp0 = m.transform_point(&p0);
     let mut min: Point<Real> = wp0;
     let mut max: Point<Real> = wp0;
 
@@ -78,21 +86,29 @@ where
 }
 
 /// Computes the [`Aabb`] of a set of points.
-pub fn local_point_cloud_aabb<'a, I>(pts: I) -> Aabb
+pub fn local_point_cloud_aabb_ref<'a, I>(pts: I) -> Aabb
 where
     I: IntoIterator<Item = &'a Point<Real>>,
+{
+    local_point_cloud_aabb(pts.into_iter().copied())
+}
+
+/// Computes the [`Aabb`] of a set of points.
+pub fn local_point_cloud_aabb<I>(pts: I) -> Aabb
+where
+    I: IntoIterator<Item = Point<Real>>,
 {
     let mut it = pts.into_iter();
 
     let p0 = it.next().expect(
         "Point cloud Aabb construction: the input iterator should yield at least one point.",
     );
-    let mut min: Point<Real> = *p0;
-    let mut max: Point<Real> = *p0;
+    let mut min: Point<Real> = p0;
+    let mut max: Point<Real> = p0;
 
     for pt in it {
-        min = min.inf(pt);
-        max = max.sup(pt);
+        min = min.inf(&pt);
+        max = max.sup(&pt);
     }
 
     Aabb::new(min, max)

src/bounding_volume/mod.rs

@@ -65,6 +65,9 @@ mod simd_aabb;
 pub mod details {
     #[cfg(feature = "dim3")]
     pub use super::aabb_utils::support_map_aabb;
-    pub use super::aabb_utils::{local_point_cloud_aabb, local_support_map_aabb, point_cloud_aabb};
+    pub use super::aabb_utils::{
+        local_point_cloud_aabb, local_point_cloud_aabb_ref, local_support_map_aabb,
+        point_cloud_aabb, point_cloud_aabb_ref,
+    };
     pub use super::bounding_sphere_utils::point_cloud_bounding_sphere;
 }