Skip to main content

embedded_matrix/
matrix3x3.rs

1use crate::{check_det, MatrixError};
2use core::ops::{Add, AddAssign, Index, Mul, MulAssign, Neg, Sub, SubAssign};
3
4/// Matrice 3×3 en `f32`, stockée en row-major.
5///
6/// ```
7/// use embedded_matrix::Matrix3x3;
8///
9/// let m = Matrix3x3::identity();
10/// assert!((m.det() - 1.0).abs() < 1e-5);
11/// assert!((m.trace() - 3.0).abs() < 1e-5);
12/// ```
13#[derive(Debug, Clone, Copy, PartialEq, Default)]
14pub struct Matrix3x3 {
15    data: [[f32; 3]; 3],
16}
17
18impl Matrix3x3 {
19    // ── Constructeurs ────────────────────────────────────────
20
21    /// Crée une matrice à partir d'un tableau row-major.
22    #[inline]
23    pub const fn new(data: [[f32; 3]; 3]) -> 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], [0.0, 1.0, 0.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; 3]; 3])
37    }
38
39    /// Matrice diagonale.
40    #[inline]
41    pub const fn from_diagonal(d: [f32; 3]) -> Self {
42        Self::new([[d[0], 0.0, 0.0], [0.0, d[1], 0.0], [0.0, 0.0, d[2]]])
43    }
44
45    /// Rotation 3D autour de l'axe X (radians, règle de la main droite).
46    ///
47    /// Disponible avec la feature `trig` (utilise `embedded-trig-f32`).
48    #[cfg(feature = "trig")]
49    #[inline]
50    pub fn rotation_x(angle: f32) -> Self {
51        let (s, c) = embedded_trig_f32::sincos(angle);
52        Self::new([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
53    }
54
55    /// Rotation 3D autour de l'axe Y (radians, règle de la main droite).
56    ///
57    /// Disponible avec la feature `trig`.
58    #[cfg(feature = "trig")]
59    #[inline]
60    pub fn rotation_y(angle: f32) -> Self {
61        let (s, c) = embedded_trig_f32::sincos(angle);
62        Self::new([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
63    }
64
65    /// Rotation 3D autour de l'axe Z (radians, règle de la main droite).
66    ///
67    /// Disponible avec la feature `trig`.
68    ///
69    /// ```
70    /// use embedded_matrix::Matrix3x3;
71    /// use embedded_trig_f32::consts::FRAC_PI_2;
72    ///
73    /// let r = Matrix3x3::rotation_z(FRAC_PI_2);
74    /// let p = r * [1.0, 0.0, 5.0];
75    /// assert!(p[0].abs() < 1e-6 && (p[1] - 1.0).abs() < 1e-6 && p[2] == 5.0);
76    /// ```
77    #[cfg(feature = "trig")]
78    #[inline]
79    pub fn rotation_z(angle: f32) -> Self {
80        let (s, c) = embedded_trig_f32::sincos(angle);
81        Self::new([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
82    }
83
84    // ── Accès ────────────────────────────────────────────────
85
86    /// Retourne l'élément à la ligne `row` et colonne `col` (0-indexé).
87    ///
88    /// # Panics
89    /// Panique si `row` ou `col` vaut 3 ou plus.
90    #[inline]
91    pub fn get(&self, row: usize, col: usize) -> f32 {
92        self.data[row][col]
93    }
94
95    /// Donne accès aux données brutes row-major.
96    #[inline]
97    pub fn as_array(&self) -> &[[f32; 3]; 3] {
98        &self.data
99    }
100
101    // ── Opérations arithmétiques ─────────────────────────────
102
103    /// Addition terme à terme.
104    #[inline]
105    #[allow(clippy::should_implement_trait)]
106    pub fn add(&self, rhs: &Self) -> Self {
107        let (a, b) = (&self.data, &rhs.data);
108        Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] + b[i][j])))
109    }
110
111    /// Soustraction terme à terme.
112    #[inline]
113    #[allow(clippy::should_implement_trait)]
114    pub fn sub(&self, rhs: &Self) -> Self {
115        let (a, b) = (&self.data, &rhs.data);
116        Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] - b[i][j])))
117    }
118
119    /// Produit matriciel standard.
120    #[inline]
121    #[allow(clippy::should_implement_trait)]
122    pub fn mul(&self, rhs: &Self) -> Self {
123        let (a, b) = (&self.data, &rhs.data);
124        Self::new(core::array::from_fn(|i| {
125            core::array::from_fn(|j| a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j])
126        }))
127    }
128
129    /// Multiplication par un scalaire.
130    #[inline]
131    pub fn scale(&self, s: f32) -> Self {
132        let a = &self.data;
133        Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[i][j] * s)))
134    }
135
136    /// Produit matrice × vecteur colonne.
137    #[inline]
138    pub fn mul_vec(&self, v: [f32; 3]) -> [f32; 3] {
139        let a = &self.data;
140        core::array::from_fn(|i| a[i][0] * v[0] + a[i][1] * v[1] + a[i][2] * v[2])
141    }
142
143    // ── Propriétés ───────────────────────────────────────────
144
145    /// Transposée.
146    #[inline]
147    pub fn transpose(&self) -> Self {
148        let a = &self.data;
149        Self::new(core::array::from_fn(|i| core::array::from_fn(|j| a[j][i])))
150    }
151
152    /// Déterminant par développement selon la première ligne (cofacteurs).
153    #[inline]
154    pub fn det(&self) -> f32 {
155        let d = &self.data;
156        d[0][0] * (d[1][1] * d[2][2] - d[1][2] * d[2][1])
157            - d[0][1] * (d[1][0] * d[2][2] - d[1][2] * d[2][0])
158            + d[0][2] * (d[1][0] * d[2][1] - d[1][1] * d[2][0])
159    }
160
161    /// Trace (somme des éléments diagonaux).
162    #[inline]
163    pub fn trace(&self) -> f32 {
164        self.data[0][0] + self.data[1][1] + self.data[2][2]
165    }
166
167    /// Résout le système `A·x = b`.
168    ///
169    /// Calculé comme `A⁻¹·b` : même coût et même critère de singularité que
170    /// [`inv`](Self::inv). Si vous avez plusieurs seconds membres pour la même
171    /// matrice, calculez `inv()` une seule fois.
172    ///
173    /// ```
174    /// use embedded_matrix::Matrix3x3;
175    ///
176    /// let a = Matrix3x3::new([[1.0, 2.0, 3.0], [0.0, 1.0, 4.0], [5.0, 6.0, 0.0]]);
177    /// let x = a.solve([1.0, 2.0, 3.0]).unwrap();
178    /// assert!((x[0] - 27.0).abs() < 1e-3);
179    /// assert!((x[1] + 22.0).abs() < 1e-3);
180    /// assert!((x[2] - 6.0).abs() < 1e-3);
181    /// ```
182    ///
183    /// # Errors
184    /// - [`MatrixError::SingularMatrix`] : même critère que [`inv`](Self::inv).
185    /// - [`MatrixError::NonFiniteValue`] : NaN / ±∞ dans la matrice, dans `b`,
186    ///   ou débordement.
187    pub fn solve(&self, b: [f32; 3]) -> Result<[f32; 3], MatrixError> {
188        let x = self.inv()?.mul_vec(b);
189        if x.iter().all(|v| v.is_finite()) {
190            Ok(x)
191        } else {
192            Err(MatrixError::NonFiniteValue)
193        }
194    }
195
196    /// Compare deux matrices terme à terme avec une tolérance absolue.
197    /// Renvoie `false` si un terme est NaN.
198    #[inline]
199    pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
200        let (a, b) = (&self.data, &other.data);
201        (0..3).all(|i| (0..3).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
202    }
203
204    /// Inverse par la méthode des cofacteurs (matrice adjointe / déterminant).
205    ///
206    /// # Errors
207    /// - [`MatrixError::SingularMatrix`] si `|det|` est négligeable devant la
208    ///   somme des valeurs absolues des termes du déterminant (seuil relatif,
209    ///   voir la doc de la crate).
210    /// - [`MatrixError::NonFiniteValue`] si la matrice contient NaN / ±∞
211    ///   ou si le calcul déborde.
212    pub fn inv(&self) -> Result<Self, MatrixError> {
213        let d = &self.data;
214
215        // Cofacteurs de la première ligne : réutilisés pour le déterminant.
216        let c00 = d[1][1] * d[2][2] - d[1][2] * d[2][1];
217        let c01 = d[1][2] * d[2][0] - d[1][0] * d[2][2];
218        let c02 = d[1][0] * d[2][1] - d[1][1] * d[2][0];
219
220        let det = d[0][0] * c00 + d[0][1] * c01 + d[0][2] * c02;
221
222        // Échelle de l'erreur d'arrondi sur `det`.
223        let bound = d[0][0].abs() * ((d[1][1] * d[2][2]).abs() + (d[1][2] * d[2][1]).abs())
224            + d[0][1].abs() * ((d[1][0] * d[2][2]).abs() + (d[1][2] * d[2][0]).abs())
225            + d[0][2].abs() * ((d[1][0] * d[2][1]).abs() + (d[1][1] * d[2][0]).abs());
226
227        check_det(det, bound)?;
228        let inv_det = 1.0 / det;
229
230        // Matrice adjointe (transposée de la matrice des cofacteurs) / det.
231        Ok(Self::new([
232            [
233                c00 * inv_det,
234                (d[0][2] * d[2][1] - d[0][1] * d[2][2]) * inv_det,
235                (d[0][1] * d[1][2] - d[0][2] * d[1][1]) * inv_det,
236            ],
237            [
238                c01 * inv_det,
239                (d[0][0] * d[2][2] - d[0][2] * d[2][0]) * inv_det,
240                (d[0][2] * d[1][0] - d[0][0] * d[1][2]) * inv_det,
241            ],
242            [
243                c02 * inv_det,
244                (d[0][1] * d[2][0] - d[0][0] * d[2][1]) * inv_det,
245                (d[0][0] * d[1][1] - d[0][1] * d[1][0]) * inv_det,
246            ],
247        ]))
248    }
249}
250
251// ── Conversions et accès par indice ──────────────────────────
252
253impl From<[[f32; 3]; 3]> for Matrix3x3 {
254    #[inline]
255    fn from(data: [[f32; 3]; 3]) -> Self {
256        Self::new(data)
257    }
258}
259
260impl From<Matrix3x3> for [[f32; 3]; 3] {
261    #[inline]
262    fn from(m: Matrix3x3) -> Self {
263        m.data
264    }
265}
266
267/// `m[(ligne, colonne)]` : panique si hors limites.
268impl Index<(usize, usize)> for Matrix3x3 {
269    type Output = f32;
270    #[inline]
271    fn index(&self, (row, col): (usize, usize)) -> &f32 {
272        &self.data[row][col]
273    }
274}
275
276// ── core::ops ────────────────────────────────────────────────
277
278/// `a + b` : addition terme à terme.
279impl Add for Matrix3x3 {
280    type Output = Self;
281    #[inline]
282    fn add(self, rhs: Self) -> Self {
283        Matrix3x3::add(&self, &rhs)
284    }
285}
286
287/// `a - b` : soustraction terme à terme.
288impl Sub for Matrix3x3 {
289    type Output = Self;
290    #[inline]
291    fn sub(self, rhs: Self) -> Self {
292        Matrix3x3::sub(&self, &rhs)
293    }
294}
295
296/// `a * b` : produit matriciel.
297impl Mul for Matrix3x3 {
298    type Output = Self;
299    #[inline]
300    fn mul(self, rhs: Self) -> Self {
301        Matrix3x3::mul(&self, &rhs)
302    }
303}
304
305/// `a * s` : multiplication par un scalaire `f32`.
306impl Mul<f32> for Matrix3x3 {
307    type Output = Self;
308    #[inline]
309    fn mul(self, s: f32) -> Self {
310        self.scale(s)
311    }
312}
313
314/// `s * a` : multiplication par un scalaire `f32` à gauche.
315impl Mul<Matrix3x3> for f32 {
316    type Output = Matrix3x3;
317    #[inline]
318    fn mul(self, m: Matrix3x3) -> Matrix3x3 {
319        m.scale(self)
320    }
321}
322
323/// `a * v` : matrice × vecteur colonne.
324impl Mul<[f32; 3]> for Matrix3x3 {
325    type Output = [f32; 3];
326    #[inline]
327    fn mul(self, v: [f32; 3]) -> [f32; 3] {
328        self.mul_vec(v)
329    }
330}
331
332/// `-a` : négation terme à terme.
333impl Neg for Matrix3x3 {
334    type Output = Self;
335    #[inline]
336    fn neg(self) -> Self {
337        self.scale(-1.0)
338    }
339}
340
341impl AddAssign for Matrix3x3 {
342    #[inline]
343    fn add_assign(&mut self, rhs: Self) {
344        *self = *self + rhs;
345    }
346}
347
348impl SubAssign for Matrix3x3 {
349    #[inline]
350    fn sub_assign(&mut self, rhs: Self) {
351        *self = *self - rhs;
352    }
353}
354
355impl MulAssign for Matrix3x3 {
356    #[inline]
357    fn mul_assign(&mut self, rhs: Self) {
358        *self = *self * rhs;
359    }
360}
361
362impl MulAssign<f32> for Matrix3x3 {
363    #[inline]
364    fn mul_assign(&mut self, s: f32) {
365        *self = self.scale(s);
366    }
367}