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 #[cfg(feature = "trig")]
59 #[inline]
60 pub fn rotation(angle: f32) -> Self {
61 let (s, c) = embedded_trig_f32::sincos(angle);
62 Self::new([[c, -s], [s, c]])
63 }
64
65 #[inline]
72 pub fn get(&self, row: usize, col: usize) -> f32 {
73 self.data[row][col]
74 }
75
76 #[inline]
78 pub fn as_array(&self) -> &[[f32; 2]; 2] {
79 &self.data
80 }
81
82 #[inline]
86 #[allow(clippy::should_implement_trait)]
87 pub fn add(&self, rhs: &Self) -> Self {
88 let (a, b) = (&self.data, &rhs.data);
89 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] + b[i][j])))
90 }
91
92 #[inline]
94 #[allow(clippy::should_implement_trait)]
95 pub fn sub(&self, rhs: &Self) -> Self {
96 let (a, b) = (&self.data, &rhs.data);
97 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] - b[i][j])))
98 }
99
100 #[inline]
102 #[allow(clippy::should_implement_trait)]
103 pub fn mul(&self, rhs: &Self) -> Self {
104 let (a, b) = (&self.data, &rhs.data);
105 Self::new(core::array::from_fn(|i| {
106 core::array::from_fn(|j| a[i][0] * b[0][j] + a[i][1] * b[1][j])
107 }))
108 }
109
110 #[inline]
112 pub fn scale(&self, s: f32) -> Self {
113 let a = &self.data;
114 Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] * s)))
115 }
116
117 #[inline]
119 pub fn mul_vec(&self, v: [f32; 2]) -> [f32; 2] {
120 let a = &self.data;
121 [a[0][0] * v[0] + a[0][1] * v[1], a[1][0] * v[0] + a[1][1] * v[1]]
122 }
123
124 #[inline]
128 pub fn transpose(&self) -> Self {
129 let a = &self.data;
130 Self::new([[a[0][0], a[1][0]], [a[0][1], a[1][1]]])
131 }
132
133 #[inline]
135 pub fn det(&self) -> f32 {
136 let a = &self.data;
137 a[0][0] * a[1][1] - a[0][1] * a[1][0]
138 }
139
140 #[inline]
142 pub fn trace(&self) -> f32 {
143 self.data[0][0] + self.data[1][1]
144 }
145
146 pub fn solve(&self, b: [f32; 2]) -> Result<[f32; 2], MatrixError> {
164 let [[a, m], [c, d]] = self.data;
165 let det = a * d - m * c;
166 check_det(det, (a * d).abs() + (m * c).abs())?;
167 let x = [(b[0] * d - m * b[1]) / det, (a * b[1] - c * b[0]) / det];
168 if x[0].is_finite() && x[1].is_finite() {
169 Ok(x)
170 } else {
171 Err(MatrixError::NonFiniteValue)
172 }
173 }
174
175 #[inline]
178 pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
179 let (a, b) = (&self.data, &other.data);
180 (0..2).all(|i| (0..2).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
181 }
182
183 pub fn inv(&self) -> Result<Self, MatrixError> {
191 let [[a, b], [c, d]] = self.data;
192 let det = a * d - b * c;
193 check_det(det, (a * d).abs() + (b * c).abs())?;
194 let inv_det = 1.0 / det;
195 Ok(Self::new([
196 [d * inv_det, -b * inv_det],
197 [-c * inv_det, a * inv_det],
198 ]))
199 }
200}
201
202impl From<[[f32; 2]; 2]> for Matrix2x2 {
205 #[inline]
206 fn from(data: [[f32; 2]; 2]) -> Self {
207 Self::new(data)
208 }
209}
210
211impl From<Matrix2x2> for [[f32; 2]; 2] {
212 #[inline]
213 fn from(m: Matrix2x2) -> Self {
214 m.data
215 }
216}
217
218impl Index<(usize, usize)> for Matrix2x2 {
220 type Output = f32;
221 #[inline]
222 fn index(&self, (row, col): (usize, usize)) -> &f32 {
223 &self.data[row][col]
224 }
225}
226
227impl Add for Matrix2x2 {
231 type Output = Self;
232 #[inline]
233 fn add(self, rhs: Self) -> Self {
234 Matrix2x2::add(&self, &rhs)
235 }
236}
237
238impl Sub for Matrix2x2 {
240 type Output = Self;
241 #[inline]
242 fn sub(self, rhs: Self) -> Self {
243 Matrix2x2::sub(&self, &rhs)
244 }
245}
246
247impl Mul for Matrix2x2 {
249 type Output = Self;
250 #[inline]
251 fn mul(self, rhs: Self) -> Self {
252 Matrix2x2::mul(&self, &rhs)
253 }
254}
255
256impl Mul<f32> for Matrix2x2 {
258 type Output = Self;
259 #[inline]
260 fn mul(self, s: f32) -> Self {
261 self.scale(s)
262 }
263}
264
265impl Mul<Matrix2x2> for f32 {
267 type Output = Matrix2x2;
268 #[inline]
269 fn mul(self, m: Matrix2x2) -> Matrix2x2 {
270 m.scale(self)
271 }
272}
273
274impl Mul<[f32; 2]> for Matrix2x2 {
276 type Output = [f32; 2];
277 #[inline]
278 fn mul(self, v: [f32; 2]) -> [f32; 2] {
279 self.mul_vec(v)
280 }
281}
282
283impl Neg for Matrix2x2 {
285 type Output = Self;
286 #[inline]
287 fn neg(self) -> Self {
288 self.scale(-1.0)
289 }
290}
291
292impl AddAssign for Matrix2x2 {
293 #[inline]
294 fn add_assign(&mut self, rhs: Self) {
295 *self = *self + rhs;
296 }
297}
298
299impl SubAssign for Matrix2x2 {
300 #[inline]
301 fn sub_assign(&mut self, rhs: Self) {
302 *self = *self - rhs;
303 }
304}
305
306impl MulAssign for Matrix2x2 {
307 #[inline]
308 fn mul_assign(&mut self, rhs: Self) {
309 *self = *self * rhs;
310 }
311}
312
313impl MulAssign<f32> for Matrix2x2 {
314 #[inline]
315 fn mul_assign(&mut self, s: f32) {
316 *self = self.scale(s);
317 }
318}