embedded_matrix/
matrix3x3.rs1use crate::{check_det, MatrixError};
2use core::ops::{Add, AddAssign, Index, Mul, MulAssign, Neg, Sub, SubAssign};
3
4#[derive(Debug, Clone, Copy, PartialEq, Default)]
14pub struct Matrix3x3 {
15 data: [[f32; 3]; 3],
16}
17
18impl Matrix3x3 {
19 #[inline]
23 pub const fn new(data: [[f32; 3]; 3]) -> Self {
24 Self { data }
25 }
26
27 #[inline]
29 pub const fn identity() -> Self {
30 Self::new([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
31 }
32
33 #[inline]
35 pub const fn zero() -> Self {
36 Self::new([[0.0; 3]; 3])
37 }
38
39 #[inline]
41 pub const fn from_diagonal(d: [f32; 3]) -> Self {
42 Self::new([[d[0], 0.0, 0.0], [0.0, d[1], 0.0], [0.0, 0.0, d[2]]])
43 }
44
45 #[inline]
52 pub fn get(&self, row: usize, col: usize) -> f32 {
53 self.data[row][col]
54 }
55
56 #[inline]
58 pub fn as_array(&self) -> &[[f32; 3]; 3] {
59 &self.data
60 }
61
62 #[inline]
66 #[allow(clippy::should_implement_trait)]
67 pub fn add(&self, rhs: &Self) -> Self {
68 let (a, b) = (&self.data, &rhs.data);
69 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] + b[i][j])))
70 }
71
72 #[inline]
74 #[allow(clippy::should_implement_trait)]
75 pub fn sub(&self, rhs: &Self) -> Self {
76 let (a, b) = (&self.data, &rhs.data);
77 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] - b[i][j])))
78 }
79
80 #[inline]
82 #[allow(clippy::should_implement_trait)]
83 pub fn mul(&self, rhs: &Self) -> Self {
84 let (a, b) = (&self.data, &rhs.data);
85 Self::new(core::array::from_fn(|i| {
86 core::array::from_fn(|j| a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j])
87 }))
88 }
89
90 #[inline]
92 pub fn scale(&self, s: f32) -> Self {
93 let a = &self.data;
94 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] * s)))
95 }
96
97 #[inline]
99 pub fn mul_vec(&self, v: [f32; 3]) -> [f32; 3] {
100 let a = &self.data;
101 core::array::from_fn(|i| a[i][0] * v[0] + a[i][1] * v[1] + a[i][2] * v[2])
102 }
103
104 #[inline]
108 pub fn transpose(&self) -> Self {
109 let a = &self.data;
110 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[j][i])))
111 }
112
113 #[inline]
115 pub fn det(&self) -> f32 {
116 let d = &self.data;
117 d[0][0] * (d[1][1] * d[2][2] - d[1][2] * d[2][1])
118 - d[0][1] * (d[1][0] * d[2][2] - d[1][2] * d[2][0])
119 + d[0][2] * (d[1][0] * d[2][1] - d[1][1] * d[2][0])
120 }
121
122 #[inline]
124 pub fn trace(&self) -> f32 {
125 self.data[0][0] + self.data[1][1] + self.data[2][2]
126 }
127
128 #[inline]
131 pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
132 let (a, b) = (&self.data, &other.data);
133 (0..3).all(|i| (0..3).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
134 }
135
136 pub fn inv(&self) -> Result<Self, MatrixError> {
145 let d = &self.data;
146
147 let c00 = d[1][1] * d[2][2] - d[1][2] * d[2][1];
149 let c01 = d[1][2] * d[2][0] - d[1][0] * d[2][2];
150 let c02 = d[1][0] * d[2][1] - d[1][1] * d[2][0];
151
152 let det = d[0][0] * c00 + d[0][1] * c01 + d[0][2] * c02;
153
154 let bound = d[0][0].abs() * ((d[1][1] * d[2][2]).abs() + (d[1][2] * d[2][1]).abs())
156 + d[0][1].abs() * ((d[1][0] * d[2][2]).abs() + (d[1][2] * d[2][0]).abs())
157 + d[0][2].abs() * ((d[1][0] * d[2][1]).abs() + (d[1][1] * d[2][0]).abs());
158
159 check_det(det, bound)?;
160 let inv_det = 1.0 / det;
161
162 Ok(Self::new([
164 [
165 c00 * inv_det,
166 (d[0][2] * d[2][1] - d[0][1] * d[2][2]) * inv_det,
167 (d[0][1] * d[1][2] - d[0][2] * d[1][1]) * inv_det,
168 ],
169 [
170 c01 * inv_det,
171 (d[0][0] * d[2][2] - d[0][2] * d[2][0]) * inv_det,
172 (d[0][2] * d[1][0] - d[0][0] * d[1][2]) * inv_det,
173 ],
174 [
175 c02 * inv_det,
176 (d[0][1] * d[2][0] - d[0][0] * d[2][1]) * inv_det,
177 (d[0][0] * d[1][1] - d[0][1] * d[1][0]) * inv_det,
178 ],
179 ]))
180 }
181}
182
183impl From<[[f32; 3]; 3]> for Matrix3x3 {
186 #[inline]
187 fn from(data: [[f32; 3]; 3]) -> Self {
188 Self::new(data)
189 }
190}
191
192impl From<Matrix3x3> for [[f32; 3]; 3] {
193 #[inline]
194 fn from(m: Matrix3x3) -> Self {
195 m.data
196 }
197}
198
199impl Index<(usize, usize)> for Matrix3x3 {
201 type Output = f32;
202 #[inline]
203 fn index(&self, (row, col): (usize, usize)) -> &f32 {
204 &self.data[row][col]
205 }
206}
207
208impl Add for Matrix3x3 {
212 type Output = Self;
213 #[inline]
214 fn add(self, rhs: Self) -> Self {
215 Matrix3x3::add(&self, &rhs)
216 }
217}
218
219impl Sub for Matrix3x3 {
221 type Output = Self;
222 #[inline]
223 fn sub(self, rhs: Self) -> Self {
224 Matrix3x3::sub(&self, &rhs)
225 }
226}
227
228impl Mul for Matrix3x3 {
230 type Output = Self;
231 #[inline]
232 fn mul(self, rhs: Self) -> Self {
233 Matrix3x3::mul(&self, &rhs)
234 }
235}
236
237impl Mul<f32> for Matrix3x3 {
239 type Output = Self;
240 #[inline]
241 fn mul(self, s: f32) -> Self {
242 self.scale(s)
243 }
244}
245
246impl Mul<Matrix3x3> for f32 {
248 type Output = Matrix3x3;
249 #[inline]
250 fn mul(self, m: Matrix3x3) -> Matrix3x3 {
251 m.scale(self)
252 }
253}
254
255impl Mul<[f32; 3]> for Matrix3x3 {
257 type Output = [f32; 3];
258 #[inline]
259 fn mul(self, v: [f32; 3]) -> [f32; 3] {
260 self.mul_vec(v)
261 }
262}
263
264impl Neg for Matrix3x3 {
266 type Output = Self;
267 #[inline]
268 fn neg(self) -> Self {
269 self.scale(-1.0)
270 }
271}
272
273impl AddAssign for Matrix3x3 {
274 #[inline]
275 fn add_assign(&mut self, rhs: Self) {
276 *self = *self + rhs;
277 }
278}
279
280impl SubAssign for Matrix3x3 {
281 #[inline]
282 fn sub_assign(&mut self, rhs: Self) {
283 *self = *self - rhs;
284 }
285}
286
287impl MulAssign for Matrix3x3 {
288 #[inline]
289 fn mul_assign(&mut self, rhs: Self) {
290 *self = *self * rhs;
291 }
292}
293
294impl MulAssign<f32> for Matrix3x3 {
295 #[inline]
296 fn mul_assign(&mut self, s: f32) {
297 *self = self.scale(s);
298 }
299}