use std::collections::{HashMap, HashSet};
use std::num::NonZeroUsize;
use kiddo::{ImmutableKdTree, SquaredEuclidean};
use crate::Point;
pub struct KdTree {
tree: ImmutableKdTree<f64, 3>,
}
impl KdTree {
pub fn new(points: &[Point]) -> Self {
let tree = if points.is_empty() {
ImmutableKdTree::new_from_slice(&[[f64::NAN; 3]]).expect("kd-tree build")
} else {
ImmutableKdTree::new_from_slice(points).expect("kd-tree build")
};
KdTree { tree }
}
pub fn knn(&self, p: &Point, k: usize) -> Vec<(usize, f64)> {
let Some(k) = NonZeroUsize::new(k) else { return Vec::new() };
self.tree
.query(p)
.nearest_n::<SquaredEuclidean<f64>>(k)
.execute()
.into_iter()
.filter(|r| r.distance.is_finite())
.map(|r| (r.item as usize, r.distance.sqrt()))
.collect()
}
pub fn knn_by_index(&self, p: &Point, k: usize) -> Vec<(usize, f64)> {
let mut nb = self.knn(p, k);
if let Some(&(_, dk)) = nb.last().filter(|_| nb.len() == k) {
nb = self.within(p, dk * (1.0 + 1e-9) + f64::MIN_POSITIVE).into_iter().filter(|&(_, d)| d <= dk).collect();
}
nb.sort_by(|a, b| a.1.total_cmp(&b.1).then(a.0.cmp(&b.0)));
nb.truncate(k);
nb
}
pub fn nearest(&self, p: &Point) -> Option<(usize, f64)> {
let r = self.tree.query(p).nearest_one::<SquaredEuclidean<f64>>().execute();
r.distance.is_finite().then(|| (r.item as usize, r.distance.sqrt()))
}
pub fn within(&self, p: &Point, radius: f64) -> Vec<(usize, f64)> {
self.tree
.query(p)
.within::<SquaredEuclidean<f64>>(radius * radius)
.unsorted()
.execute()
.into_iter()
.filter(|r| r.distance.is_finite())
.map(|r| (r.item as usize, r.distance.sqrt()))
.collect()
}
pub fn count_within(&self, p: &Point, radius: f64) -> usize {
self.within(p, radius).len()
}
}
pub type VoxelKey = [i64; 3];
#[inline]
pub fn voxel_key(p: &Point, origin: &Point, size: f64) -> VoxelKey {
[
((p[0] - origin[0]) / size).floor() as i64,
((p[1] - origin[1]) / size).floor() as i64,
((p[2] - origin[2]) / size).floor() as i64,
]
}
pub fn voxel_groups(points: &[Point], origin: &Point, size: f64) -> (Vec<VoxelKey>, Vec<Vec<usize>>) {
let mut map: HashMap<VoxelKey, usize> = HashMap::new();
let mut keys = Vec::new();
let mut groups: Vec<Vec<usize>> = Vec::new();
for (i, p) in points.iter().enumerate() {
let k = voxel_key(p, origin, size);
let g = *map.entry(k).or_insert_with(|| {
keys.push(k);
groups.push(Vec::new());
groups.len() - 1
});
groups[g].push(i);
}
(keys, groups)
}
pub fn first_in_voxel(points: &[Point], idx: impl IntoIterator<Item = usize>, origin: &Point, size: f64) -> Vec<usize> {
let mut seen: HashSet<VoxelKey> = HashSet::new();
idx.into_iter().filter(|&i| seen.insert(voxel_key(&points[i], origin, size))).collect()
}
pub fn min_corner_of(points: &[Point], idx: &[usize]) -> Point {
let mut lo = [f64::INFINITY; 3];
for &i in idx {
for k in 0..3 {
lo[k] = lo[k].min(points[i][k]);
}
}
if idx.is_empty() {
[0.0; 3]
} else {
lo
}
}
pub fn min_corner(points: &[Point]) -> Point {
let mut lo = [f64::INFINITY; 3];
for p in points {
for k in 0..3 {
lo[k] = lo[k].min(p[k]);
}
}
if points.is_empty() {
[0.0; 3]
} else {
lo
}
}
pub fn max_corner(points: &[Point]) -> Point {
let mut hi = [f64::NEG_INFINITY; 3];
for p in points {
for k in 0..3 {
hi[k] = hi[k].max(p[k]);
}
}
if points.is_empty() {
[0.0; 3]
} else {
hi
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn knn_by_index_breaks_ties_by_index_in_any_subset() {
let pts: Vec<Point> = (0..1000).map(|i| [(i % 10) as f64, ((i / 10) % 10) as f64, (i / 100) as f64]).collect();
let whole = KdTree::new(&pts);
let keep: Vec<usize> = (0..pts.len()).filter(|&i| pts[i][0] < 7.0).collect();
let sub_pts: Vec<Point> = keep.iter().map(|&i| pts[i]).collect();
let sub = KdTree::new(&sub_pts);
for (j, &i) in keep.iter().enumerate() {
if pts[i][0] > 4.0 {
continue;
}
let a = whole.knn_by_index(&pts[i], 7);
let b: Vec<(usize, f64)> = sub.knn_by_index(&sub_pts[j], 7).into_iter().map(|(m, d)| (keep[m], d)).collect();
assert_eq!(a, b, "point {i}");
assert!(a.windows(2).all(|w| w[0].1 < w[1].1 || (w[0].1 == w[1].1 && w[0].0 < w[1].0)));
}
assert!(whole.knn_by_index(&pts[0], 0).is_empty());
assert_eq!(KdTree::new(&pts[..3]).knn_by_index(&pts[0], 7).len(), 3);
}
}