Bullet Collision Detection & Physics Library
btImplicitQRSVD.h
Go to the documentation of this file.
1 
42 #ifndef btImplicitQRSVD_h
43 #define btImplicitQRSVD_h
44 
45 #include "btMatrix3x3.h"
47 {
48 public:
50  btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
51  {
52  }
53  btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
54  {}
55  btScalar& operator()(int i, int j)
56  {
57  if (i == 0 && j == 0)
58  return m_00;
59  if (i == 1 && j == 0)
60  return m_10;
61  if (i == 0 && j == 1)
62  return m_01;
63  if (i == 1 && j == 1)
64  return m_11;
65  btAssert(false);
66  return m_00;
67  }
68  const btScalar& operator()(int i, int j) const
69  {
70  if (i == 0 && j == 0)
71  return m_00;
72  if (i == 1 && j == 0)
73  return m_10;
74  if (i == 0 && j == 1)
75  return m_01;
76  if (i == 1 && j == 1)
77  return m_11;
78  btAssert(false);
79  return m_00;
80  }
81  void setIdentity()
82  {
83  m_00 = 1;
84  m_11 = 1;
85  m_01 = 0;
86  m_10 = 0;
87  }
88 };
89 
90 static inline btScalar copySign(btScalar x, btScalar y) {
91  if ((x < 0 && y > 0) || (x > 0 && y < 0))
92  return -x;
93  return x;
94 }
95 
115 public:
116  int rowi;
117  int rowk;
120 
121  inline GivensRotation(int rowi_in, int rowk_in)
122  : rowi(rowi_in)
123  , rowk(rowk_in)
124  , c(1)
125  , s(0)
126  {
127  }
128 
129  inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
130  : rowi(rowi_in)
131  , rowk(rowk_in)
132  {
133  compute(a, b);
134  }
135 
137 
138  inline void transposeInPlace()
139  {
140  s = -s;
141  }
142 
148  inline void compute(const btScalar a, const btScalar b)
149  {
150  btScalar d = a * a + b * b;
151  c = 1;
152  s = 0;
153  if (d > SIMD_EPSILON) {
154  btScalar sqrtd = btSqrt(d);
155  if (sqrtd>SIMD_EPSILON)
156  {
157  btScalar t = btScalar(1.0)/sqrtd;
158  c = a * t;
159  s = -b * t;
160  }
161  }
162  }
163 
169  inline void computeUnconventional(const btScalar a, const btScalar b)
170  {
171  btScalar d = a * a + b * b;
172  c = 0;
173  s = 1;
174  if (d > SIMD_EPSILON) {
175  btScalar t = btScalar(1.0)/btSqrt(d);
176  s = a * t;
177  c = b * t;
178  }
179  }
183  inline void fill(const btMatrix3x3& R) const
184  {
185  btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
186  A.setIdentity();
187  A[rowi][rowi] = c;
188  A[rowk][rowi] = -s;
189  A[rowi][rowk] = s;
190  A[rowk][rowk] = c;
191  }
192 
193  inline void fill(const btMatrix2x2& R) const
194  {
195  btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
196  A(rowi,rowi) = c;
197  A(rowk,rowi) = -s;
198  A(rowi,rowk) = s;
199  A(rowk,rowk) = c;
200  }
201 
209  inline void rowRotation(btMatrix3x3& A) const
210  {
211  for (int j = 0; j < 3; j++) {
212  btScalar tau1 = A[rowi][j];
213  btScalar tau2 = A[rowk][j];
214  A[rowi][j] = c * tau1 - s * tau2;
215  A[rowk][j] = s * tau1 + c * tau2;
216  }
217  }
218  inline void rowRotation(btMatrix2x2& A) const
219  {
220  for (int j = 0; j < 2; j++) {
221  btScalar tau1 = A(rowi,j);
222  btScalar tau2 = A(rowk,j);
223  A(rowi,j) = c * tau1 - s * tau2;
224  A(rowk,j) = s * tau1 + c * tau2;
225  }
226  }
227 
235  inline void columnRotation(btMatrix3x3& A) const
236  {
237  for (int j = 0; j < 3; j++) {
238  btScalar tau1 = A[j][rowi];
239  btScalar tau2 = A[j][rowk];
240  A[j][rowi] = c * tau1 - s * tau2;
241  A[j][rowk] = s * tau1 + c * tau2;
242  }
243  }
244  inline void columnRotation(btMatrix2x2& A) const
245  {
246  for (int j = 0; j < 2; j++) {
247  btScalar tau1 = A(j,rowi);
248  btScalar tau2 = A(j,rowk);
249  A(j,rowi) = c * tau1 - s * tau2;
250  A(j,rowk) = s * tau1 + c * tau2;
251  }
252  }
253 
257  inline void operator*=(const GivensRotation& A)
258  {
259  btScalar new_c = c * A.c - s * A.s;
260  btScalar new_s = s * A.c + c * A.s;
261  c = new_c;
262  s = new_s;
263  }
264 
268  inline GivensRotation operator*(const GivensRotation& A) const
269  {
270  GivensRotation r(*this);
271  r *= A;
272  return r;
273  }
274 };
275 
287 {
288 
295  GivensRotation r1(H[0][0], H[1][0], 0, 1);
304  GivensRotation r2(1, 2);
305  if (H[1][0] != 0)
306  r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]);
307  else
308  r2.compute(H[0][1], H[0][2]);
309 
310  r1.rowRotation(H);
311 
312  /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
313  r2.columnRotation(H);
314  r2.columnRotation(V);
315 
322  GivensRotation r3(H[1][1], H[2][1], 1, 2);
323  r3.rowRotation(H);
324 
325  // Save this till end for better cache coherency
326  // r1.rowRotation(u_transpose);
327  // r3.rowRotation(u_transpose);
328  r1.columnRotation(U);
329  r3.columnRotation(U);
330 }
331 
343 {
344  U.setIdentity();
345  V.setIdentity();
346 
354  GivensRotation r(H[1][0], H[2][0], 1, 2);
355  r.rowRotation(H);
356  // r.rowRotation(u_transpose);
357  r.columnRotation(U);
358  // zeroChase(H, u_transpose, V);
359  zeroChase(H, U, V);
360 }
361 
373 {
374  U.setIdentity();
375  V.setIdentity();
376 
384  GivensRotation r1(H[0][1], H[0][2], 1, 2);
385  r1.columnRotation(H);
386  r1.columnRotation(V);
387 
395  r1.computeUnconventional(H[1][2], H[2][2]);
396  r1.rowRotation(H);
397  r1.columnRotation(U);
398 
406  GivensRotation r2(H[2][0], H[2][1], 0, 1);
407  r2.columnRotation(H);
408  r2.columnRotation(V);
409 
416  r2.computeUnconventional(H[0][1], H[1][1]);
417  r2.rowRotation(H);
418  r2.columnRotation(U);
419 }
420 
431 inline void polarDecomposition(const btMatrix2x2& A,
432  GivensRotation& R,
433  const btMatrix2x2& S_Sym)
434 {
435  btScalar a = (A(0, 0) + A(1, 1)), b = (A(1, 0) - A(0, 1));
436  btScalar denominator = btSqrt(a*a+b*b);
437  R.c = (btScalar)1;
438  R.s = (btScalar)0;
439  if (denominator > SIMD_EPSILON) {
440  /*
441  No need to use a tolerance here because x(0) and x(1) always have
442  smaller magnitude then denominator, therefore overflow never happens.
443  In Bullet, we use a tolerance anyway.
444  */
445  R.c = a / denominator;
446  R.s = -b / denominator;
447  }
448  btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
449  S = A;
450  R.rowRotation(S);
451 }
452 
453 inline void polarDecomposition(const btMatrix2x2& A,
454  const btMatrix2x2& R,
455  const btMatrix2x2& S_Sym)
456 {
457  GivensRotation r(0, 1);
458  polarDecomposition(A, r, S_Sym);
459  r.fill(R);
460 }
461 
470  const btMatrix2x2& A,
471  GivensRotation& U,
472  const btMatrix2x2& Sigma,
473  GivensRotation& V,
474  const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
475 {
476  btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
477  sigma.setIdentity();
478  btMatrix2x2 S_Sym;
479  polarDecomposition(A, U, S_Sym);
480  btScalar cosine, sine;
481  btScalar x = S_Sym(0, 0);
482  btScalar y = S_Sym(0, 1);
483  btScalar z = S_Sym(1, 1);
484  if (y == 0) {
485  // S is already diagonal
486  cosine = 1;
487  sine = 0;
488  sigma(0,0) = x;
489  sigma(1,1) = z;
490  }
491  else {
492  btScalar tau = 0.5 * (x - z);
493  btScalar val = tau * tau + y * y;
494  if (val > SIMD_EPSILON)
495  {
496  btScalar w = btSqrt(val);
497  // w > y > 0
498  btScalar t;
499  if (tau > 0) {
500  // tau + w > w > y > 0 ==> division is safe
501  t = y / (tau + w);
502  }
503  else {
504  // tau - w < -w < -y < 0 ==> division is safe
505  t = y / (tau - w);
506  }
507  cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
508  sine = -t * cosine;
509  /*
510  V = [cosine -sine; sine cosine]
511  Sigma = V'SV. Only compute the diagonals for efficiency.
512  Also utilize symmetry of S and don't form V yet.
513  */
514  btScalar c2 = cosine * cosine;
515  btScalar csy = 2 * cosine * sine * y;
516  btScalar s2 = sine * sine;
517  sigma(0,0) = c2 * x - csy + s2 * z;
518  sigma(1,1) = s2 * x + csy + c2 * z;
519  } else
520  {
521  cosine = 1;
522  sine = 0;
523  sigma(0,0) = x;
524  sigma(1,1) = z;
525  }
526  }
527 
528  // Sorting
529  // Polar already guarantees negative sign is on the small magnitude singular value.
530  if (sigma(0,0) < sigma(1,1)) {
531  std::swap(sigma(0,0), sigma(1,1));
532  V.c = -sine;
533  V.s = cosine;
534  }
535  else {
536  V.c = cosine;
537  V.s = sine;
538  }
539  U *= V;
540 }
541 
550  const btMatrix2x2& A,
551  const btMatrix2x2& U,
552  const btMatrix2x2& Sigma,
553  const btMatrix2x2& V,
554  const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
555 {
556  GivensRotation gv(0, 1);
557  GivensRotation gu(0, 1);
558  singularValueDecomposition(A, gu, Sigma, gv);
559 
560  gu.fill(U);
561  gv.fill(V);
562 }
563 
572 inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
573 {
574  btScalar d = (btScalar)0.5 * (a1 - a2);
575  btScalar bs = b1 * b1;
576  btScalar val = d * d + bs;
577  if (val>SIMD_EPSILON)
578  {
579  btScalar denom = btFabs(d) + btSqrt(val);
580 
581  btScalar mu = a2 - copySign(bs / (denom), d);
582  // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
583  return mu;
584  }
585  return a2;
586 }
587 
591 template <int t>
592 inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
593 {
594  int other = (t == 1) ? 0 : 2;
595  GivensRotation u(0, 1);
596  GivensRotation v(0, 1);
597  sigma[other] = B[other][other];
598 
599  btMatrix2x2 B_sub, sigma_sub;
600  if (t == 0)
601  {
602  B_sub.m_00 = B[0][0];
603  B_sub.m_10 = B[1][0];
604  B_sub.m_01 = B[0][1];
605  B_sub.m_11 = B[1][1];
606  sigma_sub.m_00 = sigma[0];
607  sigma_sub.m_11 = sigma[1];
608 // singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
609  singularValueDecomposition(B_sub, u, sigma_sub, v);
610  B[0][0] = B_sub.m_00;
611  B[1][0] = B_sub.m_10;
612  B[0][1] = B_sub.m_01;
613  B[1][1] = B_sub.m_11;
614  sigma[0] = sigma_sub.m_00;
615  sigma[1] = sigma_sub.m_11;
616  }
617  else
618  {
619  B_sub.m_00 = B[1][1];
620  B_sub.m_10 = B[2][1];
621  B_sub.m_01 = B[1][2];
622  B_sub.m_11 = B[2][2];
623  sigma_sub.m_00 = sigma[1];
624  sigma_sub.m_11 = sigma[2];
625  // singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
626  singularValueDecomposition(B_sub, u, sigma_sub, v);
627  B[1][1] = B_sub.m_00;
628  B[2][1] = B_sub.m_10;
629  B[1][2] = B_sub.m_01;
630  B[2][2] = B_sub.m_11;
631  sigma[1] = sigma_sub.m_00;
632  sigma[2] = sigma_sub.m_11;
633  }
634  u.rowi += t;
635  u.rowk += t;
636  v.rowi += t;
637  v.rowk += t;
638  u.columnRotation(U);
639  v.columnRotation(V);
640 }
641 
645 inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
646 {
647  sigma[i] = -sigma[i];
648  U[0][i] = -U[0][i];
649  U[1][i] = -U[1][i];
650  U[2][i] = -U[2][i];
651 }
652 
653 inline void flipSign(int i, btMatrix3x3& U)
654 {
655  U[0][i] = -U[0][i];
656  U[1][i] = -U[1][i];
657  U[2][i] = -U[2][i];
658 }
659 
660 inline void swapCol(btMatrix3x3& A, int i, int j)
661 {
662  for (int d = 0; d < 3; ++d)
663  std::swap(A[d][i], A[d][j]);
664 }
668 inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
669 {
670  if (t == 0)
671  {
672  // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
673  if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
674  if (sigma[1] < 0) {
675  flipSign(1, U, sigma);
676  flipSign(2, U, sigma);
677  }
678  return;
679  }
680 
681  //fix sign of sigma for both cases
682  if (sigma[2] < 0) {
683  flipSign(1, U, sigma);
684  flipSign(2, U, sigma);
685  }
686 
687  //swap sigma(1) and sigma(2) for both cases
688  std::swap(sigma[1], sigma[2]);
689  // swap the col 1 and col 2 for U,V
690  swapCol(U,1,2);
691  swapCol(V,1,2);
692 
693  // Case: |sigma(2)| >= sigma(0) > |simga(1)|
694  if (sigma[1] > sigma[0]) {
695  std::swap(sigma[0], sigma[1]);
696  swapCol(U,0,1);
697  swapCol(V,0,1);
698  }
699 
700  // Case: sigma(0) >= |sigma(2)| > |simga(1)|
701  else {
702  flipSign(2, U);
703  flipSign(2, V);
704  }
705  }
706  else if (t == 1)
707  {
708  // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
709  if (btFabs(sigma[0]) >= sigma[1]) {
710  if (sigma[0] < 0) {
711  flipSign(0, U, sigma);
712  flipSign(2, U, sigma);
713  }
714  return;
715  }
716 
717  //swap sigma(0) and sigma(1) for both cases
718  std::swap(sigma[0], sigma[1]);
719  swapCol(U, 0, 1);
720  swapCol(V, 0, 1);
721 
722  // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
723  if (btFabs(sigma[1]) < btFabs(sigma[2])) {
724  std::swap(sigma[1], sigma[2]);
725  swapCol(U, 1, 2);
726  swapCol(V, 1, 2);
727  }
728 
729  // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
730  else {
731  flipSign(1, U);
732  flipSign(1, V);
733  }
734 
735  // fix sign for both cases
736  if (sigma[1] < 0) {
737  flipSign(1, U, sigma);
738  flipSign(2, U, sigma);
739  }
740  }
741 }
742 
751  btMatrix3x3& U,
752  btVector3& sigma,
753  btMatrix3x3& V,
754  btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
755 {
756  using std::fabs;
757  btMatrix3x3 B = A;
758  U.setIdentity();
759  V.setIdentity();
760 
761  makeUpperBidiag(B, U, V);
762 
763  int count = 0;
764  btScalar mu = (btScalar)0;
765  GivensRotation r(0, 1);
766 
767  btScalar alpha_1 = B[0][0];
768  btScalar beta_1 = B[0][1];
769  btScalar alpha_2 = B[1][1];
770  btScalar alpha_3 = B[2][2];
771  btScalar beta_2 = B[1][2];
772  btScalar gamma_1 = alpha_1 * beta_1;
773  btScalar gamma_2 = alpha_2 * beta_2;
774  btScalar val = alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2;
775  if (val > SIMD_EPSILON)
776  {
777  tol *= btMax((btScalar)0.5 * btSqrt(val), (btScalar)1);
778  }
782  int max_count = 100;
783 
784  while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
785  && btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
786  && btFabs(alpha_3) > tol
787  && count < max_count) {
788  mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
789 
790  r.compute(alpha_1 * alpha_1 - mu, gamma_1);
791  r.columnRotation(B);
792 
793  r.columnRotation(V);
794  zeroChase(B, U, V);
795 
796  alpha_1 = B[0][0];
797  beta_1 = B[0][1];
798  alpha_2 = B[1][1];
799  alpha_3 = B[2][2];
800  beta_2 = B[1][2];
801  gamma_1 = alpha_1 * beta_1;
802  gamma_2 = alpha_2 * beta_2;
803  count++;
804  }
815  if (btFabs(beta_2) <= tol) {
816  process<0>(B, U, sigma, V);
817  sort(U, sigma, V,0);
818  }
825  else if (btFabs(beta_1) <= tol) {
826  process<1>(B, U, sigma, V);
827  sort(U, sigma, V,1);
828  }
835  else if (btFabs(alpha_2) <= tol) {
842  GivensRotation r1(1, 2);
843  r1.computeUnconventional(B[1][2], B[2][2]);
844  r1.rowRotation(B);
845  r1.columnRotation(U);
846 
847  process<0>(B, U, sigma, V);
848  sort(U, sigma, V, 0);
849  }
856  else if (btFabs(alpha_3) <= tol) {
863  GivensRotation r1(1, 2);
864  r1.compute(B[1][1], B[1][2]);
865  r1.columnRotation(B);
866  r1.columnRotation(V);
873  GivensRotation r2(0, 2);
874  r2.compute(B[0][0], B[0][2]);
875  r2.columnRotation(B);
876  r2.columnRotation(V);
877 
878  process<0>(B, U, sigma, V);
879  sort(U, sigma, V, 0);
880  }
887  else if (btFabs(alpha_1) <= tol) {
894  GivensRotation r1(0, 1);
895  r1.computeUnconventional(B[0][1], B[1][1]);
896  r1.rowRotation(B);
897  r1.columnRotation(U);
898 
905  GivensRotation r2(0, 2);
906  r2.computeUnconventional(B[0][2], B[2][2]);
907  r2.rowRotation(B);
908  r2.columnRotation(U);
909 
910  process<1>(B, U, sigma, V);
911  sort(U, sigma, V, 1);
912  }
913 
914  return count;
915 }
916 #endif /* btImplicitQRSVD_h */
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btMatrix2x2::m_01
btScalar m_01
Definition: btImplicitQRSVD.h:49
wilkinsonShift
btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
compute wilkinsonShift of the block a1 b1 b1 a2 based on the wilkinsonShift formula mu = c + d - sign...
Definition: btImplicitQRSVD.h:572
GivensRotation::operator*=
void operator*=(const GivensRotation &A)
Multiply givens must be for same row and column.
Definition: btImplicitQRSVD.h:257
U
unsigned int U
Definition: btGjkEpa3.h:78
polarDecomposition
void polarDecomposition(const btMatrix2x2 &A, GivensRotation &R, const btMatrix2x2 &S_Sym)
2x2 polar decomposition.
Definition: btImplicitQRSVD.h:431
GivensRotation::transposeInPlace
void transposeInPlace()
Definition: btImplicitQRSVD.h:138
GivensRotation::rowk
int rowk
Definition: btImplicitQRSVD.h:117
btMatrix2x2::m_11
btScalar m_11
Definition: btImplicitQRSVD.h:49
sort
void sort(btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V, int t)
Helper function of 3X3 SVD for sorting singular values.
Definition: btImplicitQRSVD.h:668
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMatrix2x2::operator()
const btScalar & operator()(int i, int j) const
Definition: btImplicitQRSVD.h:68
GivensRotation::rowRotation
void rowRotation(btMatrix2x2 &A) const
Definition: btImplicitQRSVD.h:218
btMatrix2x2
Bullet Continuous Collision Detection and Physics Library Copyright (c) 2019 Google Inc.
Definition: btImplicitQRSVD.h:46
GivensRotation::GivensRotation
GivensRotation(int rowi_in, int rowk_in)
Definition: btImplicitQRSVD.h:121
btMatrix3x3.h
GivensRotation::computeUnconventional
void computeUnconventional(const btScalar a, const btScalar b)
This function computes c and s so that ( c -s ) ( a ) = ( 0 ) s c b ( * )
Definition: btImplicitQRSVD.h:169
btMatrix2x2::m_00
btScalar m_00
Definition: btImplicitQRSVD.h:49
process
void process(btMatrix3x3 &B, btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V)
Helper function of 3X3 SVD for processing 2X2 SVD.
Definition: btImplicitQRSVD.h:592
makeLambdaShape
void makeLambdaShape(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
make a 3X3 matrix to lambda shape original form of H: x x x x x x x x x after : x 0 0 x x 0 x 0 x
Definition: btImplicitQRSVD.h:372
copySign
static btScalar copySign(btScalar x, btScalar y)
Definition: btImplicitQRSVD.h:90
GivensRotation::columnRotation
void columnRotation(btMatrix2x2 &A) const
Definition: btImplicitQRSVD.h:244
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
GivensRotation::fill
void fill(const btMatrix2x2 &R) const
Definition: btImplicitQRSVD.h:193
GivensRotation::c
btScalar c
Definition: btImplicitQRSVD.h:118
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
singularValueDecomposition
void singularValueDecomposition(const btMatrix2x2 &A, GivensRotation &U, const btMatrix2x2 &Sigma, GivensRotation &V, const btScalar tol=64 *std::numeric_limits< btScalar >::epsilon())
2x2 SVD (singular value decomposition) A=USV'
Definition: btImplicitQRSVD.h:469
GivensRotation::s
btScalar s
Definition: btImplicitQRSVD.h:119
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMatrix2x2::m_10
btScalar m_10
Definition: btImplicitQRSVD.h:49
GivensRotation::compute
void compute(const btScalar a, const btScalar b)
Compute c and s from a and b so that ( c -s ) ( a ) = ( * ) s c b ( 0 )
Definition: btImplicitQRSVD.h:148
zeroChase
void zeroChase(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
zero chasing the 3X3 matrix to bidiagonal form original form of H: x x 0 x x x 0 0 x after zero chase...
Definition: btImplicitQRSVD.h:286
makeUpperBidiag
void makeUpperBidiag(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
make a 3X3 matrix to upper bidiagonal form original form of H: x x x x x x x x x after zero chase: x ...
Definition: btImplicitQRSVD.h:342
swapCol
void swapCol(btMatrix3x3 &A, int i, int j)
Definition: btImplicitQRSVD.h:660
GivensRotation::columnRotation
void columnRotation(btMatrix3x3 &A) const
This function does something like c s 0 A ( -s c 0 ) -> A 0 0 1 It only affects column i and column k...
Definition: btImplicitQRSVD.h:235
GivensRotation::rowRotation
void rowRotation(btMatrix3x3 &A) const
This function does something like c -s 0 ( s c 0 ) A -> A 0 0 1 It only affects row i and row k of A.
Definition: btImplicitQRSVD.h:209
btMatrix2x2::btMatrix2x2
btMatrix2x2()
Definition: btImplicitQRSVD.h:50
GivensRotation::GivensRotation
GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
Definition: btImplicitQRSVD.h:129
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
GivensRotation::fill
void fill(const btMatrix3x3 &R) const
Fill the R with the entries of this rotation.
Definition: btImplicitQRSVD.h:183
GivensRotation
Class for givens rotation.
Definition: btImplicitQRSVD.h:114
btMatrix2x2::setIdentity
void setIdentity()
Definition: btImplicitQRSVD.h:81
btMatrix2x2::operator()
btScalar & operator()(int i, int j)
Definition: btImplicitQRSVD.h:55
flipSign
void flipSign(int i, btMatrix3x3 &U, btVector3 &sigma)
Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma.
Definition: btImplicitQRSVD.h:645
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
GivensRotation::operator*
GivensRotation operator*(const GivensRotation &A) const
Multiply givens must be for same row and column.
Definition: btImplicitQRSVD.h:268
btMatrix2x2::btMatrix2x2
btMatrix2x2(const btMatrix2x2 &other)
Definition: btImplicitQRSVD.h:53
GivensRotation::rowi
int rowi
Definition: btImplicitQRSVD.h:116
GivensRotation::~GivensRotation
~GivensRotation()
Definition: btImplicitQRSVD.h:136