mapping_algorithms/utils/
mod.rs

1// SPDX-License-Identifier: MIT
2/*
3 * Copyright (c) [2023 - Present] Emily Matheys <emilymatt96@gmail.com>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights
8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 * copies of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 * SOFTWARE.
22 */
23
24use nalgebra::{Const, DimMin, Point, RealField, SMatrix, Scalar};
25use num_traits::NumOps;
26
27#[cfg_attr(
28    feature = "tracing",
29    tracing::instrument("Calculate Distance Squared", skip_all, level = "trace")
30)]
31pub(crate) fn distance_squared<T, const N: usize>(point_a: &Point<T, N>, point_b: &Point<T, N>) -> T
32where
33    T: Copy + Default + NumOps + Scalar,
34{
35    point_a
36        .iter()
37        .zip(point_b.iter())
38        .map(|(&x, &y)| {
39            let diff = x - y;
40            diff * diff
41        })
42        .fold(T::default(), |acc, x| acc + x)
43}
44
45#[cfg_attr(
46    feature = "tracing",
47    tracing::instrument("Verify Matrix Determinant", skip_all, level = "info")
48)]
49pub(crate) fn verify_rotation_matrix_determinant<T, const N: usize>(
50    mut u: SMatrix<T, N, N>,
51    v_t: SMatrix<T, N, N>,
52) -> SMatrix<T, N, N>
53where
54    T: Copy + RealField,
55    Const<N>: DimMin<Const<N>, Output = Const<N>>,
56{
57    if (u * v_t).determinant() < T::zero() {
58        u.column_mut(N - 1)
59            .iter_mut()
60            .for_each(|element| *element *= T::one().neg()); // Reverse the last column
61    }
62    u * v_t
63}
64
65#[cfg(test)]
66pub(crate) mod tests {
67    use super::*;
68    use nalgebra::{Matrix2, Point3};
69
70    #[test]
71    fn test_distance_squared() {
72        let point_a = Point3::new(1.0, 2.0, 3.0);
73        let point_b = Point3::new(4.0, 5.0, 6.0);
74        assert_eq!(distance_squared(&point_a, &point_b), 27.0)
75    }
76
77    #[test]
78    fn test_verify_rotation_matrix_determinant() {
79        let mat_a = Matrix2::new(2.0, 3.0, 2.0, 1.0);
80        let mat_b = Matrix2::new(-1.0, 0.0, 0.0, -1.0);
81
82        let regular_dot = mat_a * mat_b;
83        assert!(regular_dot.determinant() < 0.0);
84
85        let func_dot = verify_rotation_matrix_determinant(mat_a, mat_b);
86
87        // Verify second column is actually reversed by this function
88        assert!(func_dot.determinant() >= 0.0);
89        assert_eq!(func_dot.m12, -regular_dot.m12);
90        assert_eq!(func_dot.m22, -regular_dot.m22);
91    }
92}
mapping_algorithms/utils/mod.rs

mapping_algorithms/utils/
mod.rs
