fenris_geometry/
util.rs

1use fenris_traits::Real;
2use nalgebra::allocator::Allocator;
3use nalgebra::{DefaultAllocator, DimName, OVector, UnitVector3, Vector3};
4
5pub fn compute_orthonormal_vectors_3d<T: Real>(vector: &UnitVector3<T>) -> [UnitVector3<T>; 2] {
6    // Ported from
7    // https://github.com/dimforge/parry/blob/ac8dcf0197066cd2413a20e4420961b4694996c0/src/utils/wops.rs#L120-L138
8    // originally based on the Pixar paper "Building an Orthonormal Basis, Revisited",
9    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
10    let v = vector;
11    let sign = T::copysign(T::one(), v.z);
12    let a = -T::one() / (sign + v.z);
13    let b = v.x * v.y * a;
14
15    [
16        Vector3::new(T::one() + sign * v.x * v.x * a, sign * b, -sign * v.x),
17        Vector3::new(b, sign + v.y * v.y * a, -v.y),
18    ]
19    .map(UnitVector3::new_unchecked)
20}
21
22/// Compares two arrays for *shift-invariant* equality with the given comparator function.
23///
24/// If `X` and `Y` are the two arrays, then the two arrays are shift-invariant equal if X can be shifted/rotated
25/// by some constant `n` such that `Shift(X) == Y`.
26pub fn slices_are_equal_shift_invariant<T, C: Fn(&T, &T) -> bool>(x: &[T], y: &[T], comparator: C) -> bool {
27    let n = x.len();
28    if y.len() != n {
29        return false;
30    } else if n == 0 {
31        // Empty arrays are always equal
32        return true;
33    }
34
35    'outer_loop: for i_start in 0..n {
36        for (j, y_j) in y.iter().enumerate() {
37            let x_j_shifted = &x[(j + i_start) % n];
38            if !comparator(x_j_shifted, y_j) {
39                continue 'outer_loop;
40            }
41        }
42        return true;
43    }
44
45    return false;
46}
47
48// Base macro for line segment assertions
49#[doc(hidden)]
50#[macro_export]
51macro_rules! assert_line_segments_approx_equal_base {
52    ($msg_handler:expr, $segment1:expr, $segment2:expr, abstol = $tol:expr) => {{
53        use fenris_geometry::util::slices_are_equal_shift_invariant;
54        use matrixcompare::comparators::AbsoluteElementwiseComparator;
55        use matrixcompare::compare_matrices;
56        use nalgebra::Point2;
57
58        let tol = $tol.clone();
59        // Type check: Makes for an easier error message than failing specific methods
60        let (segment1, segment2): (&LineSegment2d<_>, &LineSegment2d<_>) = (&$segment1, &$segment2);
61        let vertices1 = [segment1.start().clone(), segment1.end().clone()];
62        let vertices2 = [segment2.start().clone(), segment2.end().clone()];
63        let comparator = AbsoluteElementwiseComparator { tol };
64        let comparator = |a: &Point2<f64>, b: &Point2<f64>| compare_matrices(&a.coords, &b.coords, &comparator).is_ok();
65        let vertices_are_shift_invariant_equal = slices_are_equal_shift_invariant(&vertices1, &vertices2, comparator);
66        if !vertices_are_shift_invariant_equal {
67            let msg = format!(
68                "Line segments are not (approximately) equal to absolute tolerance {tol}.
69Segment1: {:?}
70Segment2: {:?}",
71                segment1, segment2
72            );
73
74            return $msg_handler(msg);
75        }
76    }};
77}
78
79#[macro_export]
80macro_rules! assert_line_segments_approx_equal {
81    ($segment1:expr, $segment2:expr, abstol = $tol:expr) => {{
82        let msg_handler = |msg| panic!("{}", msg);
83        $crate::assert_line_segments_approx_equal_base!(msg_handler, $segment1, $segment2, abstol = $tol);
84    }};
85}
86
87/// Returns an iterator that iterates over the elements of $\{ 0, \dots, n - 1 \}^N$, where $n$ is
88/// the number of indices in the index set $\{ 0, \dots, n - 1 \}$.
89///
90/// TODO: Currently only implicitly tested through [`AxisAlignedBoundingBox::corners_iter`].
91pub(crate) fn index_set_nth_power_iter<D: DimName>(n: usize) -> impl Iterator<Item = OVector<usize, D>>
92where
93    DefaultAllocator: Allocator<usize, D>,
94{
95    let mut multi_idx = OVector::from_element(0);
96    let mut i = 0;
97    std::iter::from_fn(move || {
98        if i < n.pow(D::dim() as u32) {
99            let item = Some(multi_idx.clone());
100            for j in 0..D::dim() {
101                if multi_idx[j] + 1 < n {
102                    multi_idx[j] += 1;
103                    break;
104                } else {
105                    multi_idx[j] = 0;
106                }
107            }
108            i += 1;
109            item
110        } else {
111            None
112        }
113    })
114}
fenris_geometry/util.rs

fenris_geometry/
util.rs
