diff --git a/dune/gfe/localprojectedfefunction.hh b/dune/gfe/localprojectedfefunction.hh
index 80fa763d558a2b6aad1e4191018f554caaa8a376..9f9c5d77c8b9a1924bb90d6a1e6c4e03b25a36e9 100644
--- a/dune/gfe/localprojectedfefunction.hh
+++ b/dune/gfe/localprojectedfefunction.hh
@@ -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;
}
diff --git a/dune/gfe/polardecomposition.hh b/dune/gfe/polardecomposition.hh
new file mode 100644
index 0000000000000000000000000000000000000000..6f3ecb862113d5e241bc267d0bdd701ec3e43406
--- /dev/null
+++ b/dune/gfe/polardecomposition.hh
@@ -0,0 +1,326 @@
+// -*- 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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2f725374e6975ed9aeae9ce6fe65d28a4107faad..b292a33af2b6da6b5cdb459a61db0020f7a67a3a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -8,6 +8,7 @@ set(TESTS
localgfetestfunctiontest
localprojectedfefunctiontest
orthogonalmatrixtest
+ polardecompositiontest
rigidbodymotiontest
rotationtest
svdtest
diff --git a/test/polardecompositiontest.cc b/test/polardecompositiontest.cc
new file mode 100644
index 0000000000000000000000000000000000000000..cc10676d24424d59fc4316dace27efa2a9fb9720
--- /dev/null
+++ b/test/polardecompositiontest.cc
@@ -0,0 +1,243 @@
+// -*- 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;
+}
+