42#ifndef btImplicitQRSVD_h
43#define btImplicitQRSVD_h
211 for (
int j = 0;
j < 3;
j++) {
220 for (
int j = 0;
j < 2;
j++) {
237 for (
int j = 0;
j < 3;
j++) {
246 for (
int j = 0;
j < 2;
j++) {
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]);
308 r2.compute(
H[0][1],
H[0][2]);
313 r2.columnRotation(
H);
314 r2.columnRotation(
V);
328 r1.columnRotation(
U);
329 r3.columnRotation(
U);
385 r1.columnRotation(
H);
386 r1.columnRotation(
V);
395 r1.computeUnconventional(
H[1][2],
H[2][2]);
397 r1.columnRotation(
U);
407 r2.columnRotation(
H);
408 r2.columnRotation(
V);
416 r2.computeUnconventional(
H[0][1],
H[1][1]);
418 r2.columnRotation(
U);
435 btScalar a = (
A(0, 0) +
A(1, 1)), b = (
A(1, 0) -
A(0, 1));
445 R.c = a / denominator;
446 R.s = -b / denominator;
474 const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
554 const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
594 int other = (
t == 1) ? 0 : 2;
597 sigma[other] =
B[other][other];
662 for (
int d = 0; d < 3; ++d)
663 std::swap(
A[d][i],
A[d][
j]);
754 btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
843 r1.computeUnconventional(
B[1][2],
B[2][2]);
845 r1.columnRotation(
U);
864 r1.compute(
B[1][1],
B[1][2]);
865 r1.columnRotation(
B);
866 r1.columnRotation(
V);
874 r2.compute(
B[0][0],
B[0][2]);
875 r2.columnRotation(
B);
876 r2.columnRotation(
V);
895 r1.computeUnconventional(
B[0][1],
B[1][1]);
897 r1.columnRotation(
U);
906 r2.computeUnconventional(
B[0][2],
B[2][2]);
908 r2.columnRotation(
U);
void process(btMatrix3x3 &B, btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V)
Helper function of 3X3 SVD for processing 2X2 SVD.
void zeroChase(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
zero chasing the 3X3 matrix to bidiagonal form original form of H: x x 0 x x x 0 0 x after zero chase...
static btScalar copySign(btScalar x, btScalar y)
btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
compute wilkinsonShift of the block a1 b1 b1 a2 based on the wilkinsonShift formula mu = c + d - sign...
void singularValueDecomposition(const btMatrix2x2 &A, GivensRotation &U, const btMatrix2x2 &Sigma, GivensRotation &V, const btScalar tol=64 *std::numeric_limits< btScalar >::epsilon())
2x2 SVD (singular value decomposition) A=USV'
void swapCol(btMatrix3x3 &A, int i, int j)
void polarDecomposition(const btMatrix2x2 &A, GivensRotation &R, const btMatrix2x2 &S_Sym)
2x2 polar decomposition.
void makeLambdaShape(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
make a 3X3 matrix to lambda shape original form of H: x x x x x x x x x after : x 0 0 x x 0 x 0 x
void sort(btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V, int t)
Helper function of 3X3 SVD for sorting singular values.
void flipSign(int i, btMatrix3x3 &U, btVector3 &sigma)
Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma.
void makeUpperBidiag(btMatrix3x3 &H, btMatrix3x3 &U, btMatrix3x3 &V)
make a 3X3 matrix to upper bidiagonal form original form of H: x x x x x x x x x after zero chase: x ...
const T & btMax(const T &a, const T &b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
btScalar btFabs(btScalar x)
Class for givens rotation.
void fill(const btMatrix2x2 &R) const
GivensRotation(int rowi_in, int rowk_in)
void operator*=(const GivensRotation &A)
Multiply givens must be for same row and column.
void fill(const btMatrix3x3 &R) const
Fill the R with the entries of this rotation.
GivensRotation operator*(const GivensRotation &A) const
Multiply givens must be for same row and column.
void rowRotation(btMatrix3x3 &A) const
This function does something like c -s 0 ( s c 0 ) A -> A 0 0 1 It only affects row i and row k of A.
void columnRotation(btMatrix2x2 &A) const
GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
void computeUnconventional(const btScalar a, const btScalar b)
This function computes c and s so that ( c -s ) ( a ) = ( 0 ) s c b ( * )
void rowRotation(btMatrix2x2 &A) const
void compute(const btScalar a, const btScalar b)
Compute c and s from a and b so that ( c -s ) ( a ) = ( * ) s c b ( 0 )
void columnRotation(btMatrix3x3 &A) const
This function does something like c s 0 A ( -s c 0 ) -> A 0 0 1 It only affects column i and column k...
Bullet Continuous Collision Detection and Physics Library Copyright (c) 2019 Google Inc.
const btScalar & operator()(int i, int j) const
btMatrix2x2(const btMatrix2x2 &other)
btScalar & operator()(int i, int j)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void setIdentity()
Set the matrix to the identity.
btVector3 can be used to represent 3D points and vectors.