oxihuman_core/
matrix3.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3
4//! 3×3 matrix with determinant, inverse, transpose, and multiply.
5
6#![allow(dead_code)]
7
8/// A 3×3 matrix stored in row-major order.
9#[allow(dead_code)]
10#[derive(Debug, Clone, Copy, PartialEq)]
11pub struct Mat3 {
12    /// Row-major: `m[row][col]`.
13    pub m: [[f32; 3]; 3],
14}
15
16/// Create a Mat3 from 9 row-major values.
17#[allow(clippy::too_many_arguments)]
18#[allow(dead_code)]
19pub fn mat3(
20    a00: f32,
21    a01: f32,
22    a02: f32,
23    a10: f32,
24    a11: f32,
25    a12: f32,
26    a20: f32,
27    a21: f32,
28    a22: f32,
29) -> Mat3 {
30    Mat3 {
31        m: [[a00, a01, a02], [a10, a11, a12], [a20, a21, a22]],
32    }
33}
34
35/// Identity matrix.
36#[allow(dead_code)]
37pub fn mat3_identity() -> Mat3 {
38    mat3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
39}
40
41/// Zero matrix.
42#[allow(dead_code)]
43pub fn mat3_zero() -> Mat3 {
44    mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
45}
46
47/// Transpose a 3×3 matrix.
48#[allow(dead_code)]
49pub fn mat3_transpose(m: &Mat3) -> Mat3 {
50    mat3(
51        m.m[0][0], m.m[1][0], m.m[2][0], m.m[0][1], m.m[1][1], m.m[2][1], m.m[0][2], m.m[1][2],
52        m.m[2][2],
53    )
54}
55
56/// Determinant of a 3×3 matrix.
57#[allow(dead_code)]
58pub fn mat3_det(m: &Mat3) -> f32 {
59    m.m[0][0] * (m.m[1][1] * m.m[2][2] - m.m[1][2] * m.m[2][1])
60        - m.m[0][1] * (m.m[1][0] * m.m[2][2] - m.m[1][2] * m.m[2][0])
61        + m.m[0][2] * (m.m[1][0] * m.m[2][1] - m.m[1][1] * m.m[2][0])
62}
63
64/// Inverse of a 3×3 matrix. Returns None if singular.
65#[allow(dead_code)]
66pub fn mat3_inverse(m: &Mat3) -> Option<Mat3> {
67    let det = mat3_det(m);
68    if det.abs() < 1e-10 {
69        return None;
70    }
71    let inv = 1.0 / det;
72    Some(mat3(
73        (m.m[1][1] * m.m[2][2] - m.m[1][2] * m.m[2][1]) * inv,
74        (m.m[0][2] * m.m[2][1] - m.m[0][1] * m.m[2][2]) * inv,
75        (m.m[0][1] * m.m[1][2] - m.m[0][2] * m.m[1][1]) * inv,
76        (m.m[1][2] * m.m[2][0] - m.m[1][0] * m.m[2][2]) * inv,
77        (m.m[0][0] * m.m[2][2] - m.m[0][2] * m.m[2][0]) * inv,
78        (m.m[0][2] * m.m[1][0] - m.m[0][0] * m.m[1][2]) * inv,
79        (m.m[1][0] * m.m[2][1] - m.m[1][1] * m.m[2][0]) * inv,
80        (m.m[0][1] * m.m[2][0] - m.m[0][0] * m.m[2][1]) * inv,
81        (m.m[0][0] * m.m[1][1] - m.m[0][1] * m.m[1][0]) * inv,
82    ))
83}
84
85/// Multiply two 3×3 matrices.
86#[allow(dead_code)]
87pub fn mat3_mul(a: &Mat3, b: &Mat3) -> Mat3 {
88    let mut r = mat3_zero();
89    for i in 0..3 {
90        for j in 0..3 {
91            for k in 0..3 {
92                r.m[i][j] += a.m[i][k] * b.m[k][j];
93            }
94        }
95    }
96    r
97}
98
99/// Add two 3×3 matrices.
100#[allow(dead_code)]
101pub fn mat3_add(a: &Mat3, b: &Mat3) -> Mat3 {
102    let mut r = mat3_zero();
103    for i in 0..3 {
104        for j in 0..3 {
105            r.m[i][j] = a.m[i][j] + b.m[i][j];
106        }
107    }
108    r
109}
110
111/// Scale a 3×3 matrix by a scalar.
112#[allow(dead_code)]
113pub fn mat3_scale(m: &Mat3, s: f32) -> Mat3 {
114    let mut r = mat3_zero();
115    for i in 0..3 {
116        for j in 0..3 {
117            r.m[i][j] = m.m[i][j] * s;
118        }
119    }
120    r
121}
122
123/// Multiply a 3×3 matrix by a 3D column vector.
124#[allow(dead_code)]
125pub fn mat3_mul_vec(m: &Mat3, v: [f32; 3]) -> [f32; 3] {
126    [
127        m.m[0][0] * v[0] + m.m[0][1] * v[1] + m.m[0][2] * v[2],
128        m.m[1][0] * v[0] + m.m[1][1] * v[1] + m.m[1][2] * v[2],
129        m.m[2][0] * v[0] + m.m[2][1] * v[1] + m.m[2][2] * v[2],
130    ]
131}
132
133/// Trace of the matrix.
134#[allow(dead_code)]
135pub fn mat3_trace(m: &Mat3) -> f32 {
136    m.m[0][0] + m.m[1][1] + m.m[2][2]
137}
138
139/// Check approximate equality.
140#[allow(dead_code)]
141pub fn mat3_approx_eq(a: &Mat3, b: &Mat3, eps: f32) -> bool {
142    for i in 0..3 {
143        for j in 0..3 {
144            if (a.m[i][j] - b.m[i][j]).abs() >= eps {
145                return false;
146            }
147        }
148    }
149    true
150}
151
152/// Create a 3D rotation matrix around the Z axis.
153#[allow(dead_code)]
154pub fn mat3_rot_z(theta: f32) -> Mat3 {
155    let c = theta.cos();
156    let s = theta.sin();
157    mat3(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0)
158}
159
160/// Outer product of two 3D vectors: result_ij = a_i * b_j.
161#[allow(dead_code)]
162pub fn mat3_outer(a: [f32; 3], b: [f32; 3]) -> Mat3 {
163    mat3(
164        a[0] * b[0],
165        a[0] * b[1],
166        a[0] * b[2],
167        a[1] * b[0],
168        a[1] * b[1],
169        a[1] * b[2],
170        a[2] * b[0],
171        a[2] * b[1],
172        a[2] * b[2],
173    )
174}
175
176#[cfg(test)]
177mod tests {
178    use super::*;
179    use std::f32::consts::PI;
180
181    #[test]
182    fn test_identity_det() {
183        let id = mat3_identity();
184        assert!((mat3_det(&id) - 1.0).abs() < 1e-5);
185    }
186
187    #[test]
188    fn test_inverse() {
189        let m = mat3(1.0, 2.0, 0.0, 0.0, 1.0, 3.0, 0.0, 0.0, 1.0);
190        let inv = mat3_inverse(&m).expect("should succeed");
191        let prod = mat3_mul(&m, &inv);
192        assert!(mat3_approx_eq(&prod, &mat3_identity(), 1e-5));
193    }
194
195    #[test]
196    fn test_singular_inverse() {
197        let m = mat3(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
198        assert!(mat3_inverse(&m).is_none());
199    }
200
201    #[test]
202    fn test_transpose() {
203        let m = mat3(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
204        let t = mat3_transpose(&m);
205        assert!((t.m[0][1] - 4.0).abs() < 1e-5);
206        assert!((t.m[1][0] - 2.0).abs() < 1e-5);
207    }
208
209    #[test]
210    fn test_mul_identity() {
211        let m = mat3(1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0);
212        let id = mat3_identity();
213        let result = mat3_mul(&m, &id);
214        assert!(mat3_approx_eq(&result, &m, 1e-5));
215    }
216
217    #[test]
218    fn test_mul_vec() {
219        let m = mat3_identity();
220        let v = mat3_mul_vec(&m, [1.0, 2.0, 3.0]);
221        assert!((v[0] - 1.0).abs() < 1e-5);
222        assert!((v[1] - 2.0).abs() < 1e-5);
223        assert!((v[2] - 3.0).abs() < 1e-5);
224    }
225
226    #[test]
227    fn test_rotation_z() {
228        let r = mat3_rot_z(PI / 2.0);
229        let v = mat3_mul_vec(&r, [1.0, 0.0, 0.0]);
230        assert!(v[0].abs() < 1e-5);
231        assert!((v[1] - 1.0).abs() < 1e-5);
232    }
233
234    #[test]
235    fn test_trace() {
236        let m = mat3(1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0);
237        assert!((mat3_trace(&m) - 6.0).abs() < 1e-5);
238    }
239
240    #[test]
241    fn test_outer_product() {
242        let a = [1.0f32, 0.0, 0.0];
243        let b = [0.0f32, 1.0, 0.0];
244        let o = mat3_outer(a, b);
245        assert!((o.m[0][1] - 1.0).abs() < 1e-5);
246        assert!(o.m[0][0].abs() < 1e-5);
247    }
248
249    #[test]
250    fn test_scale() {
251        let m = mat3_identity();
252        let s = mat3_scale(&m, 2.0);
253        assert!((s.m[0][0] - 2.0).abs() < 1e-5);
254    }
255}
oxihuman_core/matrix3.rs

oxihuman_core/
matrix3.rs
