del_geo_core/
mat3_array_of_rows.rs

1//! methods for 3x3 matrix
2
3pub trait Mat3ArrayOfArray<Real>
4where
5    Self: Sized,
6{
7    fn det_inv(&self) -> (Real, Self);
8    fn inverse(&self) -> Self;
9    fn matmul(&self, b: &Self) -> Self;
10}
11
12impl<Real> Mat3ArrayOfArray<Real> for [[Real; 3]; 3]
13where
14    Real: num_traits::Float,
15{
16    fn det_inv(&self) -> (Real, Self) {
17        det_inv(self)
18    }
19    fn inverse(&self) -> Self {
20        inverse(self)
21    }
22    fn matmul(&self, b: &Self) -> Self {
23        matmul(self, b)
24    }
25}
26
27pub fn det_inv<T>(a: &[[T; 3]; 3]) -> (T, [[T; 3]; 3])
28where
29    T: num_traits::Float,
30{
31    let det =
32        a[0][0] * a[1][1] * a[2][2] + a[1][0] * a[2][1] * a[0][2] + a[2][0] * a[0][1] * a[1][2]
33            - a[0][0] * a[2][1] * a[1][2]
34            - a[2][0] * a[1][1] * a[0][2]
35            - a[1][0] * a[0][1] * a[2][2];
36    let inv_det = T::one() / det;
37    let ainv = [
38        [
39            inv_det * (a[1][1] * a[2][2] - a[1][2] * a[2][1]),
40            inv_det * (a[0][2] * a[2][1] - a[0][1] * a[2][2]),
41            inv_det * (a[0][1] * a[1][2] - a[0][2] * a[1][1]),
42        ],
43        [
44            inv_det * (a[1][2] * a[2][0] - a[1][0] * a[2][2]),
45            inv_det * (a[0][0] * a[2][2] - a[0][2] * a[2][0]),
46            inv_det * (a[0][2] * a[1][0] - a[0][0] * a[1][2]),
47        ],
48        [
49            inv_det * (a[1][0] * a[2][1] - a[1][1] * a[2][0]),
50            inv_det * (a[0][1] * a[2][0] - a[0][0] * a[2][1]),
51            inv_det * (a[0][0] * a[1][1] - a[0][1] * a[1][0]),
52        ],
53    ];
54    (det, ainv)
55}
56
57pub fn inverse<T>(a: &[[T; 3]; 3]) -> [[T; 3]; 3]
58where
59    T: num_traits::Float,
60{
61    a.det_inv().1
62}
63
64pub fn matmul<T>(a: &[[T; 3]; 3], b: &[[T; 3]; 3]) -> [[T; 3]; 3]
65where
66    T: num_traits::Float,
67{
68    let mut result = [[T::zero(); 3]; 3];
69    for i in 0..3 {
70        for j in 0..3 {
71            #[allow(clippy::needless_range_loop)]
72            for k in 0..3 {
73                result[i][j] = result[i][j] + a[i][k] * b[k][j];
74            }
75        }
76    }
77    result
78}
79
80#[test]
81fn test_inverse_matmul() {
82    let a = [[0f64, 2., 4.], [3., 5., 4.], [6., 7., 8.]];
83    let ainv = a.inverse();
84    let ainv_a = ainv.matmul(&a);
85    for i in 0..3 {
86        for j in 0..3 {
87            if i == j {
88                assert!((1.0 - ainv_a[i][j]).abs() < f64::EPSILON);
89            } else {
90                assert!(ainv_a[i][j].abs() < f64::EPSILON);
91            }
92        }
93    }
94}
95
96pub fn from_identity<T>() -> [[T; 3]; 3]
97where
98    T: num_traits::Float,
99{
100    let zero = T::zero();
101    let one = T::one();
102    [[one, zero, zero], [zero, one, zero], [zero, zero, one]]
103}
del_geo_core/mat3_array_of_rows.rs

del_geo_core/
mat3_array_of_rows.rs
