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    // ── 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 3 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; 3]; 3] {
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] + a[i][2] * b[2][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; 3]) -> [f32; 3] {
100        let a = &self.data;
101        core::array::from_fn(|i| a[i][0] * v[0] + a[i][1] * v[1] + a[i][2] * v[2])
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(core::array::from_fn(|i| core::array::from_fn(|j| a[j][i])))
111    }
112
113    /// Déterminant par développement selon la première ligne (cofacteurs).
114    #[inline]
115    pub fn det(&self) -> f32 {
116        let d = &self.data;
117        d[0][0] * (d[1][1] * d[2][2] - d[1][2] * d[2][1])
118            - d[0][1] * (d[1][0] * d[2][2] - d[1][2] * d[2][0])
119            + d[0][2] * (d[1][0] * d[2][1] - d[1][1] * d[2][0])
120    }
121
122    /// Trace (somme des éléments diagonaux).
123    #[inline]
124    pub fn trace(&self) -> f32 {
125        self.data[0][0] + self.data[1][1] + self.data[2][2]
126    }
127
128    /// Compare deux matrices terme à terme avec une tolérance absolue.
129    /// Renvoie `false` si un terme est NaN.
130    #[inline]
131    pub fn approx_eq(&self, other: &Self, tol: f32) -> bool {
132        let (a, b) = (&self.data, &other.data);
133        (0..3).all(|i| (0..3).all(|j| (a[i][j] - b[i][j]).abs() <= tol))
134    }
135
136    /// Inverse par la méthode des cofacteurs (matrice adjointe / déterminant).
137    ///
138    /// # Errors
139    /// - [`MatrixError::SingularMatrix`] si `|det|` est négligeable devant la
140    ///   somme des valeurs absolues des termes du déterminant (seuil relatif,
141    ///   voir la doc de la crate).
142    /// - [`MatrixError::NonFiniteValue`] si la matrice contient NaN / ±∞
143    ///   ou si le calcul déborde.
144    pub fn inv(&self) -> Result<Self, MatrixError> {
145        let d = &self.data;
146
147        // Cofacteurs de la première ligne : réutilisés pour le déterminant.
148        let c00 = d[1][1] * d[2][2] - d[1][2] * d[2][1];
149        let c01 = d[1][2] * d[2][0] - d[1][0] * d[2][2];
150        let c02 = d[1][0] * d[2][1] - d[1][1] * d[2][0];
151
152        let det = d[0][0] * c00 + d[0][1] * c01 + d[0][2] * c02;
153
154        // Échelle de l'erreur d'arrondi sur `det`.
155        let bound = d[0][0].abs() * ((d[1][1] * d[2][2]).abs() + (d[1][2] * d[2][1]).abs())
156            + d[0][1].abs() * ((d[1][0] * d[2][2]).abs() + (d[1][2] * d[2][0]).abs())
157            + d[0][2].abs() * ((d[1][0] * d[2][1]).abs() + (d[1][1] * d[2][0]).abs());
158
159        check_det(det, bound)?;
160        let inv_det = 1.0 / det;
161
162        // Matrice adjointe (transposée de la matrice des cofacteurs) / det.
163        Ok(Self::new([
164            [
165                c00 * inv_det,
166                (d[0][2] * d[2][1] - d[0][1] * d[2][2]) * inv_det,
167                (d[0][1] * d[1][2] - d[0][2] * d[1][1]) * inv_det,
168            ],
169            [
170                c01 * inv_det,
171                (d[0][0] * d[2][2] - d[0][2] * d[2][0]) * inv_det,
172                (d[0][2] * d[1][0] - d[0][0] * d[1][2]) * inv_det,
173            ],
174            [
175                c02 * inv_det,
176                (d[0][1] * d[2][0] - d[0][0] * d[2][1]) * inv_det,
177                (d[0][0] * d[1][1] - d[0][1] * d[1][0]) * inv_det,
178            ],
179        ]))
180    }
181}
182
183// ── Conversions et accès par indice ──────────────────────────
184
185impl From<[[f32; 3]; 3]> for Matrix3x3 {
186    #[inline]
187    fn from(data: [[f32; 3]; 3]) -> Self {
188        Self::new(data)
189    }
190}
191
192impl From<Matrix3x3> for [[f32; 3]; 3] {
193    #[inline]
194    fn from(m: Matrix3x3) -> Self {
195        m.data
196    }
197}
198
199/// `m[(ligne, colonne)]` : panique si hors limites.
200impl Index<(usize, usize)> for Matrix3x3 {
201    type Output = f32;
202    #[inline]
203    fn index(&self, (row, col): (usize, usize)) -> &f32 {
204        &self.data[row][col]
205    }
206}
207
208// ── core::ops ────────────────────────────────────────────────
209
210/// `a + b` : addition terme à terme.
211impl Add for Matrix3x3 {
212    type Output = Self;
213    #[inline]
214    fn add(self, rhs: Self) -> Self {
215        Matrix3x3::add(&self, &rhs)
216    }
217}
218
219/// `a - b` : soustraction terme à terme.
220impl Sub for Matrix3x3 {
221    type Output = Self;
222    #[inline]
223    fn sub(self, rhs: Self) -> Self {
224        Matrix3x3::sub(&self, &rhs)
225    }
226}
227
228/// `a * b` : produit matriciel.
229impl Mul for Matrix3x3 {
230    type Output = Self;
231    #[inline]
232    fn mul(self, rhs: Self) -> Self {
233        Matrix3x3::mul(&self, &rhs)
234    }
235}
236
237/// `a * s` : multiplication par un scalaire `f32`.
238impl Mul<f32> for Matrix3x3 {
239    type Output = Self;
240    #[inline]
241    fn mul(self, s: f32) -> Self {
242        self.scale(s)
243    }
244}
245
246/// `s * a` : multiplication par un scalaire `f32` à gauche.
247impl Mul<Matrix3x3> for f32 {
248    type Output = Matrix3x3;
249    #[inline]
250    fn mul(self, m: Matrix3x3) -> Matrix3x3 {
251        m.scale(self)
252    }
253}
254
255/// `a * v` : matrice × vecteur colonne.
256impl Mul<[f32; 3]> for Matrix3x3 {
257    type Output = [f32; 3];
258    #[inline]
259    fn mul(self, v: [f32; 3]) -> [f32; 3] {
260        self.mul_vec(v)
261    }
262}
263
264/// `-a` : négation terme à terme.
265impl Neg for Matrix3x3 {
266    type Output = Self;
267    #[inline]
268    fn neg(self) -> Self {
269        self.scale(-1.0)
270    }
271}
272
273impl AddAssign for Matrix3x3 {
274    #[inline]
275    fn add_assign(&mut self, rhs: Self) {
276        *self = *self + rhs;
277    }
278}
279
280impl SubAssign for Matrix3x3 {
281    #[inline]
282    fn sub_assign(&mut self, rhs: Self) {
283        *self = *self - rhs;
284    }
285}
286
287impl MulAssign for Matrix3x3 {
288    #[inline]
289    fn mul_assign(&mut self, rhs: Self) {
290        *self = *self * rhs;
291    }
292}
293
294impl MulAssign<f32> for Matrix3x3 {
295    #[inline]
296    fn mul_assign(&mut self, s: f32) {
297        *self = self.scale(s);
298    }
299}