Bullet Collision Detection & Physics Library: btImplicitQRSVD.h Source File

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"
46 class btMatrix2x2
47 {
48 public:
49 btScalar m_00, m_01, m_10, m_11;
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
114 class GivensRotation {
115 public:
116 int rowi;
117 int rowk;
118 btScalar c;
119 btScalar s;
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
136 ~GivensRotation() {}
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
286 inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
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
342 inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
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
372 inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
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
469 inline void singularValueDecomposition(
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
549 inline void singularValueDecomposition(
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
750 inline int singularValueDecomposition(const btMatrix3x3& A,
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:47
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:48
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
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