cadcore_math/
mat3.rs

1//! 3×3 matrix (`Mat3`) for rotations and linear maps.
2
3use std::fmt;
4use std::ops::{Mul, MulAssign};
5
6use crate::{Vec3, UnitVec3, EPS};
7
8/// Column-major 3×3 matrix.
9///
10/// Columns are stored as `col[0..2]`, so `mat * v` is `sum_i col[i]*v[i]`.
11#[derive(Clone, Copy, PartialEq)]
12pub struct Mat3 {
13    /// Columns: `col[j]` is the j-th column.
14    pub col: [Vec3; 3],
15}
16
17impl Mat3 {
18    /// Identity matrix.
19    pub const IDENTITY: Self = Self {
20        col: [Vec3::X, Vec3::Y, Vec3::Z],
21    };
22
23    /// Zero matrix.
24    pub const ZERO: Self = Self {
25        col: [Vec3::ZERO, Vec3::ZERO, Vec3::ZERO],
26    };
27
28    /// Construct from columns.
29    #[inline]
30    pub const fn from_cols(c0: Vec3, c1: Vec3, c2: Vec3) -> Self {
31        Self { col: [c0, c1, c2] }
32    }
33
34    /// Construct from rows.
35    #[inline]
36    pub fn from_rows(r0: Vec3, r1: Vec3, r2: Vec3) -> Self {
37        Self::from_cols(
38            Vec3::new(r0.x, r1.x, r2.x),
39            Vec3::new(r0.y, r1.y, r2.y),
40            Vec3::new(r0.z, r1.z, r2.z),
41        )
42    }
43
44    /// Rotation matrix that maps the +Z axis to `dir` and +X axis to `right`,
45    /// forming a right-handed orthonormal frame.
46    ///
47    /// `right` must be perpendicular to `dir`.  Use [`UnitVec3::perp_basis`]
48    /// to construct a consistent `right` if you don't have one.
49    pub fn from_axes(right: UnitVec3, up: UnitVec3, forward: UnitVec3) -> Self {
50        Self::from_cols(right.as_vec(), up.as_vec(), forward.as_vec())
51    }
52
53    /// Rotation by `angle` radians around `axis` (must be unit length).
54    pub fn rotation(axis: UnitVec3, angle: f64) -> Self {
55        let (s, c) = angle.sin_cos();
56        let t = 1.0 - c;
57        let Vec3 { x, y, z } = axis.as_vec();
58        Self::from_rows(
59            Vec3::new(t*x*x + c,   t*x*y - s*z, t*x*z + s*y),
60            Vec3::new(t*x*y + s*z, t*y*y + c,   t*y*z - s*x),
61            Vec3::new(t*x*z - s*y, t*y*z + s*x, t*z*z + c  ),
62        )
63    }
64
65    /// Transpose.
66    #[inline]
67    pub fn transpose(self) -> Self {
68        Self::from_rows(self.col[0], self.col[1], self.col[2])
69    }
70
71    /// Determinant.
72    pub fn det(self) -> f64 {
73        let [c0, c1, c2] = self.col;
74        c0.dot(c1.cross(c2))
75    }
76
77    /// Inverse (returns `None` if singular).
78    ///
79    /// Uses the cofactor/adjugate method: `M⁻¹ = adj(M)ᵀ / det`.
80    /// The three `Vec3`s below are the *columns* of the inverse.
81    pub fn try_inverse(self) -> Option<Self> {
82        let d = self.det();
83        if d.abs() < EPS { return None; }
84        let inv_d = 1.0 / d;
85        let [c0, c1, c2] = self.col;
86
87        // Each variable is a *column* of the inverse (= a column of adj(M) / det).
88        // Column 0 of adj(M) = cofactors of row 0: C_{00}, C_{10}, C_{20}
89        let ic0 = Vec3::new(
90            ( c1.y * c2.z - c1.z * c2.y) * inv_d,  //  C_{00} = +(e·i - f·h)
91            -(c0.y * c2.z - c0.z * c2.y) * inv_d,  //  C_{10} = -(b·i - c·h) wait...
92            ( c0.y * c1.z - c0.z * c1.y) * inv_d,  //  C_{20} = +(b·f - c·e)
93        );
94        // Column 1 of adj(M)
95        let ic1 = Vec3::new(
96            -(c1.x * c2.z - c1.z * c2.x) * inv_d,  //  C_{01} = -(d·i - f·g)
97            ( c0.x * c2.z - c0.z * c2.x) * inv_d,  //  C_{11} = +(a·i - c·g)
98            -(c0.x * c1.z - c0.z * c1.x) * inv_d,  //  C_{21} = -(a·f - c·d)
99        );
100        // Column 2 of adj(M)
101        let ic2 = Vec3::new(
102            ( c1.x * c2.y - c1.y * c2.x) * inv_d,  //  C_{02} = +(d·h - e·g)
103            -(c0.x * c2.y - c0.y * c2.x) * inv_d,  //  C_{12} = -(a·h - b·g)
104            ( c0.x * c1.y - c0.y * c1.x) * inv_d,  //  C_{22} = +(a·e - b·d)
105        );
106        Some(Self::from_cols(ic0, ic1, ic2))
107    }
108
109    /// Apply the matrix to a vector: `M * v`.
110    #[inline]
111    pub fn apply(self, v: Vec3) -> Vec3 {
112        self.col[0] * v.x + self.col[1] * v.y + self.col[2] * v.z
113    }
114}
115
116impl Mul for Mat3 {
117    type Output = Self;
118    fn mul(self, rhs: Self) -> Self {
119        Self::from_cols(
120            self.apply(rhs.col[0]),
121            self.apply(rhs.col[1]),
122            self.apply(rhs.col[2]),
123        )
124    }
125}
126
127impl MulAssign for Mat3 {
128    fn mul_assign(&mut self, rhs: Self) { *self = *self * rhs; }
129}
130
131impl Mul<Vec3> for Mat3 {
132    type Output = Vec3;
133    #[inline] fn mul(self, v: Vec3) -> Vec3 { self.apply(v) }
134}
135
136impl fmt::Debug for Mat3 {
137    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
138        let [c0, c1, c2] = self.col;
139        write!(f,
140            "Mat3[\n  [{:.4} {:.4} {:.4}]\n  [{:.4} {:.4} {:.4}]\n  [{:.4} {:.4} {:.4}]\n]",
141            c0.x, c1.x, c2.x,
142            c0.y, c1.y, c2.y,
143            c0.z, c1.z, c2.z,
144        )
145    }
146}
147
148#[cfg(test)]
149mod tests {
150    use super::*;
151    use crate::PI;
152
153    #[test]
154    fn rotation_90_deg_around_z() {
155        let m = Mat3::rotation(UnitVec3::Z, PI / 2.0);
156        let v = m * Vec3::X;
157        assert!((v - Vec3::Y).length() < 1e-12);
158    }
159
160    #[test]
161    fn inverse_times_original_is_identity() {
162        let m = Mat3::rotation(
163            UnitVec3::try_from_vec(Vec3::new(1.0, 2.0, 3.0).normalize()).unwrap(),
164            1.23,
165        );
166        let inv = m.try_inverse().unwrap();
167        let prod = m * inv;
168        for i in 0..3 {
169            let diff = prod.col[i] - Mat3::IDENTITY.col[i];
170            assert!(diff.length() < 1e-10, "column {i}: {:?}", diff);
171        }
172    }
173}
cadcore_math/mat3.rs

cadcore_math/
mat3.rs
