Bullet Collision Detection & Physics Library
|
Go to the documentation of this file.
42 #ifndef btImplicitQRSVD_h
43 #define btImplicitQRSVD_h
91 if ((x < 0 && y > 0) || (x > 0 && y < 0))
211 for (
int j = 0; j < 3; j++) {
214 A[
rowi][j] =
c * tau1 -
s * tau2;
215 A[
rowk][j] =
s * tau1 +
c * tau2;
220 for (
int j = 0; j < 2; j++) {
223 A(
rowi,j) =
c * tau1 -
s * tau2;
224 A(
rowk,j) =
s * tau1 +
c * tau2;
237 for (
int j = 0; j < 3; j++) {
240 A[j][
rowi] =
c * tau1 -
s * tau2;
241 A[j][
rowk] =
s * tau1 +
c * tau2;
246 for (
int j = 0; j < 2; j++) {
249 A(j,
rowi) =
c * tau1 -
s * tau2;
250 A(j,
rowk) =
s * tau1 +
c * tau2;
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]);
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())
476 btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
515 btScalar csy = 2 * cosine * sine * y;
517 sigma(0,0) = c2 * x - csy + s2 * z;
518 sigma(1,1) = s2 * x + csy + c2 * z;
530 if (sigma(0,0) < sigma(1,1)) {
531 std::swap(sigma(0,0), sigma(1,1));
554 const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
594 int other = (t == 1) ? 0 : 2;
597 sigma[other] = B[other][other];
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];
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;
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];
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;
647 sigma[i] = -sigma[i];
662 for (
int d = 0; d < 3; ++d)
663 std::swap(A[d][i], A[d][j]);
688 std::swap(sigma[1], sigma[2]);
694 if (sigma[1] > sigma[0]) {
695 std::swap(sigma[0], sigma[1]);
709 if (
btFabs(sigma[0]) >= sigma[1]) {
718 std::swap(sigma[0], sigma[1]);
724 std::swap(sigma[1], sigma[2]);
754 btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
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;
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);
790 r.
compute(alpha_1 * alpha_1 - mu, gamma_1);
801 gamma_1 = alpha_1 * beta_1;
802 gamma_2 = alpha_2 * beta_2;
815 if (
btFabs(beta_2) <= tol) {
816 process<0>(B,
U, sigma, V);
825 else if (
btFabs(beta_1) <= tol) {
826 process<1>(B,
U, sigma, V);
835 else if (
btFabs(alpha_2) <= tol) {
847 process<0>(B,
U, sigma, V);
848 sort(
U, sigma, V, 0);
856 else if (
btFabs(alpha_3) <= tol) {
878 process<0>(B,
U, sigma, V);
879 sort(
U, sigma, V, 0);
887 else if (
btFabs(alpha_1) <= tol) {
910 process<1>(B,
U, sigma, V);
911 sort(
U, sigma, V, 1);
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 operator*=(const GivensRotation &A)
Multiply givens must be for same row and column.
void polarDecomposition(const btMatrix2x2 &A, GivensRotation &R, const btMatrix2x2 &S_Sym)
2x2 polar decomposition.
void sort(btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V, int t)
Helper function of 3X3 SVD for sorting singular values.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
const btScalar & operator()(int i, int j) const
void rowRotation(btMatrix2x2 &A) const
Bullet Continuous Collision Detection and Physics Library Copyright (c) 2019 Google Inc.
GivensRotation(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 process(btMatrix3x3 &B, btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V)
Helper function of 3X3 SVD for processing 2X2 SVD.
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
static btScalar copySign(btScalar x, btScalar y)
void columnRotation(btMatrix2x2 &A) const
const T & btMax(const T &a, const T &b)
void fill(const btMatrix2x2 &R) const
btScalar btFabs(btScalar x)
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'
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btVector3 can be used to represent 3D points and vectors.
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 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...
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 ...
void swapCol(btMatrix3x3 &A, int i, int j)
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...
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.
GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
void setIdentity()
Set the matrix to the identity.
void fill(const btMatrix3x3 &R) const
Fill the R with the entries of this rotation.
Class for givens rotation.
btScalar & operator()(int i, int j)
void flipSign(int i, btMatrix3x3 &U, btVector3 &sigma)
Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma.
btScalar btSqrt(btScalar y)
GivensRotation operator*(const GivensRotation &A) const
Multiply givens must be for same row and column.
btMatrix2x2(const btMatrix2x2 &other)