From 4d64dcda3cf5c0ccde53674337ecdb57b2e6d5b3 Mon Sep 17 00:00:00 2001
From: Jonathan Youett <youett@mi.fu-berlin.de>
Date: Tue, 18 Oct 2011 12:19:15 +0000
Subject: [PATCH] Add the cayley and inverse cayley mapping. By now these
formulas use the matrix representation of a skew matrix/rotation which is not
very pretty. I will replace it by a quaternion counterpart when I figured out
how to do it properly
[[Imported from SVN: r7943]]
---
dune/gfe/rotation.hh | 60 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 60 insertions(+)
diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index 2b82d924..c1b6748b 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -450,6 +450,66 @@ public:
return APseudoInv;
}
+ /** \brief The cayley mapping from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$. */
+ static Rotation<3,T> cayley(const SkewMatrix<3,T>& s) {
+ Rotation<3,T> q;
+
+ Dune::FieldVector<T,3> vAxial = v.axial();
+ T norm = 0.25*vAxial.two_norm2() + 1;
+
+ Dune::FieldMatrix<T,3,3> mat = s.toMatrix();
+ mat *= 0.5;
+ Dune::FieldMatrix<T,3,3> skewSquare = mat.rightmultiply(mat);
+ mat += skewSquare;
+ mat *= 2/norm;
+
+ for (int i=0;i<3;i++)
+ mat[i][i] += 1;
+
+ q.set(mat);
+ return q;
+ }
+
+ /** \brief The inverse of the cayley mapping. */
+ static SkewMatrix<3,T> cayleyInv(const Rotation<3,T> q) {
+
+ Dune::FieldMatrix<T,3,3> mat;
+
+ // compute the trace of the rotation matrix
+ double trace = -(*this)[0]*(*this)[0] -(*this)[1]*(*this)[1] -(*this)[2]*(*this)[2]+3*(*this)[3]*(*this)[3];
+
+ if ( (trace+1)>1e-6 && (trace+1)<-1e-6) { // if this term doesn't vanish we can use a direct formula
+
+ (*this).matrix(mat);
+ Dune::FieldMatrix<T,3,3> matT;
+ (*this).inverse().matrix(matT);
+ mat -= matT;
+ mat *= 1/(1+trace);
+ }
+ else { // use the formula that involves the computation of an inverse
+ Dune::FieldMatrix<T,3,3> inv;
+ (*this).matrix(inv);
+ Dune::FieldMatrix<T,3,3> notInv = inv;
+
+ for (int i=0;i<3;i++) {
+ inv[i][i] +=1;
+ notInv[i][i] -=1;
+ }
+ inv.invert();
+ mat = notInv.leftmultiply(inv);
+ mat *= 2;
+ }
+
+ // result is a skew symmetric matrix
+ SkewMatrix<3,T> res;
+ res.axial()[0] = mat[2][1];
+ res.axial()[1] = mat[0][2];
+ res.axial()[2] = mat[1][0];
+
+ return res;
+
+ }
+
static T distance(const Rotation<3,T>& a, const Rotation<3,T>& b) {
Quaternion<T> diff = a;
--
GitLab