glamx/
svd3.rs

1//! SVD decomposition for 3x3 matrices.
2//!
3//! Uses eigendecomposition of A^T * A to compute singular values and right singular vectors,
4//! then derives left singular vectors from those.
5
6#[cfg(not(feature = "std"))]
7use simba::scalar::ComplexField;
8
9#[cfg(feature = "f64")]
10use crate::eigen3::DSymmetricEigen3;
11use crate::eigen3::SymmetricEigen3;
12use crate::SymmetricEigen3A;
13
14/// Macro to generate Svd3 for a specific scalar type.
15macro_rules! impl_svd3 {
16    ($Svd3:ident, $Mat3:ty, $Vec3:ty, $SymmetricEigen3:ty, $Real:ty $(, #[$attr:meta])*) => {
17        #[doc = concat!("The SVD of a 3x3 matrix (", stringify!($Real), " precision).")]
18        #[derive(Clone, Copy, Debug, PartialEq)]
19        #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
20        $(#[$attr])*
21        pub struct $Svd3 {
22            /// The left orthogonal matrix U.
23            pub u: $Mat3,
24            /// The singular values (diagonal entries of S), sorted in descending order.
25            pub s: $Vec3,
26            /// The transposed right orthogonal matrix V^T.
27            pub vt: $Mat3,
28        }
29
30        impl $Svd3 {
31            /// Creates a new SVD from components.
32            #[inline]
33            pub fn new(u: $Mat3, s: $Vec3, vt: $Mat3) -> Self {
34                Self { u, s, vt }
35            }
36
37            /// Computes the SVD of a 3x3 matrix.
38            ///
39            /// Returns the decomposition `M = U * S * V^T` where:
40            /// - `U` is an orthogonal matrix (left singular vectors as columns)
41            /// - `S` is a diagonal matrix (stored as a vector of singular values)
42            /// - `V^T` is the transpose of an orthogonal matrix
43            ///
44            /// The singular values are sorted in descending order (s.x >= s.y >= s.z).
45            #[inline]
46            pub fn from_matrix(a: $Mat3) -> Self {
47                // Compute A^T * A (symmetric positive semi-definite)
48                let ata = a.transpose() * a;
49
50                // Eigendecomposition of A^T * A gives:
51                // - eigenvalues = singular values squared
52                // - eigenvectors = right singular vectors (columns of V)
53                let eigen = <$SymmetricEigen3>::new(ata).reverse(); // reverse to get descending order
54
55                // Singular values are square roots of eigenvalues
56                let s = <$Vec3>::new(
57                    eigen.eigenvalues.x.max(0.0).sqrt(),
58                    eigen.eigenvalues.y.max(0.0).sqrt(),
59                    eigen.eigenvalues.z.max(0.0).sqrt(),
60                );
61
62                // V is the eigenvector matrix
63                let v = eigen.eigenvectors;
64
65                // Compute U = A * V * S^(-1) for each column
66                // U.col(i) = A * V.col(i) / s[i]
67                let epsilon: $Real = 1e-10;
68
69                // Compute U columns with Gram-Schmidt orthogonalization
70                let u_col0_raw = if s.x > epsilon {
71                    (a * v.col(0)) / s.x
72                } else {
73                    v.col(0)
74                };
75                let u_col0 = u_col0_raw.normalize();
76
77                let u_col1_raw = if s.y > epsilon {
78                    (a * v.col(1)) / s.y
79                } else {
80                    Self::any_orthogonal(u_col0)
81                };
82                // Orthogonalize against u_col0
83                let u_col1 = (u_col1_raw - u_col0 * u_col1_raw.dot(u_col0)).normalize();
84
85                let u_col2_raw = if s.z > epsilon {
86                    (a * v.col(2)) / s.z
87                } else {
88                    u_col0.cross(u_col1)
89                };
90                // Orthogonalize against u_col0 and u_col1
91                let u_col2_orth = u_col2_raw - u_col0 * u_col2_raw.dot(u_col0) - u_col1 * u_col2_raw.dot(u_col1);
92                let u_col2 = u_col2_orth.normalize();
93
94                let u = <$Mat3>::from_cols(u_col0, u_col1, u_col2);
95
96                Self {
97                    u,
98                    s,
99                    vt: v.transpose(),
100                }
101            }
102
103            /// Find any unit vector orthogonal to the given vector.
104            #[inline]
105            fn any_orthogonal(v: $Vec3) -> $Vec3 {
106                let abs_v = <$Vec3>::new(v.x.abs(), v.y.abs(), v.z.abs());
107                let other = if abs_v.x <= abs_v.y && abs_v.x <= abs_v.z {
108                    <$Vec3>::X
109                } else if abs_v.y <= abs_v.z {
110                    <$Vec3>::Y
111                } else {
112                    <$Vec3>::Z
113                };
114                v.cross(other).normalize()
115            }
116
117            /// Rebuilds the matrix this SVD is the decomposition of.
118            ///
119            /// Returns `U * S * V^T`.
120            #[inline]
121            pub fn recompose(&self) -> $Mat3 {
122                let u_s = <$Mat3>::from_cols(
123                    self.u.col(0) * self.s.x,
124                    self.u.col(1) * self.s.y,
125                    self.u.col(2) * self.s.z,
126                );
127                u_s * self.vt
128            }
129        }
130    };
131}
132
133impl_svd3!(Svd3, glam::Mat3, glam::Vec3, SymmetricEigen3, f32);
134impl_svd3!(Svd3A, glam::Mat3A, glam::Vec3A, SymmetricEigen3A, f32);
135#[cfg(feature = "f64")]
136impl_svd3!(DSvd3, glam::DMat3, glam::DVec3, DSymmetricEigen3, f64);
137
138// f32 <-> f64 conversions
139#[cfg(feature = "f64")]
140impl From<Svd3> for DSvd3 {
141    #[inline]
142    fn from(svd: Svd3) -> Self {
143        Self {
144            u: svd.u.as_dmat3(),
145            s: svd.s.as_dvec3(),
146            vt: svd.vt.as_dmat3(),
147        }
148    }
149}
150
151#[cfg(feature = "f64")]
152impl From<DSvd3> for Svd3 {
153    #[inline]
154    fn from(svd: DSvd3) -> Self {
155        Self {
156            u: svd.u.as_mat3(),
157            s: svd.s.as_vec3(),
158            vt: svd.vt.as_mat3(),
159        }
160    }
161}
162
163#[cfg(feature = "f64")]
164impl From<Svd3A> for DSvd3 {
165    #[inline]
166    fn from(svd: Svd3A) -> Self {
167        Self {
168            u: svd.u.as_dmat3(),
169            s: svd.s.as_dvec3(),
170            vt: svd.vt.as_dmat3(),
171        }
172    }
173}
174
175#[cfg(feature = "f64")]
176impl From<DSvd3> for Svd3A {
177    #[inline]
178    fn from(svd: DSvd3) -> Self {
179        Self {
180            u: svd.u.as_mat3().into(),
181            s: svd.s.as_vec3a(),
182            vt: svd.vt.as_mat3().into(),
183        }
184    }
185}
186
187// Svd3 <-> Svd3A conversions
188impl From<Svd3> for Svd3A {
189    #[inline]
190    fn from(svd: Svd3) -> Self {
191        Self {
192            u: glam::Mat3A::from(svd.u),
193            s: svd.s.into(),
194            vt: glam::Mat3A::from(svd.vt),
195        }
196    }
197}
198
199impl From<Svd3A> for Svd3 {
200    #[inline]
201    fn from(svd: Svd3A) -> Self {
202        Self {
203            u: glam::Mat3::from(svd.u),
204            s: svd.s.into(),
205            vt: glam::Mat3::from(svd.vt),
206        }
207    }
208}
209
210#[cfg(test)]
211mod tests {
212    use super::*;
213    use approx::assert_relative_eq;
214
215    #[test]
216    fn svd_3x3_identity() {
217        let mat = glam::Mat3::IDENTITY;
218        let svd = Svd3::from_matrix(mat);
219
220        assert_relative_eq!(svd.s, glam::Vec3::new(1.0, 1.0, 1.0), epsilon = 1e-5);
221        assert_relative_eq!(svd.recompose(), mat, epsilon = 1e-5);
222    }
223
224    #[test]
225    fn svd_3x3_diagonal() {
226        let mat = glam::Mat3::from_diagonal(glam::Vec3::new(4.0, 3.0, 2.0));
227        let svd = Svd3::from_matrix(mat);
228
229        // Singular values should be sorted in descending order
230        assert_relative_eq!(svd.s, glam::Vec3::new(4.0, 3.0, 2.0), epsilon = 1e-5);
231        assert_relative_eq!(svd.recompose(), mat, epsilon = 1e-5);
232    }
233
234    #[test]
235    fn svd_3x3_general() {
236        // Use a full-rank matrix
237        let mat =
238            glam::Mat3::from_cols_array_2d(&[[1.0, 4.0, 7.0], [2.0, 5.0, 9.0], [3.0, 6.0, 10.0]]);
239        let svd = Svd3::from_matrix(mat);
240
241        // Verify recomposition (some numerical error from orthogonalization)
242        assert_relative_eq!(svd.recompose(), mat, epsilon = 1e-2);
243
244        // Verify singular values are sorted in descending order
245        assert!(svd.s.x >= svd.s.y);
246        assert!(svd.s.y >= svd.s.z);
247
248        // Verify U is orthogonal
249        assert_relative_eq!(
250            svd.u * svd.u.transpose(),
251            glam::Mat3::IDENTITY,
252            epsilon = 1e-5
253        );
254
255        // Verify V^T is orthogonal
256        assert_relative_eq!(
257            svd.vt * svd.vt.transpose(),
258            glam::Mat3::IDENTITY,
259            epsilon = 1e-5
260        );
261    }
262
263    #[test]
264    fn svd_3x3_rotation() {
265        let mat = glam::Mat3::from_rotation_x(0.5) * glam::Mat3::from_rotation_y(0.3);
266        let svd = Svd3::from_matrix(mat);
267
268        // A rotation matrix has singular values of 1
269        assert_relative_eq!(svd.s, glam::Vec3::ONE, epsilon = 1e-5);
270        assert_relative_eq!(svd.recompose(), mat, epsilon = 1e-5);
271    }
272
273    #[cfg(feature = "f64")]
274    #[test]
275    fn svd_3x3_f64() {
276        let mat =
277            glam::DMat3::from_cols_array_2d(&[[1.0, 4.0, 7.0], [2.0, 5.0, 9.0], [3.0, 6.0, 10.0]]);
278        let svd = DSvd3::from_matrix(mat);
279
280        assert_relative_eq!(svd.recompose(), mat, epsilon = 1e-8);
281    }
282
283    #[test]
284    fn svd_3x3_rank_deficient() {
285        // Test with a rank-2 matrix (third singular value is 0)
286        let mat =
287            glam::Mat3::from_cols_array_2d(&[[1.0, 4.0, 7.0], [2.0, 5.0, 8.0], [3.0, 6.0, 9.0]]);
288        let svd = Svd3::from_matrix(mat);
289
290        // Verify recomposition (larger tolerance for rank-deficient matrices)
291        assert_relative_eq!(svd.recompose(), mat, epsilon = 0.02);
292
293        // Third singular value should be small (matrix is nearly rank 2)
294        assert!(svd.s.z < 0.1);
295    }
296}
glamx/svd3.rs

glamx/
svd3.rs
