1 /**
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2019 Google Inc. http://bulletphysics.org
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
10 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
11 3. This notice may not be removed or altered from any source distribution.
12
13 Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
14
15 Permission is hereby granted, free of charge, to any person obtaining a copy of
16 this software and associated documentation files (the "Software"), to deal in
17 the Software without restriction, including without limitation the rights to
18 use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
19 of the Software, and to permit persons to whom the Software is furnished to do
20 so, subject to the following conditions:
21
22 The above copyright notice and this permission notice shall be included in all
23 copies or substantial portions of the Software.
24
25 If the code is used in an article, the following paper shall be cited:
26 @techreport{qrsvd:2016,
27 title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
28 author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
29 year={2016},
30 institution={University of California Los Angeles}
31 }
32
33 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
34 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
35 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
36 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
37 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
38 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
39 SOFTWARE.
40 **/
41
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;
btMatrix2x2()50 btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
51 {
52 }
btMatrix2x2(const btMatrix2x2 & other)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 {}
operator()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 }
operator()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 }
setIdentity()81 void setIdentity()
82 {
83 m_00 = 1;
84 m_11 = 1;
85 m_01 = 0;
86 m_10 = 0;
87 }
88 };
89
copySign(btScalar x,btScalar y)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
96 /**
97 Class for givens rotation.
98 Row rotation G*A corresponds to something like
99 c -s 0
100 ( s c 0 ) A
101 0 0 1
102 Column rotation A G' corresponds to something like
103 c -s 0
104 A ( s c 0 )
105 0 0 1
106
107 c and s are always computed so that
108 ( c -s ) ( a ) = ( * )
109 s c b ( 0 )
110
111 Assume rowi<rowk.
112 */
113
114 class GivensRotation {
115 public:
116 int rowi;
117 int rowk;
118 btScalar c;
119 btScalar s;
120
GivensRotation(int rowi_in,int rowk_in)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
GivensRotation(btScalar a,btScalar b,int rowi_in,int rowk_in)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
~GivensRotation()136 ~GivensRotation() {}
137
transposeInPlace()138 inline void transposeInPlace()
139 {
140 s = -s;
141 }
142
143 /**
144 Compute c and s from a and b so that
145 ( c -s ) ( a ) = ( * )
146 s c b ( 0 )
147 */
compute(const btScalar a,const btScalar b)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
164 /**
165 This function computes c and s so that
166 ( c -s ) ( a ) = ( 0 )
167 s c b ( * )
168 */
computeUnconventional(const btScalar a,const btScalar b)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 }
180 /**
181 Fill the R with the entries of this rotation
182 */
fill(const btMatrix3x3 & R)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
fill(const btMatrix2x2 & R)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
202 /**
203 This function does something like
204 c -s 0
205 ( s c 0 ) A -> A
206 0 0 1
207 It only affects row i and row k of A.
208 */
rowRotation(btMatrix3x3 & A)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 }
rowRotation(btMatrix2x2 & A)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
228 /**
229 This function does something like
230 c s 0
231 A ( -s c 0 ) -> A
232 0 0 1
233 It only affects column i and column k of A.
234 */
columnRotation(btMatrix3x3 & A)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 }
columnRotation(btMatrix2x2 & A)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
254 /**
255 Multiply givens must be for same row and column
256 **/
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
265 /**
266 Multiply givens must be for same row and column
267 **/
268 inline GivensRotation operator*(const GivensRotation& A) const
269 {
270 GivensRotation r(*this);
271 r *= A;
272 return r;
273 }
274 };
275
276 /**
277 \brief zero chasing the 3X3 matrix to bidiagonal form
278 original form of H: x x 0
279 x x x
280 0 0 x
281 after zero chase:
282 x x 0
283 0 x x
284 0 0 x
285 */
zeroChase(btMatrix3x3 & H,btMatrix3x3 & U,btMatrix3x3 & V)286 inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
287 {
288
289 /**
290 Reduce H to of form
291 x x +
292 0 x x
293 0 0 x
294 */
295 GivensRotation r1(H[0][0], H[1][0], 0, 1);
296 /**
297 Reduce H to of form
298 x x 0
299 0 x x
300 0 + x
301 Can calculate r2 without multiplying by r1 since both entries are in first two
302 rows thus no need to divide by sqrt(a^2+b^2)
303 */
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
316 /**
317 Reduce H to of form
318 x x 0
319 0 x x
320 0 0 x
321 */
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
332 /**
333 \brief make a 3X3 matrix to upper bidiagonal form
334 original form of H: x x x
335 x x x
336 x x x
337 after zero chase:
338 x x 0
339 0 x x
340 0 0 x
341 */
makeUpperBidiag(btMatrix3x3 & H,btMatrix3x3 & U,btMatrix3x3 & V)342 inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
343 {
344 U.setIdentity();
345 V.setIdentity();
346
347 /**
348 Reduce H to of form
349 x x x
350 x x x
351 0 x x
352 */
353
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
362 /**
363 \brief make a 3X3 matrix to lambda shape
364 original form of H: x x x
365 * x x x
366 * x x x
367 after :
368 * x 0 0
369 * x x 0
370 * x 0 x
371 */
makeLambdaShape(btMatrix3x3 & H,btMatrix3x3 & U,btMatrix3x3 & V)372 inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
373 {
374 U.setIdentity();
375 V.setIdentity();
376
377 /**
378 Reduce H to of form
379 * x x 0
380 * x x x
381 * x x x
382 */
383
384 GivensRotation r1(H[0][1], H[0][2], 1, 2);
385 r1.columnRotation(H);
386 r1.columnRotation(V);
387
388 /**
389 Reduce H to of form
390 * x x 0
391 * x x 0
392 * x x x
393 */
394
395 r1.computeUnconventional(H[1][2], H[2][2]);
396 r1.rowRotation(H);
397 r1.columnRotation(U);
398
399 /**
400 Reduce H to of form
401 * x x 0
402 * x x 0
403 * x 0 x
404 */
405
406 GivensRotation r2(H[2][0], H[2][1], 0, 1);
407 r2.columnRotation(H);
408 r2.columnRotation(V);
409
410 /**
411 Reduce H to of form
412 * x 0 0
413 * x x 0
414 * x 0 x
415 */
416 r2.computeUnconventional(H[0][1], H[1][1]);
417 r2.rowRotation(H);
418 r2.columnRotation(U);
419 }
420
421 /**
422 \brief 2x2 polar decomposition.
423 \param[in] A matrix.
424 \param[out] R Robustly a rotation matrix.
425 \param[out] S_Sym Symmetric. Whole matrix is stored
426
427 Polar guarantees negative sign is on the small magnitude singular value.
428 S is guaranteed to be the closest one to identity.
429 R is guaranteed to be the closest rotation to A.
430 */
polarDecomposition(const btMatrix2x2 & A,GivensRotation & R,const btMatrix2x2 & S_Sym)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
polarDecomposition(const btMatrix2x2 & A,const btMatrix2x2 & R,const btMatrix2x2 & S_Sym)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
462 /**
463 \brief 2x2 SVD (singular value decomposition) A=USV'
464 \param[in] A Input matrix.
465 \param[out] U Robustly a rotation matrix in Givens form
466 \param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
467 \param[out] V Robustly a rotation matrix in Givens form
468 */
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
542 /**
543 \brief 2x2 SVD (singular value decomposition) A=USV'
544 \param[in] A Input matrix.
545 \param[out] U Robustly a rotation matrix.
546 \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
547 \param[out] V Robustly a rotation matrix.
548 */
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
564 /**
565 \brief compute wilkinsonShift of the block
566 a1 b1
567 b1 a2
568 based on the wilkinsonShift formula
569 mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
570
571 */
wilkinsonShift(const btScalar a1,const btScalar b1,const btScalar a2)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
588 /**
589 \brief Helper function of 3X3 SVD for processing 2X2 SVD
590 */
591 template <int t>
process(btMatrix3x3 & B,btMatrix3x3 & U,btVector3 & sigma,btMatrix3x3 & V)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
642 /**
643 \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
644 */
flipSign(int i,btMatrix3x3 & U,btVector3 & sigma)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
flipSign(int i,btMatrix3x3 & U)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
swapCol(btMatrix3x3 & A,int i,int j)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 }
665 /**
666 \brief Helper function of 3X3 SVD for sorting singular values
667 */
sort(btMatrix3x3 & U,btVector3 & sigma,btMatrix3x3 & V,int t)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
743 /**
744 \brief 3X3 SVD (singular value decomposition) A=USV'
745 \param[in] A Input matrix.
746 \param[out] U is a rotation matrix.
747 \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
748 \param[out] V is a rotation matrix.
749 */
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 }
779 /**
780 Do implicit shift QR until A^T A is block diagonal
781 */
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 }
805 /**
806 Handle the cases of one of the alphas and betas being 0
807 Sorted by ease of handling and then frequency
808 of occurrence
809
810 If B is of form
811 x x 0
812 0 x 0
813 0 0 x
814 */
815 if (btFabs(beta_2) <= tol) {
816 process<0>(B, U, sigma, V);
817 sort(U, sigma, V,0);
818 }
819 /**
820 If B is of form
821 x 0 0
822 0 x x
823 0 0 x
824 */
825 else if (btFabs(beta_1) <= tol) {
826 process<1>(B, U, sigma, V);
827 sort(U, sigma, V,1);
828 }
829 /**
830 If B is of form
831 x x 0
832 0 0 x
833 0 0 x
834 */
835 else if (btFabs(alpha_2) <= tol) {
836 /**
837 Reduce B to
838 x x 0
839 0 0 0
840 0 0 x
841 */
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 }
850 /**
851 If B is of form
852 x x 0
853 0 x x
854 0 0 0
855 */
856 else if (btFabs(alpha_3) <= tol) {
857 /**
858 Reduce B to
859 x x +
860 0 x 0
861 0 0 0
862 */
863 GivensRotation r1(1, 2);
864 r1.compute(B[1][1], B[1][2]);
865 r1.columnRotation(B);
866 r1.columnRotation(V);
867 /**
868 Reduce B to
869 x x 0
870 + x 0
871 0 0 0
872 */
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 }
881 /**
882 If B is of form
883 0 x 0
884 0 x x
885 0 0 x
886 */
887 else if (btFabs(alpha_1) <= tol) {
888 /**
889 Reduce B to
890 0 0 +
891 0 x x
892 0 0 x
893 */
894 GivensRotation r1(0, 1);
895 r1.computeUnconventional(B[0][1], B[1][1]);
896 r1.rowRotation(B);
897 r1.columnRotation(U);
898
899 /**
900 Reduce B to
901 0 0 0
902 0 x x
903 0 + x
904 */
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 */
917