spatialrust_math/
eigen.rs

1use crate::{Mat3, Vec3};
2
3/// Result of a symmetric 3x3 eigendecomposition.
4#[derive(Clone, Copy, Debug, PartialEq)]
5pub struct SymmetricEigen3 {
6    /// Eigenvalues sorted in ascending order.
7    pub eigenvalues: [f64; 3],
8    /// Eigenvectors stored as columns in row-major matrix form.
9    pub eigenvectors: Mat3<f64>,
10}
11
12/// Computes eigenvalues and eigenvectors of a symmetric 3x3 matrix.
13///
14/// Uses Jacobi iterations, suitable for normal estimation and local covariance analysis.
15#[must_use]
16pub fn symmetric_eigen3(matrix: Mat3<f64>) -> SymmetricEigen3 {
17    let mut a = matrix;
18    let mut v = Mat3::<f64>::identity();
19
20    for _ in 0..32 {
21        let mut p = 0;
22        let mut q = 1;
23        let mut max = a.m[0][1].abs();
24        if a.m[0][2].abs() > max {
25            max = a.m[0][2].abs();
26            p = 0;
27            q = 2;
28        }
29        if a.m[1][2].abs() > max {
30            max = a.m[1][2].abs();
31            p = 1;
32            q = 2;
33        }
34        if max < 1e-12 {
35            break;
36        }
37
38        let app = a.m[p][p];
39        let aqq = a.m[q][q];
40        let apq = a.m[p][q];
41        let phi = 0.5 * (aqq - app) / apq;
42        let t = phi.signum() / (phi.abs() + (1.0 + phi * phi).sqrt());
43        let c = 1.0 / (1.0 + t * t).sqrt();
44        let s = t * c;
45
46        let app_new = c * c * app - 2.0 * s * c * apq + s * s * aqq;
47        let aqq_new = s * s * app + 2.0 * s * c * apq + c * c * aqq;
48        a.m[p][p] = app_new;
49        a.m[q][q] = aqq_new;
50        a.m[p][q] = 0.0;
51        a.m[q][p] = 0.0;
52
53        for r in 0..3 {
54            if r != p && r != q {
55                let arp = a.m[r][p];
56                let arq = a.m[r][q];
57                a.m[r][p] = c * arp - s * arq;
58                a.m[p][r] = a.m[r][p];
59                a.m[r][q] = s * arp + c * arq;
60                a.m[q][r] = a.m[r][q];
61            }
62        }
63
64        for r in 0..3 {
65            let vr_p = v.m[r][p];
66            let vr_q = v.m[r][q];
67            v.m[r][p] = c * vr_p - s * vr_q;
68            v.m[r][q] = s * vr_p + c * vr_q;
69        }
70    }
71
72    let eigenvalues = [a.m[0][0], a.m[1][1], a.m[2][2]];
73    let mut order = [0usize, 1, 2];
74    order.sort_by(|&i, &j| eigenvalues[i].partial_cmp(&eigenvalues[j]).unwrap());
75
76    let sorted_values = [eigenvalues[order[0]], eigenvalues[order[1]], eigenvalues[order[2]]];
77    let sorted_vectors = Mat3::from_rows(
78        [v.m[0][order[0]], v.m[0][order[1]], v.m[0][order[2]]],
79        [v.m[1][order[0]], v.m[1][order[1]], v.m[1][order[2]]],
80        [v.m[2][order[0]], v.m[2][order[1]], v.m[2][order[2]]],
81    );
82
83    SymmetricEigen3 { eigenvalues: sorted_values, eigenvectors: sorted_vectors }
84}
85
86/// Returns the eigenvector for the smallest eigenvalue.
87#[must_use]
88pub fn smallest_eigenvector(matrix: Mat3<f64>) -> Vec3<f64> {
89    let result = symmetric_eigen3(matrix);
90    Vec3::new(result.eigenvectors.m[0][0], result.eigenvectors.m[1][0], result.eigenvectors.m[2][0])
91        .normalize()
92}
93
94#[cfg(test)]
95mod tests {
96    use super::{smallest_eigenvector, symmetric_eigen3};
97    use crate::{tolerance::approx_eq_f64, Mat3};
98
99    #[test]
100    fn diagonal_eigenvalues() {
101        let m = Mat3::from_rows([1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]);
102        let result = symmetric_eigen3(m);
103        assert!(approx_eq_f64(result.eigenvalues[0], 1.0, 1e-9));
104        assert!(approx_eq_f64(result.eigenvalues[1], 2.0, 1e-9));
105        assert!(approx_eq_f64(result.eigenvalues[2], 3.0, 1e-9));
106    }
107
108    #[test]
109    fn plane_normal_from_covariance() {
110        let m = Mat3::from_rows([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.01]);
111        let normal = smallest_eigenvector(m);
112        assert!(approx_eq_f64(normal.z.abs(), 1.0, 1e-6));
113    }
114}
spatialrust_math/eigen.rs

spatialrust_math/
eigen.rs
