Skip to content
Snippets Groups Projects
Commit 289fc7fe authored by Oliver Sander's avatar Oliver Sander Committed by sander@PCPOOL.MI.FU-BERLIN.DE
Browse files

add assignment from scalar, the inverse exponential, its derivative, and conjugation

[[Imported from SVN: r1678]]
parent ac21922a
No related branches found
No related tags found
No related merge requests found
......@@ -43,6 +43,13 @@ public:
(*this)[3] = std::cos(angle/2);
}
/** \brief Assignment from a scalar */
Quaternion<T>& operator=(const T& v) {
for (int i=0; i<4; i++)
(*this)[i] = v;
return (*this);
}
/** \brief Return the identity element */
static Quaternion<T> identity() {
Quaternion<T> id;
......@@ -102,6 +109,60 @@ public:
return result;
}
/** \brief The inverse of the exponential map */
static Dune::FieldVector<T,3> expInv(const Quaternion<T>& q) {
// Compute v = exp^{-1} q
// Due to numerical dirt, q[3] may be larger than 1.
// In that case, use 1 instead of q[3].
Dune::FieldVector<T,3> v;
if (q[3] > 1.0) {
v = 0;
} else {
T invSinc = 1/sincHalf(2*std::acos(q[3]));
v[0] = q[0] * invSinc;
v[1] = q[1] * invSinc;
v[2] = q[2] * invSinc;
}
return v;
}
/** \brief The derivative of the inverse of the exponential map, evaluated at q */
static Dune::FieldMatrix<T,3,4> DexpInv(const Quaternion<T>& q) {
// Compute v = exp^{-1} q
Dune::FieldVector<T,3> v = expInv(q);
// The derivative of exp at v
Dune::FieldMatrix<T,4,3> A = Dexp(v);
// Compute the Moore-Penrose pseudo inverse A^+ = (A^T A)^{-1} A^T
Dune::FieldMatrix<T,3,3> ATA;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++) {
ATA[i][j] = 0;
for (int k=0; k<4; k++)
ATA[i][j] += A[k][i] * A[k][j];
}
ATA.invert();
Dune::FieldMatrix<T,3,4> APseudoInv;
for (int i=0; i<3; i++)
for (int j=0; j<4; j++) {
APseudoInv[i][j] = 0;
for (int k=0; k<3; k++)
APseudoInv[i][j] += ATA[i][k] * A[j][k];
}
return APseudoInv;
}
/** \brief Right quaternion multiplication */
Quaternion<T> mult(const Quaternion<T>& other) const {
Quaternion<T> q;
......@@ -235,14 +296,21 @@ public:
return result;
}
/** \brief Invert the quaternion */
void invert() {
/** \brief Conjugate the quaternion */
void conjugate() {
(*this)[0] *= -1;
(*this)[1] *= -1;
(*this)[2] *= -1;
}
/** \brief Invert the quaternion */
void invert() {
conjugate();
(*this) /= this->two_norm2();
}
static Dune::FieldVector<T,3> difference(const Quaternion<T>& a, const Quaternion<T>& b) {
......
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