Skip to content
Snippets Groups Projects
Commit be93c8bd authored by Lisa Julia Nebel's avatar Lisa Julia Nebel
Browse files

Move the polar decomposition to a separate file and add the algorithm by... 

Move the polar decomposition to a separate file and add the algorithm by Higham and Noferini (from Robin Fraenzel)

In the test for the Higham and Noferini algorithm, we see:
Unfortunately, for matrices that are close to an orthogonal matrix, this algorithm is
about 2x slower. One can benefit from the Higham and Noferini algorithm only if the
matrix is quite far away from an orthogonal one.
parent bbad25b9
No related branches found
No related tags found
1 merge request!59Correct the iterative calculation of the polar decomposition and its derivative
Hide whitespace changes
Inline Side-by-side
dune/gfe/localprojectedfefunction.hh
+ 2
− 24
View file @ be93c8bd
......@@ -10,6 +10,7 @@
#include <dune/gfe/rotation.hh>
#include <dune/gfe/rigidbodymotion.hh>
#include <dune/gfe/linearalgebra.hh>
#include <dune/gfe/polardecomposition.hh>
namespace Dune {
......@@ -174,29 +175,6 @@ namespace Dune {
static const int spaceDim = TargetSpace::TangentVector::dimension;
static FieldMatrix<field_type,3,3> polarFactor(const FieldMatrix<field_type,3,3>& matrix)
{
int maxIterations = 100;
double tol = 0.001;
// Use Higham's method
auto polar = matrix;
for (size_t i=0; i<maxIterations; i++)
{
auto oldPolar = polar;
auto polarInvert = polar;
polarInvert.invert();
for (size_t j=0; j<polar.N(); j++)
for (size_t k=0; k<polar.M(); k++)
polar[j][k] = 0.5 * (polar[j][k] + polarInvert[k][j]);
oldPolar -= polar;
if (oldPolar.frobenius_norm() < tol) {
break;
}
}
return polar;
}
/**
* \param A The argument of the projection
* \param polar The image of the projection, i.e., the polar factor of A
......@@ -309,7 +287,7 @@ namespace Dune {
}
// Project back onto SO(3)
result.set(polarFactor(interpolatedMatrix));
result.set(Dune::GFE::PolarDecomposition()(interpolatedMatrix));
return result;
}
......
dune/gfe/polardecomposition.hh 0 → 100644
+ 326
− 0
View file @ be93c8bd
// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
// vi: set et ts=4 sw=2 sts=2:
#ifndef DUNE_GFE_POLARDECOMPOSITION_HH
#define DUNE_GFE_POLARDECOMPOSITION_HH
#include <dune/common/fmatrix.hh>
#include <dune/common/version.hh>
#include <dune/common/exceptions.hh>
#include <dune/gfe/rotation.hh>
#include <dune/gfe/linearalgebra.hh>
///////////////////////////////////////////////////////////////////////////////////////////
// Two Methods to compute the Polar Factor of a 3x3 matrix
///////////////////////////////////////////////////////////////////////////////////////////
namespace Dune::GFE {
class PolarDecomposition
{
public:
/** \brief Compute the polar factor of a matrix iteratively
Attention: For matrices that are quite far away from an orthogonal matrix,
this might return a matrix with determinant = -1 ! */
template <class field_type>
FieldMatrix<field_type,3,3> operator() (const FieldMatrix<field_type,3,3>& matrix, double tol = 0.001) const
{
int maxIterations = 100;
// Use Higham's method
auto polar = matrix;
for (size_t i=0; i<maxIterations; i++)
{
auto oldPolar = polar;
auto polarInvert = polar;
polarInvert.invert();
for (size_t j=0; j<polar.N(); j++)
for (size_t k=0; k<polar.M(); k++)
polar[j][k] = 0.5 * (polar[j][k] + polarInvert[k][j]);
oldPolar -= polar;
if (oldPolar.frobenius_norm() < tol) {
break;
}
}
return polar;
}
};
class HighamNoferiniPolarDecomposition
{
public:
/** \brief Compute the polar factor part of a matrix using the paper
"An algorithm to compute the polar decomposition of a 3 × 3 matrix" from Higham and Noferini (2015)
* \param matrix Compute the polar factor part of this matrix
* \param tau1 tolerance, default value taken from Higham and Noferini
* \param tau2 tolerance, default value taken from Higham and Noferini
*/
template <class field_type>
FieldMatrix<field_type, 3, 3> operator() (const FieldMatrix<field_type, 3, 3>& matrix, double tau1 = 0.0001, double tau2 = 0.0001) const
{
auto normedMatrix = matrix;
normedMatrix /= normedMatrix.frobenius_norm();
FieldMatrix<field_type,4,4> bilinearmatrix = bilinearMatrix(normedMatrix);
auto detB = determinantLaplace(bilinearmatrix);
auto detM = normedMatrix.determinant();
field_type domEV = 0;
if (detM < 0) {
detM = -detM;
bilinearmatrix = -bilinearmatrix;
}
if ( detB + 1./3 > tau1) { // estimate dominant eigenvalue if well separated => use formula
domEV = dominantEV( detB, detM );
} else { // eignevalue nearly a double root => use newtons method
auto coefficients = characteristicCoefficients(bilinearmatrix);
domEV = dominantEVNewton(coefficients,detB);
}
// compute iterationmatrix B_S
FieldMatrix<field_type,4,4> BS = {{domEV,0,0,0},{0,domEV,0,0},{0,0,domEV,0},{0,0,0,domEV}};
BS -= bilinearmatrix;
if (detB < 1 - tau2) {
Rotation<field_type,3> v(obtainQuaternion(BS));
FieldMatrix<field_type,3,3> mat;
v.matrix(mat);
return mat;
} else {
DUNE_THROW(NotImplemented, "The eigenvalues are not wellseparated => inverse iteration won't converge!");
}
}
protected:
/** \brief Compute the bilinear matrix for a given 3x3 matrix*/
template <class field_type>
static FieldMatrix<field_type,4,4> bilinearMatrix(const FieldMatrix<field_type,3,3> matrix)
{
FieldMatrix<field_type,4,4> bilinearmatrix;
bilinearmatrix[0][0] = matrix[0][0] + matrix[1][1] + matrix[2][2];
bilinearmatrix[0][1] = matrix[1][2] - matrix[2][1];
bilinearmatrix[0][2] = matrix[2][0] - matrix[0][2];
bilinearmatrix[0][3] = matrix[0][1] - matrix[1][0];
bilinearmatrix[1][0] = bilinearmatrix[0][1];
bilinearmatrix[1][1] = matrix[0][0] - matrix[1][1] - matrix[2][2];
bilinearmatrix[1][2] = matrix[0][1] + matrix[1][0];
bilinearmatrix[1][3] = matrix[2][0] + matrix[0][2];
bilinearmatrix[2][0] = bilinearmatrix[0][2];
bilinearmatrix[2][1] = bilinearmatrix[1][2];
bilinearmatrix[2][2] = matrix[1][1] - matrix[0][0] - matrix[2][2];
bilinearmatrix[2][3] = matrix[1][2] + matrix[2][1];
bilinearmatrix[3][0] = bilinearmatrix[0][3];
bilinearmatrix[3][1] = bilinearmatrix[1][3];
bilinearmatrix[3][2] = bilinearmatrix[2][3];
bilinearmatrix[3][3] = matrix[2][2] - matrix[0][0] - matrix[1][1];
return bilinearmatrix;
}
/** \brief Compute the determinant of a 4x4 matrix using the Laplace method
The implementation is faster than calling matrix.determinant()*/
template <class field_type>
static field_type determinantLaplace( const FieldMatrix<field_type,4,4>& matrix)
{
field_type T2233 = matrix[2][2] * matrix[3][3];
field_type T3223 = matrix[3][2] * matrix[2][3];
field_type T1 = T2233 - T3223;
field_type T1233 = matrix[1][2] * matrix[3][3];
field_type T3213 = matrix[3][2] * matrix[1][3];
field_type T2 = T1233 - T3213;
field_type T1223 = matrix[1][2] * matrix[2][3];
field_type T2213 = matrix[2][2] * matrix[1][3];
field_type T3 = T1223 - T2213;
field_type T0233 = matrix[0][2] * matrix[3][3];
field_type T3203 = matrix[3][2] * matrix[0][3];
field_type T4 = T3203 - T0233;
field_type T0223 = matrix[0][2] * matrix[2][3];
field_type T2203 = matrix[2][2] * matrix[0][3];
field_type T5 = T0223- T2203;
field_type T0213 = matrix[0][2] * matrix[1][3];
field_type T1203 = matrix[1][2] * matrix[0][3];
field_type T6 = T0213-T1203;
return matrix[0][0] * (matrix[1][1] * T1 - matrix[2][1] * T3 + matrix[3][1] * T3)
- matrix[1][0] * (matrix[0][1] * T1 + matrix[2][1] * T4 + matrix[3][1] * T5)
+ matrix[2][0] * (matrix[0][1] * T2 + matrix[1][1] * T4 + matrix[3][1] * T6)
- matrix[3][0] * (matrix[0][1] * T2 - matrix[1][1] * T5 + matrix[2][1] * T6);
}
/** \brief Return the factors a,b,c of the characteristic polynomial x^4+a*x^3+b*x^2+c*x + detB */
template <class field_type>
static FieldVector<field_type,3> characteristicCoefficients(const FieldMatrix<field_type,4,4>& matrix)
{
field_type a = - Dune::GFE::trace(matrix);
field_type b = matrix[0][0] * (matrix[1][1] + matrix[2][2] + matrix[3][3])
+ matrix[3][3] * (matrix[1][1] + matrix[2][2])
- (matrix[1][0] * matrix[0][1] + matrix[2][0] * matrix[0][2] +
matrix[3][0] * matrix[0][3] + matrix[1][2] * matrix[2][1] +
matrix[1][3] * matrix[3][1] + matrix[3][2] * matrix[2][3]);
field_type c = matrix[0][0] * (matrix[1][2] * matrix[2][1] + matrix[1][3] * matrix[3][1] + matrix[2][3] * matrix[3][2] -(matrix[2][2] * matrix[3][3] + matrix[1][1] * (matrix[2][2] + matrix[3][3])))
+ matrix[0][1] * (matrix[1][0] * (matrix[2][2] + matrix[3][3]) -(matrix[1][2] * matrix[2][0] + matrix[1][3] * matrix[3][0]))
+ matrix[0][2] * (matrix[2][0] * (matrix[1][1] + matrix[3][3]) -(matrix[1][0] * matrix[2][1] + matrix[2][3] * matrix[3][0]))
+ matrix[0][3] * (matrix[3][0] * (matrix[1][1] + matrix[2][2]) -(matrix[1][0] * matrix[3][1] + matrix[2][0] * matrix[3][2]))
+ matrix[1][1] * (matrix[2][3] * matrix[3][2] - matrix[2][2] * matrix[3][3])
+ matrix[1][2] * (matrix[2][1] * matrix[3][3] - matrix[2][3] * matrix[3][1])
+ matrix[1][3] * (matrix[2][2] * matrix[3][1] - matrix[2][1] * matrix[3][2]);
return {a, b, c};
}
/** \brief Return the dominant Eigenvalue of the matrix B */
template <class field_type>
static field_type dominantEV(field_type detB, field_type detM )
{
auto c = 8 * detM;
auto sigma0 = 1 + 3 * detB;
auto sigma1 = 27/16*c*c + 9 * detB - 1;
auto alpha = sigma1/std::pow(sigma0, 1.5 );
auto z = 4/3 * (1 + std::sqrt(sigma0) * std::cos(std::acos(alpha)/3));
auto s = std::sqrt(z)/ 2;
auto x = 4-z+c/s;
if (x > 0)
return s + std::sqrt(x)/2;
else
return s;
}
/** \brief Return the dominant Eigenvalue of the matrix B.
This algorithm corresponds to Algorithm 3.4 in
"An algorithm to compute the polar decomposition
of a 3 × 3 matrix" from Higham and Noferini (2015)
* \param coefficients Coefficients of the characteristic polynomial for the matrix b
* \param detB Determinant of the matrix B
* \param tol Tolerance for the Newton method, the value is taken from the paper by Higham and Noferini
*/
template <class field_type>
static field_type dominantEVNewton(const FieldVector<field_type,3>& coefficients, field_type detB, double tol = 10e-5)
{
field_type charPol = 0;
field_type charPolDeriv = 0;
field_type lambdaOld = 3;
field_type domEV = std::sqrt(3);
while ((lambdaOld - domEV)/domEV > tol) {
lambdaOld = domEV;
charPol = detB + domEV*( coefficients[2] + domEV*( coefficients[1] + domEV*( coefficients[0]+ domEV ) ) );
charPolDeriv= coefficients[2] + domEV * ( 2*coefficients[1]+ domEV * ( 3*coefficients[0] + 4*domEV) );
domEV = domEV- charPol / charPolDeriv;
}
return domEV;
}
/** \brief Return quaternion vector resulting from step 7 and 8 in Algorithm 3.2 from Higham and Noferini (2015)
Step 7: Calculate the LDLT factorization of the given matrix with diagonal pivoting
Step 8: Using L and the pivotmatix, calculate the vector v
* \param shiftedB shifted matrix B as given in chapter 3 of Higham an Noferini: shiftedB = lambda * Id - B, where lambda is the dominant eigenvalue of B
\returns v the respective polar factor in quaternion form
*\
*/
template <class field_type>
static FieldVector<field_type,4> obtainQuaternion(const FieldMatrix<field_type,4,4> shiftedB)
{
FieldMatrix<field_type,4,4> P = 0;
FieldMatrix<field_type,4,4> Pleft = 0;
FieldMatrix<field_type,4,4> Pright = 0;
FieldMatrix<field_type,4,4> L = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
FieldVector<field_type,4> D = {0,0,0,0};
FieldVector<field_type,4> Bdiag = {shiftedB[0][0], shiftedB[1][1], shiftedB[2][2], shiftedB[3][3]};
int maxi = 0; // Index of the maximal element
int secmax = 0;
int secmin = 0;
int mini = 0; // Index of the minimal element
// Find Pivotmatrix
for (int i = 0; i < 4; ++i) // going through the diagonal searching for the maximum
if ( Bdiag[maxi] < Bdiag[i] ) // found a bigger one but smaler than the older ones
maxi = i;
Pleft[0][maxi] = 1;
Pright[maxi][0] = 1; // Largest element to the first position
for (int i = 0; i < 4; ++i) // going through the diagonal searching for the minimum
if ( Bdiag[mini] > Bdiag[i] ) // found a smaler one but smaler than the older ones
mini = i;
Pleft[3][mini] = 1;
Pright[mini][3] = 1; // Smalest element to the last position
for (int i = 0; i < 4; ++i) {
if ( i != maxi & i != mini ) {
for (int j = 0; j < 4; ++j) {
if ( j != maxi & j != mini & j != i ) {
if ( Bdiag[i] < Bdiag[j] ) {
Pleft[1][j] = 1; // Second largest element at the second position
Pright[j][1] = 1;
Pleft[2][i] = 1; // Third largest element at the third position
Pright[i][2] = 1;
secmin = i;
secmax = j;
} else {
Pleft[2][j] = 1;
Pright[j][2] = 1;
Pleft[1][i] = 1;
Pright[i][1] = 1;
secmin = j;
secmax = i;
}
}
}
}
}
//P = Pleft*shiftedB*Pright;
P[maxi][maxi] = shiftedB[0][0];
P[maxi][secmax] = shiftedB[0][1];
P[maxi][secmin] = shiftedB[0][2];
P[maxi][mini] = shiftedB[0][3];
P[secmax][maxi] = shiftedB[1][0];
P[secmax][secmax] = shiftedB[1][1];
P[secmax][secmin] = shiftedB[1][2];
P[secmax][mini] = shiftedB[1][3];
P[secmin][maxi] = shiftedB[2][0];
P[secmin][secmax] = shiftedB[2][1];
P[secmin][secmin] = shiftedB[2][2];
P[secmin][mini] = shiftedB[2][3];
P[mini][maxi] = shiftedB[3][0];
P[mini][secmax] = shiftedB[3][1];
P[mini][secmin] = shiftedB[3][2];
P[mini][mini] = shiftedB[3][3];
// Choleskydecomposition
for (int k = 0; k < 4; ++k) { //columns
D[k] = P[k][k];
for (int j = 0; j < k; ++j)//sum
D[k] -= L[k][j]*L[k][j] * D[j];
for (int i = k+1; i < 4; ++i) { //rows
L[i][k] = P[i][k];
for (int j = 0; j < k; ++j)//sum
L[i][k] -= L[i][j]*L[k][j] * D[j];
L[i][k] /= D[k];
}
}
FieldVector<field_type,4> v = {0,0,0,0};
FieldVector<field_type,4> unnormed = {0,0,0,1};
auto neg = -L[3][2];
unnormed[0] = L[1][0] * ( L[3][1] + L[2][1]*neg ) + L[2][0] * L[3][2] - L[3][0];
unnormed[1] = L[2][1] * L[3][2] - L[3][1];
unnormed[2] = neg;
unnormed /= unnormed.two_norm();
v[0] = unnormed[maxi];
v[1] = unnormed[secmax];
v[2] = unnormed[secmin];
v[3] = unnormed[mini];
return v;
}
};
}
#endif
\ No newline at end of file
test/CMakeLists.txt
+ 1
− 0
View file @ be93c8bd
......@@ -8,6 +8,7 @@ set(TESTS
localgfetestfunctiontest
localprojectedfefunctiontest
orthogonalmatrixtest
polardecompositiontest
rigidbodymotiontest
rotationtest
svdtest
......
test/polardecompositiontest.cc 0 → 100644
+ 243
− 0
View file @ be93c8bd
// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
// vi: set et ts=4 sw=2 sts=2:
#include <dune/common/fmatrix.hh>
#include <dune/common/fvector.hh>
#include <dune/gfe/polardecomposition.hh>
#include <dune/gfe/rotation.hh>
#include <chrono>
#include <fstream>
#include <random>
using namespace Dune;
using field_type = double;
class PolarDecompositionTest : public Dune::GFE::HighamNoferiniPolarDecomposition {
public:
using HighamNoferiniPolarDecomposition::HighamNoferiniPolarDecomposition;
using HighamNoferiniPolarDecomposition::bilinearMatrix;
using HighamNoferiniPolarDecomposition::determinantLaplace;
using HighamNoferiniPolarDecomposition::dominantEVNewton;
using HighamNoferiniPolarDecomposition::characteristicCoefficients;
using HighamNoferiniPolarDecomposition::obtainQuaternion;
};
/** \brief Check if the matrix is not orthogonal */
static bool isNotOrthogonal(const FieldMatrix<field_type,3,3>& matrix) {
bool notOrthogonal = std::abs(1-matrix.determinant()) > 1e-12;
notOrthogonal = notOrthogonal or (( matrix[0]*matrix[1] ) > 1e-12);
notOrthogonal = notOrthogonal or (( matrix[0]*matrix[2] ) > 1e-12);
notOrthogonal = notOrthogonal or (( matrix[1]*matrix[2] ) > 1e-12);
return notOrthogonal;
}
/** \brief Return the time needed for 1000 polar decompositions of the given matrix using the iterative algorithm
and the algorithm by Higham. */
static FieldVector<field_type,2> measureTimeForPolarDecomposition(const FieldMatrix<field_type, 3, 3>& matrix, double tol = 10e-3)
{
FieldMatrix<field_type, 3, 3> Q1;
FieldMatrix<field_type, 3, 3> Q2;
FieldVector<field_type,2> actualtime;
std::chrono::steady_clock::time_point begindecomold = std::chrono::steady_clock::now();
for (int i = 0; i < 1000; ++i) {
Q1 = Dune::GFE::PolarDecomposition()(matrix, tol);
}
std::chrono::steady_clock::time_point enddecomold = std::chrono::steady_clock::now();
actualtime[0] = (enddecomold - begindecomold).count();
std::chrono::steady_clock::time_point begindecom = std::chrono::steady_clock::now();
for (int i = 0; i < 1000; ++i) {
Q2 = Dune::GFE::HighamNoferiniPolarDecomposition()(matrix, tol, tol);
}
std::chrono::steady_clock::time_point enddecom = std::chrono::steady_clock::now();
actualtime[1] = (enddecom - begindecom).count();
return actualtime;
}
/** \brief Returns a 3x3-Matrix with random entries between 0 and upper Bound*/
static FieldMatrix<field_type, 3, 3> randomMatrix3d(double upperBound = 1.0)
{
const int dim = 3;
FieldMatrix<field_type, dim, dim> matrix;
std::random_device rd; // Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
std::uniform_real_distribution<> dis(0.0, upperBound); // equally distributed between 0 and upper bound
for (int i = 0; i < dim; ++i)
for (int j = 0; j < dim; ++j)
matrix[i][j] = dis(gen);
return matrix;
}
/** \brief Returns an "almost orthogonal" 3x3-Matrix where a random perturbation
between 0 and maxPerturbation is added to each entry.*/
static FieldMatrix<field_type, 3, 3> randomMatrixAlmostOrthogonal(double maxPerturbation = 0.1)
{
const int dim = 3;
FieldVector<field_type,4> f;
std::random_device rd; // Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
std::uniform_real_distribution<> dis(0.0, 1.0); // equally distributed between 0 and upper bound
for (int i = 0; i < 4; ++i)
f[i] = dis(gen);
Rotation<field_type,3> q(f);
q.normalize();
assert(std::abs(1-q.two_norm()) < 1e-12);
// Turn it into a matrix
FieldMatrix<double,3,3> matrix;
q.matrix(matrix);
if (isNotOrthogonal(matrix))
DUNE_THROW(Exception, "Expected the matrix " << matrix << " to be orthogonal, but it is not!");
//Now perturb this a little bit
for (int i = 0; i < dim; ++i)
for (int j = 0; j < dim; ++j)
matrix[i][j] += dis(gen)*maxPerturbation;
return matrix;
}
static double timeTest(double perturbationFromSO3 = 1.0) {
// Define matrices we want to use for testing
int numberOfTests = 100;
std::vector<double> timeOld(numberOfTests);
std::vector<double> timeNew(numberOfTests);
double totalTimeOld = 0;
double totalTimeNew = 0;
for (int j = 0; j < numberOfTests; ++j) { // testing loop
FieldMatrix<field_type,3,3> N;
// Only measure the time if the decomposition is unique and if both algorithms will retun an orthogonal matrix!
// Attention: For matrices that are quite far away from an orthogonal matrix, Dune::GFE::PolarDecomposition() might return a matrix with determinant = -1 !
double normOfDifference = 10;
FieldMatrix<field_type,3,3> Q1;
FieldMatrix<field_type,3,3> Q2;
while(isNotOrthogonal(Q1) or isNotOrthogonal(Q2) or normOfDifference > 0.001) {
N = randomMatrixAlmostOrthogonal(perturbationFromSO3);
Q1 = PolarDecompositionTest()(N);
Q2 = Dune::GFE::PolarDecomposition()(N);
normOfDifference = (Q1-Q2).frobenius_norm();
}
auto actualtime = measureTimeForPolarDecomposition(N);
timeOld[j] = actualtime[0];
timeNew[j] = actualtime[1];
}
std::sort(timeOld.begin(), timeOld.end());
std::sort(timeNew.begin(), timeNew.end());
for (int j = 0; j < numberOfTests; ++j) {
totalTimeOld += timeOld[j];
totalTimeNew += timeNew[j];
}
std::cout << "Perturbation from an orthogonal matrix: " << perturbationFromSO3 << std::endl;
std::cout << "Average (old): " << totalTimeOld/numberOfTests << "[ns]" << std::endl;
std::cout << "Average (new): " << totalTimeNew/numberOfTests << "[ns]" << std::endl;
std::cout << "Relative: " << totalTimeOld/totalTimeNew << std::endl << std::endl;
return totalTimeOld/totalTimeNew;
}
static bool testBilinearMatrix() {
FieldMatrix<field_type,3,3> M = { { 20, 0, -3 },
{ 3, 2.0, 310 },
{ 5, 0.22, 0 } };
M /= M.frobenius_norm();
FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
{ 0.997822, -0.00322107, 0.0708635, 0.00644213 },
{ 0.0257685, 0.0708635, 0.00322107, 0.999239 },
{ 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
auto Btest = PolarDecompositionTest::bilinearMatrix(M);
Btest -= B;
return Btest.frobenius_norm() < 0.001;
}
static bool test4dDeterminant() {
FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
{ 0.997822, -0.00322107, 0.0708635, 0.00644213 },
{ 0.0257685, 0.0708635, 0.00322107, 0.999239 },
{ 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
double detBtest = PolarDecompositionTest::determinantLaplace(B);
detBtest -= B.determinant();
return std::abs(detBtest) < 0.001;
}
static bool testdominantEVNewton() {
field_type detB = 0.992928;
field_type detM = 0.000620104;
FieldVector<field_type,3> minpol = {0, -1.99999, -0.00496083};
double correctDominantEV = 1.05397;
auto dominantEVNewton = PolarDecompositionTest::dominantEVNewton(minpol, detB);
return std::abs(correctDominantEV - dominantEVNewton) < 0.001;
}
static bool testCharacteristicPolynomial4D() {
FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
{ 0.997822, -0.00322107, 0.0708635, 0.00644213 },
{ 0.0257685, 0.0708635, 0.00322107, 0.999239 },
{ 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
FieldVector<field_type,3> minpol = {0, -1.99999, -0.00496083};
auto coefficients = PolarDecompositionTest::characteristicCoefficients(B);
coefficients -= minpol;
return coefficients.two_norm() < 0.001;
}
static bool testQuaternionFunction() {
FieldMatrix<field_type,4,4> BS = { { 1.04431, -0.997822, -0.0257685, -0.00644213 },
{ -0.997822, 1.05719, -0.0708635, -0.00644213 },
{ -0.0257685, -0.0708635, 1.05075, -0.999239 },
{ -0.00644213, -0.00644213, -0.999239, 1.06363 } };
FieldVector<field_type, 4 > v = { 0.508566, 0.516353, 0.499062, 0.475055 };
auto vtest = PolarDecompositionTest::obtainQuaternion(BS);
vtest -= v;
return vtest.two_norm() < 0.001;
}
int main (int argc, char *argv[]) try
{
//test the correctness of the algorithm
auto testMatrix = randomMatrix3d();
auto Q = PolarDecompositionTest()(testMatrix,1e-12,1e-12);
if (isNotOrthogonal(Q)) {
std::cerr << "The polar decomposition did not return an orthogonal matrix when decomposing: " << std::endl << testMatrix << std::endl;
return 1;
}
if (testBilinearMatrix()) {
std::cerr << "The test calculation of the bilinearMatrix is wrong!" << std::endl;
return 1;
}
if (!test4dDeterminant()) {
std::cerr << "The test calculation of the 4d determinant is wrong!" << std::endl;
return 1;
}
if (!testdominantEVNewton()) {
std::cerr << "The test calculation of the dominant eigenvalue is wrong!" << std::endl;
return 1;
}
if (!testCharacteristicPolynomial4D()) {
std::cerr << "The test calculation of the characteristic polynomial is wrong!" << std::endl;
return 1;
}
if (!testQuaternionFunction()) {
std::cerr << "The test calculation of the quaternion function is wrong!" << std::endl;
return 1;
}
timeTest(.1);
timeTest(2);
timeTest(30);
return 0;
} catch (Exception& e) {
std::cout << e.what() << std::endl;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment