Implement AxisAlignedBoundingBox::corners_iter

Author: Andlon

fenris-geometry/src/lib.rs

@@ -14,6 +14,7 @@ mod primitives;
 pub use polygon::*;
 pub use polytope::*;
 pub use primitives::*;
+use crate::util::{index_set_nth_power_iter};
 
 pub mod polymesh;
 pub mod sdf;
@@ -239,6 +240,23 @@ where
         let max = self.max().map(|b_i| b_i + distance);
         Self::new(min, max)
     }
+
+    /// Creates an iterator over the corners of the bounding box.
+    pub fn corners_iter<'a>(&'a self) -> impl 'a + Iterator<Item=OPoint<T, D>>
+    where
+        DefaultAllocator: Allocator<usize, D>
+    {
+        // We can enumerate the corners by looking at {0, 1}^D, i.e. the D-th power of the
+        // set {0, 1}, and associating 0 and 1 with min and max coordinates for the i-th axis.
+        index_set_nth_power_iter::<D>(2)
+            .map(move |multi_idx| {
+                OVector::<T, D>::from_fn(|idx, _| match multi_idx[idx] {
+                    0 => self.min[idx].clone(),
+                    1 => self.max[idx].clone(),
+                    _ => unreachable!()
+                }).into()
+            })
+    }
 }
 
 fn intervals_intersect<T: Real>([l1, u1]: [T; 2], [l2, u2]: [T; 2]) -> bool {

fenris-geometry/src/util.rs

@@ -1,5 +1,6 @@
 use fenris_traits::Real;
-use nalgebra::{UnitVector3, Vector3};
+use nalgebra::{DefaultAllocator, DimName, OVector, UnitVector3, Vector3};
+use nalgebra::allocator::Allocator;
 
 pub fn compute_orthonormal_vectors_3d<T: Real>(vector: &UnitVector3<T>) -> [UnitVector3<T>; 2] {
     // Ported from
@@ -82,3 +83,32 @@ macro_rules! assert_line_segments_approx_equal {
         $crate::assert_line_segments_approx_equal_base!(msg_handler, $segment1, $segment2, abstol = $tol);
     }};
 }
+
+/// Returns an iterator that iterates over the elements of $\{ 0, \dots, n - 1 \}^N$, where $n$ is
+/// the number of indices in the index set $\{ 0, \dots, n - 1 \}$.
+///
+/// TODO: Currently only implicitly tested through [`AxisAlignedBoundingBox::corners_iter`].
+pub(crate) fn index_set_nth_power_iter<D: DimName>(n: usize) -> impl Iterator<Item = OVector<usize, D>>
+where
+    DefaultAllocator: Allocator<usize, D>
+{
+    let mut multi_idx = OVector::from_element(0);
+    let mut i = 0;
+    std::iter::from_fn(move || {
+        if i < n.pow(D::dim() as u32) {
+            let item = Some(multi_idx.clone());
+            for j in 0..D::dim() {
+                if multi_idx[j] + 1 < n {
+                    multi_idx[j] += 1;
+                    break;
+                } else {
+                    multi_idx[j] = 0;
+                }
+            }
+            i += 1;
+            item
+        } else {
+            None
+        }
+    })
+}

fenris-geometry/tests/unit_tests/aabb.rs

@@ -1,5 +1,7 @@
 use fenris_geometry::AxisAlignedBoundingBox;
-use nalgebra::{point, U2};
+use nalgebra::{DefaultAllocator, DimName, OPoint, point, U2};
+use nalgebra::allocator::Allocator;
+use fenris::allocators::DimAllocator;
 
 #[test]
 fn aabb_intersects_2d() {
@@ -39,4 +41,75 @@ fn aabb_intersects_2d() {
     assert_intersection!(Aabb::new(point![1.5, 1.5], point![4.5, 3.5]));
     assert_intersection!(Aabb::new(point![0.0, 0.0], point![2.0, 2.0]));
     assert_intersection!(Aabb::new(point![0.0, 0.0], point![5.0, 4.0]));
+}
+
+/// Helper function for collecting corners from corner iterator.
+fn corners<D: DimName>(aabb:& AxisAlignedBoundingBox<f64, D>) -> Vec<OPoint<f64, D>>
+where
+    DefaultAllocator: DimAllocator<f64, D>
+{
+    aabb.corners_iter().collect()
+}
+
+fn compare_unordered_points<D: DimName>(points1: &[OPoint<f64, D>], points2: &[OPoint<f64, D>]) -> bool
+where
+    DefaultAllocator: Allocator<f64, D>
+{
+    assert_eq!(points1.len(), points2.len());
+    // This is O(n^2) but is acceptable for use in tests here
+    points1.iter()
+        .all(|p1| points2.contains(p1))
+}
+
+macro_rules! assert_unordered_eq {
+    ($p1:expr, $p2:expr) => {
+        {
+            let eq = compare_unordered_points((&$p1).as_ref(), (&$p2).as_ref());
+            if !eq {
+                dbg!(&$p1, &$p2);
+                assert!(eq, "Point lists do not contain the same points");
+            }
+        }
+    }
+}
+
+
+#[test]
+fn test_aabb_corners_iter() {
+
+    // 1D
+    {
+        let aabb = AxisAlignedBoundingBox::new(point![3.0], point![4.0]);
+        assert_eq!(corners(&aabb), vec![ point![3.0 ], point![4.0] ]);
+    }
+
+    // 2D
+    {
+        let aabb = AxisAlignedBoundingBox::new(point![3.0, 4.0], point![5.0, 6.0]);
+        let expected = [
+            [3.0, 4.0],
+            [3.0, 6.0],
+            [5.0, 4.0],
+            [5.0, 6.0]
+        ].map(OPoint::from);
+        assert_unordered_eq!(corners(&aabb), &expected);
+    }
+
+    // 3D
+    {
+        let aabb = AxisAlignedBoundingBox::new(point![1.0, 2.0, 3.0], point![4.0, 5.0, 6.0]);
+        let expected = [
+            [1.0, 2.0, 3.0],
+            [1.0, 2.0, 6.0],
+            [1.0, 5.0, 3.0],
+            [1.0, 5.0, 6.0],
+            [4.0, 2.0, 3.0],
+            [4.0, 2.0, 6.0],
+            [4.0, 5.0, 3.0],
+            [4.0, 5.0, 6.0]
+        ].map(OPoint::from);
+        assert_unordered_eq!(corners(&aabb), expected);
+    }
+
+
 }