From 99f1cb5e50c549f63a23e7eda722eeac66cb09b7 Mon Sep 17 00:00:00 2001 From: Andreas Longva Date: Thu, 10 Nov 2022 17:19:31 +0100 Subject: [PATCH] Implement AABB::closest_point_to and related methods --- fenris-geometry/src/lib.rs | 37 +++++-- fenris-geometry/tests/unit_tests/aabb.rs | 129 ++++++++++++++++++++++- 2 files changed, 154 insertions(+), 12 deletions(-) diff --git a/fenris-geometry/src/lib.rs b/fenris-geometry/src/lib.rs index 05f8cc87..109fec7a 100644 --- a/fenris-geometry/src/lib.rs +++ b/fenris-geometry/src/lib.rs @@ -151,7 +151,7 @@ where D: DimName, DefaultAllocator: Allocator, { - /// Computes the minimal bounding box which encloses both `this` and `other`. + /// Computes the minimal bounding box which encloses both `self` and `other`. pub fn enclose(&self, other: &AxisAlignedBoundingBox) -> Self { let min = self .min @@ -266,16 +266,33 @@ where }) } + /// Computes the point in the bounding box closest to the given point. + pub fn closest_point_to(&self, point: &OPoint) -> OPoint { + point + .coords + .zip_zip_map(&self.min.coords, &self.max.coords, |p_i, a_i, b_i| { + if p_i <= a_i { + a_i + } else if p_i >= b_i { + b_i + } else { + p_i + } + }) + .into() + } + + /// Computes the distance between the bounding box and the given point. + pub fn dist_to(&self, point: &OPoint) -> T { + self.dist2_to(point).sqrt() + } + + /// Computes the squared distance between the bounding box and the given point. + pub fn dist2_to(&self, point: &OPoint) -> T { + (self.closest_point_to(point) - point).norm_squared() + } + /// Compute the point in the bounding box furthest away from the given point. - /// - /// # Panics - /// - /// Panics if two distances cannot be ordered. This typically only happens if - /// one of the numbers is not a number (NaN) or the comparison is not sensible, such as - /// comparing two infinities. Since given finite coordinates no distance should be infinite, - /// this method will realistically only panic in cases where one of the points - /// --- either of the bounding box or the query point --- has components that are not - /// finite numbers. #[replace_float_literals(T::from_f64(literal).unwrap())] pub fn furthest_point_to(&self, point: &OPoint) -> OPoint where diff --git a/fenris-geometry/tests/unit_tests/aabb.rs b/fenris-geometry/tests/unit_tests/aabb.rs index cbe9bfef..9fea0569 100644 --- a/fenris-geometry/tests/unit_tests/aabb.rs +++ b/fenris-geometry/tests/unit_tests/aabb.rs @@ -1,10 +1,12 @@ use fenris::allocators::DimAllocator; use fenris_geometry::proptest::{aabb2, aabb3, point2, point3}; -use fenris_geometry::AxisAlignedBoundingBox; +use fenris_geometry::{AxisAlignedBoundingBox, AxisAlignedBoundingBox2d, AxisAlignedBoundingBox3d}; use matrixcompare::assert_scalar_eq; use nalgebra::allocator::Allocator; -use nalgebra::distance_squared; +use nalgebra::proptest::vector; +use nalgebra::{distance, distance_squared, Const, Point}; use nalgebra::{point, DefaultAllocator, DimName, OPoint, U2}; +use proptest::collection::vec; use proptest::prelude::*; #[test] @@ -74,6 +76,21 @@ macro_rules! assert_unordered_eq { }}; } +fn point_in_aabb(aabb: AxisAlignedBoundingBox>) -> impl Strategy> { + // Bias generation 0.0 and 1.0 values to ensure that we generate values also on the boundary + // of the box + let values = prop_oneof![ + 1 => Just(0.0), + 1 => Just(1.0), + 5 => 0.0 ..= 1.0]; + vector(values, Const::) + .prop_map(move |v| { + // Coordinates are in [0, 1] interval, transform to [a_i, b_i] + v.zip_zip_map(&aabb.min().coords, &aabb.max().coords, |p, a, b| (1.0 - p) * a + p * b) + }) + .prop_map(Point::from) +} + #[test] fn test_aabb_corners_iter() { // 1D @@ -132,6 +149,57 @@ fn test_furthest_point_2d() { } } +#[test] +fn test_closest_point() { + // Helper macro for succinct checks + macro_rules! assert_closest_point { + ($aabb:expr, $p:expr => $expected:expr) => {{ + let aabb = $aabb; + let p = $p; + let q = aabb.closest_point_to(&p); + assert_eq!(&q, &$expected); + assert_eq!(distance(&q, &p), aabb.dist_to(&p)); + assert_eq!(distance_squared(&q, &p), aabb.dist2_to(&p)); + }}; + } + + // 2D + { + let a = point![2.0, 3.0]; + let b = point![3.0, 5.0]; + let aabb = AxisAlignedBoundingBox2d::new(a, b); + // Outside points + assert_closest_point!(aabb, point![1.0, 1.0] => point![2.0, 3.0]); + assert_closest_point!(aabb, point![2.0, 2.0] => point![2.0, 3.0]); + assert_closest_point!(aabb, point![1.0, 4.0] => point![2.0, 4.0]); + assert_closest_point!(aabb, point![1.0, 5.0] => point![2.0, 5.0]); + assert_closest_point!(aabb, point![-1.0, 6.0] => point![2.0, 5.0]); + assert_closest_point!(aabb, point![2.5, 7.0] => point![2.5, 5.0]); + assert_closest_point!(aabb, point![4.0, 6.0] => point![3.0, 5.0]); + assert_closest_point!(aabb, point![6.0, 4.0] => point![3.0, 4.0]); + assert_closest_point!(aabb, point![5.0, 2.0] => point![3.0, 3.0]); + + // Inside points + assert_closest_point!(aabb, point![2.5, 4.0] => point![2.5, 4.0]); + assert_closest_point!(aabb, point![2.3, 4.6] => point![2.3, 4.6]); + } + + // 3D. We only test a few points since the impl is the same as in 2D and + // we have proptests that should cover things quite extensively + { + let a = point![2.0, 3.0, 1.0]; + let b = point![3.0, 5.0, 6.0]; + let aabb = AxisAlignedBoundingBox3d::new(a, b); + // Outside points + assert_closest_point!(aabb, point![1.0, 1.0, 1.0] => point![2.0, 3.0, 1.0]); + assert_closest_point!(aabb, point![4.0, 6.0, 8.0] => point![3.0, 5.0, 6.0]); + assert_closest_point!(aabb, point![1.0, 4.0, 5.0] => point![2.0, 4.0, 5.0]); + + // Inside points + assert_closest_point!(aabb, point![2.5, 4.0, 3.0] => point![2.5, 4.0, 3.0]); + } +} + proptest! { #[test] @@ -186,4 +254,61 @@ proptest! { prop_assert!(aabb.contains_point(&q)); } + #[test] + fn aabb_dists_agree_with_closest_point_2d(point in point2(), aabb in aabb2()) { + let q = aabb.closest_point_to(&point); + let dist2 = distance_squared(&q, &point); + prop_assert_eq!(aabb.dist2_to(&point), dist2); + prop_assert_eq!(aabb.dist_to(&point), dist2.sqrt()); + } + + #[test] + fn aabb_dists_agree_with_closest_point_3d(point in point3(), aabb in aabb3()) { + let q = aabb.closest_point_to(&point); + let dist2 = distance_squared(&q, &point); + prop_assert_eq!(aabb.dist2_to(&point), dist2); + prop_assert_eq!(aabb.dist_to(&point), dist2.sqrt()); + } + + #[test] + fn aabb_closest_point_2d_closer_than_other_points( + p in point2(), + (aabb, test_points) in aabb2() + .prop_flat_map(|aabb| (Just(aabb), vec(point_in_aabb(aabb), 0 .. 50))) + ) { + let q = aabb.closest_point_to(&p); + prop_assert!(aabb.contains_point(&q)); + for test_point in test_points { + assert!(aabb.contains_point(&test_point)); + prop_assert!(distance(&q, &p) <= distance(&p, &test_point)); + } + } + + #[test] + fn aabb_closest_point_3d_closer_than_other_points( + p in point3(), + (aabb, test_points) in aabb3() + .prop_flat_map(|aabb| (Just(aabb), vec(point_in_aabb(aabb), 0 .. 50))) + ) { + let q = aabb.closest_point_to(&p); + prop_assert!(aabb.contains_point(&q)); + for test_point in test_points { + assert!(aabb.contains_point(&test_point)); + prop_assert!(distance(&q, &p) <= distance(&p, &test_point)); + } + } + + #[test] + fn aabb_closest_point_of_internal_point_2d( + (aabb, p) in aabb2().prop_flat_map(|aabb| (Just(aabb), point_in_aabb(aabb))) + ) { + prop_assert_eq!(aabb.closest_point_to(&p), p); + } + + #[test] + fn aabb_closest_point_of_internal_point_3d( + (aabb, p) in aabb3().prop_flat_map(|aabb| (Just(aabb), point_in_aabb(aabb))) + ) { + prop_assert_eq!(aabb.closest_point_to(&p), p); + } }