matext4cgmath/
exp_decomp.rs

1use crate::*;
2
3impl<F: BaseFloat> Exponential for Matrix2<F> {}
4impl<F: BaseFloat> Exponential for Matrix3<F> {}
5impl<F: BaseFloat> Exponential for Matrix4<F> {}
6
7impl<F: BaseFloat> Decomposition for Matrix2<F> {
8    fn iwasawa_decomposition(self) -> Option<(Self, Self, Self)> {
9        let v0 = self[0];
10        let mag0 = dot(v0, v0);
11        if mag0 <= F::zero() {
12            return None;
13        }
14        let n0 = dot(self[1], v0) / mag0;
15        let v1 = self[1] - v0 * n0;
16        let mag1 = dot(v1, v1);
17        if mag1 <= F::zero() {
18            return None;
19        }
20        let (a0, a1) = (F::sqrt(mag0), F::sqrt(mag1));
21        Some((
22            Matrix2::from_cols(v0 / a0, v1 / a1),
23            Matrix2::from_diagonal(Vector2::new(a0, a1)),
24            Matrix2::new(F::one(), F::zero(), n0, F::one()),
25        ))
26    }
27}
28
29impl<F: BaseFloat> Decomposition for Matrix3<F> {
30    fn iwasawa_decomposition(self) -> Option<(Self, Self, Self)> {
31        let v0 = self[0];
32        let mag0 = dot(v0, v0);
33        if mag0 <= F::zero() {
34            return None;
35        }
36        let n0 = dot(self[1], v0) / mag0;
37        let v1 = self[1] - v0 * n0;
38        let mag1 = dot(v1, v1);
39        if mag1 <= F::zero() {
40            return None;
41        }
42        let n1 = dot(self[2], v0) / mag0;
43        let n2 = dot(self[2], v1) / mag1;
44        let v2 = self[2] - v0 * n1 - v1 * n2;
45        let mag2 = dot(v2, v2);
46        if mag2 <= F::zero() {
47            return None;
48        }
49        let (a0, a1, a2) = (F::sqrt(mag0), F::sqrt(mag1), F::sqrt(mag2));
50        #[rustfmt::skip]
51        let res = Some((
52            Matrix3::from_cols(v0 / a0, v1 / a1, v2 / a2),
53            Matrix3::from_diagonal(Vector3::new(a0, a1, a2)),
54            Matrix3::new(
55                F::one(), F::zero(), F::zero(),
56                n0, F::one(), F::zero(),
57                n1, n2, F::one(),
58            ),
59        ));
60        res
61    }
62}
63
64impl<F: BaseFloat> Decomposition for Matrix4<F> {
65    fn iwasawa_decomposition(self) -> Option<(Self, Self, Self)> {
66        let v0 = self[0];
67        let mag0 = dot(v0, v0);
68        if mag0 <= F::zero() {
69            return None;
70        }
71        let n0 = dot(self[1], v0) / mag0;
72        let v1 = self[1] - v0 * n0;
73        let mag1 = dot(v1, v1);
74        if mag1 <= F::zero() {
75            return None;
76        }
77        let n1 = dot(self[2], v0) / mag0;
78        let n2 = dot(self[2], v1) / mag1;
79        let v2 = self[2] - v0 * n1 - v1 * n2;
80        let mag2 = dot(v2, v2);
81        if mag2 <= F::zero() {
82            return None;
83        }
84        let n3 = dot(self[3], v0) / mag0;
85        let n4 = dot(self[3], v1) / mag1;
86        let n5 = dot(self[3], v2) / mag2;
87        let v3 = self[3] - v0 * n3 - v1 * n4 - v2 * n5;
88        let mag3 = dot(v3, v3);
89        if mag3 <= F::zero() {
90            return None;
91        }
92        let (a0, a1, a2, a3) = (F::sqrt(mag0), F::sqrt(mag1), F::sqrt(mag2), F::sqrt(mag3));
93        #[rustfmt::skip]
94        let res = Some((
95            Matrix4::from_cols(v0 / a0, v1 / a1, v2 / a2, v3 / a3),
96            Matrix4::from_diagonal(Vector4::new(a0, a1, a2, a3)),
97            Matrix4::new(
98                F::one(), F::zero(), F::zero(), F::zero(),
99                n0, F::one(), F::zero(), F::zero(),
100                n1, n2, F::one(), F::zero(),
101                n3, n4, n5, F::one(),
102            ),
103        ));
104        res
105    }
106}
matext4cgmath/exp_decomp.rs

matext4cgmath/
exp_decomp.rs
