diff --git a/src/bvh/bvh_impl.rs b/src/bvh/bvh_impl.rs index 6bdc9b9..d194627 100644 --- a/src/bvh/bvh_impl.rs +++ b/src/bvh/bvh_impl.rs @@ -231,12 +231,70 @@ impl Bvh { } 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 + PointDistance>( + &self, + origin: nalgebra::Point, + 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::(); } + #[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::>(); + + 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() { diff --git a/src/bvh/bvh_node.rs b/src/bvh/bvh_node.rs index a00be06..4224315 100644 --- a/src/bvh/bvh_node.rs +++ b/src/bvh/bvh_node.rs @@ -324,12 +324,17 @@ impl BvhNode { /// [`Aabb`]: ../aabb/struct.Aabb.html /// [`Bvh`]: struct.Bvh.html /// - pub(crate) fn nearest_to_recursive<'a, Shape: Bounded + PointDistance>( + pub(crate) fn nearest_to_recursive< + 'a, + Shape: Bounded + PointDistance, + Update: FnMut((&'a Shape, T), &mut T) -> (), + >( nodes: &[BvhNode], node_index: usize, query: nalgebra::Point, 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 BvhNode { // 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); } } }