dali-physics/third-party/bullet3/src/LinearMath/btImplicitQRSVD.h

   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 #include <limits>
  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
  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
 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
 143     /**
 144      Compute c and s from a and b so that
 145      ( c -s ) ( a )  =  ( * )
 146      s  c     b       ( 0 )
 147      */
 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      */
 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      */
 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
 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      */
 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
 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      */
 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
 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  */
 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  */
 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  */
 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  */
 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
 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  */
 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>
 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  */
 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 }
 665 /**
 666  \brief Helper function of 3X3 SVD for sorting singular values
 667  */
 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 */