glam_matrix_extras/
mat_ext.rs

1use core::ops::Mul;
2
3#[cfg(feature = "f64")]
4use glam::{DMat2, DMat3, DMat4, DVec2, DVec3, DVec4};
5use glam::{Mat2, Mat3, Mat3A, Mat4, Vec2, Vec3, Vec3A, Vec4};
6
7// TODO: Implement optimized versions of the `inverse_or_zero` method.
8
9/// An extension trait for matrices.
10pub trait SquareMatExt {
11    /// The vector type associated with the matrix.
12    type Vector;
13
14    /// Creates a new matrix from the outer product `a * b^T`.
15    #[must_use]
16    fn from_outer_product(a: Self::Vector, b: Self::Vector) -> Self;
17
18    /// Returns the diagonal of the matrix.
19    #[must_use]
20    fn diagonal(&self) -> Self::Vector;
21
22    /// Returns the inverse of `self`, or a zero matrix if the matrix is not invertible.
23    #[must_use]
24    fn inverse_or_zero(&self) -> Self;
25
26    /// Returns `true` if the matrix is symmetric.
27    #[must_use]
28    fn is_symmetric(&self) -> bool;
29}
30
31impl SquareMatExt for Mat2 {
32    type Vector = Vec2;
33
34    #[inline]
35    fn from_outer_product(a: Vec2, b: Vec2) -> Self {
36        Mat2::from_cols(a * b.x, a * b.y)
37    }
38
39    #[inline]
40    fn inverse_or_zero(&self) -> Self {
41        let inverse = self.inverse();
42        if inverse.is_finite() {
43            inverse
44        } else {
45            Mat2::ZERO
46        }
47    }
48
49    #[inline]
50    fn diagonal(&self) -> Vec2 {
51        Vec2::new(self.x_axis.x, self.y_axis.y)
52    }
53
54    #[inline]
55    fn is_symmetric(&self) -> bool {
56        self.x_axis.y == self.y_axis.x
57    }
58}
59
60#[cfg(feature = "f64")]
61impl SquareMatExt for DMat2 {
62    type Vector = DVec2;
63
64    #[inline]
65    fn from_outer_product(a: DVec2, b: DVec2) -> Self {
66        DMat2::from_cols(a * b.x, a * b.y)
67    }
68
69    #[inline]
70    fn inverse_or_zero(&self) -> Self {
71        let inverse = self.inverse();
72        if inverse.is_finite() {
73            inverse
74        } else {
75            DMat2::ZERO
76        }
77    }
78
79    #[inline]
80    fn diagonal(&self) -> DVec2 {
81        DVec2::new(self.x_axis.x, self.y_axis.y)
82    }
83
84    #[inline]
85    fn is_symmetric(&self) -> bool {
86        self.x_axis.y == self.y_axis.x
87    }
88}
89
90impl SquareMatExt for Mat3 {
91    type Vector = Vec3;
92
93    #[inline]
94    fn from_outer_product(a: Vec3, b: Vec3) -> Self {
95        Mat3::from_cols(a * b.x, a * b.y, a * b.z)
96    }
97
98    #[inline]
99    fn inverse_or_zero(&self) -> Self {
100        let tmp0 = self.y_axis.cross(self.z_axis);
101        let tmp1 = self.z_axis.cross(self.x_axis);
102        let tmp2 = self.x_axis.cross(self.y_axis);
103        let det = self.z_axis.dot(tmp2);
104        if det != 0.0 {
105            let inv_det = Vec3::splat(det.recip());
106            Self::from_cols(tmp0.mul(inv_det), tmp1.mul(inv_det), tmp2.mul(inv_det)).transpose()
107        } else {
108            Mat3::ZERO
109        }
110    }
111
112    #[inline]
113    fn diagonal(&self) -> Vec3 {
114        Vec3::new(self.x_axis.x, self.y_axis.y, self.z_axis.z)
115    }
116
117    #[inline]
118    fn is_symmetric(&self) -> bool {
119        self.x_axis.y == self.y_axis.x
120            && self.x_axis.z == self.z_axis.x
121            && self.y_axis.z == self.z_axis.y
122    }
123}
124
125#[cfg(feature = "f64")]
126impl SquareMatExt for DMat3 {
127    type Vector = DVec3;
128
129    #[inline]
130    fn from_outer_product(a: DVec3, b: DVec3) -> Self {
131        DMat3::from_cols(a * b.x, a * b.y, a * b.z)
132    }
133
134    #[inline]
135    fn inverse_or_zero(&self) -> Self {
136        let tmp0 = self.y_axis.cross(self.z_axis);
137        let tmp1 = self.z_axis.cross(self.x_axis);
138        let tmp2 = self.x_axis.cross(self.y_axis);
139        let det = self.z_axis.dot(tmp2);
140        if det != 0.0 {
141            let inv_det = DVec3::splat(det.recip());
142            Self::from_cols(tmp0.mul(inv_det), tmp1.mul(inv_det), tmp2.mul(inv_det)).transpose()
143        } else {
144            DMat3::ZERO
145        }
146    }
147
148    #[inline]
149    fn diagonal(&self) -> DVec3 {
150        DVec3::new(self.x_axis.x, self.y_axis.y, self.z_axis.z)
151    }
152
153    #[inline]
154    fn is_symmetric(&self) -> bool {
155        self.x_axis.y == self.y_axis.x
156            && self.x_axis.z == self.z_axis.x
157            && self.y_axis.z == self.z_axis.y
158    }
159}
160
161impl SquareMatExt for Mat3A {
162    type Vector = Vec3A;
163
164    #[inline]
165    fn from_outer_product(a: Vec3A, b: Vec3A) -> Self {
166        Mat3A::from_cols(a * b.x, a * b.y, a * b.z)
167    }
168
169    #[inline]
170    fn inverse_or_zero(&self) -> Self {
171        let tmp0 = self.y_axis.cross(self.z_axis);
172        let tmp1 = self.z_axis.cross(self.x_axis);
173        let tmp2 = self.x_axis.cross(self.y_axis);
174        let det = self.z_axis.dot(tmp2);
175        if det != 0.0 {
176            let inv_det = Vec3A::splat(det.recip());
177            Self::from_cols(tmp0.mul(inv_det), tmp1.mul(inv_det), tmp2.mul(inv_det)).transpose()
178        } else {
179            Mat3A::ZERO
180        }
181    }
182
183    #[inline]
184    fn diagonal(&self) -> Vec3A {
185        Vec3A::new(self.x_axis.x, self.y_axis.y, self.z_axis.z)
186    }
187
188    #[inline]
189    fn is_symmetric(&self) -> bool {
190        self.x_axis.y == self.y_axis.x
191            && self.x_axis.z == self.z_axis.x
192            && self.y_axis.z == self.z_axis.y
193    }
194}
195
196impl SquareMatExt for Mat4 {
197    type Vector = Vec4;
198
199    #[inline]
200    fn from_outer_product(a: Vec4, b: Vec4) -> Self {
201        Mat4::from_cols(a * b.x, a * b.y, a * b.z, a * b.w)
202    }
203
204    #[inline]
205    fn inverse_or_zero(&self) -> Self {
206        let inverse = self.inverse();
207        if inverse.is_finite() {
208            inverse
209        } else {
210            Mat4::ZERO
211        }
212    }
213
214    #[inline]
215    fn is_symmetric(&self) -> bool {
216        self.x_axis.y == self.y_axis.x
217            && self.x_axis.z == self.z_axis.x
218            && self.x_axis.w == self.w_axis.x
219            && self.y_axis.z == self.z_axis.y
220            && self.y_axis.w == self.w_axis.y
221            && self.z_axis.w == self.w_axis.z
222    }
223
224    #[inline]
225    fn diagonal(&self) -> Vec4 {
226        Vec4::new(self.x_axis.x, self.y_axis.y, self.z_axis.z, self.w_axis.w)
227    }
228}
229
230#[cfg(feature = "f64")]
231impl SquareMatExt for DMat4 {
232    type Vector = DVec4;
233
234    #[inline]
235    fn from_outer_product(a: DVec4, b: DVec4) -> Self {
236        DMat4::from_cols(a * b.x, a * b.y, a * b.z, a * b.w)
237    }
238
239    #[inline]
240    fn inverse_or_zero(&self) -> Self {
241        let inverse = self.inverse();
242        if inverse.is_finite() {
243            inverse
244        } else {
245            DMat4::ZERO
246        }
247    }
248
249    #[inline]
250    fn is_symmetric(&self) -> bool {
251        self.x_axis.y == self.y_axis.x
252            && self.x_axis.z == self.z_axis.x
253            && self.x_axis.w == self.w_axis.x
254            && self.y_axis.z == self.z_axis.y
255            && self.y_axis.w == self.w_axis.y
256            && self.z_axis.w == self.w_axis.z
257    }
258
259    #[inline]
260    fn diagonal(&self) -> DVec4 {
261        DVec4::new(self.x_axis.x, self.y_axis.y, self.z_axis.z, self.w_axis.w)
262    }
263}
glam_matrix_extras/mat_ext.rs

glam_matrix_extras/
mat_ext.rs
