Add nearest iterator

src/bvh/bvh_impl.rs

@@ -231,12 +231,70 @@ impl<T: BHValue, const D: usize> Bvh<T, D> {
         }
 
         let mut best_candidate = None;
-        BvhNode::nearest_to_recursive(&self.nodes, 0, origin, shapes, &mut best_candidate);
+        let mut best_distance = T::max_value();
+        let mut closure = |(shape, dist): (&'a Shape, T), best_distance: &mut T| {
+            if dist.lt(best_distance) {
+                *best_distance = dist;
+                best_candidate.replace((shape, dist));
+            }
+        };
+
+        BvhNode::nearest_to_recursive(
+            &self.nodes,
+            0,
+            origin,
+            shapes,
+            &mut best_distance,
+            &mut closure,
+        );
 
         // Return the best shape and its distance. We had a distance squared previously.
         best_candidate.map(|best| (best.0, best.1.sqrt()))
     }
 
+    /// Traverses the [`Bvh`].
+    ///
+    /// Returns a collection of the nearest shapes and their distances.
+    /// The collection is not sorted to prevent `T: Ord` constraint.
+    ///
+    /// [`Bvh`]: struct.Bvh.html
+    /// [`Aabb`]: ../aabb/struct.Aabb.html
+    ///
+    pub fn all_nearest_to<'a, Shape: Bounded<T, D> + PointDistance<T, D>>(
+        &self,
+        origin: nalgebra::Point<T, D>,
+        shapes: &'a [Shape],
+    ) -> Vec<(&'a Shape, T)>
+    where
+        Self: marker::Sized,
+    {
+        let mut candidates = Vec::new();
+
+        if self.nodes.is_empty() {
+            return candidates;
+        }
+
+        let mut best_distance = T::max_value();
+        let mut closure = |(shape, dist): (&'a Shape, T), _: &mut T| {
+            candidates.push((shape, dist));
+        };
+
+        BvhNode::nearest_to_recursive(
+            &self.nodes,
+            0,
+            origin,
+            shapes,
+            &mut best_distance,
+            &mut closure,
+        );
+
+        // Return the best shape and its distance. We had a distance squared previously.
+        for (_, d) in candidates.as_mut_slice() {
+            *d = d.sqrt();
+        }
+        candidates
+    }
+
     /// Prints the [`Bvh`] in a tree-like visualization.
     ///
     /// [`Bvh`]: struct.Bvh.html
@@ -549,8 +607,8 @@ mod tests {
     use crate::{
         bounding_hierarchy::BoundingHierarchy,
         testbase::{
-            TBvh3, TBvhNode3, TPoint3, TRay3, TVector3, UnitBox, build_empty_bh, build_some_bh,
-            nearest_to_some_bh, traverse_some_bh,
+            TBvh3, TBvhNode3, TPoint3, TRay3, TVector3, Triangle, UnitBox, build_empty_bh,
+            build_some_bh, nearest_to_some_bh, traverse_some_bh,
         },
     };
 
@@ -585,6 +643,43 @@ mod tests {
         nearest_to_some_bh::<TBvh3>();
     }
 
+    #[test]
+    fn test_all_nearest_to_bvh() {
+        let mut tris = [
+            //
+            ([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]),
+            ([0.0, 0.0, 2.0], [1.0, 0.0, 2.0], [0.0, 1.0, 2.0]),
+            ([0.0, 0.0, 4.0], [1.0, 0.0, 4.0], [0.0, 1.0, 4.0]),
+        ]
+        .into_iter()
+        .map(|(a, b, c)| (TPoint3::from(a), TPoint3::from(b), TPoint3::from(c)))
+        .map(|(a, b, c)| Triangle::new(a, b, c))
+        .collect::<alloc::vec::Vec<_>>();
+
+        let bh = TBvh3::build(&mut tris);
+
+        let mut shapes = bh.all_nearest_to(
+            // Query near the last triangle
+            TPoint3::new(0.5, 0.5, 4.0),
+            &mut tris,
+        );
+
+        // Sort by distance.
+        shapes.sort_by(|(_, d_lf), (_, d_rt)| d_lf.total_cmp(d_rt));
+
+        assert_eq!(shapes.len(), 3);
+
+        let nearest = shapes.first().map(|(s, _)| s).unwrap();
+        assert_eq!(nearest.a, TPoint3::new(0.0, 0.0, 4.0));
+        assert_eq!(nearest.b, TPoint3::new(1.0, 0.0, 4.0));
+        assert_eq!(nearest.c, TPoint3::new(0.0, 1.0, 4.0));
+
+        let furthest = shapes.last().map(|(s, _)| s).unwrap();
+        assert_eq!(furthest.a, TPoint3::new(0.0, 0.0, 0.0));
+        assert_eq!(furthest.b, TPoint3::new(1.0, 0.0, 0.0));
+        assert_eq!(furthest.c, TPoint3::new(0.0, 1.0, 0.0));
+    }
+
     #[test]
     /// Verify contents of the bounding hierarchy for a fixed scene structure
     fn test_bvh_shape_indices() {

src/bvh/bvh_node.rs

@@ -324,12 +324,17 @@ impl<T: BHValue, const D: usize> BvhNode<T, D> {
     /// [`Aabb`]: ../aabb/struct.Aabb.html
     /// [`Bvh`]: struct.Bvh.html
     ///
-    pub(crate) fn nearest_to_recursive<'a, Shape: Bounded<T, D> + PointDistance<T, D>>(
+    pub(crate) fn nearest_to_recursive<
+        'a,
+        Shape: Bounded<T, D> + PointDistance<T, D>,
+        Update: FnMut((&'a Shape, T), &mut T) -> (),
+    >(
         nodes: &[BvhNode<T, D>],
         node_index: usize,
         query: nalgebra::Point<T, D>,
         shapes: &'a [Shape],
-        best_candidate: &mut Option<(&'a Shape, T)>,
+        best_distance: &mut T,
+        update: &mut Update,
     ) {
         match nodes[node_index] {
             BvhNode::Node {
@@ -352,23 +357,22 @@ impl<T: BHValue, const D: usize> BvhNode<T, D> {
 
                 // Traverse children
                 for (index, child_dist) in children {
-                    // Node might contain a better shape: check it.
-                    // TODO: to be replaced by `Option::is_none_or` after 2025-10 for 1 year MSRV.
-                    #[allow(clippy::unnecessary_map_or)]
-                    if best_candidate.map_or(true, |(_, best_dist)| child_dist < best_dist) {
-                        Self::nearest_to_recursive(nodes, index, query, shapes, best_candidate);
+                    if child_dist.lt(best_distance) {
+                        Self::nearest_to_recursive(
+                            nodes,
+                            index,
+                            query,
+                            shapes,
+                            best_distance,
+                            update,
+                        );
                     }
                 }
             }
             BvhNode::Leaf { shape_index, .. } => {
                 // This leaf might contain a better shape: check it directly with its exact distance (squared).
                 let dist = shapes[shape_index].distance_squared(query);
-
-                // TODO: to be replaced by `Option::is_none_or` after 2025-10 for 1 year MSRV.
-                #[allow(clippy::unnecessary_map_or)]
-                if best_candidate.map_or(true, |(_, best_dist)| dist < best_dist) {
-                    *best_candidate = Some((&shapes[shape_index], dist));
-                }
+                update((&shapes[shape_index], dist), best_distance);
             }
         }
     }