ogeom_math/
matrix.rs

1//! Small dense matrices: the linear part of a transform.
2//!
3//! Fixed 3×3 and 2×2, row-major, stored inline. Deliberately not general: these
4//! sit on the hot path of every transform application in the kernel, and the
5//! sizes are known. General linear algebra lives in [`crate::solve`].
6
7use core::ops::{Add, Mul, Neg, Sub};
8
9use ogeom_core::{OgeomResult, ogeom_bail};
10
11use crate::{Direction, Vector, Vector2};
12
13/// A 3×3 matrix, row-major.
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct Matrix3 {
16    /// Rows, each `[m[i][0], m[i][1], m[i][2]]`.
17    pub rows: [[f64; 3]; 3],
18}
19
20/// A 2×2 matrix, row-major.
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub struct Matrix2 {
23    /// Rows.
24    pub rows: [[f64; 2]; 2],
25}
26
27impl Default for Matrix3 {
28    fn default() -> Self {
29        Self::IDENTITY
30    }
31}
32
33impl Default for Matrix2 {
34    fn default() -> Self {
35        Self::IDENTITY
36    }
37}
38
39impl Matrix3 {
40    /// The identity.
41    pub const IDENTITY: Self = Self::new([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]);
42    /// All zeros.
43    pub const ZERO: Self = Self::new([[0.0; 3]; 3]);
44
45    /// From rows.
46    #[must_use]
47    pub const fn new(rows: [[f64; 3]; 3]) -> Self {
48        Self { rows }
49    }
50
51    /// From column vectors.
52    #[must_use]
53    pub const fn from_columns(a: Vector, b: Vector, c: Vector) -> Self {
54        Self::new([[a.x, b.x, c.x], [a.y, b.y, c.y], [a.z, b.z, c.z]])
55    }
56
57    /// From row vectors.
58    #[must_use]
59    pub const fn from_rows(a: Vector, b: Vector, c: Vector) -> Self {
60        Self::new([[a.x, a.y, a.z], [b.x, b.y, b.z], [c.x, c.y, c.z]])
61    }
62
63    /// A uniform scaling.
64    #[must_use]
65    pub const fn scaling(s: f64) -> Self {
66        Self::new([[s, 0.0, 0.0], [0.0, s, 0.0], [0.0, 0.0, s]])
67    }
68
69    /// A non-uniform scaling along the coordinate axes.
70    #[must_use]
71    pub const fn scaling_xyz(x: f64, y: f64, z: f64) -> Self {
72        Self::new([[x, 0.0, 0.0], [0.0, y, 0.0], [0.0, 0.0, z]])
73    }
74
75    /// A right-handed rotation of `angle` radians about `axis`.
76    ///
77    /// Rodrigues' formula. Orthonormal to within rounding for any unit `axis`,
78    /// which [`Direction`] guarantees.
79    #[must_use]
80    pub fn rotation(axis: Direction, angle: f64) -> Self {
81        let (s, c) = angle.sin_cos();
82        let t = 1.0 - c;
83        let (x, y, z) = (axis.x(), axis.y(), axis.z());
84        Self::new([
85            [
86                t.mul_add(x * x, c),
87                t.mul_add(x * y, -(s * z)),
88                t.mul_add(x * z, s * y),
89            ],
90            [
91                t.mul_add(x * y, s * z),
92                t.mul_add(y * y, c),
93                t.mul_add(y * z, -(s * x)),
94            ],
95            [
96                t.mul_add(x * z, -(s * y)),
97                t.mul_add(y * z, s * x),
98                t.mul_add(z * z, c),
99            ],
100        ])
101    }
102
103    /// Reflection in the plane through the origin with normal `n`.
104    #[must_use]
105    pub fn reflection(n: Direction) -> Self {
106        let (x, y, z) = (n.x(), n.y(), n.z());
107        Self::new([
108            [(-2.0f64).mul_add(x * x, 1.0), -2.0 * x * y, -2.0 * x * z],
109            [-2.0 * x * y, (-2.0f64).mul_add(y * y, 1.0), -2.0 * y * z],
110            [-2.0 * x * z, -2.0 * y * z, (-2.0f64).mul_add(z * z, 1.0)],
111        ])
112    }
113
114    /// Element at `(row, col)`.
115    ///
116    /// # Errors
117    ///
118    /// [`OgeomError::Range`](ogeom_core::OgeomError::Range) if either index exceeds 2.
119    pub fn get(&self, row: usize, col: usize) -> OgeomResult<f64> {
120        if row > 2 || col > 2 {
121            ogeom_bail!(Range, "matrix index ({row}, {col}) of 3x3");
122        }
123        Ok(self.rows[row][col])
124    }
125
126    /// Column `i` as a vector.
127    ///
128    /// # Errors
129    ///
130    /// [`OgeomError::Range`](ogeom_core::OgeomError::Range) if `i > 2`.
131    pub fn column(&self, i: usize) -> OgeomResult<Vector> {
132        if i > 2 {
133            ogeom_bail!(Range, "matrix column {i} of 3");
134        }
135        Ok(Vector::new(
136            self.rows[0][i],
137            self.rows[1][i],
138            self.rows[2][i],
139        ))
140    }
141
142    /// Row `i` as a vector.
143    ///
144    /// # Errors
145    ///
146    /// [`OgeomError::Range`](ogeom_core::OgeomError::Range) if `i > 2`.
147    pub fn row(&self, i: usize) -> OgeomResult<Vector> {
148        if i > 2 {
149            ogeom_bail!(Range, "matrix row {i} of 3");
150        }
151        Ok(Vector::from_array(self.rows[i]))
152    }
153
154    /// The transpose.
155    #[must_use]
156    pub const fn transposed(&self) -> Self {
157        let m = &self.rows;
158        Self::new([
159            [m[0][0], m[1][0], m[2][0]],
160            [m[0][1], m[1][1], m[2][1]],
161            [m[0][2], m[1][2], m[2][2]],
162        ])
163    }
164
165    /// The determinant.
166    #[must_use]
167    pub fn determinant(&self) -> f64 {
168        let m = &self.rows;
169        m[0][0].mul_add(
170            m[1][1].mul_add(m[2][2], -(m[1][2] * m[2][1])),
171            m[0][1].mul_add(
172                -m[1][0].mul_add(m[2][2], -(m[1][2] * m[2][0])),
173                m[0][2] * m[1][0].mul_add(m[2][1], -(m[1][1] * m[2][0])),
174            ),
175        )
176    }
177
178    /// The trace.
179    #[must_use]
180    pub fn trace(&self) -> f64 {
181        self.rows[0][0] + self.rows[1][1] + self.rows[2][2]
182    }
183
184    /// The inverse.
185    ///
186    /// # Errors
187    ///
188    /// [`OgeomError::Numeric`](ogeom_core::OgeomError::Numeric) if the matrix is
189    /// singular.
190    ///
191    /// Singularity is a question of numerical conditioning, not of modelling
192    /// tolerance, so this takes no [`Tolerances`](ogeom_core::Tolerances).
193    /// The determinant is compared against the rounding error incurred
194    /// computing it: for an `n x n` matrix
195    /// with entries bounded by `s`, that is on the order of `n * n! * eps *
196    /// s^n`. The bound scales with the entries and has no absolute floor,
197    /// because a matrix scaled by `1e-3` has a determinant scaled by `1e-9` and
198    /// is no less invertible for it.
199    pub fn inverse(&self) -> OgeomResult<Self> {
200        let d = self.determinant();
201        let scale = self
202            .rows
203            .iter()
204            .flatten()
205            .fold(0.0_f64, |acc, v| acc.max(v.abs()));
206        if d.abs() <= 18.0 * f64::EPSILON * scale * scale * scale {
207            ogeom_bail!(Numeric, "matrix is singular (determinant {d})");
208        }
209        let m = &self.rows;
210        let cof =
211            |a: usize, b: usize, c: usize, e: usize| m[a][b].mul_add(m[c][e], -(m[a][e] * m[c][b]));
212        // Adjugate (transposed cofactor matrix), divided by the determinant.
213        Ok(Self::new([
214            [
215                cof(1, 1, 2, 2) / d,
216                -cof(0, 1, 2, 2) / d,
217                cof(0, 1, 1, 2) / d,
218            ],
219            [
220                -cof(1, 0, 2, 2) / d,
221                cof(0, 0, 2, 2) / d,
222                -cof(0, 0, 1, 2) / d,
223            ],
224            [
225                cof(1, 0, 2, 1) / d,
226                -cof(0, 0, 2, 1) / d,
227                cof(0, 0, 1, 1) / d,
228            ],
229        ]))
230    }
231
232    /// Whether the matrix is orthonormal: its rows form an orthonormal basis,
233    /// so it represents a rotation or a reflection and its inverse is its
234    /// transpose.
235    #[must_use]
236    pub fn is_orthonormal(&self, eps: f64) -> bool {
237        let p = *self * self.transposed();
238        p.is_equal(&Self::IDENTITY, eps)
239    }
240
241    /// Whether every element agrees with `other` within `eps`.
242    ///
243    /// Takes a bare epsilon rather than [`Tolerances`](ogeom_core::Tolerances): matrix entries are
244    /// dimensionless ratios, and a length tolerance does not apply to them.
245    #[must_use]
246    pub fn is_equal(&self, other: &Self, eps: f64) -> bool {
247        self.rows
248            .iter()
249            .flatten()
250            .zip(other.rows.iter().flatten())
251            .all(|(a, b)| (a - b).abs() <= eps)
252    }
253
254    /// Whether every element is finite.
255    #[must_use]
256    pub fn is_finite(&self) -> bool {
257        self.rows.iter().flatten().all(|v| v.is_finite())
258    }
259}
260
261impl Matrix2 {
262    /// The identity.
263    pub const IDENTITY: Self = Self::new([[1.0, 0.0], [0.0, 1.0]]);
264    /// All zeros.
265    pub const ZERO: Self = Self::new([[0.0; 2]; 2]);
266
267    /// From rows.
268    #[must_use]
269    pub const fn new(rows: [[f64; 2]; 2]) -> Self {
270        Self { rows }
271    }
272
273    /// A counter-clockwise rotation of `angle` radians.
274    #[must_use]
275    pub fn rotation(angle: f64) -> Self {
276        let (s, c) = angle.sin_cos();
277        Self::new([[c, -s], [s, c]])
278    }
279
280    /// A uniform scaling.
281    #[must_use]
282    pub const fn scaling(s: f64) -> Self {
283        Self::new([[s, 0.0], [0.0, s]])
284    }
285
286    /// The transpose.
287    #[must_use]
288    pub const fn transposed(&self) -> Self {
289        let m = &self.rows;
290        Self::new([[m[0][0], m[1][0]], [m[0][1], m[1][1]]])
291    }
292
293    /// The determinant.
294    #[must_use]
295    pub fn determinant(&self) -> f64 {
296        let m = &self.rows;
297        m[0][0].mul_add(m[1][1], -(m[0][1] * m[1][0]))
298    }
299
300    /// The inverse.
301    ///
302    /// # Errors
303    ///
304    /// [`OgeomError::Numeric`](ogeom_core::OgeomError::Numeric) if the matrix is
305    /// singular. See [`Matrix3::inverse`] for why this takes no tolerance.
306    pub fn inverse(&self) -> OgeomResult<Self> {
307        let d = self.determinant();
308        let scale = self
309            .rows
310            .iter()
311            .flatten()
312            .fold(0.0_f64, |acc, v| acc.max(v.abs()));
313        if d.abs() <= 4.0 * f64::EPSILON * scale * scale {
314            ogeom_bail!(Numeric, "matrix is singular (determinant {d})");
315        }
316        let m = &self.rows;
317        Ok(Self::new([
318            [m[1][1] / d, -m[0][1] / d],
319            [-m[1][0] / d, m[0][0] / d],
320        ]))
321    }
322
323    /// Whether every element agrees with `other` within `eps`.
324    #[must_use]
325    pub fn is_equal(&self, other: &Self, eps: f64) -> bool {
326        self.rows
327            .iter()
328            .flatten()
329            .zip(other.rows.iter().flatten())
330            .all(|(a, b)| (a - b).abs() <= eps)
331    }
332}
333
334impl Mul<Vector> for Matrix3 {
335    type Output = Vector;
336    fn mul(self, v: Vector) -> Vector {
337        let m = &self.rows;
338        Vector::new(
339            m[0][0].mul_add(v.x, m[0][1].mul_add(v.y, m[0][2] * v.z)),
340            m[1][0].mul_add(v.x, m[1][1].mul_add(v.y, m[1][2] * v.z)),
341            m[2][0].mul_add(v.x, m[2][1].mul_add(v.y, m[2][2] * v.z)),
342        )
343    }
344}
345
346impl Mul for Matrix3 {
347    type Output = Self;
348    fn mul(self, o: Self) -> Self {
349        let mut out = [[0.0_f64; 3]; 3];
350        for (i, row) in out.iter_mut().enumerate() {
351            for (j, cell) in row.iter_mut().enumerate() {
352                *cell = self.rows[i][0].mul_add(
353                    o.rows[0][j],
354                    self.rows[i][1].mul_add(o.rows[1][j], self.rows[i][2] * o.rows[2][j]),
355                );
356            }
357        }
358        Self::new(out)
359    }
360}
361
362impl Mul<f64> for Matrix3 {
363    type Output = Self;
364    fn mul(self, s: f64) -> Self {
365        let mut out = self.rows;
366        for cell in out.iter_mut().flatten() {
367            *cell *= s;
368        }
369        Self::new(out)
370    }
371}
372
373impl Add for Matrix3 {
374    type Output = Self;
375    fn add(self, o: Self) -> Self {
376        let mut out = self.rows;
377        for (i, row) in out.iter_mut().enumerate() {
378            for (j, cell) in row.iter_mut().enumerate() {
379                *cell += o.rows[i][j];
380            }
381        }
382        Self::new(out)
383    }
384}
385
386impl Sub for Matrix3 {
387    type Output = Self;
388    fn sub(self, o: Self) -> Self {
389        self + (-o)
390    }
391}
392
393impl Neg for Matrix3 {
394    type Output = Self;
395    fn neg(self) -> Self {
396        self * -1.0
397    }
398}
399
400impl Mul<Vector2> for Matrix2 {
401    type Output = Vector2;
402    fn mul(self, v: Vector2) -> Vector2 {
403        let m = &self.rows;
404        Vector2::new(
405            m[0][0].mul_add(v.x, m[0][1] * v.y),
406            m[1][0].mul_add(v.x, m[1][1] * v.y),
407        )
408    }
409}
410
411impl Mul for Matrix2 {
412    type Output = Self;
413    fn mul(self, o: Self) -> Self {
414        let mut out = [[0.0_f64; 2]; 2];
415        for (i, row) in out.iter_mut().enumerate() {
416            for (j, cell) in row.iter_mut().enumerate() {
417                *cell = self.rows[i][0].mul_add(o.rows[0][j], self.rows[i][1] * o.rows[1][j]);
418            }
419        }
420        Self::new(out)
421    }
422}
423
424#[cfg(test)]
425#[allow(clippy::unwrap_used)]
426mod tests {
427    use super::*;
428    use approx::assert_relative_eq;
429    use ogeom_core::Tolerances;
430
431    /// Only needed where a [`Direction`] has to be constructed.
432    const T: Tolerances = Tolerances::millimetres();
433    /// Matrix entries are dimensionless, so comparisons take a bare epsilon.
434    /// A few hundred ulps covers accumulated rounding in a product of rotations.
435    const EPS: f64 = 1e-13;
436
437    #[test]
438    fn identity_is_neutral() {
439        let v = Vector::new(1.0, 2.0, 3.0);
440        assert_eq!(Matrix3::IDENTITY * v, v);
441        let m = Matrix3::rotation(Direction::Z, 0.7);
442        assert!((m * Matrix3::IDENTITY).is_equal(&m, EPS));
443        assert!((Matrix3::IDENTITY * m).is_equal(&m, EPS));
444    }
445
446    #[test]
447    fn rotation_is_orthonormal_and_preserves_length() {
448        let axis = Direction::from_coords(1.0, 2.0, 3.0, T).unwrap();
449        for k in 0..8 {
450            let m = Matrix3::rotation(axis, f64::from(k) * 0.7);
451            assert!(m.is_orthonormal(EPS));
452            assert_relative_eq!(m.determinant(), 1.0, epsilon = 1e-14);
453            let v = Vector::new(3.0, -1.0, 2.0);
454            assert_relative_eq!((m * v).magnitude(), v.magnitude(), epsilon = 1e-13);
455        }
456    }
457
458    #[test]
459    fn rotation_about_z_matches_the_hand_computation() {
460        let m = Matrix3::rotation(Direction::Z, core::f64::consts::FRAC_PI_2);
461        let v = m * Vector::X;
462        assert_relative_eq!(v.x, 0.0, epsilon = 1e-15);
463        assert_relative_eq!(v.y, 1.0, epsilon = 1e-15);
464        assert_relative_eq!(v.z, 0.0, epsilon = 1e-15);
465    }
466
467    #[test]
468    fn rotation_composes_additively_in_angle() {
469        let axis = Direction::from_coords(0.0, 1.0, 1.0, T).unwrap();
470        let a = Matrix3::rotation(axis, 0.3);
471        let b = Matrix3::rotation(axis, 0.4);
472        let ab = Matrix3::rotation(axis, 0.7);
473        assert!((a * b).is_equal(&ab, EPS));
474    }
475
476    #[test]
477    fn reflection_is_an_involution_with_negative_determinant() {
478        let n = Direction::from_coords(1.0, 1.0, 0.0, T).unwrap();
479        let m = Matrix3::reflection(n);
480        assert_relative_eq!(m.determinant(), -1.0, epsilon = 1e-14);
481        assert!((m * m).is_equal(&Matrix3::IDENTITY, EPS));
482        // A vector in the mirror plane is unmoved; the normal is negated.
483        let in_plane = Vector::new(1.0, -1.0, 0.0);
484        assert!((m * in_plane).is_equal(in_plane, T));
485        assert!((m * n.vector()).is_equal(-n.vector(), T));
486    }
487
488    #[test]
489    fn inverse_round_trips() {
490        let m = Matrix3::new([[2.0, 1.0, 0.0], [1.0, 3.0, 1.0], [0.0, 1.0, 4.0]]);
491        let inv = m.inverse().unwrap();
492        assert!((m * inv).is_equal(&Matrix3::IDENTITY, EPS));
493        assert!((inv * m).is_equal(&Matrix3::IDENTITY, EPS));
494    }
495
496    #[test]
497    fn singular_matrices_are_refused() {
498        // Third row is the sum of the first two.
499        let m = Matrix3::new([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [5.0, 7.0, 9.0]]);
500        assert!(m.inverse().is_err());
501        assert!(Matrix3::ZERO.inverse().is_err());
502    }
503
504    #[test]
505    fn singularity_threshold_is_relative_not_absolute() {
506        // A well-conditioned matrix scaled down by 1e-3 has a determinant of
507        // 1e-9, below an absolute confusion threshold, but perfectly
508        // invertible. An absolute test would reject it.
509        let m = Matrix3::IDENTITY * 1e-3;
510        assert_relative_eq!(m.determinant(), 1e-9, max_relative = 1e-12);
511        let inv = m.inverse().unwrap();
512        assert!((m * inv).is_equal(&Matrix3::IDENTITY, EPS));
513        // And the same matrix scaled up stays invertible too.
514        let big = Matrix3::IDENTITY * 1e6;
515        assert!((big * big.inverse().unwrap()).is_equal(&Matrix3::IDENTITY, EPS));
516    }
517
518    #[test]
519    fn orthonormal_inverse_equals_transpose() {
520        let axis = Direction::from_coords(2.0, -1.0, 0.5, T).unwrap();
521        let m = Matrix3::rotation(axis, 1.1);
522        assert!(m.inverse().unwrap().is_equal(&m.transposed(), EPS));
523        assert!(!Matrix3::scaling(2.0).is_orthonormal(EPS));
524    }
525
526    #[test]
527    fn determinant_and_trace() {
528        let m = Matrix3::scaling_xyz(2.0, 3.0, 4.0);
529        assert_relative_eq!(m.determinant(), 24.0);
530        assert_relative_eq!(m.trace(), 9.0);
531    }
532
533    #[test]
534    fn columns_and_rows_are_bounds_checked() {
535        let m = Matrix3::from_columns(Vector::X, Vector::Y, Vector::Z);
536        assert_eq!(m.column(0).unwrap(), Vector::X);
537        assert_eq!(m.row(1).unwrap(), Vector::Y);
538        assert!(m.column(3).is_err());
539        assert!(m.row(3).is_err());
540        assert!(m.get(0, 3).is_err());
541        assert!(m.is_equal(&Matrix3::IDENTITY, EPS));
542    }
543
544    #[test]
545    fn matrix2_rotation_and_inverse() {
546        let m = Matrix2::rotation(core::f64::consts::FRAC_PI_2);
547        let v = m * Vector2::X;
548        assert_relative_eq!(v.x, 0.0, epsilon = 1e-15);
549        assert_relative_eq!(v.y, 1.0, epsilon = 1e-15);
550        assert_relative_eq!(m.determinant(), 1.0, epsilon = 1e-15);
551        assert!((m * m.inverse().unwrap()).is_equal(&Matrix2::IDENTITY, EPS));
552        assert!(Matrix2::ZERO.inverse().is_err());
553    }
554}
ogeom_math/matrix.rs

ogeom_math/
matrix.rs
