Skip to content
Snippets Groups Projects
Commit 3176314d authored by Oliver Sander's avatar Oliver Sander Committed by sander
Browse files

Move methods exp with similar arguments next to each other

The methods 

Rotation<T,3> exp(const Rotation<T,3>& p, const EmbeddedTangentVector& v) 

and

otation<T,3> exp(const Rotation<T,3>& p, const Dune::FieldVector<T,4>& v)

do literally the same.  So they should be next to each other in the code.

[[Imported from SVN: r9559]]
parent 874cb377
No related branches found
No related tags found
No related merge requests found
......@@ -297,6 +297,29 @@ public:
// The actual exponential map
return exp(p, vMatrix);
}
static Rotation<T,3> exp(const Rotation<T,3>& p, const Dune::FieldVector<T,4>& v) {
assert( std::abs(p*v) < 1e-8 );
// The vector v as a quaternion
Quaternion<T> vQuat(v);
// left multiplication by the inverse base point yields a tangent vector at the identity
Quaternion<T> vAtIdentity = p.inverse().mult(vQuat);
assert( std::abs(vAtIdentity[3]) < 1e-8 );
// vAtIdentity as a skew matrix
SkewMatrix<T,3> vMatrix;
vMatrix.axial()[0] = 2*vAtIdentity[0];
vMatrix.axial()[1] = 2*vAtIdentity[1];
vMatrix.axial()[2] = 2*vAtIdentity[2];
// The actual exponential map
return exp(p, vMatrix);
}
/** \brief The exponential map from a given point $p \in SO(3)$.
\param v A tangent vector.
......@@ -360,27 +383,6 @@ public:
return skew;
}
static Rotation<T,3> exp(const Rotation<T,3>& p, const Dune::FieldVector<T,4>& v) {
assert( std::abs(p*v) < 1e-8 );
// The vector v as a quaternion
Quaternion<T> vQuat(v);
// left multiplication by the inverse base point yields a tangent vector at the identity
Quaternion<T> vAtIdentity = p.inverse().mult(vQuat);
assert( std::abs(vAtIdentity[3]) < 1e-8 );
// vAtIdentity as a skew matrix
SkewMatrix<T,3> vMatrix;
vMatrix.axial()[0] = 2*vAtIdentity[0];
vMatrix.axial()[1] = 2*vAtIdentity[1];
vMatrix.axial()[2] = 2*vAtIdentity[2];
// The actual exponential map
return exp(p, vMatrix);
}
static Dune::FieldMatrix<T,4,3> Dexp(const SkewMatrix<T,3>& v) {
Dune::FieldMatrix<T,4,3> result(0);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment