embedded_matrix/
matrix2x2.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 Matrix2x2 {
15 data: [[f32; 2]; 2],
16}
17
18impl Matrix2x2 {
19 #[inline]
23 pub const fn new(data: [[f32; 2]; 2]) -> Self {
24 Self { data }
25 }
26
27 #[inline]
29 pub const fn identity() -> Self {
30 Self::new([[1.0, 0.0], [0.0, 1.0]])
31 }
32
33 #[inline]
35 pub const fn zero() -> Self {
36 Self::new([[0.0; 2]; 2])
37 }
38
39 #[inline]
41 pub const fn from_diagonal(d: [f32; 2]) -> Self {
42 Self::new([[d[0], 0.0], [0.0, d[1]]])
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; 2]; 2] {
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])
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; 2]) -> [f32; 2] {
100 let a = &self.data;
101 [a[0][0] * v[0] + a[0][1] * v[1], a[1][0] * v[0] + a[1][1] * v[1]]
102 }
103
104 #[inline]
108 pub fn transpose(&self) -> Self {
109 let a = &self.data;
110 Self::new([[a[0][0], a[1][0]], [a[0][1], a[1][1]]])
111 }
112
113 #[inline]
115 pub fn det(&self) -> f32 {
116 let a = &self.data;
117 a[0][0] * a[1][1] - a[0][1] * a[1][0]
118 }
119
120 #[inline]
122 pub fn trace(&self) -> f32 {
123 self.data[0][0] + self.data[1][1]
124 }
125
126 #[inline]
129 pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
130 let (a, b) = (&self.data, &other.data);
131 (0..2).all(|i| (0..2).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
132 }
133
134 pub fn inv(&self) -> Result<Self, MatrixError> {
142 let [[a, b], [c, d]] = self.data;
143 let det = a * d - b * c;
144 check_det(det, (a * d).abs() + (b * c).abs())?;
145 let inv_det = 1.0 / det;
146 Ok(Self::new([
147 [d * inv_det, -b * inv_det],
148 [-c * inv_det, a * inv_det],
149 ]))
150 }
151}
152
153impl From<[[f32; 2]; 2]> for Matrix2x2 {
156 #[inline]
157 fn from(data: [[f32; 2]; 2]) -> Self {
158 Self::new(data)
159 }
160}
161
162impl From<Matrix2x2> for [[f32; 2]; 2] {
163 #[inline]
164 fn from(m: Matrix2x2) -> Self {
165 m.data
166 }
167}
168
169impl Index<(usize, usize)> for Matrix2x2 {
171 type Output = f32;
172 #[inline]
173 fn index(&self, (row, col): (usize, usize)) -> &f32 {
174 &self.data[row][col]
175 }
176}
177
178impl Add for Matrix2x2 {
182 type Output = Self;
183 #[inline]
184 fn add(self, rhs: Self) -> Self {
185 Matrix2x2::add(&self, &rhs)
186 }
187}
188
189impl Sub for Matrix2x2 {
191 type Output = Self;
192 #[inline]
193 fn sub(self, rhs: Self) -> Self {
194 Matrix2x2::sub(&self, &rhs)
195 }
196}
197
198impl Mul for Matrix2x2 {
200 type Output = Self;
201 #[inline]
202 fn mul(self, rhs: Self) -> Self {
203 Matrix2x2::mul(&self, &rhs)
204 }
205}
206
207impl Mul<f32> for Matrix2x2 {
209 type Output = Self;
210 #[inline]
211 fn mul(self, s: f32) -> Self {
212 self.scale(s)
213 }
214}
215
216impl Mul<Matrix2x2> for f32 {
218 type Output = Matrix2x2;
219 #[inline]
220 fn mul(self, m: Matrix2x2) -> Matrix2x2 {
221 m.scale(self)
222 }
223}
224
225impl Mul<[f32; 2]> for Matrix2x2 {
227 type Output = [f32; 2];
228 #[inline]
229 fn mul(self, v: [f32; 2]) -> [f32; 2] {
230 self.mul_vec(v)
231 }
232}
233
234impl Neg for Matrix2x2 {
236 type Output = Self;
237 #[inline]
238 fn neg(self) -> Self {
239 self.scale(-1.0)
240 }
241}
242
243impl AddAssign for Matrix2x2 {
244 #[inline]
245 fn add_assign(&mut self, rhs: Self) {
246 *self = *self + rhs;
247 }
248}
249
250impl SubAssign for Matrix2x2 {
251 #[inline]
252 fn sub_assign(&mut self, rhs: Self) {
253 *self = *self - rhs;
254 }
255}
256
257impl MulAssign for Matrix2x2 {
258 #[inline]
259 fn mul_assign(&mut self, rhs: Self) {
260 *self = *self * rhs;
261 }
262}
263
264impl MulAssign<f32> for Matrix2x2 {
265 #[inline]
266 fn mul_assign(&mut self, s: f32) {
267 *self = self.scale(s);
268 }
269}