Skip to main content

embedded_matrix/
matrix2x2.rs

1use crate::{check_det, MatrixError};
2use core::ops::{Add, AddAssign, Index, Mul, MulAssign, Neg, Sub, SubAssign};
3
4/// Matrice 2×2 en `f32`, stockée en row-major.
5///
6/// ```
7/// use embedded_matrix::Matrix2x2;
8///
9/// let m = Matrix2x2::new([[1.0, 2.0], [3.0, 4.0]]);
10/// assert!((m.det() - (-2.0)).abs() < 1e-5);
11/// assert_eq!(m[(1, 0)], 3.0);
12/// ```
13#[derive(Debug, Clone, Copy, PartialEq, Default)]
14pub struct Matrix2x2 {
15    data: [[f32; 2]; 2],
16}
17
18impl Matrix2x2 {
19    // ── Constructeurs ────────────────────────────────────────
20
21    /// Crée une matrice à partir d'un tableau row-major.
22    #[inline]
23    pub const fn new(data: [[f32; 2]; 2]) -> Self {
24        Self { data }
25    }
26
27    /// Matrice identité.
28    #[inline]
29    pub const fn identity() -> Self {
30        Self::new([[1.0, 0.0], [0.0, 1.0]])
31    }
32
33    /// Matrice nulle.
34    #[inline]
35    pub const fn zero() -> Self {
36        Self::new([[0.0; 2]; 2])
37    }
38
39    /// Matrice diagonale.
40    #[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    // ── Accès ────────────────────────────────────────────────
46
47    /// Retourne l'élément à la ligne `row` et colonne `col` (0-indexé).
48    ///
49    /// # Panics
50    /// Panique si `row` ou `col` vaut 2 ou plus.
51    #[inline]
52    pub fn get(&self, row: usize, col: usize) -> f32 {
53        self.data[row][col]
54    }
55
56    /// Donne accès aux données brutes row-major.
57    #[inline]
58    pub fn as_array(&self) -> &[[f32; 2]; 2] {
59        &self.data
60    }
61
62    // ── Opérations arithmétiques ─────────────────────────────
63
64    /// Addition terme à terme.
65    #[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    /// Soustraction terme à terme.
73    #[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    /// Produit matriciel standard.
81    #[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    /// Multiplication par un scalaire.
91    #[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    /// Produit matrice × vecteur colonne.
98    #[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    // ── Propriétés ───────────────────────────────────────────
105
106    /// Transposée.
107    #[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    /// Déterminant : `ad - bc`.
114    #[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    /// Trace (somme des éléments diagonaux).
121    #[inline]
122    pub fn trace(&self) -> f32 {
123        self.data[0][0] + self.data[1][1]
124    }
125
126    /// Compare deux matrices terme à terme avec une tolérance absolue.
127    /// Renvoie `false` si un terme est NaN.
128    #[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    /// Inverse de la matrice.
135    ///
136    /// # Errors
137    /// - [`MatrixError::SingularMatrix`] si `|det|` est négligeable devant
138    ///   `|ad| + |bc|` (seuil relatif, voir la doc de la crate).
139    /// - [`MatrixError::NonFiniteValue`] si la matrice contient NaN / ±∞
140    ///   ou si le calcul déborde.
141    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
153// ── Conversions et accès par indice ──────────────────────────
154
155impl 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
169/// `m[(ligne, colonne)]` : panique si hors limites.
170impl 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
178// ── core::ops ────────────────────────────────────────────────
179
180/// `a + b` : addition terme à terme.
181impl Add for Matrix2x2 {
182    type Output = Self;
183    #[inline]
184    fn add(self, rhs: Self) -> Self {
185        Matrix2x2::add(&self, &rhs)
186    }
187}
188
189/// `a - b` : soustraction terme à terme.
190impl Sub for Matrix2x2 {
191    type Output = Self;
192    #[inline]
193    fn sub(self, rhs: Self) -> Self {
194        Matrix2x2::sub(&self, &rhs)
195    }
196}
197
198/// `a * b` : produit matriciel.
199impl Mul for Matrix2x2 {
200    type Output = Self;
201    #[inline]
202    fn mul(self, rhs: Self) -> Self {
203        Matrix2x2::mul(&self, &rhs)
204    }
205}
206
207/// `a * s` : multiplication par un scalaire `f32`.
208impl Mul<f32> for Matrix2x2 {
209    type Output = Self;
210    #[inline]
211    fn mul(self, s: f32) -> Self {
212        self.scale(s)
213    }
214}
215
216/// `s * a` : multiplication par un scalaire `f32` à gauche.
217impl Mul<Matrix2x2> for f32 {
218    type Output = Matrix2x2;
219    #[inline]
220    fn mul(self, m: Matrix2x2) -> Matrix2x2 {
221        m.scale(self)
222    }
223}
224
225/// `a * v` : matrice × vecteur colonne.
226impl 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
234/// `-a` : négation terme à terme.
235impl 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}