use crate::Result;
use crate::common::{IndexMask, PCoords};
use kiddo::{ImmutableKdTree, SquaredEuclidean};
use std::num::NonZeroUsize;
pub trait KdTreeSearch<const D: usize> {
fn nearest_one(&self, point: &impl PCoords<D>) -> (usize, f64);
fn nearest(&self, point: &impl PCoords<D>, count: usize) -> Vec<(usize, f64)>;
fn within(&self, point: &impl PCoords<D>, radius: f64) -> Vec<(usize, f64)>;
fn len(&self) -> usize;
fn is_empty(&self) -> bool {
self.len() == 0
}
}
pub struct KdTree<const D: usize> {
tree: ImmutableKdTree<f64, D>,
len: usize,
}
impl<const D: usize> KdTree<D> {
pub fn try_new(points: &[impl PCoords<D>]) -> Result<Self> {
let entries: Vec<[f64; D]> = points.iter().map(|p| p.coords().into()).collect();
if entries.is_empty() {
return Ok(Self {
tree: ImmutableKdTree::new_from_slice(&[])
.map_err(|e| format!("Failed to build an empty k-d tree: {e}"))?,
len: 0,
});
}
let tree = ImmutableKdTree::new_from_slice(&entries).map_err(|e| {
format!(
"Failed to build a k-d tree over {} points: {e}",
entries.len()
)
})?;
Ok(Self {
tree,
len: entries.len(),
})
}
fn key(point: &impl PCoords<D>) -> [f64; D] {
point.coords().into()
}
}
impl<const D: usize> KdTreeSearch<D> for KdTree<D> {
fn nearest_one(&self, point: &impl PCoords<D>) -> (usize, f64) {
if self.len == 0 {
return (usize::MAX, f64::INFINITY);
}
let found = self
.tree
.query(&Self::key(point))
.nearest_one::<SquaredEuclidean<f64>>()
.execute();
(found.item as usize, found.distance.sqrt())
}
fn nearest(&self, point: &impl PCoords<D>, count: usize) -> Vec<(usize, f64)> {
let Some(count) = NonZeroUsize::new(count.min(self.len)) else {
return Vec::new();
};
self.tree
.query(&Self::key(point))
.nearest_n::<SquaredEuclidean<f64>>(count)
.execute()
.into_iter()
.map(|f| (f.item as usize, f.distance.sqrt()))
.collect()
}
fn within(&self, point: &impl PCoords<D>, radius: f64) -> Vec<(usize, f64)> {
if self.len == 0 {
return Vec::new();
}
self.tree
.query(&Self::key(point))
.within::<SquaredEuclidean<f64>>(radius * radius)
.execute()
.into_iter()
.map(|f| (f.item as usize, f.distance.sqrt()))
.collect()
}
fn len(&self) -> usize {
self.len
}
}
pub struct PartialKdTree<const D: usize> {
tree: KdTree<D>,
index_map: Vec<usize>,
}
impl<const D: usize> PartialKdTree<D> {
pub fn try_new(all_points: &[impl PCoords<D>], mask: &IndexMask) -> Result<Self> {
if mask.len() != all_points.len() {
panic!("Mask length must match the length of all_points");
}
let mut points = Vec::new();
let mut index_map = Vec::new();
for i in mask.iter_true() {
points.push(all_points[i].coords());
index_map.push(i);
}
let tree = KdTree::try_new(&points)?;
Ok(Self { tree, index_map })
}
}
impl<const D: usize> KdTreeSearch<D> for PartialKdTree<D> {
fn nearest_one(&self, point: &impl PCoords<D>) -> (usize, f64) {
let (i, d) = self.tree.nearest_one(point);
(self.index_map[i], d)
}
fn nearest(&self, point: &impl PCoords<D>, count: usize) -> Vec<(usize, f64)> {
let result = self.tree.nearest(point, count);
result
.iter()
.map(|(i, d)| (self.index_map[*i], *d))
.collect::<Vec<_>>()
}
fn within(&self, point: &impl PCoords<D>, radius: f64) -> Vec<(usize, f64)> {
let result = self.tree.within(point, radius);
result
.iter()
.map(|(i, d)| (self.index_map[*i], *d))
.collect::<Vec<_>>()
}
fn len(&self) -> usize {
self.tree.len()
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::indices::index_vec;
use crate::common::points::dist;
use crate::tests::stanford_bun_2;
use crate::{Point2, Point3};
use approx::assert_relative_eq;
use rand::prelude::SliceRandom;
#[test]
fn kd_tree_build() {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 1.0),
Point2::new(2.0, 2.0),
];
let tree = KdTree::try_new(&points).expect("KD tree creation failed");
assert_eq!(tree.len(), 3);
}
#[test]
fn kd_tree_nearest() {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 1.0),
Point2::new(2.0, 2.0),
];
let tree = KdTree::try_new(&points).expect("KD tree creation failed");
let (i, d) = tree.nearest_one(&Point2::new(1.25, 1.25));
assert_eq!(i, 1);
assert_relative_eq!(d, (0.25 * 0.25 * 2.0_f64).sqrt(), epsilon = 1e-6);
}
#[test]
fn kd_tree_check_distances_within() {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 0.0),
Point2::new(2.0, 0.0),
];
let tree = KdTree::try_new(&points).expect("KD tree creation failed");
let within = tree.within(&Point2::new(3.5, 0.0), 2.0);
assert_eq!(within.len(), 1);
assert_eq!(within[0].0, 2);
assert_relative_eq!(within[0].1, 1.5, epsilon = 1e-6);
}
#[test]
fn partial_kd_tree_maps() {
let points = (0..20)
.flat_map(|i| (0..20).map(move |j| Point2::new(i as f64, j as f64)))
.collect::<Vec<_>>();
let fixed_tree = KdTree::try_new(&points).expect("KD tree creation failed");
for _ in 0..1000 {
let mut test_select = index_vec(None, points.len());
test_select.shuffle(&mut rand::rng());
let mut mask = IndexMask::new(points.len(), false);
for &i in test_select.iter().take(points.len() / 2) {
mask.set(i, true);
}
let indices = mask.to_indices();
let partial_tree =
PartialKdTree::try_new(&points, &mask).expect("KD tree creation failed");
for &i in indices.iter() {
let p = &points[i];
let (j, _d) = fixed_tree.nearest_one(p);
let (k, e) = partial_tree.nearest_one(p);
assert_eq!(i, j);
assert_eq!(i, k);
assert_relative_eq!(0.0, e, epsilon = 1e-6);
assert_relative_eq!(0.0, e, epsilon = 1e-6);
}
}
}
#[test]
fn partial_kd_tree_nearest() -> Result<()> {
let mesh = stanford_bun_2();
let even = (0..mesh.points().len()).step_by(2).collect::<Vec<_>>();
let mask = IndexMask::try_from_indices(&even, mesh.points().len())?;
let reduced_points = mask.clone_indices_of(mesh.points())?;
let reduced_tree = KdTree::try_new(&reduced_points)?;
let partial_tree = PartialKdTree::try_new(mesh.points(), &mask)?;
let n = 3;
for p in mesh.points().iter() {
let mut result = partial_tree
.nearest(p, n)
.iter()
.map(|x| x.0)
.collect::<Vec<_>>();
let mut reduced_result = reduced_tree
.nearest(p, n)
.iter()
.map(|x| x.0)
.collect::<Vec<_>>();
result.sort();
reduced_result.sort();
assert_eq!(result.len(), reduced_result.len());
for (a, b) in result.iter().zip(reduced_result.iter()) {
assert_relative_eq!(mesh.points()[*a], reduced_points[*b], epsilon = 1e-6);
}
}
Ok(())
}
#[test]
fn kd_tree_nearest_one_matches_brute_force_on_stanford_bun_vertices() {
let mesh = stanford_bun_2();
let vertices = mesh.points().to_vec();
let tree = KdTree::try_new(&vertices).expect("KD tree creation failed");
for (expected_index, query) in vertices.iter().enumerate() {
let (actual_index, actual_distance) = tree.nearest_one(query);
assert_eq!(actual_index, expected_index);
assert_relative_eq!(actual_distance, 0.0, epsilon = 1e-6);
let expected = brute_force_nearest_n(&vertices, query, 1);
assert_eq!(expected.len(), 1);
assert_eq!(expected[0].0, expected_index);
assert_relative_eq!(expected[0].1, 0.0, epsilon = 1e-6);
}
}
#[test]
fn kd_tree_nearest_matches_brute_force_on_stanford_bun_vertices() {
let mesh = stanford_bun_2();
let vertices = mesh.points().to_vec();
let tree = KdTree::try_new(&vertices).expect("KD tree creation failed");
for query in &vertices {
let expected = brute_force_nearest_n(&vertices, query, 3);
let actual = tree.nearest(query, 3);
assert_eq!(actual.len(), expected.len());
for ((actual_index, actual_distance), (expected_index, expected_distance)) in
actual.iter().zip(expected.iter())
{
assert_eq!(actual_index, expected_index);
assert_relative_eq!(actual_distance, expected_distance, epsilon = 1e-6);
}
}
}
#[test]
fn kd_tree_within_matches_brute_force_on_stanford_bun_vertices() {
let mesh = stanford_bun_2();
let vertices = mesh.points().to_vec();
let tree = KdTree::try_new(&vertices).expect("KD tree creation failed");
let search_dist = mesh.aabb().extents().x * 0.1;
for query in vertices.iter() {
let expected = vertices
.iter()
.filter(|p| dist(query, *p) < search_dist)
.count();
let tested = tree.within(query, search_dist).len();
assert_eq!(tested, expected);
}
}
#[test]
fn kd_tree_within_matches_brute_force_at_scale() {
const N: usize = 500_000;
const QUERIES: usize = 100;
let points = fibonacci_sphere(N, 100.0);
let tree = KdTree::try_new(&points).expect("KD tree creation failed");
let radius = 30.0 * 200.0 / (N as f64).sqrt();
for q in (0..QUERIES).map(|i| points[(i * (N / QUERIES)) % N]) {
let mut expected: Vec<usize> = points
.iter()
.enumerate()
.filter(|(_, p)| dist(&q, *p) <= radius)
.map(|(i, _)| i)
.collect();
let mut actual: Vec<usize> = tree.within(&q, radius).iter().map(|(i, _)| *i).collect();
assert!(
expected.len() > 50,
"radius {radius} returned only {} neighbors, too few to be a meaningful check",
expected.len()
);
expected.sort_unstable();
actual.sort_unstable();
assert_eq!(
actual, expected,
"radius query missed or invented neighbors"
);
}
}
fn fibonacci_sphere(n: usize, radius: f64) -> Vec<Point3> {
let golden = std::f64::consts::PI * (3.0 - 5.0_f64.sqrt());
(0..n)
.map(|i| {
let y = 1.0 - (i as f64 / (n as f64 - 1.0)) * 2.0;
let r = (1.0 - y * y).max(0.0).sqrt();
let theta = golden * i as f64;
Point3::new(
radius * theta.cos() * r,
radius * y,
radius * theta.sin() * r,
)
})
.collect()
}
pub fn brute_force_nearest_n(
points: &[Point3],
query: &Point3,
count: usize,
) -> Vec<(usize, f64)> {
let mut neighbors: Vec<(usize, f64)> = points
.iter()
.enumerate()
.map(|(i, p)| {
let dx = p.x - query.x;
let dy = p.y - query.y;
let dz = p.z - query.z;
let dist = (dx * dx + dy * dy + dz * dz).sqrt();
(i, dist)
})
.collect();
neighbors.sort_by(|a, b| a.1.total_cmp(&b.1));
neighbors.truncate(count);
neighbors
}
}