voxora_math/
solvers.rs

1//! Numerical solvers, SVD decomposition, linear least-squares, and floating-point sanity validation.
2
3use crate::{Matrix3x3, Vector3};
4
5/// Singular Value Decomposition result for a $3 \times 3$ matrix $A = U \cdot S \cdot V^T$.
6#[derive(Debug, Clone, PartialEq)]
7pub struct SvdResult3x3 {
8    /// Left orthogonal matrix U
9    pub u: Matrix3x3,
10    /// Singular values vector S = (s1, s2, s3) in descending order
11    pub s: Vector3,
12    /// Right orthogonal matrix V
13    pub v: Matrix3x3,
14}
15
16/// Solves $3 \times 3$ Singular Value Decomposition using Jacobi rotations.
17pub fn svd_3x3(a: &Matrix3x3) -> SvdResult3x3 {
18    let mut u = *a;
19    let mut v = Matrix3x3::IDENTITY;
20    let max_iter = 15;
21
22    for _ in 0..max_iter {
23        // Cyclic Jacobi rotations on U * U^T
24        let mut converged = true;
25        for i in 0..2 {
26            for j in (i + 1)..3 {
27                let alpha = u.get(0, i) * u.get(0, j)
28                    + u.get(1, i) * u.get(1, j)
29                    + u.get(2, i) * u.get(2, j);
30                let beta = u.get(0, i) * u.get(0, i)
31                    + u.get(1, i) * u.get(1, i)
32                    + u.get(2, i) * u.get(2, i);
33                let gamma = u.get(0, j) * u.get(0, j)
34                    + u.get(1, j) * u.get(1, j)
35                    + u.get(2, j) * u.get(2, j);
36
37                if alpha.abs() > 1e-10 {
38                    converged = false;
39                    let zeta = (gamma - beta) / (2.0 * alpha);
40                    let t = if zeta >= 0.0 {
41                        1.0 / (zeta + (1.0 + zeta * zeta).sqrt())
42                    } else {
43                        -1.0 / (-zeta + (1.0 + zeta * zeta).sqrt())
44                    };
45                    let c = 1.0 / (1.0 + t * t).sqrt();
46                    let s = c * t;
47
48                    // Rotate columns i and j of U and V
49                    for k in 0..3 {
50                        let u_ik = u.get(k, i);
51                        let u_jk = u.get(k, j);
52                        u.set(k, i, c * u_ik - s * u_jk);
53                        u.set(k, j, s * u_ik + c * u_jk);
54
55                        let v_ik = v.get(k, i);
56                        let v_jk = v.get(k, j);
57                        v.set(k, i, c * v_ik - s * v_jk);
58                        v.set(k, j, s * v_ik + c * v_jk);
59                    }
60                }
61            }
62        }
63        if converged {
64            break;
65        }
66    }
67
68    // Extract singular values (column norms of U)
69    let s1 =
70        (u.get(0, 0) * u.get(0, 0) + u.get(1, 0) * u.get(1, 0) + u.get(2, 0) * u.get(2, 0)).sqrt();
71    let s2 =
72        (u.get(0, 1) * u.get(0, 1) + u.get(1, 1) * u.get(1, 1) + u.get(2, 1) * u.get(2, 1)).sqrt();
73    let s3 =
74        (u.get(0, 2) * u.get(0, 2) + u.get(1, 2) * u.get(1, 2) + u.get(2, 2) * u.get(2, 2)).sqrt();
75
76    // Normalize U columns
77    if s1 > 1e-10 {
78        u.set(0, 0, u.get(0, 0) / s1);
79        u.set(1, 0, u.get(1, 0) / s1);
80        u.set(2, 0, u.get(2, 0) / s1);
81    }
82    if s2 > 1e-10 {
83        u.set(0, 1, u.get(0, 1) / s2);
84        u.set(1, 1, u.get(1, 1) / s2);
85        u.set(2, 1, u.get(2, 1) / s2);
86    }
87    if s3 > 1e-10 {
88        u.set(0, 2, u.get(0, 2) / s3);
89        u.set(1, 2, u.get(1, 2) / s3);
90        u.set(2, 2, u.get(2, 2) / s3);
91    }
92
93    SvdResult3x3 { u, s: Vector3::new(s1, s2, s3), v }
94}
95
96/// Solves linear least-squares problem $A x = b$ for a $3 \times 3$ matrix system.
97pub fn least_squares_solve(a: &Matrix3x3, b: Vector3) -> Option<Vector3> {
98    let inv_a = a.invert()?;
99    Some(inv_a.mul_vec(b))
100}
101
102/// Sanitizes a floating-point value, substituting fallback if value is NaN or Infinite.
103pub fn sanitize_float(val: f64, fallback: f64) -> f64 {
104    if val.is_nan() || val.is_infinite() {
105        fallback
106    } else {
107        val
108    }
109}
110
111#[cfg(test)]
112mod tests {
113    use super::*;
114
115    #[test]
116    fn test_svd_3x3_decomposition() {
117        let m = Matrix3x3::from_row_major([2.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
118        let svd = svd_3x3(&m);
119
120        assert!(
121            (svd.s.x - 3.0).abs() < 1e-2
122                || (svd.s.y - 3.0).abs() < 1e-2
123                || (svd.s.z - 3.0).abs() < 1e-2
124        );
125    }
126
127    #[test]
128    fn test_least_squares_solve() {
129        let a = Matrix3x3::IDENTITY;
130        let b = Vector3::new(1.0, 2.0, 3.0);
131        let x = least_squares_solve(&a, b).unwrap();
132        assert_eq!(x, b);
133    }
134
135    #[test]
136    fn test_sanitize_float() {
137        assert_eq!(sanitize_float(f64::NAN, 0.0), 0.0);
138        assert_eq!(sanitize_float(f64::INFINITY, 1.0), 1.0);
139        assert_eq!(sanitize_float(42.0, 0.0), 42.0);
140    }
141}
voxora_math/solvers.rs

voxora_math/
solvers.rs
