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::{UnitVec3, Vec3, 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 {
84            return None;
85        }
86        let inv_d = 1.0 / d;
87        let [c0, c1, c2] = self.col;
88
89        // Each variable is a *column* of the inverse (= a column of adj(M) / det).
90        // Column 0 of adj(M) = cofactors of row 0: C_{00}, C_{10}, C_{20}
91        let ic0 = Vec3::new(
92            (c1.y * c2.z - c1.z * c2.y) * inv_d,  //  C_{00} = +(e·i - f·h)
93            -(c0.y * c2.z - c0.z * c2.y) * inv_d, //  C_{10} = -(b·i - c·h) wait...
94            (c0.y * c1.z - c0.z * c1.y) * inv_d,  //  C_{20} = +(b·f - c·e)
95        );
96        // Column 1 of adj(M)
97        let ic1 = Vec3::new(
98            -(c1.x * c2.z - c1.z * c2.x) * inv_d, //  C_{01} = -(d·i - f·g)
99            (c0.x * c2.z - c0.z * c2.x) * inv_d,  //  C_{11} = +(a·i - c·g)
100            -(c0.x * c1.z - c0.z * c1.x) * inv_d, //  C_{21} = -(a·f - c·d)
101        );
102        // Column 2 of adj(M)
103        let ic2 = Vec3::new(
104            (c1.x * c2.y - c1.y * c2.x) * inv_d,  //  C_{02} = +(d·h - e·g)
105            -(c0.x * c2.y - c0.y * c2.x) * inv_d, //  C_{12} = -(a·h - b·g)
106            (c0.x * c1.y - c0.y * c1.x) * inv_d,  //  C_{22} = +(a·e - b·d)
107        );
108        Some(Self::from_cols(ic0, ic1, ic2))
109    }
110
111    /// Apply the matrix to a vector: `M * v`.
112    #[inline]
113    pub fn apply(self, v: Vec3) -> Vec3 {
114        self.col[0] * v.x + self.col[1] * v.y + self.col[2] * v.z
115    }
116}
117
118impl Mul for Mat3 {
119    type Output = Self;
120    fn mul(self, rhs: Self) -> Self {
121        Self::from_cols(
122            self.apply(rhs.col[0]),
123            self.apply(rhs.col[1]),
124            self.apply(rhs.col[2]),
125        )
126    }
127}
128
129impl MulAssign for Mat3 {
130    fn mul_assign(&mut self, rhs: Self) {
131        *self = *self * rhs;
132    }
133}
134
135impl Mul<Vec3> for Mat3 {
136    type Output = Vec3;
137    #[inline]
138    fn mul(self, v: Vec3) -> Vec3 {
139        self.apply(v)
140    }
141}
142
143impl fmt::Debug for Mat3 {
144    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
145        let [c0, c1, c2] = self.col;
146        write!(
147            f,
148            "Mat3[\n  [{:.4} {:.4} {:.4}]\n  [{:.4} {:.4} {:.4}]\n  [{:.4} {:.4} {:.4}]\n]",
149            c0.x, c1.x, c2.x, c0.y, c1.y, c2.y, c0.z, c1.z, c2.z,
150        )
151    }
152}
153
154#[cfg(test)]
155mod tests {
156    use super::*;
157    use crate::PI;
158
159    #[test]
160    fn rotation_90_deg_around_z() {
161        let m = Mat3::rotation(UnitVec3::Z, PI / 2.0);
162        let v = m * Vec3::X;
163        assert!((v - Vec3::Y).length() < 1e-12);
164    }
165
166    #[test]
167    fn inverse_times_original_is_identity() {
168        let m = Mat3::rotation(
169            UnitVec3::try_from_vec(Vec3::new(1.0, 2.0, 3.0).normalize()).unwrap(),
170            1.23,
171        );
172        let inv = m.try_inverse().unwrap();
173        let prod = m * inv;
174        for i in 0..3 {
175            let diff = prod.col[i] - Mat3::IDENTITY.col[i];
176            assert!(diff.length() < 1e-10, "column {i}: {:?}", diff);
177        }
178    }
179}
cadcore_math/mat3.rs

cadcore_math/
mat3.rs
