1use crate::{Matrix3, SVD, U3};
2use simba::scalar::RealField;
3
4pub fn svd_ordered3<T: RealField>(
10 m: &Matrix3<T>,
11 compute_u: bool,
12 compute_v: bool,
13 eps: T,
14 niter: usize,
15) -> Option<SVD<T, U3, U3>> {
16 let s = m.tr_mul(m);
17 let mut v = s.try_symmetric_eigen(eps, niter)?.eigenvectors;
18 let mut b = m * &v;
19
20 let mut rho0 = b.column(0).norm_squared();
23 let mut rho1 = b.column(1).norm_squared();
24 let mut rho2 = b.column(2).norm_squared();
25
26 if rho0 < rho1 {
27 b.swap_columns(0, 1);
28 b.column_mut(1).neg_mut();
29 v.swap_columns(0, 1);
30 v.column_mut(1).neg_mut();
31 std::mem::swap(&mut rho0, &mut rho1);
32 }
33 if rho0 < rho2 {
34 b.swap_columns(0, 2);
35 b.column_mut(2).neg_mut();
36 v.swap_columns(0, 2);
37 v.column_mut(2).neg_mut();
38 std::mem::swap(&mut rho0, &mut rho2);
39 }
40 if rho1 < rho2 {
41 b.swap_columns(1, 2);
42 b.column_mut(2).neg_mut();
43 v.swap_columns(1, 2);
44 v.column_mut(2).neg_mut();
45 std::mem::swap(&mut rho0, &mut rho2);
46 }
47
48 let qr = b.qr();
49
50 Some(SVD {
51 u: if compute_u { Some(qr.q()) } else { None },
52 singular_values: qr.diag_internal().map(|e| e.abs()),
53 v_t: if compute_v { Some(v.transpose()) } else { None },
54 })
55}