diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index e83e17800b5e2d2fc67a32dc5a7304fdbc0f58c0..f5ebf1bf6139f1376e5b995090fc99e6118e4f9e 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -964,6 +964,108 @@ public:
}
+ /** \brief Derivative of the map from orthogonal matrices to unit quaternions
+
+ We tacitly assume that the matrix really is orthogonal */
+ static Tensor3<T,4,3,3> derivativeOfMatrixToQuaternion(const Dune::FieldMatrix<T,3,3>& m)
+ {
+ Tensor3<T,4,3,3> result;
+
+ Dune::FieldVector<T,3> p;
+
+ // The following equations for the derivation of a unit quaternion from a rotation
+ // matrix comes from 'E. Salamin, Application of Quaternions to Computation with
+ // Rotations, Technical Report, Stanford, 1974'
+
+ p[0] = (1 + m[0][0] - m[1][1] - m[2][2]) / 4;
+ p[1] = (1 - m[0][0] + m[1][1] - m[2][2]) / 4;
+ p[2] = (1 - m[0][0] - m[1][1] + m[2][2]) / 4;
+ p[3] = (1 + m[0][0] + m[1][1] + m[2][2]) / 4;
+
+ // avoid rounding problems
+ if (p[0] >= p[1] && p[0] >= p[2] && p[0] >= p[3])
+ {
+ result[0] = {{1,0,0},{0,-1,0},{0,0,-1}};
+ result[0] *= 1.0/(8.0*std::sqrt(p[0]));
+
+ T denom = 32 * std::pow(p[0],1.5);
+ T offDiag = 1.0/(4*std::sqrt(p[0]));
+
+ result[1] = { {-(m[0][1]+m[1][0]) / denom, offDiag, 0},
+ {offDiag, (m[0][1]+m[1][0]) / denom, 0},
+ {0, 0, (m[0][1]+m[1][0]) / denom}};
+
+ result[2] = { {-(m[0][2]+m[2][0]) / denom, 0, offDiag},
+ {0, (m[0][2]+m[2][0]) / denom, 0},
+ {offDiag, 0, (m[0][2]+m[2][0]) / denom}};
+
+ result[3] = { {-(m[2][1]-m[1][2]) / denom, 0, 0},
+ {0, (m[2][1]-m[1][2]) / denom, -offDiag},
+ {0, offDiag, (m[2][1]-m[1][2]) / denom}};
+ }
+ else if (p[1] >= p[0] && p[1] >= p[2] && p[1] >= p[3])
+ {
+ result[1] = {{-1,0,0},{0,1,0},{0,0,-1}};
+ result[1] *= 1.0/(8.0*std::sqrt(p[1]));
+
+ T denom = 32 * std::pow(p[1],1.5);
+ T offDiag = 1.0/(4*std::sqrt(p[1]));
+
+ result[0] = { {(m[0][1]+m[1][0]) / denom, offDiag, 0},
+ {offDiag, -(m[0][1]+m[1][0]) / denom, 0},
+ {0, 0, (m[0][1]+m[1][0]) / denom}};
+
+ result[2] = { {(m[1][2]+m[2][1]) / denom, 0 , 0},
+ {0, -(m[1][2]+m[2][1]) / denom, offDiag},
+ {0, offDiag, (m[1][2]+m[2][1]) / denom}};
+
+ result[3] = { {(m[0][2]-m[2][0]) / denom, 0, offDiag},
+ {0, -(m[0][2]-m[2][0]) / denom, 0},
+ {-offDiag, 0, (m[0][2]-m[2][0]) / denom}};
+ }
+ else if (p[2] >= p[0] && p[2] >= p[1] && p[2] >= p[3])
+ {
+ result[2] = {{-1,0,0},{0,-1,0},{0,0,1}};
+ result[2] *= 1.0/(8.0*std::sqrt(p[2]));
+
+ T denom = 32 * std::pow(p[2],1.5);
+ T offDiag = 1.0/(4*std::sqrt(p[2]));
+
+ result[0] = { {(m[0][2]+m[2][0]) / denom, 0, offDiag},
+ {0, (m[0][2]+m[2][0]) / denom, 0},
+ {offDiag, 0, -(m[0][2]+m[2][0]) / denom}};
+
+ result[1] = { {(m[1][2]+m[2][1]) / denom, 0 , 0},
+ {0, (m[1][2]+m[2][1]) / denom, offDiag},
+ {0, offDiag, -(m[1][2]+m[2][1]) / denom}};
+
+ result[3] = { {(m[1][0]-m[0][1]) / denom, -offDiag, 0},
+ {offDiag, (m[1][0]-m[0][1]) / denom, 0},
+ {0, 0, -(m[1][0]-m[0][1]) / denom}};
+ }
+ else
+ {
+ result[3] = {{1,0,0},{0,1,0},{0,0,1}};
+ result[3] *= 1.0/(8.0*std::sqrt(p[3]));
+
+ T denom = 32 * std::pow(p[3],1.5);
+ T offDiag = 1.0/(4*std::sqrt(p[3]));
+
+ result[0] = { {-(m[2][1]-m[1][2]) / denom, 0, 0},
+ {0, -(m[2][1]-m[1][2]) / denom, -offDiag},
+ {0, offDiag, -(m[2][1]-m[1][2]) / denom}};
+
+ result[1] = { {-(m[0][2]-m[2][0]) / denom, 0, offDiag},
+ {0, -(m[0][2]-m[2][0]) / denom, 0},
+ {-offDiag, 0, -(m[0][2]-m[2][0]) / denom}};
+
+ result[2] = { {-(m[1][0]-m[0][1]) / denom, -offDiag, 0},
+ {offDiag, -(m[1][0]-m[0][1]) / denom, 0},
+ {0, 0, -(m[1][0]-m[0][1]) / denom}};
+ }
+ return result;
+ }
+
/** \brief Create three vectors which form an orthonormal basis of \mathbb{H} together
with this one.
diff --git a/test/rotationtest.cc b/test/rotationtest.cc
index c097935543f350f24fa891ca67ee2148c351e201..5208d3487fe0bcae473f5a0c50320e84d8197a4d 100644
--- a/test/rotationtest.cc
+++ b/test/rotationtest.cc
@@ -9,7 +9,6 @@
#include <dune/grid/onedgrid.hh>
#include <dune/gfe/rotation.hh>
-#include <dune/gfe/svd.hh>
#warning Do not include rodlocalstiffness.hh
#include <dune/gfe/rodlocalstiffness.hh>
#include "valuefactory.hh"
@@ -304,6 +303,45 @@ void testRotation(Rotation<double,3> q)
}
+ //////////////////////////////////////////////////////////////////////
+ // Check whether the derivativeOfMatrixToQuaternion methods works
+ //////////////////////////////////////////////////////////////////////
+
+ Tensor3<double,4,3,3> derivative = Rotation<double,3>::derivativeOfMatrixToQuaternion(matrix);
+
+ const double eps = 1e-8;
+ Tensor3<double,4,3,3> derivativeFD;
+
+ for (size_t i=0; i<3; i++)
+ {
+ for (size_t j=0; j<3; j++)
+ {
+ auto forwardMatrix = matrix;
+ forwardMatrix[i][j] += eps;
+ auto backwardMatrix = matrix;
+ backwardMatrix[i][j] -= eps;
+
+ Rotation<double,3> forwardRotation, backwardRotation;
+ forwardRotation.set(forwardMatrix);
+ backwardRotation.set(backwardMatrix);
+
+ for (size_t k=0; k<4; k++)
+ derivativeFD[k][i][j] = (forwardRotation.globalCoordinates()[k] - backwardRotation.globalCoordinates()[k]) / (2*eps);
+
+ }
+ }
+
+ if ((derivative - derivativeFD).infinity_norm() > 1e-6)
+ {
+ std::cout << "At matrix:\n" << matrix << std::endl;
+
+ std::cout << "Derivative of matrix to quaternion map does not match its FD approximation" << std::endl;
+ std::cout << "Analytical derivative:" << std::endl;
+ std::cout << derivative << std::endl;
+ std::cout << "Finite difference approximation" << std::endl;
+ std::cout << derivativeFD << std::endl;
+ abort();
+ }
}
int main (int argc, char *argv[]) try