mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
clamp stress for NeoHookean in singular value space
This commit is contained in:
Xuchen Han
parent
e87df18544
commit
9546390fd6
@ -18,6 +18,8 @@ subject to the following restrictions:
|
||||
|
||||
#include "btDeformableLagrangianForce.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "LinearMath/btImplicitQRSVD.h"
|
||||
#include "Eigen"
|
||||
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
|
||||
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
|
||||
{
|
||||
@ -72,11 +74,12 @@ public:
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 C = dF + dF.transpose();
|
||||
btMatrix3x3 dP;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
|
||||
@ -155,6 +158,7 @@ public:
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
// btScalar max_p = 0;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
@ -166,6 +170,51 @@ public:
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j],P);
|
||||
|
||||
btMatrix3x3 U, V;
|
||||
btVector3 sigma;
|
||||
singularValueDecomposition(P, U, sigma, V);
|
||||
sigma[0] = btMin(sigma[0], (btScalar)1000);
|
||||
sigma[1] = btMin(sigma[1], (btScalar)1000);
|
||||
sigma[2] = btMin(sigma[2], (btScalar)1000);
|
||||
sigma[0] = btMax(sigma[0], (btScalar)-1000);
|
||||
sigma[1] = btMax(sigma[1], (btScalar)-1000);
|
||||
sigma[2] = btMax(sigma[2], (btScalar)-1000);
|
||||
btMatrix3x3 Sigma;
|
||||
Sigma.setIdentity();
|
||||
Sigma[0][0] = sigma[0];
|
||||
Sigma[1][1] = sigma[1];
|
||||
Sigma[2][2] = sigma[2];
|
||||
// max_p = btMax(max_p, sigma[0]);
|
||||
// max_p = btMax(max_p, sigma[1]);
|
||||
// max_p = btMax(max_p, sigma[2]);
|
||||
P = U * Sigma * V.transpose();
|
||||
|
||||
// Eigen::Matrix<double, 3,3> eigenP;
|
||||
// eigenP << P[0][0], P[0][1], P[0][2],
|
||||
// P[1][0],P[1][1],P[1][2],
|
||||
// P[2][0],P[2][1],P[2][2];
|
||||
// Eigen::JacobiSVD<Eigen::Matrix<double, 3,3> > svd(eigenP, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
// Eigen::Matrix<double, 3,3> P_hat = svd.singularValues().asDiagonal();
|
||||
// P_hat(0,0) = btMin((btScalar)P_hat(0,0), (btScalar)20);
|
||||
// P_hat(1,1) = btMin((btScalar)P_hat(1,1), (btScalar)20);
|
||||
// P_hat(2,2) = btMin((btScalar)P_hat(2,2), (btScalar)20);
|
||||
// eigenP = svd.matrixU() * P_hat * svd.matrixV().transpose();
|
||||
// max_p = btMax(max_p, (btScalar)P_hat(0,0));
|
||||
// max_p = btMax(max_p, (btScalar)P_hat(1,1));
|
||||
// max_p = btMax(max_p, (btScalar)P_hat(2,2));
|
||||
//
|
||||
// P[0][0] = eigenP(0,0);
|
||||
// P[0][1] = eigenP(0,1);
|
||||
// P[0][2] = eigenP(0,2);
|
||||
// P[1][0] = eigenP(1,0);
|
||||
// P[1][1] = eigenP(1,1);
|
||||
// P[1][2] = eigenP(1,2);
|
||||
// P[2][0] = eigenP(2,0);
|
||||
// P[2][1] = eigenP(2,1);
|
||||
// P[2][2] = eigenP(2,2);
|
||||
|
||||
|
||||
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
@ -187,6 +236,7 @@ public:
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
// std::cout << max_p << std::endl;
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
@ -212,11 +262,12 @@ public:
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 C = dF + dF.transpose();
|
||||
btMatrix3x3 dP;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
@ -32,6 +32,7 @@ SET(LinearMath_HDRS
|
||||
btIDebugDraw.h
|
||||
btList.h
|
||||
btMatrix3x3.h
|
||||
btImplicitQRSVD.h
|
||||
btMinMax.h
|
||||
btMotionState.h
|
||||
btPolarDecomposition.h
|
||||
|
||||
892
src/LinearMath/btImplicitQRSVD.h
Normal file
892
src/LinearMath/btImplicitQRSVD.h
Normal file
@ -0,0 +1,892 @@
|
||||
/**
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
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 != 0) {
|
||||
btScalar t = btScalar(1.0)/btSqrt(d);
|
||||
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 != 0) {
|
||||
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 != 0) {
|
||||
/*
|
||||
No need to use a tolerance here because x(0) and x(1) always have
|
||||
smaller magnitude then denominator, therefore overflow never happens.
|
||||
*/
|
||||
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 w = btSqrt(tau * tau + y * y);
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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)
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::fabs;
|
||||
using std::copysign;
|
||||
|
||||
btScalar d = (btScalar)0.5 * (a1 - a2);
|
||||
btScalar bs = b1 * b1;
|
||||
|
||||
btScalar mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
|
||||
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
|
||||
return mu;
|
||||
}
|
||||
|
||||
/**
|
||||
\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)
|
||||
{
|
||||
using std::fabs;
|
||||
|
||||
if (t == 0)
|
||||
{
|
||||
// Case: sigma(0) > |sigma(1)| >= |sigma(2)|
|
||||
if (fabs(sigma[1]) >= fabs(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 (fabs(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 (fabs(sigma[1]) < fabs(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;
|
||||
using std::sqrt;
|
||||
using std::max;
|
||||
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;
|
||||
tol *= max((btScalar)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1);
|
||||
|
||||
/**
|
||||
Do implicit shift QR until A^T A is block diagonal
|
||||
*/
|
||||
|
||||
while (fabs(beta_2) > tol && fabs(beta_1) > tol
|
||||
&& fabs(alpha_1) > tol && fabs(alpha_2) > tol
|
||||
&& fabs(alpha_3) > tol) {
|
||||
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 (fabs(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 (fabs(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 (fabs(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 (fabs(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 (fabs(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 */
|
||||
Loading…
Reference in New Issue
Block a user