glam_matrix_extras/eigen/symmetric_eigen3.rs
1// The eigensolver is a Rust adaptation, with modifications, of the pseudocode and approach described in
2// "A Robust Eigensolver for 3 x 3 Symmetric Matrices" by David Eberly, Geometric Tools, Redmond WA 98052.
3// https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
4
5use crate::{
6 SymmetricMat3,
7 ops::{self, FloatPow},
8};
9use glam::{Mat3, Vec3, Vec3Swizzles};
10
11/// The [eigen decomposition] of a [`SymmetricMat3`].
12///
13/// [eigen decomposition]: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix
14#[derive(Clone, Copy, Debug, PartialEq)]
15#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))]
16#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
17pub struct SymmetricEigen3 {
18 /// The eigenvalues of the [`SymmetricMat3`].
19 ///
20 /// These should be in ascending order `eigen1 <= eigen2 <= eigen3`.
21 pub eigenvalues: Vec3,
22 /// The three eigenvectors of the [`SymmetricMat3`].
23 /// They should be unit length and orthogonal to the other eigenvectors.
24 ///
25 /// The eigenvectors are ordered to correspond to the eigenvalues. For example,
26 /// `eigenvectors.x_axis` corresponds to `eigenvalues.x`.
27 pub eigenvectors: Mat3,
28}
29
30impl SymmetricEigen3 {
31 /// Computes the eigen decomposition of the given [`SymmetricMat3`].
32 ///
33 /// The eigenvalues are returned in ascending order `eigen1 <= eigen2 <= eigen3`.
34 /// This can be reversed with the [`reverse`](Self::reverse) method.
35 pub fn new(mat: SymmetricMat3) -> Self {
36 let (mut eigenvalues, is_diagonal) = Self::eigenvalues(mat);
37
38 if is_diagonal {
39 // The matrix is already diagonal. Sort the eigenvalues in ascending order,
40 // ordering the eigenvectors accordingly, and return early.
41 let mut eigenvectors = Mat3::IDENTITY;
42 if eigenvalues[0] > eigenvalues[1] {
43 core::mem::swap(&mut eigenvalues.x, &mut eigenvalues.y);
44 core::mem::swap(&mut eigenvectors.x_axis, &mut eigenvectors.y_axis);
45 }
46 if eigenvalues[1] > eigenvalues[2] {
47 core::mem::swap(&mut eigenvalues.y, &mut eigenvalues.z);
48 core::mem::swap(&mut eigenvectors.y_axis, &mut eigenvectors.z_axis);
49 }
50 return Self {
51 eigenvalues,
52 eigenvectors,
53 };
54 }
55
56 // Compute the eigenvectors corresponding to the eigenvalues.
57 let eigenvector1 = Self::eigenvector1(mat, eigenvalues.x);
58 let eigenvector2 = Self::eigenvector2(mat, eigenvector1, eigenvalues.y);
59 let eigenvector3 = Self::eigenvector3(eigenvector1, eigenvector2);
60
61 Self {
62 eigenvalues,
63 eigenvectors: Mat3::from_cols(eigenvector1, eigenvector2, eigenvector3),
64 }
65 }
66
67 /// Reverses the order of the eigenvalues and their corresponding eigenvectors.
68 pub fn reverse(&self) -> Self {
69 Self {
70 eigenvalues: self.eigenvalues.zyx(),
71 eigenvectors: Mat3::from_cols(
72 self.eigenvectors.z_axis,
73 self.eigenvectors.y_axis,
74 self.eigenvectors.x_axis,
75 ),
76 }
77 }
78
79 /// Computes the eigenvalues of a [`SymmetricMat3`], also returning whether the input matrix is diagonal.
80 ///
81 /// If the matrix is already diagonal, the eigenvalues are returned as is without reordering.
82 /// Otherwise, the eigenvalues are computed and returned in ascending order
83 /// such that `eigen1 <= eigen2 <= eigen3`.
84 pub fn eigenvalues(mat: SymmetricMat3) -> (Vec3, bool) {
85 // Reference: https://en.wikipedia.org/wiki/Eigenvalue_algorithm#Symmetric_3%C3%973_matrices
86
87 let p1 = mat.m01.squared() + mat.m02.squared() + mat.m12.squared();
88
89 // Check if the matrix is nearly diagonal.
90 // Without this check, the algorithm can produce NaN values.
91 // TODO: What is the ideal threshold here?
92 if p1 < 1e-10 {
93 return (Vec3::new(mat.m00, mat.m11, mat.m22), true);
94 }
95
96 let q = (mat.m00 + mat.m11 + mat.m22) / 3.0;
97 let p2 =
98 (mat.m00 - q).squared() + (mat.m11 - q).squared() + (mat.m22 - q).squared() + 2.0 * p1;
99 let p = ops::sqrt(p2 / 6.0);
100
101 let mat_b = 1.0 / p * (mat - q * Mat3::IDENTITY);
102 let r = mat_b.determinant() / 2.0;
103
104 // r should be in the [-1, 1] range for a symmetric matrix,
105 // but computation error can leave it slightly outside this range.
106 let phi = if r <= -1.0 {
107 core::f32::consts::FRAC_PI_3
108 } else if r >= 1.0 {
109 0.0
110 } else {
111 ops::acos(r) / 3.0
112 };
113
114 // The eigenvalues satisfy eigen3 <= eigen2 <= eigen1
115 let eigen1 = q + 2.0 * p * ops::cos(phi);
116 let eigen3 = q + 2.0 * p * ops::cos(phi + 2.0 * core::f32::consts::FRAC_PI_3);
117 let eigen2 = 3.0 * q - eigen1 - eigen3; // trace(mat) = eigen1 + eigen2 + eigen3
118 (Vec3::new(eigen3, eigen2, eigen1), false)
119 }
120
121 // TODO: Fall back to QL when the eigenvalue precision is poor.
122 /// Computes the unit-length eigenvector corresponding to the `eigenvalue1` of `mat` that was
123 /// computed from the root of a cubic polynomial with a multiplicity of 1.
124 ///
125 /// If the other two eigenvalues are well separated, this method can be used for computing
126 /// all three eigenvectors. However, to avoid numerical issues when eigenvalues are close to
127 /// each other, it's recommended to use the `eigenvector2` method for the second eigenvector.
128 ///
129 /// The third eigenvector can be computed as the cross product of the first two.
130 pub fn eigenvector1(mat: SymmetricMat3, eigenvalue1: f32) -> Vec3 {
131 let cols = (mat - SymmetricMat3::from_diagonal(Vec3::splat(eigenvalue1))).to_mat3();
132 let c0xc1 = cols.x_axis.cross(cols.y_axis);
133 let c0xc2 = cols.x_axis.cross(cols.z_axis);
134 let c1xc2 = cols.y_axis.cross(cols.z_axis);
135 let d0 = c0xc1.length_squared();
136 let d1 = c0xc2.length_squared();
137 let d2 = c1xc2.length_squared();
138
139 let mut d_max = d0;
140 let mut i_max = 0;
141
142 if d1 > d_max {
143 d_max = d1;
144 i_max = 1;
145 }
146 if d2 > d_max {
147 i_max = 2;
148 }
149 if i_max == 0 {
150 c0xc1 / ops::sqrt(d0)
151 } else if i_max == 1 {
152 c0xc2 / ops::sqrt(d1)
153 } else {
154 c1xc2 / ops::sqrt(d2)
155 }
156 }
157
158 /// Computes the unit-length eigenvector corresponding to the `eigenvalue2` of `mat` that was
159 /// computed from the root of a cubic polynomial with a potential multiplicity of 2.
160 ///
161 /// The third eigenvector can be computed as the cross product of the first two.
162 pub fn eigenvector2(mat: SymmetricMat3, eigenvector1: Vec3, eigenvalue2: f32) -> Vec3 {
163 // Compute right-handed orthonormal set { U, V, W }, where W is eigenvector1.
164 let (u, v) = eigenvector1.any_orthonormal_pair();
165
166 // The unit-length eigenvector is E = x0 * U + x1 * V. We need to compute x0 and x1.
167 //
168 // Define the symmetrix 2x2 matrix M = J^T * (mat - eigenvalue2 * I), where J = [U V]
169 // and I is a 3x3 identity matrix. This means that E = J * X, where X is a column vector
170 // with rows x0 and x1. The 3x3 linear system (mat - eigenvalue2 * I) * E = 0 reduces to
171 // the 2x2 linear system M * X = 0.
172 //
173 // When eigenvalue2 != eigenvalue3, M has rank 1 and is not the zero matrix.
174 // Otherwise, it has rank 0, and it is the zero matrix.
175
176 let au = mat * u;
177 let av = mat * v;
178
179 let mut m00 = u.dot(au) - eigenvalue2;
180 let mut m01 = u.dot(av);
181 let mut m11 = v.dot(av) - eigenvalue2;
182 let (abs_m00, abs_m01, abs_m11) = (ops::abs(m00), ops::abs(m01), ops::abs(m11));
183
184 if abs_m00 >= abs_m11 {
185 let max_abs_component = abs_m00.max(abs_m01);
186 if max_abs_component > 0.0 {
187 if abs_m00 >= abs_m01 {
188 // m00 is the largest component of the row.
189 // Factor it out for normalization and discard to avoid underflow or overflow.
190 m01 /= m00;
191 m00 = 1.0 / ops::sqrt(1.0 + m01 * m01);
192 m01 *= m00;
193 } else {
194 // m01 is the largest component of the row.
195 // Factor it out for normalization and discard to avoid underflow or overflow.
196 m00 /= m01;
197 m01 = 1.0 / ops::sqrt(1.0 + m00 * m00);
198 m00 *= m01;
199 }
200 return m01 * u - m00 * v;
201 }
202 } else {
203 let max_abs_component = abs_m11.max(abs_m01);
204 if max_abs_component > 0.0 {
205 if abs_m11 >= abs_m01 {
206 // m11 is the largest component of the row.
207 // Factor it out for normalization and discard to avoid underflow or overflow.
208 m01 /= m11;
209 m11 = 1.0 / ops::sqrt(1.0 + m01 * m01);
210 m01 *= m11;
211 } else {
212 // m01 is the largest component of the row.
213 // Factor it out for normalization and discard to avoid underflow or overflow.
214 m11 /= m01;
215 m01 = 1.0 / ops::sqrt(1.0 + m11 * m11);
216 m11 *= m01;
217 }
218 return m11 * u - m01 * v;
219 }
220 }
221
222 // M is the zero matrix, any unit-length solution suffices.
223 u
224 }
225
226 /// Computes the third eigenvector as the cross product of the first two.
227 /// If the given eigenvectors are valid, the returned vector should be unit length.
228 pub fn eigenvector3(eigenvector1: Vec3, eigenvector2: Vec3) -> Vec3 {
229 eigenvector1.cross(eigenvector2)
230 }
231}
232
233#[cfg(test)]
234mod test {
235 use super::SymmetricEigen3;
236 use crate::SymmetricMat3;
237 use approx::assert_relative_eq;
238 use glam::{Mat3, Vec3};
239 use rand::{Rng, SeedableRng};
240
241 #[test]
242 fn eigen_3x3() {
243 let mat = SymmetricMat3::new(2.0, 7.0, 8.0, 6.0, 3.0, 0.0);
244 let eigen = SymmetricEigen3::new(mat);
245
246 assert_relative_eq!(
247 eigen.eigenvalues,
248 Vec3::new(-7.605, 0.577, 15.028),
249 epsilon = 0.001
250 );
251 assert_relative_eq!(
252 Mat3::from_cols(
253 eigen.eigenvectors.x_axis.abs(),
254 eigen.eigenvectors.y_axis.abs(),
255 eigen.eigenvectors.z_axis.abs()
256 ),
257 Mat3::from_cols(
258 Vec3::new(-1.075, 0.333, 1.0).normalize().abs(),
259 Vec3::new(0.542, -1.253, 1.0).normalize().abs(),
260 Vec3::new(1.359, 1.386, 1.0).normalize().abs()
261 ),
262 epsilon = 0.001
263 );
264 }
265
266 #[test]
267 fn eigen_3x3_diagonal() {
268 let mat = SymmetricMat3::from_diagonal(Vec3::new(2.0, 5.0, 3.0));
269 let eigen = SymmetricEigen3::new(mat);
270
271 assert_eq!(eigen.eigenvalues, Vec3::new(2.0, 3.0, 5.0));
272 assert_eq!(
273 Mat3::from_cols(
274 eigen.eigenvectors.x_axis.normalize().abs(),
275 eigen.eigenvectors.y_axis.normalize().abs(),
276 eigen.eigenvectors.z_axis.normalize().abs()
277 ),
278 Mat3::from_cols_array_2d(&[[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]])
279 );
280 }
281
282 #[test]
283 fn eigen_3x3_reconstruction() {
284 let mut rng = rand_chacha::ChaCha8Rng::from_seed(Default::default());
285
286 // Generate random symmetric matrices and verify that the eigen decomposition is correct.
287 for _ in 0..10_000 {
288 let eigenvalues = Vec3::new(
289 rng.random_range(0.1..100.0),
290 rng.random_range(0.1..100.0),
291 rng.random_range(0.1..100.0),
292 );
293 let eigenvectors = Mat3::from_cols(
294 Vec3::new(
295 rng.random_range(-1.0..1.0),
296 rng.random_range(-1.0..1.0),
297 rng.random_range(-1.0..1.0),
298 )
299 .normalize(),
300 Vec3::new(
301 rng.random_range(-1.0..1.0),
302 rng.random_range(-1.0..1.0),
303 rng.random_range(-1.0..1.0),
304 )
305 .normalize(),
306 Vec3::new(
307 rng.random_range(-1.0..1.0),
308 rng.random_range(-1.0..1.0),
309 rng.random_range(-1.0..1.0),
310 )
311 .normalize(),
312 );
313
314 // Construct the symmetric matrix from the eigenvalues and eigenvectors.
315 let mat1 = eigenvectors * Mat3::from_diagonal(eigenvalues) * eigenvectors.transpose();
316
317 // Compute the eigen decomposition of the constructed matrix.
318 let eigen = SymmetricEigen3::new(SymmetricMat3::from_mat3_unchecked(mat1));
319
320 // Reconstruct the matrix from the computed eigenvalues and eigenvectors.
321 let mat2 = eigen.eigenvectors
322 * Mat3::from_diagonal(eigen.eigenvalues)
323 * eigen.eigenvectors.transpose();
324
325 // The reconstructed matrix should be close to the original matrix.
326 // Note: The precision depends on how large the eigenvalues are.
327 // Larger eigenvalues can lead to larger absolute error.
328 assert_relative_eq!(mat1, mat2, epsilon = 1e-2);
329 }
330 }
331
332 #[test]
333 fn eigen_pathological() {
334 // The algorithm sometimes produces NaN eigenvalues and eigenvectors for matrices
335 // that are already nearly diagonal. There is a diagonality check that should avoid this.
336 let mat = SymmetricMat3 {
337 m00: 5.3333335,
338 m01: 3.4465857e-20,
339 m02: 0.0,
340 m11: 5.3333335,
341 m12: 0.0,
342 m22: 5.3333335,
343 };
344 let eigen = SymmetricEigen3::new(mat);
345 assert_relative_eq!(eigen.eigenvalues, Vec3::splat(5.3333335), epsilon = 1e-6);
346 assert_relative_eq!(
347 eigen.eigenvectors.x_axis.abs(),
348 Vec3::new(1.0, 0.0, 0.0),
349 epsilon = 1e-6
350 );
351 assert_relative_eq!(
352 eigen.eigenvectors.y_axis.abs(),
353 Vec3::new(0.0, 1.0, 0.0),
354 epsilon = 1e-6
355 );
356 assert_relative_eq!(
357 eigen.eigenvectors.z_axis.abs(),
358 Vec3::new(0.0, 0.0, 1.0),
359 epsilon = 1e-6
360 );
361 }
362}