ifc_lite_geometry/kernel/broadphase/
nearest.rs

1// This Source Code Form is subject to the terms of the Mozilla Public
2// License, v. 2.0. If a copy of the MPL was not distributed with this
3// file, You can obtain one at https://mozilla.org/MPL/2.0/.
4use super::{aabb_contains, Aabb, Bvh};
5
6impl Bvh {
7    /// Nearest leaf within a finite radius, visiting the nearer child first and
8    /// shrinking the conservative query radius after each exact leaf distance.
9    /// Retains candidates in the final nearest-distance plus ambiguity band;
10    /// their exact distances let the caller reject near ties without a second walk.
11    /// The callback must return the squared distance to that leaf's primitive.
12    /// Equal distances retain the original left-to-right candidate order.
13    /// Every node and callback consumes the caller's aggregate work budget;
14    /// exhaustion returns an error, never a partial nearest result.
15    pub fn nearest_point_bounded(
16        &self,
17        point: [f64; 3],
18        radius: f64,
19        remaining: &mut usize,
20        ambiguity: f64,
21        candidates: &mut Vec<(u32, f64)>,
22        mut distance_squared: impl FnMut(u32) -> f64,
23    ) -> Result<Option<(u32, f64)>, &'static str> {
24        if !point.iter().all(|v| v.is_finite()) || !radius.is_finite() || radius < 0.
25            || !ambiguity.is_finite() || ambiguity < 0. || !(radius + ambiguity).is_finite() {
26            return Err("BVH query requires a finite point and nonnegative radius");
27        }
28        candidates.clear();
29        if self.root == u32::MAX { return Ok(None); }
30        let mut pending = [0_u32; 64];
31        pending[0] = self.root;
32        let mut count = 1;
33        let mut best: Option<(u32, u32, f64)> = None;
34        let mut limit = radius + ambiguity;
35        while count > 0 {
36            count -= 1;
37            let index = pending[count];
38            charge(remaining)?;
39            let node = &self.nodes[index as usize];
40            if !aabb_contains(point, &node.aabb, self.pad + limit) { continue; }
41            if node.tri != u32::MAX {
42                charge(remaining)?;
43                let d2 = distance_squared(node.tri);
44                if !d2.is_finite() || d2 < 0. {
45                    return Err("BVH primitive distance must be finite and nonnegative");
46                }
47                if d2.sqrt() > limit { continue; }
48                candidates.push((node.tri, d2));
49                if best.is_none_or(|(_, old_index, old)| d2 < old || (d2 == old && index < old_index)) {
50                    best = Some((node.tri, index, d2));
51                    limit = d2.sqrt() + ambiguity;
52                }
53            } else {
54                if count + 2 > pending.len() { return Err("BVH query stack budget exceeded"); }
55                let left = box_distance(point, &self.nodes[node.left as usize].aabb);
56                let right = box_distance(point, &self.nodes[node.right as usize].aabb);
57                // Ordering only; pruning still uses the existing padded predicate.
58                let (near, far) = if left <= right { (node.left, node.right) } else { (node.right, node.left) };
59                pending[count] = far;
60                pending[count + 1] = near;
61                count += 2;
62            }
63        }
64        *remaining = remaining.checked_sub(candidates.len()).ok_or("BVH query work budget exhausted")?;
65        candidates.retain(|(_, d2)| d2.sqrt() <= limit);
66        Ok(best.filter(|(_, _, d2)| d2.sqrt() <= radius).map(|(id, _, d2)| (id, d2)))
67    }
68}
69fn charge(remaining: &mut usize) -> Result<(), &'static str> {
70    *remaining = remaining.checked_sub(1).ok_or("BVH query work budget exhausted")?;
71    Ok(())
72}
73fn box_distance(point: [f64; 3], bounds: &Aabb) -> f64 {
74    (0..3).map(|axis| {
75        let d = (bounds.0[axis] - point[axis]).max(point[axis] - bounds.1[axis]).max(0.);
76        d * d
77    }).sum()
78}
79
80#[cfg(test)]
81mod tests {
82    use super::*;
83    fn d2(a: [f64; 3], b: [f64; 3]) -> f64 {
84        (0..3).map(|axis| (a[axis] - b[axis]).powi(2)).sum()
85    }
86    #[test]
87    fn transfer_4381_nearest_and_band_match_independent_distances() {
88        let points: Vec<_> = (0..125).map(|i| [f64::from(i % 5), f64::from((i / 5) % 5), f64::from(i / 25)]).collect();
89        let tree = Bvh::build(&points.iter().map(|p| [*p; 3]).collect::<Vec<_>>());
90        for query in [[0.1, 0.2, 0.3], [2.5, 2.5, 2.5], [-2., 1., 3.], [4., 4., 4.]] {
91            let mut order = Vec::new();
92            tree.point_candidates(query, 10., &mut order);
93            let mut expected = None;
94            for id in order {
95                let distance = d2(points[id as usize], query);
96                if expected.is_none_or(|(_, old)| distance < old) { expected = Some((id, distance)); }
97            }
98            let mut candidates = Vec::new();
99            let actual = tree.nearest_point_bounded(query, 10., &mut 1000, 0.4, &mut candidates,
100                |id| d2(points[id as usize], query)).unwrap();
101            assert_eq!(actual, expected, "nearest and original left-to-right tie precedence");
102            let nearest = expected.unwrap().1.sqrt();
103            let expected_band: Vec<_> = points.iter().enumerate().filter_map(|(id, point)| {
104                let distance = d2(*point, query);
105                (distance.sqrt() <= nearest + 0.4).then_some((id as u32, distance))
106            }).collect();
107            candidates.sort_by_key(|(id, _)| *id);
108            assert_eq!(candidates, expected_band, "no near-tie observation may be pruned");
109        }
110    }
111    #[test]
112    fn transfer_4381_nearest_refuses_budget_and_invalid_distance() {
113        let tree = Bvh::build(&[[[0.; 3]; 3], [[2.; 3]; 3]]);
114        assert!(tree.nearest_point_bounded([0.; 3], 1., &mut 0, 0., &mut Vec::new(), |_| 0.).is_err());
115        assert!(tree.nearest_point_bounded([0.; 3], 1., &mut 100, 0., &mut Vec::new(), |_| f64::NAN).is_err());
116        assert!(tree.nearest_point_bounded([0.; 3], 1., &mut 100, -1., &mut Vec::new(), |_| 0.).is_err());
117        assert_eq!(tree.nearest_point_bounded([8.; 3], 1., &mut 100, 0., &mut Vec::new(), |_| 108.).unwrap(), None);
118        assert_eq!(Bvh::build(&[]).nearest_point_bounded([0.; 3], 1., &mut 0, 0., &mut Vec::new(), |_| 0.).unwrap(), None);
119    }
120}
ifc_lite_geometry/kernel/broadphase/nearest.rs

ifc_lite_geometry/kernel/broadphase/
nearest.rs
