1#[derive(Debug, Clone, Copy, PartialEq)]
11pub struct Matrix3(pub [[f64; 3]; 3]);
12
13impl Matrix3 {
14 pub const IDENTITY: Self = Self::diagonal([1.0, 1.0, 1.0]);
16
17 pub const fn diagonal([x, y, z]: [f64; 3]) -> Self {
19 Self([[x, 0.0, 0.0], [0.0, y, 0.0], [0.0, 0.0, z]])
20 }
21
22 pub const fn transform(&self, [x, y, z]: [f64; 3]) -> [f64; 3] {
24 let Self(m) = self;
25 [
26 m[0][0] * x + m[0][1] * y + m[0][2] * z,
27 m[1][0] * x + m[1][1] * y + m[1][2] * z,
28 m[2][0] * x + m[2][1] * y + m[2][2] * z,
29 ]
30 }
31
32 pub const fn then(&self, next: &Self) -> Self {
34 let (Self(first), Self(second)) = (self, next);
35 let mut out = [[0.0; 3]; 3];
36 let mut row = 0;
37 while row < 3 {
38 let mut col = 0;
39 while col < 3 {
40 out[row][col] =
41 second[row][0] * first[0][col] + second[row][1] * first[1][col] + second[row][2] * first[2][col];
42 col += 1;
43 }
44 row += 1;
45 }
46 Self(out)
47 }
48
49 pub const fn inverse(&self) -> Self {
59 let Self(m) = self;
60 let c00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
61 let c01 = m[1][2] * m[2][0] - m[1][0] * m[2][2];
62 let c02 = m[1][0] * m[2][1] - m[1][1] * m[2][0];
63 let det = m[0][0] * c00 + m[0][1] * c01 + m[0][2] * c02;
64 Self([
65 [c00 / det, (m[0][2] * m[2][1] - m[0][1] * m[2][2]) / det, (m[0][1] * m[1][2] - m[0][2] * m[1][1]) / det],
66 [c01 / det, (m[0][0] * m[2][2] - m[0][2] * m[2][0]) / det, (m[0][2] * m[1][0] - m[0][0] * m[1][2]) / det],
67 [c02 / det, (m[0][1] * m[2][0] - m[0][0] * m[2][1]) / det, (m[0][0] * m[1][1] - m[0][1] * m[1][0]) / det],
68 ])
69 }
70}