brepkit_math/
mat.rs

1//! Matrix types for geometric transforms.
2//!
3//! [`Mat3`] is a 3x3 matrix and [`Mat4`] is a 4x4 affine transform matrix.
4
5use std::ops::Mul;
6
7use crate::MathError;
8use crate::vec::Point3;
9
10// ---------------------------------------------------------------------------
11// Mat3
12// ---------------------------------------------------------------------------
13
14/// A 3x3 matrix stored in row-major order.
15#[derive(Debug, Clone, Copy, PartialEq)]
16#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
17pub struct Mat3(pub [[f64; 3]; 3]);
18
19impl Mat3 {
20    /// The 3x3 identity matrix.
21    #[must_use]
22    pub const fn identity() -> Self {
23        Self([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
24    }
25
26    /// Transpose the matrix.
27    #[must_use]
28    pub const fn transpose(self) -> Self {
29        let m = &self.0;
30        Self([
31            [m[0][0], m[1][0], m[2][0]],
32            [m[0][1], m[1][1], m[2][1]],
33            [m[0][2], m[1][2], m[2][2]],
34        ])
35    }
36
37    /// Compute the determinant of the matrix.
38    #[must_use]
39    pub fn determinant(self) -> f64 {
40        let m = &self.0;
41        m[0][0].mul_add(
42            m[1][1].mul_add(m[2][2], -(m[1][2] * m[2][1])),
43            m[0][1].mul_add(
44                m[1][2].mul_add(m[2][0], -(m[1][0] * m[2][2])),
45                m[0][2] * m[1][0].mul_add(m[2][1], -(m[1][1] * m[2][0])),
46            ),
47        )
48    }
49}
50
51// ---------------------------------------------------------------------------
52// Mat4
53// ---------------------------------------------------------------------------
54
55/// A 4x4 matrix stored in row-major order, typically used for affine transforms.
56#[derive(Debug, Clone, Copy, PartialEq)]
57#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
58pub struct Mat4(pub [[f64; 4]; 4]);
59
60impl Mat4 {
61    /// The 4x4 identity matrix.
62    #[must_use]
63    pub const fn identity() -> Self {
64        Self([
65            [1.0, 0.0, 0.0, 0.0],
66            [0.0, 1.0, 0.0, 0.0],
67            [0.0, 0.0, 1.0, 0.0],
68            [0.0, 0.0, 0.0, 1.0],
69        ])
70    }
71
72    /// Create a translation matrix.
73    #[must_use]
74    pub const fn translation(tx: f64, ty: f64, tz: f64) -> Self {
75        Self([
76            [1.0, 0.0, 0.0, tx],
77            [0.0, 1.0, 0.0, ty],
78            [0.0, 0.0, 1.0, tz],
79            [0.0, 0.0, 0.0, 1.0],
80        ])
81    }
82
83    /// Create a uniform or non-uniform scale matrix.
84    #[must_use]
85    pub const fn scale(sx: f64, sy: f64, sz: f64) -> Self {
86        Self([
87            [sx, 0.0, 0.0, 0.0],
88            [0.0, sy, 0.0, 0.0],
89            [0.0, 0.0, sz, 0.0],
90            [0.0, 0.0, 0.0, 1.0],
91        ])
92    }
93
94    /// Create a rotation matrix around the X axis by `angle` radians.
95    #[must_use]
96    pub fn rotation_x(angle: f64) -> Self {
97        let (s, c) = angle.sin_cos();
98        Self([
99            [1.0, 0.0, 0.0, 0.0],
100            [0.0, c, -s, 0.0],
101            [0.0, s, c, 0.0],
102            [0.0, 0.0, 0.0, 1.0],
103        ])
104    }
105
106    /// Create a rotation matrix around the Y axis by `angle` radians.
107    #[must_use]
108    pub fn rotation_y(angle: f64) -> Self {
109        let (s, c) = angle.sin_cos();
110        Self([
111            [c, 0.0, s, 0.0],
112            [0.0, 1.0, 0.0, 0.0],
113            [-s, 0.0, c, 0.0],
114            [0.0, 0.0, 0.0, 1.0],
115        ])
116    }
117
118    /// Create a rotation matrix around the Z axis by `angle` radians.
119    #[must_use]
120    pub fn rotation_z(angle: f64) -> Self {
121        let (s, c) = angle.sin_cos();
122        Self([
123            [c, -s, 0.0, 0.0],
124            [s, c, 0.0, 0.0],
125            [0.0, 0.0, 1.0, 0.0],
126            [0.0, 0.0, 0.0, 1.0],
127        ])
128    }
129
130    /// Transform a 3D point by this matrix (assumes w = 1).
131    #[must_use]
132    pub fn mul_point(self, p: Point3) -> Point3 {
133        let m = &self.0;
134        Point3::new(
135            m[0][0].mul_add(
136                p.x(),
137                m[0][1].mul_add(p.y(), m[0][2].mul_add(p.z(), m[0][3])),
138            ),
139            m[1][0].mul_add(
140                p.x(),
141                m[1][1].mul_add(p.y(), m[1][2].mul_add(p.z(), m[1][3])),
142            ),
143            m[2][0].mul_add(
144                p.x(),
145                m[2][1].mul_add(p.y(), m[2][2].mul_add(p.z(), m[2][3])),
146            ),
147        )
148    }
149
150    /// Transpose the matrix.
151    #[must_use]
152    pub const fn transpose(self) -> Self {
153        let m = &self.0;
154        Self([
155            [m[0][0], m[1][0], m[2][0], m[3][0]],
156            [m[0][1], m[1][1], m[2][1], m[3][1]],
157            [m[0][2], m[1][2], m[2][2], m[3][2]],
158            [m[0][3], m[1][3], m[2][3], m[3][3]],
159        ])
160    }
161
162    /// Compute the determinant using cofactor expansion along the first row.
163    #[must_use]
164    #[allow(clippy::similar_names)]
165    pub fn determinant(self) -> f64 {
166        let m = &self.0;
167
168        let s0 = m[0][0].mul_add(m[1][1], -(m[1][0] * m[0][1]));
169        let s1 = m[0][0].mul_add(m[1][2], -(m[1][0] * m[0][2]));
170        let s2 = m[0][0].mul_add(m[1][3], -(m[1][0] * m[0][3]));
171        let s3 = m[0][1].mul_add(m[1][2], -(m[1][1] * m[0][2]));
172        let s4 = m[0][1].mul_add(m[1][3], -(m[1][1] * m[0][3]));
173        let s5 = m[0][2].mul_add(m[1][3], -(m[1][2] * m[0][3]));
174
175        let c5 = m[2][2].mul_add(m[3][3], -(m[3][2] * m[2][3]));
176        let c4 = m[2][1].mul_add(m[3][3], -(m[3][1] * m[2][3]));
177        let c3 = m[2][1].mul_add(m[3][2], -(m[3][1] * m[2][2]));
178        let c2 = m[2][0].mul_add(m[3][3], -(m[3][0] * m[2][3]));
179        let c1 = m[2][0].mul_add(m[3][2], -(m[3][0] * m[2][2]));
180        let c0 = m[2][0].mul_add(m[3][1], -(m[3][0] * m[2][1]));
181
182        s0.mul_add(
183            c5,
184            (-s1).mul_add(
185                c4,
186                s2.mul_add(c3, s3.mul_add(c2, (-s4).mul_add(c1, s5 * c0))),
187            ),
188        )
189    }
190
191    /// Compute the inverse of the matrix using the adjugate method.
192    ///
193    /// # Errors
194    ///
195    /// Returns [`MathError::SingularMatrix`] if the determinant is approximately zero.
196    #[allow(clippy::similar_names)]
197    pub fn inverse(self) -> Result<Self, MathError> {
198        let m = &self.0;
199
200        // Reuse the 2x2 minor pattern from determinant().
201        let s0 = m[0][0].mul_add(m[1][1], -(m[1][0] * m[0][1]));
202        let s1 = m[0][0].mul_add(m[1][2], -(m[1][0] * m[0][2]));
203        let s2 = m[0][0].mul_add(m[1][3], -(m[1][0] * m[0][3]));
204        let s3 = m[0][1].mul_add(m[1][2], -(m[1][1] * m[0][2]));
205        let s4 = m[0][1].mul_add(m[1][3], -(m[1][1] * m[0][3]));
206        let s5 = m[0][2].mul_add(m[1][3], -(m[1][2] * m[0][3]));
207
208        let c5 = m[2][2].mul_add(m[3][3], -(m[3][2] * m[2][3]));
209        let c4 = m[2][1].mul_add(m[3][3], -(m[3][1] * m[2][3]));
210        let c3 = m[2][1].mul_add(m[3][2], -(m[3][1] * m[2][2]));
211        let c2 = m[2][0].mul_add(m[3][3], -(m[3][0] * m[2][3]));
212        let c1 = m[2][0].mul_add(m[3][2], -(m[3][0] * m[2][2]));
213        let c0 = m[2][0].mul_add(m[3][1], -(m[3][0] * m[2][1]));
214
215        let det = s0.mul_add(
216            c5,
217            (-s1).mul_add(
218                c4,
219                s2.mul_add(c3, s3.mul_add(c2, (-s4).mul_add(c1, s5 * c0))),
220            ),
221        );
222
223        // Scale-relative singularity check.  The determinant is computed as a
224        // sum of products s_i * c_j of 2x2 minors, so its magnitude scales as
225        // max_minor^2.  Comparing against that avoids the false-singular
226        // problem that a hardcoded threshold (1e-15) causes when matrix entries
227        // are very small or very large.
228        let max_minor = s0
229            .abs()
230            .max(s1.abs())
231            .max(s2.abs())
232            .max(s3.abs())
233            .max(s4.abs())
234            .max(s5.abs())
235            .max(c0.abs())
236            .max(c1.abs())
237            .max(c2.abs())
238            .max(c3.abs())
239            .max(c4.abs())
240            .max(c5.abs());
241        if max_minor == 0.0 {
242            return Err(MathError::SingularMatrix);
243        }
244        if det.abs() < f64::EPSILON * max_minor * max_minor {
245            return Err(MathError::SingularMatrix);
246        }
247
248        let inv_det = 1.0 / det;
249
250        Ok(Self([
251            [
252                m[1][1].mul_add(c5, m[1][3].mul_add(c3, -(m[1][2] * c4))) * inv_det,
253                (-m[0][1]).mul_add(c5, m[0][2].mul_add(c4, -(m[0][3] * c3))) * inv_det,
254                m[3][1].mul_add(s5, m[3][3].mul_add(s3, -(m[3][2] * s4))) * inv_det,
255                (-m[2][1]).mul_add(s5, m[2][2].mul_add(s4, -(m[2][3] * s3))) * inv_det,
256            ],
257            [
258                (-m[1][0]).mul_add(c5, m[1][2].mul_add(c2, -(m[1][3] * c1))) * inv_det,
259                m[0][0].mul_add(c5, m[0][3].mul_add(c1, -(m[0][2] * c2))) * inv_det,
260                (-m[3][0]).mul_add(s5, m[3][2].mul_add(s2, -(m[3][3] * s1))) * inv_det,
261                m[2][0].mul_add(s5, m[2][3].mul_add(s1, -(m[2][2] * s2))) * inv_det,
262            ],
263            [
264                m[1][0].mul_add(c4, m[1][3].mul_add(c0, -(m[1][1] * c2))) * inv_det,
265                (-m[0][0]).mul_add(c4, m[0][1].mul_add(c2, -(m[0][3] * c0))) * inv_det,
266                m[3][0].mul_add(s4, m[3][3].mul_add(s0, -(m[3][1] * s2))) * inv_det,
267                (-m[2][0]).mul_add(s4, m[2][1].mul_add(s2, -(m[2][3] * s0))) * inv_det,
268            ],
269            [
270                (-m[1][0]).mul_add(c3, m[1][1].mul_add(c1, -(m[1][2] * c0))) * inv_det,
271                m[0][0].mul_add(c3, m[0][2].mul_add(c0, -(m[0][1] * c1))) * inv_det,
272                (-m[3][0]).mul_add(s3, m[3][1].mul_add(s1, -(m[3][2] * s0))) * inv_det,
273                m[2][0].mul_add(s3, m[2][2].mul_add(s0, -(m[2][1] * s1))) * inv_det,
274            ],
275        ]))
276    }
277}
278
279impl Mul for Mat4 {
280    type Output = Self;
281
282    fn mul(self, rhs: Self) -> Self {
283        let a = &self.0;
284        let b = &rhs.0;
285        let mut out = [[0.0_f64; 4]; 4];
286        for i in 0..4 {
287            for j in 0..4 {
288                out[i][j] = a[i][0].mul_add(
289                    b[0][j],
290                    a[i][1].mul_add(b[1][j], a[i][2].mul_add(b[2][j], a[i][3] * b[3][j])),
291                );
292            }
293        }
294        Self(out)
295    }
296}
297
298#[cfg(test)]
299#[allow(clippy::expect_used)]
300mod tests {
301    use super::*;
302
303    fn approx_eq_mat4(a: &Mat4, b: &Mat4, tol: f64) -> bool {
304        for i in 0..4 {
305            for j in 0..4 {
306                if (a.0[i][j] - b.0[i][j]).abs() > tol {
307                    return false;
308                }
309            }
310        }
311        true
312    }
313
314    #[test]
315    fn identity_inverse() {
316        let inv = Mat4::identity().inverse().expect("invertible");
317        assert!(approx_eq_mat4(&inv, &Mat4::identity(), 1e-14));
318    }
319
320    #[test]
321    fn translation_inverse() {
322        let m = Mat4::translation(1.0, 2.0, 3.0);
323        let inv = m.inverse().expect("invertible");
324        let product = m * inv;
325        assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-12));
326    }
327
328    #[test]
329    fn rotation_inverse() {
330        let m = Mat4::rotation_x(0.7) * Mat4::rotation_y(1.2) * Mat4::rotation_z(0.3);
331        let inv = m.inverse().expect("invertible");
332        let product = m * inv;
333        assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-12));
334    }
335
336    #[test]
337    fn scale_inverse() {
338        let m = Mat4::scale(2.0, 3.0, 4.0);
339        let inv = m.inverse().expect("invertible");
340        let product = m * inv;
341        assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-12));
342    }
343
344    #[test]
345    fn singular_matrix() {
346        let m = Mat4([[1.0, 0.0, 0.0, 0.0]; 4]);
347        assert!(m.inverse().is_err());
348    }
349
350    #[test]
351    fn combined_transform_inverse() {
352        let m = Mat4::translation(5.0, -3.0, 2.0)
353            * Mat4::rotation_z(std::f64::consts::FRAC_PI_4)
354            * Mat4::scale(2.0, 0.5, 1.0);
355        let inv = m.inverse().expect("invertible");
356        let product = m * inv;
357        assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-10));
358    }
359
360    use proptest::prelude::*;
361
362    proptest! {
363        #[test]
364        fn prop_inverse_roundtrip(
365            tx in -10.0f64..10.0,
366            ty in -10.0f64..10.0,
367            tz in -10.0f64..10.0,
368            angle in 0.0f64..std::f64::consts::TAU,
369        ) {
370            let m = Mat4::translation(tx, ty, tz) * Mat4::rotation_z(angle);
371            let inv = m.inverse().expect("invertible");
372            let product = m * inv;
373            prop_assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-10));
374        }
375
376        /// Verify that inverse works for matrices with small and large entry
377        /// magnitudes (the old hardcoded 1e-15 threshold would reject these).
378        #[test]
379        fn prop_inverse_scaled(
380            tx in -10.0f64..10.0,
381            ty in -10.0f64..10.0,
382            tz in -10.0f64..10.0,
383            angle in 0.0f64..std::f64::consts::TAU,
384            scale_exp in prop::sample::select(&[-8_i32, -6, -4, -2, 2, 4, 6][..]),
385        ) {
386            let scale = 10.0_f64.powi(scale_exp);
387            let m = Mat4::translation(tx * scale, ty * scale, tz * scale)
388                * Mat4::rotation_z(angle)
389                * Mat4::scale(scale, scale, scale);
390            let inv = m.inverse().expect("invertible");
391            let product = m * inv;
392            // Tolerance scales with condition number; 1e-6 is generous enough
393            // for the range of scales we test.
394            prop_assert!(approx_eq_mat4(&product, &Mat4::identity(), 1e-6));
395        }
396    }
397}
brepkit_math/mat.rs

brepkit_math/
mat.rs
