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    /// Matrice de rotation 2D d'angle `angle` (radians, sens trigonométrique).
46    ///
47    /// Disponible avec la feature `trig` (utilise `embedded-trig-f32`).
48    /// Pour une rotation, l'inverse est la transposée : inutile d'appeler `inv`.
49    ///
50    /// ```
51    /// use embedded_matrix::Matrix2x2;
52    /// use embedded_trig_f32::consts::FRAC_PI_2;
53    ///
54    /// let r = Matrix2x2::rotation(FRAC_PI_2);
55    /// let p = r * [1.0, 0.0];
56    /// assert!(p[0].abs() < 1e-6 && (p[1] - 1.0).abs() < 1e-6);
57    /// ```
58    #[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    // ── Accès ────────────────────────────────────────────────
66
67    /// Retourne l'élément à la ligne `row` et colonne `col` (0-indexé).
68    ///
69    /// # Panics
70    /// Panique si `row` ou `col` vaut 2 ou plus.
71    #[inline]
72    pub fn get(&self, row: usize, col: usize) -> f32 {
73        self.data[row][col]
74    }
75
76    /// Donne accès aux données brutes row-major.
77    #[inline]
78    pub fn as_array(&self) -> &[[f32; 2]; 2] {
79        &self.data
80    }
81
82    // ── Opérations arithmétiques ─────────────────────────────
83
84    /// Addition terme à terme.
85    #[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    /// Soustraction terme à terme.
93    #[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    /// Produit matriciel standard.
101    #[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    /// Multiplication par un scalaire.
111    #[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    /// Produit matrice × vecteur colonne.
118    #[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    // ── Propriétés ───────────────────────────────────────────
125
126    /// Transposée.
127    #[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    /// Déterminant : `ad - bc`.
134    #[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    /// Trace (somme des éléments diagonaux).
141    #[inline]
142    pub fn trace(&self) -> f32 {
143        self.data[0][0] + self.data[1][1]
144    }
145
146    /// Résout le système `A·x = b` (formule de Cramer).
147    ///
148    /// Plus direct que `inv()` suivi d'un produit. Si vous avez plusieurs
149    /// seconds membres pour la même matrice, calculez `inv()` une seule fois.
150    ///
151    /// ```
152    /// use embedded_matrix::Matrix2x2;
153    ///
154    /// let a = Matrix2x2::new([[2.0, 1.0], [1.0, 3.0]]);
155    /// let x = a.solve([3.0, 5.0]).unwrap();
156    /// assert!((x[0] - 0.8).abs() < 1e-6 && (x[1] - 1.4).abs() < 1e-6);
157    /// ```
158    ///
159    /// # Errors
160    /// - [`MatrixError::SingularMatrix`] : même critère que [`inv`](Self::inv).
161    /// - [`MatrixError::NonFiniteValue`] : NaN / ±∞ dans la matrice, dans `b`,
162    ///   ou débordement.
163    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    /// Compare deux matrices terme à terme avec une tolérance absolue.
176    /// Renvoie `false` si un terme est NaN.
177    #[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    /// Inverse de la matrice.
184    ///
185    /// # Errors
186    /// - [`MatrixError::SingularMatrix`] si `|det|` est négligeable devant
187    ///   `|ad| + |bc|` (seuil relatif, voir la doc de la crate).
188    /// - [`MatrixError::NonFiniteValue`] si la matrice contient NaN / ±∞
189    ///   ou si le calcul déborde.
190    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
202// ── Conversions et accès par indice ──────────────────────────
203
204impl 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
218/// `m[(ligne, colonne)]` : panique si hors limites.
219impl 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
227// ── core::ops ────────────────────────────────────────────────
228
229/// `a + b` : addition terme à terme.
230impl Add for Matrix2x2 {
231    type Output = Self;
232    #[inline]
233    fn add(self, rhs: Self) -> Self {
234        Matrix2x2::add(&self, &rhs)
235    }
236}
237
238/// `a - b` : soustraction terme à terme.
239impl Sub for Matrix2x2 {
240    type Output = Self;
241    #[inline]
242    fn sub(self, rhs: Self) -> Self {
243        Matrix2x2::sub(&self, &rhs)
244    }
245}
246
247/// `a * b` : produit matriciel.
248impl Mul for Matrix2x2 {
249    type Output = Self;
250    #[inline]
251    fn mul(self, rhs: Self) -> Self {
252        Matrix2x2::mul(&self, &rhs)
253    }
254}
255
256/// `a * s` : multiplication par un scalaire `f32`.
257impl Mul<f32> for Matrix2x2 {
258    type Output = Self;
259    #[inline]
260    fn mul(self, s: f32) -> Self {
261        self.scale(s)
262    }
263}
264
265/// `s * a` : multiplication par un scalaire `f32` à gauche.
266impl Mul<Matrix2x2> for f32 {
267    type Output = Matrix2x2;
268    #[inline]
269    fn mul(self, m: Matrix2x2) -> Matrix2x2 {
270        m.scale(self)
271    }
272}
273
274/// `a * v` : matrice × vecteur colonne.
275impl 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
283/// `-a` : négation terme à terme.
284impl 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}