diff --git a/Cargo.lock b/Cargo.lock index 78f55b0c..99226e30 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -853,14 +853,17 @@ dependencies = [ name = "bvh-region" version = "0.1.0" dependencies = [ + "approx", "arrayvec", "criterion", + "derive_more", "fastrand 2.3.0", "geometry", "glam", "ordered-float", "plotters", "plotters-bitmap", + "proptest", "rand", "rayon", "tracing", diff --git a/Cargo.toml b/Cargo.toml index fc500883..234c7c21 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -95,6 +95,7 @@ papaya = '0.1.4' parking_lot = '0.12.3' plotters-bitmap = '0.3.6' proc-macro2 = '1.0.89' +proptest = "1.5.0" quote = '1.0.37' rand = '0.8.5' rayon = '1.10.0' diff --git a/crates/bvh-region/Cargo.toml b/crates/bvh-region/Cargo.toml index 8ae39329..7dca6aee 100644 --- a/crates/bvh-region/Cargo.toml +++ b/crates/bvh-region/Cargo.toml @@ -11,23 +11,23 @@ name = "sort" #name = "side_by_side" [dependencies] -geometry = {workspace = true} -glam = {workspace = true, features = ["serde"]} -plotters = {workspace = true, features = [ - "plotters-bitmap", - "image" -], optional = true} -plotters-bitmap = {workspace = true, optional = true} -arrayvec = {workspace = true} -fastrand = {workspace = true} -ordered-float = {workspace = true} -rayon = {workspace = true} -tracing = {workspace = true} +arrayvec = { workspace = true } +derive_more = { workspace = true } +fastrand = { workspace = true } +geometry = { workspace = true } +glam = { workspace = true, features = ["serde"] } +ordered-float = { workspace = true } +plotters = { workspace = true, features = ["plotters-bitmap", "image"], optional = true } +plotters-bitmap = { workspace = true, optional = true } +proptest = { workspace = true } +rayon = { workspace = true } +tracing = { workspace = true } [dev-dependencies] -criterion = {workspace = true} +approx = { workspace = true } +criterion = { workspace = true } +rand = { workspace = true } #divan = {workspace = true} -rand = {workspace = true} #tango-bench = {workspace = true} [features] diff --git a/crates/bvh-region/src/build.rs b/crates/bvh-region/src/build.rs new file mode 100644 index 00000000..483773b2 --- /dev/null +++ b/crates/bvh-region/src/build.rs @@ -0,0 +1,188 @@ +use std::fmt::Debug; + +use geometry::aabb::Aabb; + +use crate::{ + Bvh, ELEMENTS_TO_ACTIVATE_LEAF, VOLUME_TO_ACTIVATE_LEAF, node::BvhNode, sort_by_largest_axis, + utils, utils::GetAabb, +}; + +/// Used to find the start addresses of the elements and nodes arrays +#[derive(Debug)] +pub struct StartAddresses { + /// Base pointer to the start of the elements array + pub start_elements_ptr: *const T, + pub start_nodes_ptr: *const BvhNode, +} + +impl StartAddresses { + const fn element_start_index(&self, slice: &[T]) -> isize { + unsafe { slice.as_ptr().offset_from(self.start_elements_ptr) } + } + + const fn node_start_index(&self, slice: &[BvhNode]) -> isize { + unsafe { slice.as_ptr().offset_from(self.start_nodes_ptr) } + } +} + +unsafe impl Send for StartAddresses {} +unsafe impl Sync for StartAddresses {} + +impl Bvh +where + T: Debug + Send + Copy + Sync, +{ + #[tracing::instrument(skip_all, fields(elements_len = elements.len()))] + pub fn build(mut elements: Vec, get_aabb: (impl GetAabb + Sync)) -> Self { + let max_threads = utils::thread_count_pow2(); + + let len = elements.len(); + + // // 1.7 works too, 2.0 is upper bound ... 1.8 is probably best + // todo: make this more mathematically derived + let capacity = ((len / ELEMENTS_TO_ACTIVATE_LEAF) as f64 * 8.0) as usize; + + // [A] + let capacity = capacity.max(16); + + let mut nodes = vec![BvhNode::DUMMY; capacity]; + + let bvh = StartAddresses { + start_elements_ptr: elements.as_ptr(), + start_nodes_ptr: nodes.as_ptr(), + }; + + #[expect( + clippy::indexing_slicing, + reason = "Look at [A]. The length is at least 16, so this is safe." + )] + let nodes_slice = &mut nodes[1..]; + + let (root, _) = build_in(&bvh, &mut elements, max_threads, 0, nodes_slice, &get_aabb); + + Self { + nodes, + elements, + root, + } + } +} + +#[allow(clippy::float_cmp)] +pub fn build_in( + addresses: &StartAddresses, + elements: &mut [T], + max_threads: usize, + nodes_idx: usize, + nodes: &mut [BvhNode], + get_aabb: &(impl GetAabb + Sync), +) -> (i32, usize) +where + T: Send + Copy + Sync + Debug, +{ + // aabb that encompasses all elements + let aabb: Aabb = elements.iter().map(get_aabb).collect(); + let volume = aabb.volume(); + + if elements.len() <= ELEMENTS_TO_ACTIVATE_LEAF || volume <= VOLUME_TO_ACTIVATE_LEAF { + let idx_start = addresses.element_start_index(elements); + + let node = BvhNode::create_leaf(aabb, idx_start as usize, elements.len()); + + let set = &mut nodes[nodes_idx..=nodes_idx]; + set[0] = node; + + let idx = addresses.node_start_index(set); + + let idx = idx as i32; + + debug_assert!(idx > 0); + + return (idx, nodes_idx + 1); + } + + sort_by_largest_axis(elements, &aabb, get_aabb); + + let element_split_idx = elements.len() / 2; + + let (left_elems, right_elems) = elements.split_at_mut(element_split_idx); + + debug_assert!(max_threads != 0); + + let original_node_idx = nodes_idx; + + let (left, right, nodes_idx, to_set) = if max_threads == 1 { + let start_idx = nodes_idx; + let (left, nodes_idx) = build_in( + addresses, + left_elems, + max_threads, + nodes_idx + 1, + nodes, + get_aabb, + ); + + let (right, nodes_idx) = build_in( + addresses, + right_elems, + max_threads, + nodes_idx, + nodes, + get_aabb, + ); + let end_idx = nodes_idx; + + debug_assert!(start_idx != end_idx); + + ( + left, + right, + nodes_idx, + &mut nodes[original_node_idx..=original_node_idx], + ) + } else { + let max_threads = max_threads >> 1; + + let (to_set, nodes) = nodes.split_at_mut(1); + + let node_split_idx = nodes.len() / 2; + let (left_nodes, right_nodes) = match true { + true => { + let (left, right) = nodes.split_at_mut(node_split_idx); + (left, right) + } + false => { + let (right, left) = nodes.split_at_mut(node_split_idx); + (left, right) + } + }; + + let (left, right) = rayon::join( + || build_in(addresses, left_elems, max_threads, 0, left_nodes, get_aabb), + || { + build_in( + addresses, + right_elems, + max_threads, + 0, + right_nodes, + get_aabb, + ) + }, + ); + + (left.0, right.0, 0, to_set) + }; + + let node = BvhNode { aabb, left, right }; + + to_set[0] = node; + let idx = unsafe { to_set.as_ptr().offset_from(addresses.start_nodes_ptr) }; + let idx = idx as i32; + + // trace!("internal nodes_idx {:03}", original_node_idx); + + debug_assert!(idx > 0); + + (idx, nodes_idx + 1) +} diff --git a/crates/bvh-region/src/lib.rs b/crates/bvh-region/src/lib.rs index b0fff3f3..cf458a18 100644 --- a/crates/bvh-region/src/lib.rs +++ b/crates/bvh-region/src/lib.rs @@ -5,11 +5,7 @@ // https://www.haroldserrano.com/blog/visualizing-the-boundary-volume-hierarchy-collision-algorithm -use std::{ - cmp::Reverse, - collections::BinaryHeap, - fmt::{Debug, Formatter}, -}; +use std::fmt::Debug; use arrayvec::ArrayVec; use geometry::aabb::Aabb; @@ -18,43 +14,19 @@ use glam::Vec3; const ELEMENTS_TO_ACTIVATE_LEAF: usize = 16; const VOLUME_TO_ACTIVATE_LEAF: f32 = 5.0; -pub trait GetAabb: Fn(&T) -> Aabb {} +mod node; +use node::BvhNode; -impl GetAabb for F where F: Fn(&T) -> Aabb {} +use crate::utils::GetAabb; + +mod build; +mod query; +mod utils; #[cfg(feature = "plot")] pub mod plot; -#[derive(Debug, Copy, Clone, PartialEq)] -struct BvhNode { - aabb: Aabb, // f32 * 6 = 24 bytes - - // if positive then it is an internal node; if negative then it is a leaf node - left: i32, - right: i32, -} - -impl BvhNode { - #[allow(dead_code)] - const EMPTY_LEAF: Self = Self { - aabb: Aabb::NULL, - left: -1, - right: 0, - }; - - const fn create_leaf(aabb: Aabb, idx_left: usize, len: usize) -> Self { - let left = idx_left as i32; - let right = len as i32; - - let left = -left; - - let left = left - 1; - - Self { aabb, left, right } - } -} - -#[derive(Clone)] +#[derive(Debug, Clone)] pub struct Bvh { nodes: Vec, elements: Vec, @@ -77,186 +49,6 @@ impl Bvh { } } -impl Debug for Bvh { - fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - f.debug_struct("Bvh") - .field("nodes", &self.nodes) - .field("elems", &self.elements) - .field("root", &self.root) - .finish() - } -} - -struct BvhBuild { - start_elements_ptr: *const T, - start_nodes_ptr: *const BvhNode, -} - -impl Debug for BvhBuild { - fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - f.debug_struct("BvhBuild") - .field("start_elements_ptr", &self.start_elements_ptr) - .field("start_nodes_ptr", &self.start_nodes_ptr) - .finish() - } -} - -unsafe impl Send for BvhBuild {} -unsafe impl Sync for BvhBuild {} - -/// get number of threads that is pow of 2 -fn thread_count_pow2() -> usize { - let max_threads_tentative = rayon::current_num_threads(); - // let max - - // does not make sense to not have a power of two - let mut max_threads = max_threads_tentative.next_power_of_two(); - - if max_threads != max_threads_tentative { - max_threads >>= 1; - } - - max_threads -} - -impl Bvh { - #[tracing::instrument(skip_all, fields(elements_len = elements.len()))] - pub fn build(mut elements: Vec, get_aabb: &(impl GetAabb + Sync)) -> Self { - let max_threads = thread_count_pow2(); - - let len = elements.len(); - - // // 1.7 works too, 2.0 is upper bound ... 1.8 is probably best - let capacity = ((len / ELEMENTS_TO_ACTIVATE_LEAF) as f64 * 8.0) as usize; - - // [A] - let capacity = capacity.max(16); - - let mut nodes = vec![BvhNode::DUMMY; capacity]; - - let bvh = BvhBuild { - start_elements_ptr: elements.as_ptr(), - start_nodes_ptr: nodes.as_ptr(), - }; - - #[expect( - clippy::indexing_slicing, - reason = "Look at [A]. The length is at least 16, so this is safe." - )] - let nodes_slice = &mut nodes[1..]; - - let (root, _) = - BvhNode::build_in(&bvh, &mut elements, max_threads, 0, nodes_slice, get_aabb); - - Self { - nodes, - elements, - root, - } - } - - /// Returns the closest element to the target and the distance squared to it. - pub fn get_closest(&self, target: Vec3, get_aabb: &impl Fn(&T) -> Aabb) -> Option<(&T, f32)> { - struct NodeOrd<'a> { - node: &'a BvhNode, - dist2: f32, - } - - impl PartialEq for NodeOrd<'_> { - fn eq(&self, other: &Self) -> bool { - self.dist2 == other.dist2 - } - } - impl PartialOrd for NodeOrd<'_> { - fn partial_cmp(&self, other: &Self) -> Option { - Some(self.cmp(other)) - } - } - - impl Eq for NodeOrd<'_> {} - - impl Ord for NodeOrd<'_> { - fn cmp(&self, other: &Self) -> std::cmp::Ordering { - self.dist2.partial_cmp(&other.dist2).unwrap() - } - } - - let mut min_dist2 = f32::MAX; - let mut min_node = None; - - let on = self.root(); - - let on = match on { - Node::Internal(internal) => internal, - Node::Leaf(leaf) => { - return leaf - .iter() - .map(|elem| { - let aabb = get_aabb(elem); - let mid = aabb.mid(); - let dist2 = (mid - target).length_squared(); - - (elem, dist2) - }) - .min_by_key(|(_, dist)| dist.to_bits()); - } - }; - - // let mut stack: SmallVec<&BvhNode, 64> = SmallVec::new(); - let mut heap: BinaryHeap<_> = std::iter::once(on) - .map(|node| Reverse(NodeOrd { node, dist2: 0.0 })) - .collect(); - - while let Some(on) = heap.pop() { - let on = on.0.node; - let dist2 = on.aabb.dist2(target); - - if dist2 > min_dist2 { - break; - } - - for child in on.children(self) { - match child { - Node::Internal(internal) => { - heap.push(Reverse(NodeOrd { - node: internal, - dist2: internal.aabb.dist2(target), - })); - } - Node::Leaf(leaf) => { - let (elem, dist2) = leaf - .iter() - .map(|elem| { - let aabb = get_aabb(elem); - let mid = aabb.mid(); - let dist = (mid - target).length_squared(); - - (elem, dist) - }) - .min_by_key(|(_, dist)| dist.to_bits()) - .unwrap(); - - if dist2 < min_dist2 { - min_dist2 = dist2; - min_node = Some(elem); - } - } - } - } - } - - min_node.map(|elem| (elem, min_dist2)) - } - - pub fn get_collisions<'a>( - &'a self, - target: Aabb, - get_aabb: impl GetAabb + 'a, - ) -> impl Iterator + 'a { - BvhIter::consume(self, target, get_aabb) - } -} - impl Bvh { fn root(&self) -> Node<'_, T> { let root = self.root; @@ -403,111 +195,6 @@ impl BvhNode { &root.nodes[right as usize] } - - #[allow(clippy::float_cmp)] - fn build_in( - root: &BvhBuild, - elements: &mut [T], - max_threads: usize, - nodes_idx: usize, - nodes: &mut [Self], - get_aabb: &(impl GetAabb + Sync), - ) -> (i32, usize) - where - T: Send + Copy + Sync + Debug, - { - // aabb that encompasses all elements - let aabb: Aabb = elements.iter().map(get_aabb).collect(); - let volume = aabb.volume(); - - if elements.len() <= ELEMENTS_TO_ACTIVATE_LEAF || volume <= VOLUME_TO_ACTIVATE_LEAF { - // flush - let idx_start = unsafe { elements.as_ptr().offset_from(root.start_elements_ptr) }; - - let node = Self::create_leaf(aabb, idx_start as usize, elements.len()); - - let set = &mut nodes[nodes_idx..=nodes_idx]; - set[0] = node; - let idx = unsafe { set.as_ptr().offset_from(root.start_nodes_ptr) }; - - let idx = idx as i32; - - debug_assert!(idx > 0); - - return (idx, nodes_idx + 1); - } - - sort_by_largest_axis(elements, &aabb, get_aabb); - - let element_split_idx = elements.len() / 2; - - let (left_elems, right_elems) = elements.split_at_mut(element_split_idx); - - debug_assert!(max_threads != 0); - - let original_node_idx = nodes_idx; - - let (left, right, nodes_idx, to_set) = if max_threads == 1 { - let start_idx = nodes_idx; - let (left, nodes_idx) = Self::build_in( - root, - left_elems, - max_threads, - nodes_idx + 1, - nodes, - get_aabb, - ); - - let (right, nodes_idx) = - Self::build_in(root, right_elems, max_threads, nodes_idx, nodes, get_aabb); - let end_idx = nodes_idx; - - debug_assert!(start_idx != end_idx); - - ( - left, - right, - nodes_idx, - &mut nodes[original_node_idx..=original_node_idx], - ) - } else { - let max_threads = max_threads >> 1; - - let (to_set, nodes) = nodes.split_at_mut(1); - - let node_split_idx = nodes.len() / 2; - // todo: remove fastrand - let (left_nodes, right_nodes) = match true { - true => { - let (left, right) = nodes.split_at_mut(node_split_idx); - (left, right) - } - false => { - let (right, left) = nodes.split_at_mut(node_split_idx); - (left, right) - } - }; - - let (left, right) = rayon::join( - || Self::build_in(root, left_elems, max_threads, 0, left_nodes, get_aabb), - || Self::build_in(root, right_elems, max_threads, 0, right_nodes, get_aabb), - ); - - (left.0, right.0, 0, to_set) - }; - - let node = Self { aabb, left, right }; - - to_set[0] = node; - let idx = unsafe { to_set.as_ptr().offset_from(root.start_nodes_ptr) }; - let idx = idx as i32; - - // trace!("internal nodes_idx {:03}", original_node_idx); - - debug_assert!(idx > 0); - - (idx, nodes_idx + 1) - } } struct BvhIter<'a, T> { diff --git a/crates/bvh-region/src/node.rs b/crates/bvh-region/src/node.rs new file mode 100644 index 00000000..db36360b --- /dev/null +++ b/crates/bvh-region/src/node.rs @@ -0,0 +1,30 @@ +use geometry::aabb::Aabb; + +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct BvhNode { + pub aabb: Aabb, // f32 * 6 = 24 bytes + + // if positive then it is an internal node; if negative then it is a leaf node + pub left: i32, + pub right: i32, +} + +impl BvhNode { + #[allow(dead_code)] + const EMPTY_LEAF: Self = Self { + aabb: Aabb::NULL, + left: -1, + right: 0, + }; + + pub const fn create_leaf(aabb: Aabb, idx_left: usize, len: usize) -> Self { + let left = idx_left as i32; + let right = len as i32; + + let left = -left; + + let left = left - 1; + + Self { aabb, left, right } + } +} diff --git a/crates/bvh-region/src/query.rs b/crates/bvh-region/src/query.rs new file mode 100644 index 00000000..e9cc8996 --- /dev/null +++ b/crates/bvh-region/src/query.rs @@ -0,0 +1 @@ +mod closest; diff --git a/crates/bvh-region/src/query/closest.rs b/crates/bvh-region/src/query/closest.rs new file mode 100644 index 00000000..c32af02f --- /dev/null +++ b/crates/bvh-region/src/query/closest.rs @@ -0,0 +1,84 @@ +use std::{cmp::Reverse, collections::BinaryHeap, fmt::Debug}; + +use geometry::aabb::Aabb; +use glam::Vec3; + +use crate::{ + Bvh, BvhIter, Node, + utils::{GetAabb, NodeOrd}, +}; + +impl Bvh { + /// Returns the closest element to the target and the distance squared to it. + pub fn get_closest(&self, target: Vec3, get_aabb: impl Fn(&T) -> Aabb) -> Option<(&T, f64)> { + let mut min_dist2 = f64::INFINITY; + let mut min_node = None; + + let on = self.root(); + + let on = match on { + Node::Internal(internal) => internal, + Node::Leaf(leaf) => { + return leaf + .iter() + .map(|elem| { + let aabb = get_aabb(elem); + let dist2 = aabb.dist2(target); + (elem, dist2) + }) + .min_by_key(|(_, dist)| dist.to_bits()); + } + }; + + // let mut stack: SmallVec<&BvhNode, 64> = SmallVec::new(); + let mut heap: BinaryHeap<_> = std::iter::once(on) + .map(|node| Reverse(NodeOrd { node, dist2: 0.0 })) + .collect(); + + while let Some(on) = heap.pop() { + let on = on.0.node; + let dist2 = on.aabb.dist2(target); + + if dist2 > min_dist2 { + break; + } + + for child in on.children(self) { + match child { + Node::Internal(internal) => { + heap.push(Reverse(NodeOrd { + node: internal, + dist2: internal.aabb.dist2(target), + })); + } + Node::Leaf(leaf) => { + let (elem, dist2) = leaf + .iter() + .map(|elem| { + let aabb = get_aabb(elem); + let dist2 = aabb.dist2(target); + (elem, dist2) + }) + .min_by_key(|(_, dist)| dist.to_bits()) + .unwrap(); + + if dist2 < min_dist2 { + min_dist2 = dist2; + min_node = Some(elem); + } + } + } + } + } + + min_node.map(|elem| (elem, min_dist2)) + } + + pub fn get_collisions<'a>( + &'a self, + target: Aabb, + get_aabb: impl GetAabb + 'a, + ) -> impl Iterator + 'a { + BvhIter::consume(self, target, get_aabb) + } +} diff --git a/crates/bvh-region/src/utils.rs b/crates/bvh-region/src/utils.rs new file mode 100644 index 00000000..2e59a0eb --- /dev/null +++ b/crates/bvh-region/src/utils.rs @@ -0,0 +1,48 @@ +use derive_more::Constructor; +use geometry::aabb::Aabb; + +use crate::node::BvhNode; + +/// get number of threads that is pow of 2 +pub fn thread_count_pow2() -> usize { + let max_threads_tentative = rayon::current_num_threads(); + // let max + + // does not make sense to not have a power of two + let mut max_threads = max_threads_tentative.next_power_of_two(); + + if max_threads != max_threads_tentative { + max_threads >>= 1; + } + + max_threads +} + +pub trait GetAabb: Fn(&T) -> Aabb {} + +impl GetAabb for F where F: Fn(&T) -> Aabb {} + +#[derive(Constructor)] +pub struct NodeOrd<'a> { + pub node: &'a BvhNode, + pub dist2: f64, +} + +impl PartialEq for NodeOrd<'_> { + fn eq(&self, other: &Self) -> bool { + self.dist2 == other.dist2 + } +} +impl PartialOrd for NodeOrd<'_> { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl Eq for NodeOrd<'_> {} + +impl Ord for NodeOrd<'_> { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.dist2.partial_cmp(&other.dist2).unwrap() + } +} diff --git a/crates/bvh-region/tests/simple.rs b/crates/bvh-region/tests/simple.rs new file mode 100644 index 00000000..db62dab6 --- /dev/null +++ b/crates/bvh-region/tests/simple.rs @@ -0,0 +1,104 @@ +use approx::assert_relative_eq; +use bvh_region::Bvh; +use geometry::aabb::Aabb; +use glam::Vec3; +use proptest::prelude::*; + +const fn copied(value: &T) -> T { + *value +} + +// Helper function to compute the squared distance from a point to an Aabb. +// This logic might differ depending on how you define Aabb. +fn aabb_distance_squared(aabb: &Aabb, p: Vec3) -> f64 { + let p = p.as_dvec3(); + let min = aabb.min.as_dvec3(); // Ensure you have aabb.min / aabb.max as Vec3 + let max = aabb.max.as_dvec3(); + let clamped = p.clamp(min, max); + p.distance_squared(clamped) +} + +#[test] +fn simple() { + let elements = vec![Aabb { + min: Vec3::new(-1.470_215_5e30, 0.0, 0.0), + max: Vec3::new(0.0, 0.0, 0.0), + }]; + + let target = Vec3::new(0.0, 0.0, 0.0); + let bvh = Bvh::build(elements.clone(), copied); + let (closest, dist2) = bvh.get_closest(target, copied).unwrap(); + + assert_eq!(closest, &elements[0]); + assert_relative_eq!(dist2, 0.0); +} + +proptest! { + #[test] + fn test_get_closest_correctness( + elements in prop::collection::vec( + // Generate random AABBs by picking two random points and making one the min and the other the max. + (any::(), any::(), any::(), any::(), any::(), any::()) + .prop_map(|(x1, y1, z1, x2, y2, z2)| { + let min_x = x1.min(x2); + let max_x = x1.max(x2); + let min_y = y1.min(y2); + let max_y = y1.max(y2); + let min_z = z1.min(z2); + let max_z = z1.max(z2); + Aabb::from([min_x, min_y, min_z, max_x, max_y, max_z]) + }), + 1..50 // vary the number of elements from small to moderate + ), + target in (any::(), any::(), any::()) + ) { + let target_vec = Vec3::new(target.0, target.1, target.2); + let bvh = Bvh::build(elements.clone(), copied); + + let closest_bvh = bvh.get_closest(target_vec, copied); + + // Compute the closest element by brute force + let mut best: Option<(&Aabb, f64)> = None; + for aabb in &elements { + let dist = aabb_distance_squared(aabb, target_vec); + if let Some((_, best_dist)) = best { + if dist < best_dist { + best = Some((aabb, dist)); + } + } else { + best = Some((aabb, dist)); + } + } + + // Compare results + match (closest_bvh, best) { + (Some((bvh_aabb, bvh_dist)), Some((brute_aabb, brute_dist))) => { + if bvh_dist.is_infinite() && brute_dist.is_infinite() { + // If both are infinite, they should return the same element + prop_assert_eq!(bvh_aabb, brute_aabb); + } else { + // Check that the distances are essentially the same + prop_assert!((bvh_dist - brute_dist).abs() < 1e-6, "Distances differ significantly; BVH: {bvh_dist}, brute force: {brute_dist}"); + + let target = Vec3::new(target.0, target.1, target.2); + let calculated_bvh_dist = bvh_aabb.dist2(target); + let calculated_brute_dist = brute_aabb.dist2(target); + prop_assert!((calculated_bvh_dist - calculated_brute_dist).abs() < 1e-6, "Distances differ significantly; BVH: {calculated_bvh_dist}, brute force: {calculated_brute_dist}"); + + // We are commenting this out because there might be some cases + // where there are multiple "correct" answers. + // prop_assert_eq!(bvh_aabb, brute_aabb); + } + + }, + (None, None) => { + // If there are no elements, both should return None + prop_assert!(true); + }, + (x,y) => { + // If one returns None and the other doesn't, there's a mismatch + prop_assert!(false, "Mismatch between BVH closest and brute force closest; BVH: {x:?}, brute force: {y:?}"); + } + } + } +} diff --git a/crates/geometry/src/aabb.rs b/crates/geometry/src/aabb.rs index 4d07bb06..22513585 100644 --- a/crates/geometry/src/aabb.rs +++ b/crates/geometry/src/aabb.rs @@ -1,4 +1,7 @@ -use std::{fmt::Display, ops::Add}; +use std::{ + fmt::{Debug, Display}, + ops::Add, +}; use glam::Vec3; use serde::{Deserialize, Serialize}; @@ -15,12 +18,22 @@ impl HasAabb for Aabb { } } -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] +#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)] pub struct Aabb { pub min: Vec3, pub max: Vec3, } +impl From<[f32; 6]> for Aabb { + fn from(value: [f32; 6]) -> Self { + let [min_x, min_y, min_z, max_x, max_y, max_z] = value; + let min = Vec3::new(min_x, min_y, min_z); + let max = Vec3::new(max_x, max_y, max_z); + + Self { min, max } + } +} + impl FromIterator for Aabb { fn from_iter>(iter: T) -> Self { let mut min = Vec3::new(f32::INFINITY, f32::INFINITY, f32::INFINITY); @@ -35,6 +48,12 @@ impl FromIterator for Aabb { } } +impl Debug for Aabb { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{self}") + } +} + impl Display for Aabb { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { // write [0.00, 0.00, 0.00] -> [1.00, 1.00, 1.00] @@ -181,20 +200,16 @@ impl Aabb { } #[must_use] - pub fn dist2(&self, point: Vec3) -> f32 { - let point = point.as_ref(); - let self_min = self.min.as_ref(); - let self_max = self.max.as_ref(); + pub fn dist2(&self, point: Vec3) -> f64 { + let point = point.as_dvec3(); + // Clamp the point into the box volume. + let clamped = point.clamp(self.min.as_dvec3(), self.max.as_dvec3()); - let mut dist2 = 0.0; - - #[expect(clippy::indexing_slicing, reason = "all of these have length of 3")] - for i in 0..3 { - dist2 += (self_min[i] - point[i]).max(0.0).powi(2); - dist2 += (self_max[i] - point[i]).min(0.0).powi(2); - } + // Distance vector from point to the clamped point inside the box. + let diff = point - clamped; - dist2 + // The squared distance. + diff.length_squared() } pub fn overlaps<'a, T>( diff --git a/crates/spatial/src/lib.rs b/crates/spatial/src/lib.rs index 332d7e16..d7d3725f 100644 --- a/crates/spatial/src/lib.rs +++ b/crates/spatial/src/lib.rs @@ -1,4 +1,3 @@ -use bvh_region::TrivialHeuristic; use flecs_ecs::{ core::{ Builder, Entity, EntityView, EntityViewGet, IdOperations, QueryAPI, QueryBuilderImpl, @@ -43,7 +42,7 @@ impl SpatialIndex { let all_entities = all_indexed_entities(world); let get_aabb = get_aabb_func(world); - self.query = bvh_region::Bvh::build::(all_entities, &get_aabb); + self.query = bvh_region::Bvh::build(all_entities, &get_aabb); } pub fn get_collisions<'a>(