1use 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 #[cfg(feature = "trig")]
49 #[inline]
50 pub fn rotation_x(angle: f32) -> Self {
51 let (s, c) = embedded_trig_f32::sincos(angle);
52 Self::new([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
53 }
54
55 #[cfg(feature = "trig")]
59 #[inline]
60 pub fn rotation_y(angle: f32) -> Self {
61 let (s, c) = embedded_trig_f32::sincos(angle);
62 Self::new([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
63 }
64
65 #[cfg(feature = "trig")]
78 #[inline]
79 pub fn rotation_z(angle: f32) -> Self {
80 let (s, c) = embedded_trig_f32::sincos(angle);
81 Self::new([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
82 }
83
84 #[inline]
91 pub fn get(&self, row: usize, col: usize) -> f32 {
92 self.data[row][col]
93 }
94
95 #[inline]
97 pub fn as_array(&self) -> &[[f32; 3]; 3] {
98 &self.data
99 }
100
101 #[inline]
105 #[allow(clippy::should_implement_trait)]
106 pub fn add(&self, rhs: &Self) -> Self {
107 let (a, b) = (&self.data, &rhs.data);
108 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] + b[i][j])))
109 }
110
111 #[inline]
113 #[allow(clippy::should_implement_trait)]
114 pub fn sub(&self, rhs: &Self) -> Self {
115 let (a, b) = (&self.data, &rhs.data);
116 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] - b[i][j])))
117 }
118
119 #[inline]
121 #[allow(clippy::should_implement_trait)]
122 pub fn mul(&self, rhs: &Self) -> Self {
123 let (a, b) = (&self.data, &rhs.data);
124 Self::new(core::array::from_fn(|i| {
125 core::array::from_fn(|j| a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j])
126 }))
127 }
128
129 #[inline]
131 pub fn scale(&self, s: f32) -> Self {
132 let a = &self.data;
133 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] * s)))
134 }
135
136 #[inline]
138 pub fn mul_vec(&self, v: [f32; 3]) -> [f32; 3] {
139 let a = &self.data;
140 core::array::from_fn(|i| a[i][0] * v[0] + a[i][1] * v[1] + a[i][2] * v[2])
141 }
142
143 #[inline]
147 pub fn transpose(&self) -> Self {
148 let a = &self.data;
149 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[j][i])))
150 }
151
152 #[inline]
154 pub fn det(&self) -> f32 {
155 let d = &self.data;
156 d[0][0] * (d[1][1] * d[2][2] - d[1][2] * d[2][1])
157 - d[0][1] * (d[1][0] * d[2][2] - d[1][2] * d[2][0])
158 + d[0][2] * (d[1][0] * d[2][1] - d[1][1] * d[2][0])
159 }
160
161 #[inline]
163 pub fn trace(&self) -> f32 {
164 self.data[0][0] + self.data[1][1] + self.data[2][2]
165 }
166
167 pub fn solve(&self, b: [f32; 3]) -> Result<[f32; 3], MatrixError> {
188 let x = self.inv()?.mul_vec(b);
189 if x.iter().all(|v| v.is_finite()) {
190 Ok(x)
191 } else {
192 Err(MatrixError::NonFiniteValue)
193 }
194 }
195
196 #[inline]
199 pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
200 let (a, b) = (&self.data, &other.data);
201 (0..3).all(|i| (0..3).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
202 }
203
204 pub fn inv(&self) -> Result<Self, MatrixError> {
213 let d = &self.data;
214
215 let c00 = d[1][1] * d[2][2] - d[1][2] * d[2][1];
217 let c01 = d[1][2] * d[2][0] - d[1][0] * d[2][2];
218 let c02 = d[1][0] * d[2][1] - d[1][1] * d[2][0];
219
220 let det = d[0][0] * c00 + d[0][1] * c01 + d[0][2] * c02;
221
222 let bound = d[0][0].abs() * ((d[1][1] * d[2][2]).abs() + (d[1][2] * d[2][1]).abs())
224 + d[0][1].abs() * ((d[1][0] * d[2][2]).abs() + (d[1][2] * d[2][0]).abs())
225 + d[0][2].abs() * ((d[1][0] * d[2][1]).abs() + (d[1][1] * d[2][0]).abs());
226
227 check_det(det, bound)?;
228 let inv_det = 1.0 / det;
229
230 Ok(Self::new([
232 [
233 c00 * inv_det,
234 (d[0][2] * d[2][1] - d[0][1] * d[2][2]) * inv_det,
235 (d[0][1] * d[1][2] - d[0][2] * d[1][1]) * inv_det,
236 ],
237 [
238 c01 * inv_det,
239 (d[0][0] * d[2][2] - d[0][2] * d[2][0]) * inv_det,
240 (d[0][2] * d[1][0] - d[0][0] * d[1][2]) * inv_det,
241 ],
242 [
243 c02 * inv_det,
244 (d[0][1] * d[2][0] - d[0][0] * d[2][1]) * inv_det,
245 (d[0][0] * d[1][1] - d[0][1] * d[1][0]) * inv_det,
246 ],
247 ]))
248 }
249}
250
251impl From<[[f32; 3]; 3]> for Matrix3x3 {
254 #[inline]
255 fn from(data: [[f32; 3]; 3]) -> Self {
256 Self::new(data)
257 }
258}
259
260impl From<Matrix3x3> for [[f32; 3]; 3] {
261 #[inline]
262 fn from(m: Matrix3x3) -> Self {
263 m.data
264 }
265}
266
267impl Index<(usize, usize)> for Matrix3x3 {
269 type Output = f32;
270 #[inline]
271 fn index(&self, (row, col): (usize, usize)) -> &f32 {
272 &self.data[row][col]
273 }
274}
275
276impl Add for Matrix3x3 {
280 type Output = Self;
281 #[inline]
282 fn add(self, rhs: Self) -> Self {
283 Matrix3x3::add(&self, &rhs)
284 }
285}
286
287impl Sub for Matrix3x3 {
289 type Output = Self;
290 #[inline]
291 fn sub(self, rhs: Self) -> Self {
292 Matrix3x3::sub(&self, &rhs)
293 }
294}
295
296impl Mul for Matrix3x3 {
298 type Output = Self;
299 #[inline]
300 fn mul(self, rhs: Self) -> Self {
301 Matrix3x3::mul(&self, &rhs)
302 }
303}
304
305impl Mul<f32> for Matrix3x3 {
307 type Output = Self;
308 #[inline]
309 fn mul(self, s: f32) -> Self {
310 self.scale(s)
311 }
312}
313
314impl Mul<Matrix3x3> for f32 {
316 type Output = Matrix3x3;
317 #[inline]
318 fn mul(self, m: Matrix3x3) -> Matrix3x3 {
319 m.scale(self)
320 }
321}
322
323impl Mul<[f32; 3]> for Matrix3x3 {
325 type Output = [f32; 3];
326 #[inline]
327 fn mul(self, v: [f32; 3]) -> [f32; 3] {
328 self.mul_vec(v)
329 }
330}
331
332impl Neg for Matrix3x3 {
334 type Output = Self;
335 #[inline]
336 fn neg(self) -> Self {
337 self.scale(-1.0)
338 }
339}
340
341impl AddAssign for Matrix3x3 {
342 #[inline]
343 fn add_assign(&mut self, rhs: Self) {
344 *self = *self + rhs;
345 }
346}
347
348impl SubAssign for Matrix3x3 {
349 #[inline]
350 fn sub_assign(&mut self, rhs: Self) {
351 *self = *self - rhs;
352 }
353}
354
355impl MulAssign for Matrix3x3 {
356 #[inline]
357 fn mul_assign(&mut self, rhs: Self) {
358 *self = *self * rhs;
359 }
360}
361
362impl MulAssign<f32> for Matrix3x3 {
363 #[inline]
364 fn mul_assign(&mut self, s: f32) {
365 *self = self.scale(s);
366 }
367}