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

glamx/
svd3.rs
