static-math 0.2.3

Fast mathematical operations with static arrays, without `unsafe` code
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

//-------------------------------------------------------------------------
// @file utils.rs
//
// @date 06/02/20 11:06:12
// @author Martin Noblia
// @email mnoblia@disroot.org
//
// @brief
//
// @detail
//
// Licence MIT:
// Copyright <2020> <Martin Noblia>
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.  THE SOFTWARE IS PROVIDED
// "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
// PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//-------------------------------------------------------------------------
use crate::dual_quaternion::DualQuaternion;
use crate::matrix3x3::M33;
use crate::matrix4x4::M44;
use crate::traits::LinearAlgebra;
use num::Float;
// NOTE(elsuizo:2020-06-02): the following function
// is a translation of the implementation that is here:
// https://floating-point-gui.de/errors/comparison/
//
/// a comparison function for floating point values
///
pub fn nearly_equal<T: Float>(a: T, b: T, epsilon: T) -> bool {
    let abs_a = a.abs();
    let abs_b = b.abs();
    let abs_diff = (a - b).abs();
    let zero = T::zero();
    // short-cut, handles infinity
    if a == b {
        true
    } else if a == zero || b == zero || (abs_a + abs_b < T::min_value()) {
        // a or b is zero or both are extremely close to it
        // relative error is less meaningful here
        abs_diff < epsilon
    } else {
        abs_diff / T::min(abs_a + abs_b, T::max_value()) < epsilon
    }
}

#[inline(always)]
pub fn nearly_zero<T: Float>(a: T) -> bool {
    nearly_equal(a, T::zero(), T::epsilon())
}

/// utility function to compare vectors of Floats
pub fn compare_vecs<T: Float>(v1: &[T], v2: &[T], epsilon: T) -> bool {
    v1.iter()
        .zip(v2)
        .map(|(a, b)| nearly_equal(*a, *b, epsilon))
        .all(|x| x)
}

pub fn compare_floats<T: Float>(num1: T, num2: T, tol: T) -> bool {
    Float::abs(num1 - num2) < tol
}

/// utility function to verify if a Matrix is a propper rotation matrix
pub fn is_rotation<T: Float + core::iter::Sum>(r: M33<T>) -> bool {
    let r2 = r * r;
    nearly_equal(r.det(), T::one(), T::epsilon()) && nearly_equal(r2.det(), T::one(), T::epsilon())
}

pub fn is_rotation_h<T: Float + core::iter::Sum>(r: M44<T>) -> bool {
    let r2 = r * r;
    let eps = T::from(1e-6).unwrap();
    nearly_equal(r.det(), T::one(), eps) && nearly_equal(r2.det(), T::one(), eps)
}

pub fn compare_dual_quaternions<T: Float>(
    a: DualQuaternion<T>,
    b: DualQuaternion<T>,
    epsilon: T,
) -> bool {
    nearly_equal(a.real().real(), b.real().real(), epsilon)
        && nearly_equal(a.real().imag()[0], b.real().imag()[0], epsilon)
        && nearly_equal(a.real().imag()[1], b.real().imag()[1], epsilon)
        && nearly_equal(a.real().imag()[2], b.real().imag()[2], epsilon)
        && nearly_equal(a.dual().real(), b.dual().real(), epsilon)
        && nearly_equal(a.dual().imag()[0], b.dual().imag()[0], epsilon)
        && nearly_equal(a.dual().imag()[1], b.dual().imag()[1], epsilon)
        && nearly_equal(a.dual().imag()[2], b.dual().imag()[2], epsilon)
}

//-------------------------------------------------------------------------
//                        tests
//-------------------------------------------------------------------------
#[cfg(test)]
mod test_utils {
    use super::*;
    #[test]
    fn test_nearly_equal() {
        let a = 0.15 + 0.15;
        let b = 0.1 + 0.2;
        assert_eq!(nearly_equal(a, b, 1e-10), true);
    }

    #[test]
    fn test_zero_signs() {
        let a = 0.0;
        let b = -0.0;
        assert_eq!(nearly_equal(a, b, 1e-10), true);
    }

    #[test]
    fn test_one_zero() {
        let b = -0.0000000005561918225744;
        let a = 0.0;
        assert_eq!(nearly_equal(a, b, 1e-8), true);
    }
}