Mathter
A configurable 3D math library for game developers.
DecomposeSVD.hpp
Go to the documentation of this file.
1 //==============================================================================
2 // This software is distributed under The Unlicense.
3 // For more information, please refer to <http://unlicense.org/>
4 //==============================================================================
5 
6 #pragma once
7 
8 #include "DecomposeQR.hpp"
9 
10 #include <algorithm>
11 
12 
13 namespace mathter {
14 
15 
18 template <class T, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
20  template <int Rows_, int Columns_>
22 
23  static constexpr int Sdim = std::min(Rows, Columns);
24  static constexpr int Udim = Rows;
25  static constexpr int Vdim = Columns;
26 
27 public:
29 
33 };
34 
35 
36 namespace impl {
37  template <class T, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
38  void Rq2x2Helper(const Matrix<T, 2, 2, Order, Layout, Packed>& A, T& x, T& y, T& z, T& c2, T& s2) {
39  T a = A(0, 0);
40  T b = A(0, 1);
41  T c = A(1, 0);
42  T d = A(1, 1);
43 
44  if (c == 0) {
45  x = a;
46  y = b;
47  z = d;
48  c2 = 1;
49  s2 = 0;
50  return;
51  }
52  T maxden = std::max(abs(c), abs(d));
53 
54  T rcmaxden = 1 / maxden;
55  c *= rcmaxden;
56  d *= rcmaxden;
57 
58  T den = 1 / sqrt(c * c + d * d);
59 
60  T numx = (-b * c + a * d);
61  T numy = (a * c + b * d);
62  x = numx * den;
63  y = numy * den;
64  z = maxden / den;
65 
66  s2 = -c * den;
67  c2 = d * den;
68  }
69 
70 
71  template <class T, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
72  void Svd2x2Helper(const Matrix<T, 2, 2, Order, Layout, Packed>& A, T& c1, T& s1, T& c2, T& s2, T& d1, T& d2) {
73  // Calculate RQ decomposition of A
74  T x, y, z;
75  Rq2x2Helper(A, x, y, z, c2, s2);
76 
77  // Calculate tangent of rotation on R[x,y;0,z] to diagonalize R^T*R
78  T scaler = T(1) / std::max(abs(x), std::max(abs(y), std::numeric_limits<T>::min()));
79  T x_ = x * scaler, y_ = y * scaler, z_ = z * scaler;
80  T numer = ((z_ - x_) * (z_ + x_)) + y_ * y_;
81  T gamma = x_ * y_;
82  numer = numer == 0 ? std::numeric_limits<T>::infinity() : numer;
83  T zeta = numer / gamma;
84 
85  T t = 2 * sign_nonzero(zeta) / (abs(zeta) + sqrt(zeta * zeta + 4));
86 
87  // Calculate sines and cosines
88  c1 = T(1) / sqrt(T(1) + t * t);
89  s1 = c1 * t;
90 
91  // Calculate U*S = R*R(c1,s1)
92  T usa = c1 * x - s1 * y;
93  T usb = s1 * x + c1 * y;
94  T usc = -s1 * z;
95  T usd = c1 * z;
96 
97  // Update V = R(c1,s1)^T*Q
98  t = c1 * c2 + s1 * s2;
99  s2 = c2 * s1 - c1 * s2;
100  c2 = t;
101 
102  // Separate U and S
103  d1 = std::hypot(usa, usc);
104  d2 = std::hypot(usb, usd);
105  T dmax = std::max(d1, d2);
106  T usmax1 = d2 > d1 ? usd : usa;
107  T usmax2 = d2 > d1 ? usb : -usc;
108 
109  T signd1 = sign_nonzero(x * z);
110  dmax *= d2 > d1 ? signd1 : 1;
111  d2 *= signd1;
112  T rcpdmax = 1 / dmax;
113 
114  c1 = dmax != T(0) ? usmax1 * rcpdmax : T(1);
115  s1 = dmax != T(0) ? usmax2 * rcpdmax : T(0);
116  }
117 
118 
119  template <class T, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
124 
125  // Precondition with QR if needed
126  if (Rows > Columns) {
127  auto [Q, R] = DecomposeQR(m);
128  B = R;
129  U = Q.template Submatrix<Rows, Columns>(0, 0);
130  V = Identity();
131  }
132  else {
133  B = m;
134  U = Identity();
135  V = Identity();
136  }
137 
138  T tolerance = T(1e-6);
139 
140  T N = NormSquared(B);
141  T s;
142 
143  do {
144  s = 0;
145 
146  for (int i = 0; i < m.ColumnCount(); ++i) {
147  for (int j = i + 1; j < m.ColumnCount(); ++j) {
148  s += B(i, j) * B(i, j) + B(j, i) * B(j, i);
149 
150  T s1, c1, s2, c2, d1, d2; // SVD of the submat row,col i,j of B
152  B(i, i), B(i, j),
153  B(j, i), B(j, j)
154  };
155  Svd2x2Helper(Bsub, c1, s1, c2, s2, d1, d2);
156 
157  // Apply givens rotations given by 2x2 SVD to working matrices
158  // B = R(c1,s1)*B*R(c2,-s2)
159  Vector<T, 4, false> givensCoeffs = { c1, -s1, s1, c1 };
160  Vector<T, 4, false> bElems;
161  for (int col = 0; col < B.ColumnCount(); ++col) {
162  bElems.Set(B(i, col), B(j, col), B(i, col), B(j, col));
163  bElems *= givensCoeffs;
164  B(i, col) = bElems(0) + bElems(1);
165  B(j, col) = bElems(2) + bElems(3);
166  }
167  auto coli = B.stripes[i];
168  B.stripes[i] = c2 * coli + -s2 * B.stripes[j];
169  B.stripes[j] = s2 * coli + c2 * B.stripes[j];
170 
171  // U = U*R(c1,s1);
172  coli = U.stripes[i];
173  U.stripes[i] = c1 * coli + -s1 * U.stripes[j];
174  U.stripes[j] = s1 * coli + c1 * U.stripes[j];
175 
176  // V = V*R(c2,s2);
177  auto coliv = V.stripes[i];
178  V.stripes[i] = c2 * coliv + -s2 * V.stripes[j];
179  V.stripes[j] = s2 * coliv + c2 * V.stripes[j];
180  }
181  }
182  } while (s > tolerance * N);
183 
185  Sout = Zero();
186  for (int i = 0; i < B.ColumnCount(); ++i) {
187  Sout(i, i) = B(i, i);
188  }
189 
191  }
192 
193  template <class T, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
195  auto [U, S, V] = DecomposeSVD(Transpose(m), std::true_type{});
197  }
198 
199 
200 } // namespace impl
201 
202 
206 template <class T, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
208  return impl::DecomposeSVD(m, std::integral_constant<bool, (Rows >= Columns)>());
209 }
210 
211 
212 
213 } // namespace mathter
auto Identity()
Creates an identity matrix or identity quaternion.
Definition: IdentityBuilder.hpp:42
auto DecomposeQR(Matrix< T, Rows, Columns, Order, Layout, Packed > m)
Calculates the QR decomposition of the matrix using Householder transforms.
Definition: DecomposeQR.hpp:34
Matrix< T, Columns, Rows, Order, Layout, Packed > Transpose(const Matrix< T, Rows, Columns, Order, Layout, Packed > &m)
Transposes the matrix in-place.
Definition: MatrixFunction.hpp:34
MatrixT< Sdim, Sdim > S
Definition: DecomposeSVD.hpp:31
Represents a vector in N-dimensional space.
Definition: Definitions.hpp:57
auto DecomposeSVD(Matrix< T, Rows, Columns, Order, Layout, Packed > m, std::true_type)
Definition: DecomposeSVD.hpp:120
T sign_nonzero(T arg)
Definition: MathUtil.hpp:11
Vector & Set(Scalars... scalars)
Sets the vector&#39;s elements to the given scalars.
Definition: VectorImpl.hpp:476
T NormSquared(const Matrix< T, Rows, Columns, Order, Layout, Packed > &m)
Calculates the square of the Frobenius norm of the matrix.
Definition: MatrixFunction.hpp:67
std::array< Vector< T, StripeDim, Packed >, StripeCount > stripes
Definition: MatrixImpl.hpp:46
A utility class that can do common operations with the singular value decomposition, i.e. solving equation systems.
Definition: DecomposeSVD.hpp:19
Definition: Approx.hpp:11
auto DecomposeSVD(Matrix< T, Rows, Columns, Order, Layout, Packed > m)
Calculates the thin SVD of the matrix.
Definition: DecomposeSVD.hpp:207
MatrixT< Sdim, Vdim > V
Definition: DecomposeSVD.hpp:32
constexpr int ColumnCount() const
Returns the number of columns of the matrix.
Definition: MatrixImpl.hpp:27
Definition: Definitions.hpp:63
DecompositionSVD(MatrixT< Udim, Sdim > U, MatrixT< Sdim, Sdim > S, MatrixT< Sdim, Vdim > V)
Definition: DecomposeSVD.hpp:28
MatrixT< Udim, Sdim > U
Definition: DecomposeSVD.hpp:30
void Rq2x2Helper(const Matrix< T, 2, 2, Order, Layout, Packed > &A, T &x, T &y, T &z, T &c2, T &s2)
Definition: DecomposeSVD.hpp:38
void Svd2x2Helper(const Matrix< T, 2, 2, Order, Layout, Packed > &A, T &c1, T &s1, T &c2, T &s2, T &d1, T &d2)
Definition: DecomposeSVD.hpp:72
auto Zero()
Creates a matrix with all elements zero.
Definition: ZeroBuilder.hpp:34