static_math/
utils.rs

1//-------------------------------------------------------------------------
2// @file utils.rs
3//
4// @date 06/02/20 11:06:12
5// @author Martin Noblia
6// @email mnoblia@disroot.org
7//
8// @brief
9//
10// @detail
11//
12// Licence MIT:
13// Copyright <2020> <Martin Noblia>
14//
15// Permission is hereby granted, free of charge, to any person obtaining a copy
16// of this software and associated documentation files (the "Software"), to deal
17// in the Software without restriction, including without limitation the rights
18// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19// copies of the Software, and to permit persons to whom the Software is
20// furnished to do so, subject to the following conditions:
21//
22// The above copyright notice and this permission notice shall be included in
23// all copies or substantial portions of the Software.  THE SOFTWARE IS PROVIDED
24// "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
25// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
26// PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
27// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
28// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
29// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
30//-------------------------------------------------------------------------
31use crate::dual_quaternion::DualQuaternion;
32use crate::matrix3x3::M33;
33use crate::matrix4x4::M44;
34use crate::traits::LinearAlgebra;
35use num::Float;
36// NOTE(elsuizo:2020-06-02): the following function
37// is a translation of the implementation that is here:
38// https://floating-point-gui.de/errors/comparison/
39//
40/// a comparison function for floating point values
41///
42pub fn nearly_equal<T: Float>(a: T, b: T, epsilon: T) -> bool {
43    let abs_a = a.abs();
44    let abs_b = b.abs();
45    let abs_diff = (a - b).abs();
46    let zero = T::zero();
47    // short-cut, handles infinity
48    if a == b {
49        true
50    } else if a == zero || b == zero || (abs_a + abs_b < T::min_value()) {
51        // a or b is zero or both are extremely close to it
52        // relative error is less meaningful here
53        abs_diff < epsilon
54    } else {
55        abs_diff / T::min(abs_a + abs_b, T::max_value()) < epsilon
56    }
57}
58
59#[inline(always)]
60pub fn nearly_zero<T: Float>(a: T) -> bool {
61    nearly_equal(a, T::zero(), T::epsilon())
62}
63
64/// utility function to compare vectors of Floats
65pub fn compare_vecs<T: Float>(v1: &[T], v2: &[T], epsilon: T) -> bool {
66    v1.iter()
67        .zip(v2)
68        .map(|(a, b)| nearly_equal(*a, *b, epsilon))
69        .all(|x| x)
70}
71
72pub fn compare_floats<T: Float>(num1: T, num2: T, tol: T) -> bool {
73    Float::abs(num1 - num2) < tol
74}
75
76/// utility function to verify if a Matrix is a propper rotation matrix
77pub fn is_rotation<T: Float + core::iter::Sum>(r: M33<T>) -> bool {
78    let r2 = r * r;
79    nearly_equal(r.det(), T::one(), T::epsilon()) && nearly_equal(r2.det(), T::one(), T::epsilon())
80}
81
82pub fn is_rotation_h<T: Float + core::iter::Sum>(r: M44<T>) -> bool {
83    let r2 = r * r;
84    let eps = T::from(1e-6).unwrap();
85    nearly_equal(r.det(), T::one(), eps) && nearly_equal(r2.det(), T::one(), eps)
86}
87
88pub fn compare_dual_quaternions<T: Float>(
89    a: DualQuaternion<T>,
90    b: DualQuaternion<T>,
91    epsilon: T,
92) -> bool {
93    nearly_equal(a.real().real(), b.real().real(), epsilon)
94        && nearly_equal(a.real().imag()[0], b.real().imag()[0], epsilon)
95        && nearly_equal(a.real().imag()[1], b.real().imag()[1], epsilon)
96        && nearly_equal(a.real().imag()[2], b.real().imag()[2], epsilon)
97        && nearly_equal(a.dual().real(), b.dual().real(), epsilon)
98        && nearly_equal(a.dual().imag()[0], b.dual().imag()[0], epsilon)
99        && nearly_equal(a.dual().imag()[1], b.dual().imag()[1], epsilon)
100        && nearly_equal(a.dual().imag()[2], b.dual().imag()[2], epsilon)
101}
102
103//-------------------------------------------------------------------------
104//                        tests
105//-------------------------------------------------------------------------
106#[cfg(test)]
107mod test_utils {
108    use super::*;
109    #[test]
110    fn test_nearly_equal() {
111        let a = 0.15 + 0.15;
112        let b = 0.1 + 0.2;
113        assert_eq!(nearly_equal(a, b, 1e-10), true);
114    }
115
116    #[test]
117    fn test_zero_signs() {
118        let a = 0.0;
119        let b = -0.0;
120        assert_eq!(nearly_equal(a, b, 1e-10), true);
121    }
122
123    #[test]
124    fn test_one_zero() {
125        let b = -0.0000000005561918225744;
126        let a = 0.0;
127        assert_eq!(nearly_equal(a, b, 1e-8), true);
128    }
129}
static_math/utils.rs

static_math/
utils.rs
