Implement AABB::closest_point_to and related methods

fenris-geometry/src/lib.rs

@@ -151,7 +151,7 @@ where
     D: DimName,
     DefaultAllocator: Allocator<T, D>,
 {
-    /// 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<T, D>) -> 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<T, D>) -> OPoint<T, D> {
+        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, D>) -> 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, D>) -> 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<T, D>) -> OPoint<T, D>
     where

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<const D: usize>(aabb: AxisAlignedBoundingBox<f64, Const<D>>) -> impl Strategy<Value = Point<f64, D>> {
+    // 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::<D>)
+        .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);
+    }
 }