1#[cfg(feature = "serde-serialize-no-std")]
2use serde::{Deserialize, Serialize};
3
4use approx::AbsDiffEq;
5use num::Zero;
6
7use crate::allocator::Allocator;
8use crate::base::{DefaultAllocator, Matrix2, OMatrix, OVector, SquareMatrix, Vector2};
9use crate::dimension::{Dim, DimDiff, DimSub, U1};
10use crate::storage::Storage;
11use simba::scalar::ComplexField;
12
13use crate::linalg::SymmetricTridiagonal;
14use crate::linalg::givens::GivensRotation;
15
16#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
18#[cfg_attr(
19 feature = "serde-serialize-no-std",
20 serde(bound(serialize = "DefaultAllocator: Allocator<D, D> +
21 Allocator<D>,
22 OVector<T::RealField, D>: Serialize,
23 OMatrix<T, D, D>: Serialize"))
24)]
25#[cfg_attr(
26 feature = "serde-serialize-no-std",
27 serde(bound(deserialize = "DefaultAllocator: Allocator<D, D> +
28 Allocator<D>,
29 OVector<T::RealField, D>: Deserialize<'de>,
30 OMatrix<T, D, D>: Deserialize<'de>"))
31)]
32#[cfg_attr(feature = "defmt", derive(defmt::Format))]
33#[derive(Clone, Debug)]
34pub struct SymmetricEigen<T: ComplexField, D: Dim>
35where
36 DefaultAllocator: Allocator<D, D> + Allocator<D>,
37{
38 pub eigenvectors: OMatrix<T, D, D>,
40
41 pub eigenvalues: OVector<T::RealField, D>,
43}
44
45impl<T: ComplexField, D: Dim> Copy for SymmetricEigen<T, D>
46where
47 DefaultAllocator: Allocator<D, D> + Allocator<D>,
48 OMatrix<T, D, D>: Copy,
49 OVector<T::RealField, D>: Copy,
50{
51}
52
53impl<T: ComplexField, D: Dim> SymmetricEigen<T, D>
54where
55 DefaultAllocator: Allocator<D, D> + Allocator<D>,
56{
57 pub fn new(m: OMatrix<T, D, D>) -> Self
61 where
62 D: DimSub<U1>,
63 DefaultAllocator: Allocator<DimDiff<D, U1>> + Allocator<DimDiff<D, U1>>,
64 {
65 Self::try_new(m, T::RealField::default_epsilon(), 0).unwrap()
66 }
67
68 pub fn try_new(m: OMatrix<T, D, D>, eps: T::RealField, max_niter: usize) -> Option<Self>
80 where
81 D: DimSub<U1>,
82 DefaultAllocator: Allocator<DimDiff<D, U1>> + Allocator<DimDiff<D, U1>>,
83 {
84 Self::do_decompose(m, true, eps, max_niter).map(|(vals, vecs)| SymmetricEigen {
85 eigenvectors: vecs.unwrap(),
86 eigenvalues: vals,
87 })
88 }
89
90 fn do_decompose(
91 mut matrix: OMatrix<T, D, D>,
92 eigenvectors: bool,
93 eps: T::RealField,
94 max_niter: usize,
95 ) -> Option<(OVector<T::RealField, D>, Option<OMatrix<T, D, D>>)>
96 where
97 D: DimSub<U1>,
98 DefaultAllocator: Allocator<DimDiff<D, U1>> + Allocator<DimDiff<D, U1>>,
99 {
100 assert!(
101 matrix.is_square(),
102 "Unable to compute the eigendecomposition of a non-square matrix."
103 );
104 let dim = matrix.nrows();
105 let m_amax = matrix.camax();
106
107 if !m_amax.is_zero() {
108 matrix.unscale_mut(m_amax.clone());
109 }
110
111 let (mut q_mat, mut diag, mut off_diag);
112
113 if eigenvectors {
114 let res = SymmetricTridiagonal::new(matrix).unpack();
115 q_mat = Some(res.0);
116 diag = res.1;
117 off_diag = res.2;
118 } else {
119 let res = SymmetricTridiagonal::new(matrix).unpack_tridiagonal();
120 q_mat = None;
121 diag = res.0;
122 off_diag = res.1;
123 }
124
125 if dim == 1 {
126 diag.scale_mut(m_amax);
127 return Some((diag, q_mat));
128 }
129
130 let mut niter = 0;
131 let (mut start, mut end) =
132 Self::delimit_subproblem(&diag, &mut off_diag, dim - 1, eps.clone());
133
134 while end != start {
135 let subdim = end - start + 1;
136
137 #[allow(clippy::comparison_chain)]
138 if subdim > 2 {
139 let m = end - 1;
140 let n = end;
141
142 let mut vec = Vector2::new(
143 diag[start].clone()
144 - wilkinson_shift(
145 diag[m].clone().clone(),
146 diag[n].clone(),
147 off_diag[m].clone().clone(),
148 ),
149 off_diag[start].clone(),
150 );
151
152 for i in start..n {
153 let j = i + 1;
154
155 match GivensRotation::cancel_y(&vec) {
156 Some((rot, norm)) => {
157 if i > start {
158 off_diag[i - 1] = norm;
160 }
161
162 let mii = diag[i].clone();
163 let mjj = diag[j].clone();
164 let mij = off_diag[i].clone();
165
166 let cc = rot.c() * rot.c();
167 let ss = rot.s() * rot.s();
168 let cs = rot.c() * rot.s();
169
170 let b = cs.clone() * crate::convert(2.0) * mij.clone();
171
172 diag[i] =
173 (cc.clone() * mii.clone() + ss.clone() * mjj.clone()) - b.clone();
174 diag[j] = (ss.clone() * mii.clone() + cc.clone() * mjj.clone()) + b;
175 off_diag[i] = cs * (mii - mjj) + mij * (cc - ss);
176
177 if i != n - 1 {
178 vec.x = off_diag[i].clone();
179 vec.y = -rot.s() * off_diag[i + 1].clone();
180 off_diag[i + 1] *= rot.c();
181 }
182
183 if let Some(ref mut q) = q_mat {
184 let rot =
185 GivensRotation::new_unchecked(rot.c(), T::from_real(rot.s()));
186 rot.inverse().rotate_rows(&mut q.fixed_columns_mut::<2>(i));
187 }
188 }
189 None => {
190 break;
191 }
192 }
193 }
194
195 if off_diag[m].clone().norm1()
196 <= eps.clone() * (diag[m].clone().norm1() + diag[n].clone().norm1())
197 {
198 end -= 1;
199 }
200 } else if subdim == 2 {
201 let m = Matrix2::new(
202 diag[start].clone(),
203 off_diag[start].clone().conjugate(),
204 off_diag[start].clone(),
205 diag[start + 1].clone(),
206 );
207 let eigvals = m.eigenvalues().unwrap();
208 let basis = Vector2::new(
209 eigvals.x.clone() - diag[start + 1].clone(),
210 off_diag[start].clone(),
211 );
212
213 diag[start] = eigvals[0].clone();
214 diag[start + 1] = eigvals[1].clone();
215
216 if let Some(ref mut q) = q_mat {
217 if let Some((rot, _)) =
218 GivensRotation::try_new(basis.x.clone(), basis.y.clone(), eps.clone())
219 {
220 let rot = GivensRotation::new_unchecked(rot.c(), T::from_real(rot.s()));
221 rot.rotate_rows(&mut q.fixed_columns_mut::<2>(start));
222 }
223 }
224
225 end -= 1;
226 }
227
228 let sub = Self::delimit_subproblem(&diag, &mut off_diag, end, eps.clone());
230
231 start = sub.0;
232 end = sub.1;
233
234 niter += 1;
235 if niter == max_niter {
236 return None;
237 }
238 }
239
240 diag.scale_mut(m_amax);
241
242 Some((diag, q_mat))
243 }
244
245 fn delimit_subproblem(
246 diag: &OVector<T::RealField, D>,
247 off_diag: &mut OVector<T::RealField, DimDiff<D, U1>>,
248 end: usize,
249 eps: T::RealField,
250 ) -> (usize, usize)
251 where
252 D: DimSub<U1>,
253 DefaultAllocator: Allocator<DimDiff<D, U1>>,
254 {
255 let mut n = end;
256
257 while n > 0 {
258 let m = n - 1;
259
260 if off_diag[m].clone().norm1()
261 > eps.clone() * (diag[n].clone().norm1() + diag[m].clone().norm1())
262 {
263 break;
264 }
265
266 n -= 1;
267 }
268
269 if n == 0 {
270 return (0, 0);
271 }
272
273 let mut new_start = n - 1;
274 while new_start > 0 {
275 let m = new_start - 1;
276
277 if off_diag[m].clone().is_zero()
278 || off_diag[m].clone().norm1()
279 <= eps.clone() * (diag[new_start].clone().norm1() + diag[m].clone().norm1())
280 {
281 off_diag[m] = T::RealField::zero();
282 break;
283 }
284
285 new_start -= 1;
286 }
287
288 (new_start, n)
289 }
290
291 #[must_use]
295 pub fn recompose(&self) -> OMatrix<T, D, D> {
296 let mut u_t = self.eigenvectors.clone();
297 for i in 0..self.eigenvalues.len() {
298 let val = self.eigenvalues[i].clone();
299 u_t.column_mut(i).scale_mut(val);
300 }
301 u_t.adjoint_mut();
302 &self.eigenvectors * u_t
303 }
304}
305
306pub fn wilkinson_shift<T: ComplexField>(tmm: T, tnn: T, tmn: T) -> T {
313 let sq_tmn = tmn.clone() * tmn;
314 if !sq_tmn.is_zero() {
315 let d = (tmm - tnn.clone()) * crate::convert(0.5);
317 tnn - sq_tmn.clone() / (d.clone() + d.clone().signum() * (d.clone() * d + sq_tmn).sqrt())
318 } else {
319 tnn
320 }
321}
322
323impl<T: ComplexField, D: DimSub<U1>, S: Storage<T, D, D>> SquareMatrix<T, D, S>
329where
330 DefaultAllocator:
331 Allocator<D, D> + Allocator<DimDiff<D, U1>> + Allocator<D> + Allocator<DimDiff<D, U1>>,
332{
333 #[must_use]
337 pub fn symmetric_eigenvalues(&self) -> OVector<T::RealField, D> {
338 SymmetricEigen::do_decompose(
339 self.clone_owned(),
340 false,
341 T::RealField::default_epsilon(),
342 0,
343 )
344 .unwrap()
345 .0
346 }
347}
348
349#[cfg(test)]
350mod test {
351 use crate::base::Matrix2;
352
353 fn expected_shift(m: Matrix2<f64>) -> f64 {
354 let vals = m.eigenvalues().unwrap();
355
356 if (vals.x - m.m22).abs() < (vals.y - m.m22).abs() {
357 vals.x
358 } else {
359 vals.y
360 }
361 }
362
363 #[cfg(feature = "rand")]
364 #[test]
365 fn wilkinson_shift_random() {
366 for _ in 0..1000 {
367 let m = Matrix2::<f64>::new_random();
368 let m = m * m.transpose();
369
370 let expected = expected_shift(m);
371 let computed = super::wilkinson_shift(m.m11, m.m22, m.m12);
372 assert!(relative_eq!(expected, computed, epsilon = 1.0e-7));
373 }
374 }
375
376 #[test]
377 fn wilkinson_shift_zero() {
378 let m = Matrix2::new(0.0, 0.0, 0.0, 0.0);
379 assert!(relative_eq!(
380 expected_shift(m),
381 super::wilkinson_shift(m.m11, m.m22, m.m12)
382 ));
383 }
384
385 #[test]
386 fn wilkinson_shift_zero_diagonal() {
387 let m = Matrix2::new(0.0, 42.0, 42.0, 0.0);
388 assert!(relative_eq!(
389 expected_shift(m),
390 super::wilkinson_shift(m.m11, m.m22, m.m12)
391 ));
392 }
393
394 #[test]
395 fn wilkinson_shift_zero_off_diagonal() {
396 let m = Matrix2::new(42.0, 0.0, 0.0, 64.0);
397 assert!(relative_eq!(
398 expected_shift(m),
399 super::wilkinson_shift(m.m11, m.m22, m.m12)
400 ));
401 }
402
403 #[test]
404 fn wilkinson_shift_zero_trace() {
405 let m = Matrix2::new(42.0, 20.0, 20.0, -42.0);
406 assert!(relative_eq!(
407 expected_shift(m),
408 super::wilkinson_shift(m.m11, m.m22, m.m12)
409 ));
410 }
411
412 #[test]
413 fn wilkinson_shift_zero_diag_diff_and_zero_off_diagonal() {
414 let m = Matrix2::new(42.0, 0.0, 0.0, 42.0);
415 assert!(relative_eq!(
416 expected_shift(m),
417 super::wilkinson_shift(m.m11, m.m22, m.m12)
418 ));
419 }
420
421 #[test]
422 fn wilkinson_shift_zero_det() {
423 let m = Matrix2::new(2.0, 4.0, 4.0, 8.0);
424 assert!(relative_eq!(
425 expected_shift(m),
426 super::wilkinson_shift(m.m11, m.m22, m.m12)
427 ));
428 }
429}