917 lines
24 KiB
C++
917 lines
24 KiB
C++
|
/**
|
||
|
|
Bullet Continuous Collision Detection and Physics Library
|
||
|
|
Copyright (c) 2019 Google Inc. http://bulletphysics.org
|
||
|
|
This software is provided 'as-is', without any express or implied warranty.
|
||
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
||
|
|
Permission is granted to anyone to use this software for any purpose,
|
||
|
|
including commercial applications, and to alter it and redistribute it freely,
|
||
|
|
subject to the following restrictions:
|
||
|
|
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.
|
||
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||
|
|
3. This notice may not be removed or altered from any source distribution.
|
||
|
|
|
||
|
|
Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
|
||
|
|
|
||
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||
|
|
this software and associated documentation files (the "Software"), to deal in
|
||
|
|
the Software without restriction, including without limitation the rights to
|
||
|
|
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
|
||
|
|
of the Software, and to permit persons to whom the Software is furnished to do
|
||
|
|
so, subject to the following conditions:
|
||
|
|
|
||
|
|
The above copyright notice and this permission notice shall be included in all
|
||
|
|
copies or substantial portions of the Software.
|
||
|
|
|
||
|
|
If the code is used in an article, the following paper shall be cited:
|
||
|
|
@techreport{qrsvd:2016,
|
||
|
|
title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
|
||
|
|
author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
|
||
|
|
year={2016},
|
||
|
|
institution={University of California Los Angeles}
|
||
|
|
}
|
||
|
|
|
||
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||
|
|
SOFTWARE.
|
||
|
|
**/
|
||
|
|
|
||
|
|
#ifndef btImplicitQRSVD_h
|
||
|
|
#define btImplicitQRSVD_h
|
||
|
|
|
||
|
|
#include "btMatrix3x3.h"
|
||
|
|
class btMatrix2x2
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
btScalar m_00, m_01, m_10, m_11;
|
||
|
|
btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
|
||
|
|
{}
|
||
|
|
btScalar& operator()(int i, int j)
|
||
|
|
{
|
||
|
|
if (i == 0 && j == 0)
|
||
|
|
return m_00;
|
||
|
|
if (i == 1 && j == 0)
|
||
|
|
return m_10;
|
||
|
|
if (i == 0 && j == 1)
|
||
|
|
return m_01;
|
||
|
|
if (i == 1 && j == 1)
|
||
|
|
return m_11;
|
||
|
|
btAssert(false);
|
||
|
|
return m_00;
|
||
|
|
}
|
||
|
|
const btScalar& operator()(int i, int j) const
|
||
|
|
{
|
||
|
|
if (i == 0 && j == 0)
|
||
|
|
return m_00;
|
||
|
|
if (i == 1 && j == 0)
|
||
|
|
return m_10;
|
||
|
|
if (i == 0 && j == 1)
|
||
|
|
return m_01;
|
||
|
|
if (i == 1 && j == 1)
|
||
|
|
return m_11;
|
||
|
|
btAssert(false);
|
||
|
|
return m_00;
|
||
|
|
}
|
||
|
|
void setIdentity()
|
||
|
|
{
|
||
|
|
m_00 = 1;
|
||
|
|
m_11 = 1;
|
||
|
|
m_01 = 0;
|
||
|
|
m_10 = 0;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
static inline btScalar copySign(btScalar x, btScalar y) {
|
||
|
|
if ((x < 0 && y > 0) || (x > 0 && y < 0))
|
||
|
|
return -x;
|
||
|
|
return x;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
Class for givens rotation.
|
||
|
|
Row rotation G*A corresponds to something like
|
||
|
|
c -s 0
|
||
|
|
( s c 0 ) A
|
||
|
|
0 0 1
|
||
|
|
Column rotation A G' corresponds to something like
|
||
|
|
c -s 0
|
||
|
|
A ( s c 0 )
|
||
|
|
0 0 1
|
||
|
|
|
||
|
|
c and s are always computed so that
|
||
|
|
( c -s ) ( a ) = ( * )
|
||
|
|
s c b ( 0 )
|
||
|
|
|
||
|
|
Assume rowi<rowk.
|
||
|
|
*/
|
||
|
|
|
||
|
|
class GivensRotation {
|
||
|
|
public:
|
||
|
|
int rowi;
|
||
|
|
int rowk;
|
||
|
|
btScalar c;
|
||
|
|
btScalar s;
|
||
|
|
|
||
|
|
inline GivensRotation(int rowi_in, int rowk_in)
|
||
|
|
: rowi(rowi_in)
|
||
|
|
, rowk(rowk_in)
|
||
|
|
, c(1)
|
||
|
|
, s(0)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
|
||
|
|
: rowi(rowi_in)
|
||
|
|
, rowk(rowk_in)
|
||
|
|
{
|
||
|
|
compute(a, b);
|
||
|
|
}
|
||
|
|
|
||
|
|
~GivensRotation() {}
|
||
|
|
|
||
|
|
inline void transposeInPlace()
|
||
|
|
{
|
||
|
|
s = -s;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
Compute c and s from a and b so that
|
||
|
|
( c -s ) ( a ) = ( * )
|
||
|
|
s c b ( 0 )
|
||
|
|
*/
|
||
|
|
inline void compute(const btScalar a, const btScalar b)
|
||
|
|
{
|
||
|
|
btScalar d = a * a + b * b;
|
||
|
|
c = 1;
|
||
|
|
s = 0;
|
||
|
|
if (d > SIMD_EPSILON) {
|
||
|
|
btScalar sqrtd = btSqrt(d);
|
||
|
|
if (sqrtd>SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
btScalar t = btScalar(1.0)/sqrtd;
|
||
|
|
c = a * t;
|
||
|
|
s = -b * t;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
This function computes c and s so that
|
||
|
|
( c -s ) ( a ) = ( 0 )
|
||
|
|
s c b ( * )
|
||
|
|
*/
|
||
|
|
inline void computeUnconventional(const btScalar a, const btScalar b)
|
||
|
|
{
|
||
|
|
btScalar d = a * a + b * b;
|
||
|
|
c = 0;
|
||
|
|
s = 1;
|
||
|
|
if (d > SIMD_EPSILON) {
|
||
|
|
btScalar t = btScalar(1.0)/btSqrt(d);
|
||
|
|
s = a * t;
|
||
|
|
c = b * t;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
Fill the R with the entries of this rotation
|
||
|
|
*/
|
||
|
|
inline void fill(const btMatrix3x3& R) const
|
||
|
|
{
|
||
|
|
btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
|
||
|
|
A.setIdentity();
|
||
|
|
A[rowi][rowi] = c;
|
||
|
|
A[rowk][rowi] = -s;
|
||
|
|
A[rowi][rowk] = s;
|
||
|
|
A[rowk][rowk] = c;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void fill(const btMatrix2x2& R) const
|
||
|
|
{
|
||
|
|
btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
|
||
|
|
A(rowi,rowi) = c;
|
||
|
|
A(rowk,rowi) = -s;
|
||
|
|
A(rowi,rowk) = s;
|
||
|
|
A(rowk,rowk) = c;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
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.
|
||
|
|
*/
|
||
|
|
inline void rowRotation(btMatrix3x3& A) const
|
||
|
|
{
|
||
|
|
for (int j = 0; j < 3; j++) {
|
||
|
|
btScalar tau1 = A[rowi][j];
|
||
|
|
btScalar tau2 = A[rowk][j];
|
||
|
|
A[rowi][j] = c * tau1 - s * tau2;
|
||
|
|
A[rowk][j] = s * tau1 + c * tau2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
inline void rowRotation(btMatrix2x2& A) const
|
||
|
|
{
|
||
|
|
for (int j = 0; j < 2; j++) {
|
||
|
|
btScalar tau1 = A(rowi,j);
|
||
|
|
btScalar tau2 = A(rowk,j);
|
||
|
|
A(rowi,j) = c * tau1 - s * tau2;
|
||
|
|
A(rowk,j) = s * tau1 + c * tau2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
This function does something like
|
||
|
|
c s 0
|
||
|
|
A ( -s c 0 ) -> A
|
||
|
|
0 0 1
|
||
|
|
It only affects column i and column k of A.
|
||
|
|
*/
|
||
|
|
inline void columnRotation(btMatrix3x3& A) const
|
||
|
|
{
|
||
|
|
for (int j = 0; j < 3; j++) {
|
||
|
|
btScalar tau1 = A[j][rowi];
|
||
|
|
btScalar tau2 = A[j][rowk];
|
||
|
|
A[j][rowi] = c * tau1 - s * tau2;
|
||
|
|
A[j][rowk] = s * tau1 + c * tau2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
inline void columnRotation(btMatrix2x2& A) const
|
||
|
|
{
|
||
|
|
for (int j = 0; j < 2; j++) {
|
||
|
|
btScalar tau1 = A(j,rowi);
|
||
|
|
btScalar tau2 = A(j,rowk);
|
||
|
|
A(j,rowi) = c * tau1 - s * tau2;
|
||
|
|
A(j,rowk) = s * tau1 + c * tau2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
Multiply givens must be for same row and column
|
||
|
|
**/
|
||
|
|
inline void operator*=(const GivensRotation& A)
|
||
|
|
{
|
||
|
|
btScalar new_c = c * A.c - s * A.s;
|
||
|
|
btScalar new_s = s * A.c + c * A.s;
|
||
|
|
c = new_c;
|
||
|
|
s = new_s;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
Multiply givens must be for same row and column
|
||
|
|
**/
|
||
|
|
inline GivensRotation operator*(const GivensRotation& A) const
|
||
|
|
{
|
||
|
|
GivensRotation r(*this);
|
||
|
|
r *= A;
|
||
|
|
return r;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief zero chasing the 3X3 matrix to bidiagonal form
|
||
|
|
original form of H: x x 0
|
||
|
|
x x x
|
||
|
|
0 0 x
|
||
|
|
after zero chase:
|
||
|
|
x x 0
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
|
||
|
|
{
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
x x +
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
GivensRotation r1(H[0][0], H[1][0], 0, 1);
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
x x 0
|
||
|
|
0 x x
|
||
|
|
0 + x
|
||
|
|
Can calculate r2 without multiplying by r1 since both entries are in first two
|
||
|
|
rows thus no need to divide by sqrt(a^2+b^2)
|
||
|
|
*/
|
||
|
|
GivensRotation r2(1, 2);
|
||
|
|
if (H[1][0] != 0)
|
||
|
|
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]);
|
||
|
|
else
|
||
|
|
r2.compute(H[0][1], H[0][2]);
|
||
|
|
|
||
|
|
r1.rowRotation(H);
|
||
|
|
|
||
|
|
/* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
|
||
|
|
r2.columnRotation(H);
|
||
|
|
r2.columnRotation(V);
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
x x 0
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
GivensRotation r3(H[1][1], H[2][1], 1, 2);
|
||
|
|
r3.rowRotation(H);
|
||
|
|
|
||
|
|
// Save this till end for better cache coherency
|
||
|
|
// r1.rowRotation(u_transpose);
|
||
|
|
// r3.rowRotation(u_transpose);
|
||
|
|
r1.columnRotation(U);
|
||
|
|
r3.columnRotation(U);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 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 x 0
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
|
||
|
|
{
|
||
|
|
U.setIdentity();
|
||
|
|
V.setIdentity();
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
x x x
|
||
|
|
x x x
|
||
|
|
0 x x
|
||
|
|
*/
|
||
|
|
|
||
|
|
GivensRotation r(H[1][0], H[2][0], 1, 2);
|
||
|
|
r.rowRotation(H);
|
||
|
|
// r.rowRotation(u_transpose);
|
||
|
|
r.columnRotation(U);
|
||
|
|
// zeroChase(H, u_transpose, V);
|
||
|
|
zeroChase(H, U, V);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 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
|
||
|
|
*/
|
||
|
|
inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
|
||
|
|
{
|
||
|
|
U.setIdentity();
|
||
|
|
V.setIdentity();
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
* x x 0
|
||
|
|
* x x x
|
||
|
|
* x x x
|
||
|
|
*/
|
||
|
|
|
||
|
|
GivensRotation r1(H[0][1], H[0][2], 1, 2);
|
||
|
|
r1.columnRotation(H);
|
||
|
|
r1.columnRotation(V);
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
* x x 0
|
||
|
|
* x x 0
|
||
|
|
* x x x
|
||
|
|
*/
|
||
|
|
|
||
|
|
r1.computeUnconventional(H[1][2], H[2][2]);
|
||
|
|
r1.rowRotation(H);
|
||
|
|
r1.columnRotation(U);
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
* x x 0
|
||
|
|
* x x 0
|
||
|
|
* x 0 x
|
||
|
|
*/
|
||
|
|
|
||
|
|
GivensRotation r2(H[2][0], H[2][1], 0, 1);
|
||
|
|
r2.columnRotation(H);
|
||
|
|
r2.columnRotation(V);
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce H to of form
|
||
|
|
* x 0 0
|
||
|
|
* x x 0
|
||
|
|
* x 0 x
|
||
|
|
*/
|
||
|
|
r2.computeUnconventional(H[0][1], H[1][1]);
|
||
|
|
r2.rowRotation(H);
|
||
|
|
r2.columnRotation(U);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 2x2 polar decomposition.
|
||
|
|
\param[in] A matrix.
|
||
|
|
\param[out] R Robustly a rotation matrix.
|
||
|
|
\param[out] S_Sym Symmetric. Whole matrix is stored
|
||
|
|
|
||
|
|
Polar guarantees negative sign is on the small magnitude singular value.
|
||
|
|
S is guaranteed to be the closest one to identity.
|
||
|
|
R is guaranteed to be the closest rotation to A.
|
||
|
|
*/
|
||
|
|
inline void polarDecomposition(const btMatrix2x2& A,
|
||
|
|
GivensRotation& R,
|
||
|
|
const btMatrix2x2& S_Sym)
|
||
|
|
{
|
||
|
|
btScalar a = (A(0, 0) + A(1, 1)), b = (A(1, 0) - A(0, 1));
|
||
|
|
btScalar denominator = btSqrt(a*a+b*b);
|
||
|
|
R.c = (btScalar)1;
|
||
|
|
R.s = (btScalar)0;
|
||
|
|
if (denominator > SIMD_EPSILON) {
|
||
|
|
/*
|
||
|
|
No need to use a tolerance here because x(0) and x(1) always have
|
||
|
|
smaller magnitude then denominator, therefore overflow never happens.
|
||
|
|
In Bullet, we use a tolerance anyway.
|
||
|
|
*/
|
||
|
|
R.c = a / denominator;
|
||
|
|
R.s = -b / denominator;
|
||
|
|
}
|
||
|
|
btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
|
||
|
|
S = A;
|
||
|
|
R.rowRotation(S);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void polarDecomposition(const btMatrix2x2& A,
|
||
|
|
const btMatrix2x2& R,
|
||
|
|
const btMatrix2x2& S_Sym)
|
||
|
|
{
|
||
|
|
GivensRotation r(0, 1);
|
||
|
|
polarDecomposition(A, r, S_Sym);
|
||
|
|
r.fill(R);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 2x2 SVD (singular value decomposition) A=USV'
|
||
|
|
\param[in] A Input matrix.
|
||
|
|
\param[out] U Robustly a rotation matrix in Givens form
|
||
|
|
\param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
|
||
|
|
\param[out] V Robustly a rotation matrix in Givens form
|
||
|
|
*/
|
||
|
|
inline void singularValueDecomposition(
|
||
|
|
const btMatrix2x2& A,
|
||
|
|
GivensRotation& U,
|
||
|
|
const btMatrix2x2& Sigma,
|
||
|
|
GivensRotation& V,
|
||
|
|
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
|
||
|
|
{
|
||
|
|
btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
|
||
|
|
sigma.setIdentity();
|
||
|
|
btMatrix2x2 S_Sym;
|
||
|
|
polarDecomposition(A, U, S_Sym);
|
||
|
|
btScalar cosine, sine;
|
||
|
|
btScalar x = S_Sym(0, 0);
|
||
|
|
btScalar y = S_Sym(0, 1);
|
||
|
|
btScalar z = S_Sym(1, 1);
|
||
|
|
if (y == 0) {
|
||
|
|
// S is already diagonal
|
||
|
|
cosine = 1;
|
||
|
|
sine = 0;
|
||
|
|
sigma(0,0) = x;
|
||
|
|
sigma(1,1) = z;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
btScalar tau = 0.5 * (x - z);
|
||
|
|
btScalar val = tau * tau + y * y;
|
||
|
|
if (val > SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
btScalar w = btSqrt(val);
|
||
|
|
// w > y > 0
|
||
|
|
btScalar t;
|
||
|
|
if (tau > 0) {
|
||
|
|
// tau + w > w > y > 0 ==> division is safe
|
||
|
|
t = y / (tau + w);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
// tau - w < -w < -y < 0 ==> division is safe
|
||
|
|
t = y / (tau - w);
|
||
|
|
}
|
||
|
|
cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
|
||
|
|
sine = -t * cosine;
|
||
|
|
/*
|
||
|
|
V = [cosine -sine; sine cosine]
|
||
|
|
Sigma = V'SV. Only compute the diagonals for efficiency.
|
||
|
|
Also utilize symmetry of S and don't form V yet.
|
||
|
|
*/
|
||
|
|
btScalar c2 = cosine * cosine;
|
||
|
|
btScalar csy = 2 * cosine * sine * y;
|
||
|
|
btScalar s2 = sine * sine;
|
||
|
|
sigma(0,0) = c2 * x - csy + s2 * z;
|
||
|
|
sigma(1,1) = s2 * x + csy + c2 * z;
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
cosine = 1;
|
||
|
|
sine = 0;
|
||
|
|
sigma(0,0) = x;
|
||
|
|
sigma(1,1) = z;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Sorting
|
||
|
|
// Polar already guarantees negative sign is on the small magnitude singular value.
|
||
|
|
if (sigma(0,0) < sigma(1,1)) {
|
||
|
|
std::swap(sigma(0,0), sigma(1,1));
|
||
|
|
V.c = -sine;
|
||
|
|
V.s = cosine;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
V.c = cosine;
|
||
|
|
V.s = sine;
|
||
|
|
}
|
||
|
|
U *= V;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 2x2 SVD (singular value decomposition) A=USV'
|
||
|
|
\param[in] A Input matrix.
|
||
|
|
\param[out] U Robustly a rotation matrix.
|
||
|
|
\param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
|
||
|
|
\param[out] V Robustly a rotation matrix.
|
||
|
|
*/
|
||
|
|
inline void singularValueDecomposition(
|
||
|
|
const btMatrix2x2& A,
|
||
|
|
const btMatrix2x2& U,
|
||
|
|
const btMatrix2x2& Sigma,
|
||
|
|
const btMatrix2x2& V,
|
||
|
|
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
|
||
|
|
{
|
||
|
|
GivensRotation gv(0, 1);
|
||
|
|
GivensRotation gu(0, 1);
|
||
|
|
singularValueDecomposition(A, gu, Sigma, gv);
|
||
|
|
|
||
|
|
gu.fill(U);
|
||
|
|
gv.fill(V);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief compute wilkinsonShift of the block
|
||
|
|
a1 b1
|
||
|
|
b1 a2
|
||
|
|
based on the wilkinsonShift formula
|
||
|
|
mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
|
||
|
|
|
||
|
|
*/
|
||
|
|
inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
|
||
|
|
{
|
||
|
|
btScalar d = (btScalar)0.5 * (a1 - a2);
|
||
|
|
btScalar bs = b1 * b1;
|
||
|
|
btScalar val = d * d + bs;
|
||
|
|
if (val>SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
btScalar denom = btFabs(d) + btSqrt(val);
|
||
|
|
|
||
|
|
btScalar mu = a2 - copySign(bs / (denom), d);
|
||
|
|
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
|
||
|
|
return mu;
|
||
|
|
}
|
||
|
|
return a2;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief Helper function of 3X3 SVD for processing 2X2 SVD
|
||
|
|
*/
|
||
|
|
template <int t>
|
||
|
|
inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
|
||
|
|
{
|
||
|
|
int other = (t == 1) ? 0 : 2;
|
||
|
|
GivensRotation u(0, 1);
|
||
|
|
GivensRotation v(0, 1);
|
||
|
|
sigma[other] = B[other][other];
|
||
|
|
|
||
|
|
btMatrix2x2 B_sub, sigma_sub;
|
||
|
|
if (t == 0)
|
||
|
|
{
|
||
|
|
B_sub.m_00 = B[0][0];
|
||
|
|
B_sub.m_10 = B[1][0];
|
||
|
|
B_sub.m_01 = B[0][1];
|
||
|
|
B_sub.m_11 = B[1][1];
|
||
|
|
sigma_sub.m_00 = sigma[0];
|
||
|
|
sigma_sub.m_11 = sigma[1];
|
||
|
|
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
|
||
|
|
singularValueDecomposition(B_sub, u, sigma_sub, v);
|
||
|
|
B[0][0] = B_sub.m_00;
|
||
|
|
B[1][0] = B_sub.m_10;
|
||
|
|
B[0][1] = B_sub.m_01;
|
||
|
|
B[1][1] = B_sub.m_11;
|
||
|
|
sigma[0] = sigma_sub.m_00;
|
||
|
|
sigma[1] = sigma_sub.m_11;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
B_sub.m_00 = B[1][1];
|
||
|
|
B_sub.m_10 = B[2][1];
|
||
|
|
B_sub.m_01 = B[1][2];
|
||
|
|
B_sub.m_11 = B[2][2];
|
||
|
|
sigma_sub.m_00 = sigma[1];
|
||
|
|
sigma_sub.m_11 = sigma[2];
|
||
|
|
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
|
||
|
|
singularValueDecomposition(B_sub, u, sigma_sub, v);
|
||
|
|
B[1][1] = B_sub.m_00;
|
||
|
|
B[2][1] = B_sub.m_10;
|
||
|
|
B[1][2] = B_sub.m_01;
|
||
|
|
B[2][2] = B_sub.m_11;
|
||
|
|
sigma[1] = sigma_sub.m_00;
|
||
|
|
sigma[2] = sigma_sub.m_11;
|
||
|
|
}
|
||
|
|
u.rowi += t;
|
||
|
|
u.rowk += t;
|
||
|
|
v.rowi += t;
|
||
|
|
v.rowk += t;
|
||
|
|
u.columnRotation(U);
|
||
|
|
v.columnRotation(V);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
|
||
|
|
*/
|
||
|
|
inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
|
||
|
|
{
|
||
|
|
sigma[i] = -sigma[i];
|
||
|
|
U[0][i] = -U[0][i];
|
||
|
|
U[1][i] = -U[1][i];
|
||
|
|
U[2][i] = -U[2][i];
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void flipSign(int i, btMatrix3x3& U)
|
||
|
|
{
|
||
|
|
U[0][i] = -U[0][i];
|
||
|
|
U[1][i] = -U[1][i];
|
||
|
|
U[2][i] = -U[2][i];
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void swapCol(btMatrix3x3& A, int i, int j)
|
||
|
|
{
|
||
|
|
for (int d = 0; d < 3; ++d)
|
||
|
|
std::swap(A[d][i], A[d][j]);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
\brief Helper function of 3X3 SVD for sorting singular values
|
||
|
|
*/
|
||
|
|
inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
|
||
|
|
{
|
||
|
|
if (t == 0)
|
||
|
|
{
|
||
|
|
// Case: sigma(0) > |sigma(1)| >= |sigma(2)|
|
||
|
|
if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
|
||
|
|
if (sigma[1] < 0) {
|
||
|
|
flipSign(1, U, sigma);
|
||
|
|
flipSign(2, U, sigma);
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//fix sign of sigma for both cases
|
||
|
|
if (sigma[2] < 0) {
|
||
|
|
flipSign(1, U, sigma);
|
||
|
|
flipSign(2, U, sigma);
|
||
|
|
}
|
||
|
|
|
||
|
|
//swap sigma(1) and sigma(2) for both cases
|
||
|
|
std::swap(sigma[1], sigma[2]);
|
||
|
|
// swap the col 1 and col 2 for U,V
|
||
|
|
swapCol(U,1,2);
|
||
|
|
swapCol(V,1,2);
|
||
|
|
|
||
|
|
// Case: |sigma(2)| >= sigma(0) > |simga(1)|
|
||
|
|
if (sigma[1] > sigma[0]) {
|
||
|
|
std::swap(sigma[0], sigma[1]);
|
||
|
|
swapCol(U,0,1);
|
||
|
|
swapCol(V,0,1);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Case: sigma(0) >= |sigma(2)| > |simga(1)|
|
||
|
|
else {
|
||
|
|
flipSign(2, U);
|
||
|
|
flipSign(2, V);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (t == 1)
|
||
|
|
{
|
||
|
|
// Case: |sigma(0)| >= sigma(1) > |sigma(2)|
|
||
|
|
if (btFabs(sigma[0]) >= sigma[1]) {
|
||
|
|
if (sigma[0] < 0) {
|
||
|
|
flipSign(0, U, sigma);
|
||
|
|
flipSign(2, U, sigma);
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//swap sigma(0) and sigma(1) for both cases
|
||
|
|
std::swap(sigma[0], sigma[1]);
|
||
|
|
swapCol(U, 0, 1);
|
||
|
|
swapCol(V, 0, 1);
|
||
|
|
|
||
|
|
// Case: sigma(1) > |sigma(2)| >= |sigma(0)|
|
||
|
|
if (btFabs(sigma[1]) < btFabs(sigma[2])) {
|
||
|
|
std::swap(sigma[1], sigma[2]);
|
||
|
|
swapCol(U, 1, 2);
|
||
|
|
swapCol(V, 1, 2);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Case: sigma(1) >= |sigma(0)| > |sigma(2)|
|
||
|
|
else {
|
||
|
|
flipSign(1, U);
|
||
|
|
flipSign(1, V);
|
||
|
|
}
|
||
|
|
|
||
|
|
// fix sign for both cases
|
||
|
|
if (sigma[1] < 0) {
|
||
|
|
flipSign(1, U, sigma);
|
||
|
|
flipSign(2, U, sigma);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief 3X3 SVD (singular value decomposition) A=USV'
|
||
|
|
\param[in] A Input matrix.
|
||
|
|
\param[out] U is a rotation matrix.
|
||
|
|
\param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
|
||
|
|
\param[out] V is a rotation matrix.
|
||
|
|
*/
|
||
|
|
inline int singularValueDecomposition(const btMatrix3x3& A,
|
||
|
|
btMatrix3x3& U,
|
||
|
|
btVector3& sigma,
|
||
|
|
btMatrix3x3& V,
|
||
|
|
btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
|
||
|
|
{
|
||
|
|
using std::fabs;
|
||
|
|
btMatrix3x3 B = A;
|
||
|
|
U.setIdentity();
|
||
|
|
V.setIdentity();
|
||
|
|
|
||
|
|
makeUpperBidiag(B, U, V);
|
||
|
|
|
||
|
|
int count = 0;
|
||
|
|
btScalar mu = (btScalar)0;
|
||
|
|
GivensRotation r(0, 1);
|
||
|
|
|
||
|
|
btScalar alpha_1 = B[0][0];
|
||
|
|
btScalar beta_1 = B[0][1];
|
||
|
|
btScalar alpha_2 = B[1][1];
|
||
|
|
btScalar alpha_3 = B[2][2];
|
||
|
|
btScalar beta_2 = B[1][2];
|
||
|
|
btScalar gamma_1 = alpha_1 * beta_1;
|
||
|
|
btScalar gamma_2 = alpha_2 * beta_2;
|
||
|
|
btScalar val = alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2;
|
||
|
|
if (val > SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
tol *= btMax((btScalar)0.5 * btSqrt(val), (btScalar)1);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
Do implicit shift QR until A^T A is block diagonal
|
||
|
|
*/
|
||
|
|
int max_count = 100;
|
||
|
|
|
||
|
|
while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
|
||
|
|
&& btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
|
||
|
|
&& btFabs(alpha_3) > tol
|
||
|
|
&& count < max_count) {
|
||
|
|
mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
|
||
|
|
|
||
|
|
r.compute(alpha_1 * alpha_1 - mu, gamma_1);
|
||
|
|
r.columnRotation(B);
|
||
|
|
|
||
|
|
r.columnRotation(V);
|
||
|
|
zeroChase(B, U, V);
|
||
|
|
|
||
|
|
alpha_1 = B[0][0];
|
||
|
|
beta_1 = B[0][1];
|
||
|
|
alpha_2 = B[1][1];
|
||
|
|
alpha_3 = B[2][2];
|
||
|
|
beta_2 = B[1][2];
|
||
|
|
gamma_1 = alpha_1 * beta_1;
|
||
|
|
gamma_2 = alpha_2 * beta_2;
|
||
|
|
count++;
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
Handle the cases of one of the alphas and betas being 0
|
||
|
|
Sorted by ease of handling and then frequency
|
||
|
|
of occurrence
|
||
|
|
|
||
|
|
If B is of form
|
||
|
|
x x 0
|
||
|
|
0 x 0
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
if (btFabs(beta_2) <= tol) {
|
||
|
|
process<0>(B, U, sigma, V);
|
||
|
|
sort(U, sigma, V,0);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
If B is of form
|
||
|
|
x 0 0
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
else if (btFabs(beta_1) <= tol) {
|
||
|
|
process<1>(B, U, sigma, V);
|
||
|
|
sort(U, sigma, V,1);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
If B is of form
|
||
|
|
x x 0
|
||
|
|
0 0 x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
else if (btFabs(alpha_2) <= tol) {
|
||
|
|
/**
|
||
|
|
Reduce B to
|
||
|
|
x x 0
|
||
|
|
0 0 0
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
GivensRotation r1(1, 2);
|
||
|
|
r1.computeUnconventional(B[1][2], B[2][2]);
|
||
|
|
r1.rowRotation(B);
|
||
|
|
r1.columnRotation(U);
|
||
|
|
|
||
|
|
process<0>(B, U, sigma, V);
|
||
|
|
sort(U, sigma, V, 0);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
If B is of form
|
||
|
|
x x 0
|
||
|
|
0 x x
|
||
|
|
0 0 0
|
||
|
|
*/
|
||
|
|
else if (btFabs(alpha_3) <= tol) {
|
||
|
|
/**
|
||
|
|
Reduce B to
|
||
|
|
x x +
|
||
|
|
0 x 0
|
||
|
|
0 0 0
|
||
|
|
*/
|
||
|
|
GivensRotation r1(1, 2);
|
||
|
|
r1.compute(B[1][1], B[1][2]);
|
||
|
|
r1.columnRotation(B);
|
||
|
|
r1.columnRotation(V);
|
||
|
|
/**
|
||
|
|
Reduce B to
|
||
|
|
x x 0
|
||
|
|
+ x 0
|
||
|
|
0 0 0
|
||
|
|
*/
|
||
|
|
GivensRotation r2(0, 2);
|
||
|
|
r2.compute(B[0][0], B[0][2]);
|
||
|
|
r2.columnRotation(B);
|
||
|
|
r2.columnRotation(V);
|
||
|
|
|
||
|
|
process<0>(B, U, sigma, V);
|
||
|
|
sort(U, sigma, V, 0);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
If B is of form
|
||
|
|
0 x 0
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
else if (btFabs(alpha_1) <= tol) {
|
||
|
|
/**
|
||
|
|
Reduce B to
|
||
|
|
0 0 +
|
||
|
|
0 x x
|
||
|
|
0 0 x
|
||
|
|
*/
|
||
|
|
GivensRotation r1(0, 1);
|
||
|
|
r1.computeUnconventional(B[0][1], B[1][1]);
|
||
|
|
r1.rowRotation(B);
|
||
|
|
r1.columnRotation(U);
|
||
|
|
|
||
|
|
/**
|
||
|
|
Reduce B to
|
||
|
|
0 0 0
|
||
|
|
0 x x
|
||
|
|
0 + x
|
||
|
|
*/
|
||
|
|
GivensRotation r2(0, 2);
|
||
|
|
r2.computeUnconventional(B[0][2], B[2][2]);
|
||
|
|
r2.rowRotation(B);
|
||
|
|
r2.columnRotation(U);
|
||
|
|
|
||
|
|
process<1>(B, U, sigma, V);
|
||
|
|
sort(U, sigma, V, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
return count;
|
||
|
|
}
|
||
|
|
#endif /* btImplicitQRSVD_h */
|