Program Listing for File MathFunctions.cpp
↰ Return to documentation for file (PositionBasedDynamics/MathFunctions.cpp)
#include "MathFunctions.h"
#include <cfloat>
using namespace PBD;
// MathFunctions
// ----------------------------------------------------------------------------------------------
void MathFunctions::jacobiRotate(Matrix3r &A, Matrix3r &R, int p, int q)
{
// rotates A through phi in pq-plane to set A(p,q) = 0
// rotation stored in R whose columns are eigenvectors of A
if (A(p, q) == 0.0)
return;
Real d = (A(p, p) - A(q, q)) / (static_cast<Real>(2.0)*A(p, q));
Real t = static_cast<Real>(1.0) / (fabs(d) + sqrt(d*d + static_cast<Real>(1.0)));
if (d < 0.0) t = -t;
Real c = static_cast<Real>(1.0) / sqrt(t*t + 1);
Real s = t*c;
A(p, p) += t*A(p, q);
A(q, q) -= t*A(p, q);
A(p, q) = A(q, p) = 0.0;
// transform A
int k;
for (k = 0; k < 3; k++) {
if (k != p && k != q) {
Real Akp = c*A(k, p) + s*A(k, q);
Real Akq = -s*A(k, p) + c*A(k, q);
A(k, p) = A(p, k) = Akp;
A(k, q) = A(q, k) = Akq;
}
}
// store rotation in R
for (k = 0; k < 3; k++) {
Real Rkp = c*R(k, p) + s*R(k, q);
Real Rkq = -s*R(k, p) + c*R(k, q);
R(k, p) = Rkp;
R(k, q) = Rkq;
}
}
// ----------------------------------------------------------------------------------------------
void MathFunctions::eigenDecomposition(const Matrix3r &A, Matrix3r &eigenVecs, Vector3r &eigenVals)
{
const int numJacobiIterations = 10;
const Real epsilon = static_cast<Real>(1e-15);
Matrix3r D = A;
// only for symmetric matrices!
eigenVecs.setIdentity(); // unit matrix
int iter = 0;
while (iter < numJacobiIterations) { // 3 off diagonal elements
// find off diagonal element with maximum modulus
int p, q;
Real a, max;
max = fabs(D(0, 1));
p = 0; q = 1;
a = fabs(D(0, 2));
if (a > max) { p = 0; q = 2; max = a; }
a = fabs(D(1, 2));
if (a > max) { p = 1; q = 2; max = a; }
// all small enough -> done
if (max < epsilon) break;
// rotate matrix with respect to that element
jacobiRotate(D, eigenVecs, p, q);
iter++;
}
eigenVals[0] = D(0, 0);
eigenVals[1] = D(1, 1);
eigenVals[2] = D(2, 2);
}
void MathFunctions::polarDecomposition(const Matrix3r &A, Matrix3r &R, Matrix3r &U, Matrix3r &D)
{
// A = SR, where S is symmetric and R is orthonormal
// -> S = (A A^T)^(1/2)
// A = U D U^T R
Matrix3r AAT;
AAT(0, 0) = A(0, 0)*A(0, 0) + A(0, 1)*A(0, 1) + A(0, 2)*A(0, 2);
AAT(1, 1) = A(1, 0)*A(1, 0) + A(1, 1)*A(1, 1) + A(1, 2)*A(1, 2);
AAT(2, 2) = A(2, 0)*A(2, 0) + A(2, 1)*A(2, 1) + A(2, 2)*A(2, 2);
AAT(0, 1) = A(0, 0)*A(1, 0) + A(0, 1)*A(1, 1) + A(0, 2)*A(1, 2);
AAT(0, 2) = A(0, 0)*A(2, 0) + A(0, 1)*A(2, 1) + A(0, 2)*A(2, 2);
AAT(1, 2) = A(1, 0)*A(2, 0) + A(1, 1)*A(2, 1) + A(1, 2)*A(2, 2);
AAT(1, 0) = AAT(0, 1);
AAT(2, 0) = AAT(0, 2);
AAT(2, 1) = AAT(1, 2);
R.setIdentity();
Vector3r eigenVals;
eigenDecomposition(AAT, U, eigenVals);
Real d0 = sqrt(eigenVals[0]);
Real d1 = sqrt(eigenVals[1]);
Real d2 = sqrt(eigenVals[2]);
D.setZero();
D(0, 0) = d0;
D(1, 1) = d1;
D(2, 2) = d2;
const Real eps = static_cast<Real>(1e-15);
Real l0 = eigenVals[0]; if (l0 <= eps) l0 = 0.0; else l0 = static_cast<Real>(1.0) / d0;
Real l1 = eigenVals[1]; if (l1 <= eps) l1 = 0.0; else l1 = static_cast<Real>(1.0) / d1;
Real l2 = eigenVals[2]; if (l2 <= eps) l2 = 0.0; else l2 = static_cast<Real>(1.0) / d2;
Matrix3r S1;
S1(0, 0) = l0*U(0, 0)*U(0, 0) + l1*U(0, 1)*U(0, 1) + l2*U(0, 2)*U(0, 2);
S1(1, 1) = l0*U(1, 0)*U(1, 0) + l1*U(1, 1)*U(1, 1) + l2*U(1, 2)*U(1, 2);
S1(2, 2) = l0*U(2, 0)*U(2, 0) + l1*U(2, 1)*U(2, 1) + l2*U(2, 2)*U(2, 2);
S1(0, 1) = l0*U(0, 0)*U(1, 0) + l1*U(0, 1)*U(1, 1) + l2*U(0, 2)*U(1, 2);
S1(0, 2) = l0*U(0, 0)*U(2, 0) + l1*U(0, 1)*U(2, 1) + l2*U(0, 2)*U(2, 2);
S1(1, 2) = l0*U(1, 0)*U(2, 0) + l1*U(1, 1)*U(2, 1) + l2*U(1, 2)*U(2, 2);
S1(1, 0) = S1(0, 1);
S1(2, 0) = S1(0, 2);
S1(2, 1) = S1(1, 2);
R = S1*A;
// stabilize
Vector3r c0, c1, c2;
c0 = R.col(0);
c1 = R.col(1);
c2 = R.col(2);
if (c0.squaredNorm() < eps)
c0 = c1.cross(c2);
else if (c1.squaredNorm() < eps)
c1 = c2.cross(c0);
else
c2 = c0.cross(c1);
R.col(0) = c0;
R.col(1) = c1;
R.col(2) = c2;
}
Real MathFunctions::oneNorm(const Matrix3r &A)
{
const Real sum1 = fabs(A(0,0)) + fabs(A(1,0)) + fabs(A(2,0));
const Real sum2 = fabs(A(0,1)) + fabs(A(1,1)) + fabs(A(2,1));
const Real sum3 = fabs(A(0,2)) + fabs(A(1,2)) + fabs(A(2,2));
Real maxSum = sum1;
if (sum2 > maxSum)
maxSum = sum2;
if (sum3 > maxSum)
maxSum = sum3;
return maxSum;
}
Real MathFunctions::infNorm(const Matrix3r &A)
{
const Real sum1 = fabs(A(0, 0)) + fabs(A(0, 1)) + fabs(A(0, 2));
const Real sum2 = fabs(A(1, 0)) + fabs(A(1, 1)) + fabs(A(1, 2));
const Real sum3 = fabs(A(2, 0)) + fabs(A(2, 1)) + fabs(A(2, 2));
Real maxSum = sum1;
if (sum2 > maxSum)
maxSum = sum2;
if (sum3 > maxSum)
maxSum = sum3;
return maxSum;
}
void MathFunctions::polarDecompositionStable(const Matrix3r &M, const Real tolerance, Matrix3r &R)
{
Matrix3r Mt = M.transpose();
Real Mone = oneNorm(M);
Real Minf = infNorm(M);
Real Eone;
Matrix3r MadjTt, Et;
do
{
MadjTt.row(0) = Mt.row(1).cross(Mt.row(2));
MadjTt.row(1) = Mt.row(2).cross(Mt.row(0));
MadjTt.row(2) = Mt.row(0).cross(Mt.row(1));
Real det = Mt(0,0) * MadjTt(0,0) + Mt(0,1) * MadjTt(0,1) + Mt(0,2) * MadjTt(0,2);
if (fabs(det) < 1.0e-12)
{
Vector3r len;
unsigned int index = 0xffffffff;
for (unsigned int i = 0; i < 3; i++)
{
len[i] = MadjTt.row(i).squaredNorm();
if (len[i] > 1.0e-12)
{
// index of valid cross product
// => is also the index of the vector in Mt that must be exchanged
index = i;
break;
}
}
if (index == 0xffffffff)
{
R.setIdentity();
return;
}
else
{
Mt.row(index) = Mt.row((index + 1) % 3).cross(Mt.row((index + 2) % 3));
MadjTt.row((index + 1) % 3) = Mt.row((index + 2) % 3).cross(Mt.row(index));
MadjTt.row((index + 2) % 3) = Mt.row(index).cross(Mt.row((index + 1) % 3));
Matrix3r M2 = Mt.transpose();
Mone = oneNorm(M2);
Minf = infNorm(M2);
det = Mt(0,0) * MadjTt(0,0) + Mt(0,1) * MadjTt(0,1) + Mt(0,2) * MadjTt(0,2);
}
}
const Real MadjTone = oneNorm(MadjTt);
const Real MadjTinf = infNorm(MadjTt);
const Real gamma = sqrt(sqrt((MadjTone*MadjTinf) / (Mone*Minf)) / fabs(det));
const Real g1 = gamma* static_cast<Real>(0.5);
const Real g2 = static_cast<Real>(0.5) / (gamma*det);
for (unsigned char i = 0; i < 3; i++)
{
for (unsigned char j = 0; j < 3; j++)
{
Et(i,j) = Mt(i,j);
Mt(i,j) = g1*Mt(i,j) + g2*MadjTt(i,j);
Et(i,j) -= Mt(i,j);
}
}
Eone = oneNorm(Et);
Mone = oneNorm(Mt);
Minf = infNorm(Mt);
} while (Eone > Mone * tolerance);
// Q = Mt^T
R = Mt.transpose();
}
void MathFunctions::svdWithInversionHandling(const Matrix3r &A, Vector3r &sigma, Matrix3r &U, Matrix3r &VT)
{
Matrix3r AT_A, V;
AT_A = A.transpose() * A;
Vector3r S;
// Eigen decomposition of A^T * A
eigenDecomposition(AT_A, V, S);
// Detect if V is a reflection .
// Make a rotation out of it by multiplying one column with -1.
const Real detV = V.determinant();
if (detV < 0.0)
{
Real minLambda = REAL_MAX;
unsigned char pos = 0;
for (unsigned char l = 0; l < 3; l++)
{
if (S[l] < minLambda)
{
pos = l;
minLambda = S[l];
}
}
V(0, pos) = -V(0, pos);
V(1, pos) = -V(1, pos);
V(2, pos) = -V(2, pos);
}
if (S[0] < 0.0) S[0] = 0.0; // safety for sqrt
if (S[1] < 0.0) S[1] = 0.0;
if (S[2] < 0.0) S[2] = 0.0;
sigma[0] = sqrt(S[0]);
sigma[1] = sqrt(S[1]);
sigma[2] = sqrt(S[2]);
VT = V.transpose();
//
// Check for values of hatF near zero
//
unsigned char chk = 0;
unsigned char pos = 0;
for (unsigned char l = 0; l < 3; l++)
{
if (fabs(sigma[l]) < 1.0e-4)
{
pos = l;
chk++;
}
}
if (chk > 0)
{
if (chk > 1)
{
U.setIdentity();
}
else
{
U = A * V;
for (unsigned char l = 0; l < 3; l++)
{
if (l != pos)
{
for (unsigned char m = 0; m < 3; m++)
{
U(m, l) *= static_cast<Real>(1.0) / sigma[l];
}
}
}
Vector3r v[2];
unsigned char index = 0;
for (unsigned char l = 0; l < 3; l++)
{
if (l != pos)
{
v[index++] = Vector3r(U(0, l), U(1, l), U(2, l));
}
}
Vector3r vec = v[0].cross(v[1]);
vec.normalize();
U(0, pos) = vec[0];
U(1, pos) = vec[1];
U(2, pos) = vec[2];
}
}
else
{
Vector3r sigmaInv(static_cast<Real>(1.0) / sigma[0], static_cast<Real>(1.0) / sigma[1], static_cast<Real>(1.0) / sigma[2]);
U = A * V;
for (unsigned char l = 0; l < 3; l++)
{
for (unsigned char m = 0; m < 3; m++)
{
U(m, l) *= sigmaInv[l];
}
}
}
const Real detU = U.determinant();
// U is a reflection => inversion
if (detU < 0.0)
{
//std::cout << "Inversion!\n";
Real minLambda = REAL_MAX;
unsigned char pos = 0;
for (unsigned char l = 0; l < 3; l++)
{
if (sigma[l] < minLambda)
{
pos = l;
minLambda = sigma[l];
}
}
// invert values of smallest singular value
sigma[pos] = -sigma[pos];
U(0, pos) = -U(0, pos);
U(1, pos) = -U(1, pos);
U(2, pos) = -U(2, pos);
}
}
// ----------------------------------------------------------------------------------------------
Real MathFunctions::cotTheta(const Vector3r &v, const Vector3r &w)
{
const Real cosTheta = v.dot(w);
const Real sinTheta = (v.cross(w)).norm();
return (cosTheta / sinTheta);
}
// ----------------------------------------------------------------------------------------------
void MathFunctions::crossProductMatrix(const Vector3r &v, Matrix3r &v_hat)
{
v_hat << 0, -v(2), v(1),
v(2), 0, -v(0),
-v(1), v(0), 0;
}
// ----------------------------------------------------------------------------------------------
void MathFunctions::extractRotation(const Matrix3r &A, Quaternionr &q, const unsigned int maxIter)
{
for (unsigned int iter = 0; iter < maxIter; iter++)
{
Matrix3r R = q.matrix();
Vector3r omega = (R.col(0).cross(A.col(0)) + R.col(1).cross(A.col(1)) + R.col(2).cross(A.col(2))) *
(1.0 / fabs(R.col(0).dot(A.col(0)) + R.col(1).dot(A.col(1)) + R.col(2).dot(A.col(2)) + 1.0e-9));
Real w = omega.norm();
if (w < 1.0e-9)
break;
q = Quaternionr(AngleAxisr(w, (1.0 / w) * omega)) * q;
q.normalize();
}
}