SVD-Impl.hpp
Go to the documentation of this file.
1 // This code is based on Jet framework.
2 // Copyright (c) 2018 Doyub Kim
3 // CubbyFlow is voxel-based fluid simulation engine for computer games.
4 // Copyright (c) 2020 CubbyFlow Team
5 // Core Part: Chris Ohk, Junwoo Hwang, Jihong Sin, Seungwoo Yoo
6 // AI Part: Dongheon Cho, Minseo Kim
7 // We are making my contributions/submissions to this project solely in our
8 // personal capacity and are not conveying any rights to any intellectual
9 // property of any third parties.
10 
11 #ifndef CUBBYFLOW_SVD_IMPL_HPP
12 #define CUBBYFLOW_SVD_IMPL_HPP
13 
14 #include <stdexcept>
15 
16 namespace CubbyFlow
17 {
18 namespace Internal
19 {
20 template <typename T>
21 T Sign(T a, T b)
22 {
23  return static_cast<double>(b) >= 0.0 ? std::fabs(a) : -std::fabs(a);
24 }
25 
26 template <typename T>
27 T Pythag(T a, T b)
28 {
29  T at = std::fabs(a);
30  T bt = std::fabs(b);
31  T ct;
32  T result;
33 
34  if (at > bt)
35  {
36  ct = bt / at;
37  result = at * std::sqrt(1 + ct * ct);
38  }
39  else if (bt > 0)
40  {
41  ct = at / bt;
42  result = bt * std::sqrt(1 + ct * ct);
43  }
44  else
45  {
46  result = 0;
47  }
48 
49  return result;
50 }
51 } // namespace Internal
52 
53 template <typename T>
54 void SVD(const MatrixMxN<T>& a, MatrixMxN<T>& u, VectorN<T>& w, MatrixMxN<T>& v)
55 {
56  const int m = static_cast<int>(a.GetRows());
57  const int n = static_cast<int>(a.GetCols());
58 
59  int i, j = 0, jj = 0, k = 0, l = 0, nm = 0;
60  T c = 0, f = 0, h = 0, s = 0, x = 0, y = 0, z = 0;
61  T anorm = 0, g = 0, scale = 0;
62 
63  if (m < n)
64  {
65  throw std::invalid_argument{
66  "Number of rows of input matrix must greater than or equal to "
67  "columns."
68  };
69  }
70 
71  // Prepare workspace
72  VectorN<T> rv1(n, T{});
73  u = a;
74  w.Resize(n, 0);
75  v.Resize(n, n, 0);
76 
77  // Householder reduction to bi-diagonal form
78  for (i = 0; i < n; i++)
79  {
80  // left-hand reduction
81  l = i + 1;
82  rv1[i] = scale * g;
83  g = s = scale = 0;
84 
85  if (i < m)
86  {
87  for (k = i; k < m; k++)
88  {
89  scale += std::fabs(u(k, i));
90  }
91 
92  if (std::fabs(static_cast<double>(scale)) >=
93  std::numeric_limits<double>::epsilon())
94  {
95  for (k = i; k < m; k++)
96  {
97  u(k, i) /= scale;
98  s += u(k, i) * u(k, i);
99  }
100 
101  f = u(i, i);
102  g = -Internal::Sign(std::sqrt(s), f);
103  h = f * g - s;
104  u(i, i) = f - g;
105 
106  if (i != n - 1)
107  {
108  for (j = l; j < n; j++)
109  {
110  s = 0;
111 
112  for (k = i; k < m; k++)
113  {
114  s += u(k, i) * u(k, j);
115  }
116 
117  f = s / h;
118 
119  for (k = i; k < m; k++)
120  {
121  u(k, j) += f * u(k, i);
122  }
123  }
124  }
125 
126  for (k = i; k < m; k++)
127  {
128  u(k, i) *= scale;
129  }
130  }
131  }
132 
133  w[i] = scale * g;
134 
135  // right-hand reduction
136  g = s = scale = 0;
137 
138  if (i < m && i != n - 1)
139  {
140  for (k = l; k < n; k++)
141  {
142  scale += std::fabs(u(i, k));
143  }
144 
145  if (std::fabs(static_cast<double>(scale)) >=
146  std::numeric_limits<double>::epsilon())
147  {
148  for (k = l; k < n; k++)
149  {
150  u(i, k) /= scale;
151  s += u(i, k) * u(i, k);
152  }
153 
154  f = u(i, l);
155  g = -Internal::Sign(std::sqrt(s), f);
156  h = f * g - s;
157  u(i, l) = f - g;
158 
159  for (k = l; k < n; k++)
160  {
161  rv1[k] = static_cast<T>(u(i, k)) / h;
162  }
163 
164  if (i != m - 1)
165  {
166  for (j = l; j < m; j++)
167  {
168  s = 0;
169 
170  for (k = l; k < n; k++)
171  {
172  s += u(j, k) * u(i, k);
173  }
174 
175  for (k = l; k < n; k++)
176  {
177  u(j, k) += s * rv1[k];
178  }
179  }
180  }
181 
182  for (k = l; k < n; k++)
183  {
184  u(i, k) *= scale;
185  }
186  }
187  }
188 
189  anorm = std::max(anorm,
190  (std::fabs(static_cast<T>(w[i])) + std::fabs(rv1[i])));
191  }
192 
193  // accumulate the right-hand transformation
194  for (i = n - 1; i >= 0; i--)
195  {
196  if (i < n - 1)
197  {
198  if (std::fabs(static_cast<double>(g)) >=
199  std::numeric_limits<double>::epsilon())
200  {
201  for (j = l; j < n; j++)
202  {
203  v(j, i) = ((u(i, j) / u(i, l)) / g);
204  }
205 
206  // T division to avoid underflow
207  for (j = l; j < n; j++)
208  {
209  s = 0;
210 
211  for (k = l; k < n; k++)
212  {
213  s += u(i, k) * v(k, j);
214  }
215 
216  for (k = l; k < n; k++)
217  {
218  v(k, j) += s * v(k, i);
219  }
220  }
221  }
222 
223  for (j = l; j < n; j++)
224  {
225  v(i, j) = v(j, i) = 0;
226  }
227  }
228 
229  v(i, i) = 1;
230  g = rv1[i];
231  l = i;
232  }
233 
234  // accumulate the left-hand transformation
235  for (i = n - 1; i >= 0; i--)
236  {
237  l = i + 1;
238  g = w[i];
239 
240  if (i < n - 1)
241  {
242  for (j = l; j < n; j++)
243  {
244  u(i, j) = 0;
245  }
246  }
247 
248  if (std::fabs(static_cast<double>(g)) >=
249  std::numeric_limits<double>::epsilon())
250  {
251  g = 1 / g;
252 
253  if (i != n - 1)
254  {
255  for (j = l; j < n; j++)
256  {
257  s = 0;
258 
259  for (k = l; k < m; k++)
260  {
261  s += u(k, i) * u(k, j);
262  }
263 
264  f = (s / u(i, i)) * g;
265 
266  for (k = i; k < m; k++)
267  {
268  u(k, j) += f * u(k, i);
269  }
270  }
271  }
272 
273  for (j = i; j < m; j++)
274  {
275  u(j, i) = u(j, i) * g;
276  }
277  }
278  else
279  {
280  for (j = i; j < m; j++)
281  {
282  u(j, i) = 0;
283  }
284  }
285 
286  ++u(i, i);
287  }
288 
289  // diagonalize the bi-diagonal form
290  for (k = n - 1; k >= 0; k--)
291  {
292  // loop over singular values
293  for (int its = 0; its < 30; its++)
294  {
295  // loop over allowed iterations
296  int flag = 1;
297 
298  for (l = k; l >= 0; l--)
299  {
300  // test for splitting
301  nm = l - 1;
302 
303  if (std::fabs(static_cast<double>(rv1[l])) <=
304  std::numeric_limits<double>::epsilon())
305  {
306  flag = 0;
307  break;
308  }
309 
310  if (std::fabs(static_cast<double>(w[nm])) <=
311  std::numeric_limits<double>::epsilon())
312  {
313  break;
314  }
315  }
316 
317  if (flag)
318  {
319  c = 0;
320  s = 1;
321 
322  for (i = l; i <= k; i++)
323  {
324  f = s * rv1[i];
325 
326  if (std::fabs(static_cast<double>(f)) <=
327  std::numeric_limits<double>::epsilon())
328  {
329  g = w[i];
330  h = Internal::Pythag(f, g);
331  w[i] = static_cast<T>(h);
332  h = 1 / h;
333  c = g * h;
334  s = -f * h;
335 
336  for (j = 0; j < m; j++)
337  {
338  y = u(j, nm);
339  z = u(j, i);
340  u(j, nm) = y * c + z * s;
341  u(j, i) = z * c - y * s;
342  }
343  }
344  }
345  }
346 
347  z = w[k];
348 
349  if (l == k)
350  {
351  // convergence
352  if (z < 0)
353  {
354  // make singular value nonnegative
355  w[k] = -z;
356 
357  for (j = 0; j < n; j++)
358  {
359  v(j, k) = -v(j, k);
360  }
361  }
362 
363  break;
364  }
365 
366  if (its >= 30)
367  {
368  throw std::logic_error{ "No convergence after 30 iterations" };
369  }
370 
371  // shift from bottom 2 x 2 minor
372  x = w[l];
373  nm = k - 1;
374  y = w[nm];
375  g = rv1[nm];
376  h = rv1[k];
377  f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2 * h * y);
378  g = Internal::Pythag(f, static_cast<T>(1));
379  f = ((x - z) * (x + z) +
380  h * ((y / (f + Internal::Sign(g, f))) - h)) /
381  x;
382 
383  // next QR transformation
384  c = s = 1;
385 
386  for (j = l; j <= nm; j++)
387  {
388  i = j + 1;
389  g = rv1[i];
390  y = w[i];
391  h = s * g;
392  g = c * g;
393  z = Internal::Pythag(f, h);
394  rv1[j] = z;
395  c = f / z;
396  s = h / z;
397  f = x * c + g * s;
398  g = g * c - x * s;
399  h = y * s;
400  y = y * c;
401 
402  for (jj = 0; jj < n; jj++)
403  {
404  x = v(jj, j);
405  z = v(jj, i);
406  v(jj, j) = x * c + z * s;
407  v(jj, i) = z * c - x * s;
408  }
409 
410  z = Internal::Pythag(f, h);
411  w[j] = z;
412 
413  if (std::fabs(static_cast<double>(z)) >=
414  std::numeric_limits<double>::epsilon())
415  {
416  z = 1 / z;
417  c = f * z;
418  s = h * z;
419  }
420 
421  f = (c * g) + (s * y);
422  x = (c * y) - (s * g);
423 
424  for (jj = 0; jj < m; jj++)
425  {
426  y = u(jj, j);
427  z = u(jj, i);
428  u(jj, j) = y * c + z * s;
429  u(jj, i) = z * c - y * s;
430  }
431  }
432 
433  rv1[l] = 0;
434  rv1[k] = f;
435  w[k] = x;
436  }
437  }
438 }
439 
440 template <typename T, size_t M, size_t N>
441 void SVD(const Matrix<T, M, N>& a, Matrix<T, M, N>& u, Vector<T, N>& w,
442  Matrix<T, N, N>& v)
443 {
444  const int m = static_cast<int>(M);
445  const int n = static_cast<int>(N);
446 
447  int i, its, j = 0, jj = 0, k = 0, l = 0, nm = 0;
448  T c = 0, f = 0, h = 0, s = 0, x = 0, y = 0, z = 0;
449  T anorm = 0, g = 0, scale = 0;
450 
451  static_assert(m >= n,
452  "Number of rows of input matrix must greater than or equal "
453  "to columns.");
454 
455  // Prepare workspace
456  Vector<T, N> rv1;
457  u = a;
458  w = Vector<T, N>{};
459  v = Matrix<T, N, N>{};
460 
461  // Householder reduction to bi-diagonal form
462  for (i = 0; i < n; i++)
463  {
464  // left-hand reduction
465  l = i + 1;
466  rv1[i] = scale * g;
467  g = s = scale = 0;
468 
469  if (i < m)
470  {
471  for (k = i; k < m; k++)
472  {
473  scale += std::fabs(u(k, i));
474  }
475 
476  if (scale)
477  {
478  for (k = i; k < m; k++)
479  {
480  u(k, i) /= scale;
481  s += u(k, i) * u(k, i);
482  }
483 
484  f = u(i, i);
485  g = -Internal::Sign(std::sqrt(s), f);
486  h = f * g - s;
487  u(i, i) = f - g;
488 
489  if (i != n - 1)
490  {
491  for (j = l; j < n; j++)
492  {
493  s = 0;
494 
495  for (k = i; k < m; k++)
496  {
497  s += u(k, i) * u(k, j);
498  }
499 
500  f = s / h;
501 
502  for (k = i; k < m; k++)
503  {
504  u(k, j) += f * u(k, i);
505  }
506  }
507  }
508 
509  for (k = i; k < m; k++)
510  {
511  u(k, i) *= scale;
512  }
513  }
514  }
515 
516  w[i] = scale * g;
517 
518  // right-hand reduction
519  g = s = scale = 0;
520 
521  if (i < m && i != n - 1)
522  {
523  for (k = l; k < n; k++)
524  {
525  scale += std::fabs(u(i, k));
526  }
527 
528  if (scale)
529  {
530  for (k = l; k < n; k++)
531  {
532  u(i, k) /= scale;
533  s += u(i, k) * u(i, k);
534  }
535 
536  f = u(i, l);
537  g = -Internal::Sign(std::sqrt(s), f);
538  h = f * g - s;
539  u(i, l) = f - g;
540 
541  for (k = l; k < n; k++)
542  {
543  rv1[k] = static_cast<T>(u(i, k)) / h;
544  }
545 
546  if (i != m - 1)
547  {
548  for (j = l; j < m; j++)
549  {
550  s = 0;
551 
552  for (k = l; k < n; k++)
553  {
554  s += u(j, k) * u(i, k);
555  }
556 
557  for (k = l; k < n; k++)
558  {
559  u(j, k) += s * rv1[k];
560  }
561  }
562  }
563 
564  for (k = l; k < n; k++)
565  {
566  u(i, k) *= scale;
567  }
568  }
569  }
570  anorm = std::max(anorm,
571  (std::fabs(static_cast<T>(w[i])) + std::fabs(rv1[i])));
572  }
573 
574  // accumulate the right-hand transformation
575  for (i = n - 1; i >= 0; i--)
576  {
577  if (i < n - 1)
578  {
579  if (g)
580  {
581  for (j = l; j < n; j++)
582  {
583  v(j, i) = ((u(i, j) / u(i, l)) / g);
584  }
585 
586  // T division to avoid underflow
587  for (j = l; j < n; j++)
588  {
589  s = 0;
590 
591  for (k = l; k < n; k++)
592  {
593  s += u(i, k) * v(k, j);
594  }
595 
596  for (k = l; k < n; k++)
597  {
598  v(k, j) += s * v(k, i);
599  }
600  }
601  }
602 
603  for (j = l; j < n; j++)
604  {
605  v(i, j) = v(j, i) = 0;
606  }
607  }
608 
609  v(i, i) = 1;
610  g = rv1[i];
611  l = i;
612  }
613 
614  // accumulate the left-hand transformation
615  for (i = n - 1; i >= 0; i--)
616  {
617  l = i + 1;
618  g = w[i];
619 
620  if (i < n - 1)
621  {
622  for (j = l; j < n; j++)
623  {
624  u(i, j) = 0;
625  }
626  }
627 
628  if (g)
629  {
630  g = 1 / g;
631 
632  if (i != n - 1)
633  {
634  for (j = l; j < n; j++)
635  {
636  s = 0;
637 
638  for (k = l; k < m; k++)
639  {
640  s += u(k, i) * u(k, j);
641  }
642 
643  f = (s / u(i, i)) * g;
644 
645  for (k = i; k < m; k++)
646  {
647  u(k, j) += f * u(k, i);
648  }
649  }
650  }
651 
652  for (j = i; j < m; j++)
653  {
654  u(j, i) = u(j, i) * g;
655  }
656  }
657  else
658  {
659  for (j = i; j < m; j++)
660  {
661  u(j, i) = 0;
662  }
663  }
664 
665  ++u(i, i);
666  }
667 
668  // diagonalize the bi-diagonal form
669  for (k = n - 1; k >= 0; k--)
670  {
671  // loop over singular values
672  for (its = 0; its < 30; its++)
673  {
674  // loop over allowed iterations
675  int flag = 1;
676 
677  for (l = k; l >= 0; l--)
678  {
679  // test for splitting
680  nm = l - 1;
681 
682  if (std::fabs(rv1[l]) + anorm == anorm)
683  {
684  flag = 0;
685  break;
686  }
687 
688  if (std::fabs(static_cast<T>(w[nm])) + anorm == anorm)
689  {
690  break;
691  }
692  }
693 
694  if (flag)
695  {
696  c = 0;
697  s = 1;
698 
699  for (i = l; i <= k; i++)
700  {
701  f = s * rv1[i];
702 
703  if (std::fabs(f) + anorm != anorm)
704  {
705  g = w[i];
706  h = Internal::Pythag(f, g);
707  w[i] = static_cast<T>(h);
708  h = 1 / h;
709  c = g * h;
710  s = -f * h;
711 
712  for (j = 0; j < m; j++)
713  {
714  y = u(j, nm);
715  z = u(j, i);
716  u(j, nm) = y * c + z * s;
717  u(j, i) = z * c - y * s;
718  }
719  }
720  }
721  }
722 
723  z = w[k];
724 
725  if (l == k)
726  {
727  // convergence
728  if (z < 0)
729  {
730  // make singular value nonnegative
731  w[k] = -z;
732 
733  for (j = 0; j < n; j++)
734  {
735  v(j, k) = -v(j, k);
736  }
737  }
738 
739  break;
740  }
741 
742  if (its >= 30)
743  {
744  throw std::logic_error{ "No convergence after 30 iterations" };
745  }
746 
747  // shift from bottom 2 x 2 minor
748  x = w[l];
749  nm = k - 1;
750  y = w[nm];
751  g = rv1[nm];
752  h = rv1[k];
753  f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2 * h * y);
754  g = Internal::Pythag(f, static_cast<T>(1));
755  f = ((x - z) * (x + z) +
756  h * ((y / (f + Internal::Sign(g, f))) - h)) /
757  x;
758 
759  // next QR transformation
760  c = s = 1;
761 
762  for (j = l; j <= nm; j++)
763  {
764  i = j + 1;
765  g = rv1[i];
766  y = w[i];
767  h = s * g;
768  g = c * g;
769  z = Internal::Pythag(f, h);
770  rv1[j] = z;
771  c = f / z;
772  s = h / z;
773  f = x * c + g * s;
774  g = g * c - x * s;
775  h = y * s;
776  y = y * c;
777 
778  for (jj = 0; jj < n; jj++)
779  {
780  x = v(jj, j);
781  z = v(jj, i);
782  v(jj, j) = x * c + z * s;
783  v(jj, i) = z * c - x * s;
784  }
785 
786  z = Internal::Pythag(f, h);
787  w[j] = z;
788 
789  if (z)
790  {
791  z = 1 / z;
792  c = f * z;
793  s = h * z;
794  }
795 
796  f = (c * g) + (s * y);
797  x = (c * y) - (s * g);
798 
799  for (jj = 0; jj < m; jj++)
800  {
801  y = u(jj, j);
802  z = u(jj, i);
803  u(jj, j) = y * c + z * s;
804  u(jj, i) = z * c - y * s;
805  }
806  }
807 
808  rv1[l] = 0;
809  rv1[k] = f;
810  w[k] = x;
811  }
812  }
813 }
814 } // namespace CubbyFlow
815 
816 #endif
CubbyFlow::Internal::Sign
T Sign(T a, T b)
Definition: SVD-Impl.hpp:21
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, MATRIX_SIZE_DYNAMIC >::GetCols
size_t GetCols() const
Definition: Matrix-Impl.hpp:1069
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, MATRIX_SIZE_DYNAMIC >::Resize
void Resize(size_t rows, size_t cols, ConstReference val=ValueType{})
Definition: Matrix-Impl.hpp:1035
CubbyFlow::Matrix
Definition: Matrix.hpp:27
CubbyFlow
Definition: pybind11Utils.hpp:20
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, MATRIX_SIZE_DYNAMIC >
Definition: Matrix.hpp:531
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, 1 >
Definition: Matrix.hpp:611
CubbyFlow::Internal::Pythag
T Pythag(T a, T b)
Definition: SVD-Impl.hpp:27
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, 1 >::Resize
void Resize(size_t rows, ConstReference val=ValueType{})
Definition: Matrix-Impl.hpp:1245
CubbyFlow::SVD
void SVD(const MatrixMxN< T > &a, MatrixMxN< T > &u, VectorN< T > &w, MatrixMxN< T > &v)
Singular value decomposition (SVD).
Definition: SVD-Impl.hpp:54
CubbyFlow::Matrix< T, MATRIX_SIZE_DYNAMIC, MATRIX_SIZE_DYNAMIC >::GetRows
size_t GetRows() const
Definition: Matrix-Impl.hpp:1063
Generated on Wed Jul 27 2022 14:50:10 for CubbyFlow by   doxygen 1.8.13