From 5af1e29597e2fc00662446e11bbc5aa71b58b272 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Thu, 30 Nov 2006 14:17:50 +0000
Subject: [PATCH] added conversions from and to orthogonal matrices
[[Imported from SVN: r1061]]
---
src/quaternion.hh | 101 +++++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 100 insertions(+), 1 deletion(-)
diff --git a/src/quaternion.hh b/src/quaternion.hh
index 45f9af69..846b5f0f 100644
--- a/src/quaternion.hh
+++ b/src/quaternion.hh
@@ -12,6 +12,16 @@ public:
/** \brief Default constructor */
Quaternion() {}
+ /** \brief Constructor with the four components */
+ Quaternion(const T& a, const T& b, const T& c, const T& d) {
+
+ (*this)[0] = a;
+ (*this)[1] = b;
+ (*this)[2] = c;
+ (*this)[3] = d;
+
+ }
+
/** \brief Copy constructor */
Quaternion(const Dune::FieldVector<T,4>& other) : Dune::FieldVector<T,4>(other) {}
@@ -42,7 +52,7 @@ public:
T normV = std::sqrt(v0*v0 + v1*v1 + v2*v2);
- // Stabilization for small |v| due Grassia
+ // Stabilization for small |v| due to Grassia
T sin = (normV < 1e-4) ? 0.5 * (normV*normV/48) : std::sin(normV/2)/normV;
// if normV == 0 then q = (0,0,0,1)
@@ -124,6 +134,95 @@ public:
return result;
}
+ /** \brief Return the corresponding orthogonal matrix */
+ void matrix(Dune::FieldMatrix<T,3,3>& m) const {
+
+ m[0][0] = (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+ m[0][1] = 2 * ( (*this)[0]*(*this)[1] - (*this)[2]*(*this)[3] );
+ m[0][2] = 2 * ( (*this)[0]*(*this)[2] + (*this)[1]*(*this)[3] );
+
+ m[1][0] = 2 * ( (*this)[0]*(*this)[1] + (*this)[2]*(*this)[3] );
+ m[1][1] = - (*this)[0]*(*this)[0] + (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+ m[1][2] = 2 * ( -(*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
+
+ m[2][0] = 2 * ( (*this)[0]*(*this)[2] - (*this)[1]*(*this)[3] );
+ m[2][1] = 2 * ( (*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
+ m[2][2] = - (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] + (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+
+ }
+
+ /** \brief Set unit quaternion from orthogonal matrix
+
+ We tacitly assume that the matrix really is orthogonal */
+ void set(const Dune::FieldMatrix<T,3,3>& m) {
+
+ // Easier writing
+ Dune::FieldVector<T,4>& p = (*this);
+ // 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]) {
+
+ p[0] = std::sqrt(p[0]);
+
+ // r_x r_y = (R_12 + R_21) / 4
+ p[1] = (m[0][1] + m[1][0]) / 4 / p[0];
+
+ // r_x r_z = (R_13 + R_31) / 4
+ p[2] = (m[0][2] + m[2][0]) / 4 / p[0];
+
+ // r_0 r_x = (R_32 - R_23) / 4
+ p[3] = (m[2][1] - m[1][2]) / 4 / p[0];
+
+ } else if (p[1] >= p[0] && p[1] >= p[2] && p[1] >= p[3]) {
+
+ p[1] = std::sqrt(p[1]);
+
+ // r_x r_y = (R_12 + R_21) / 4
+ p[0] = (m[0][1] + m[1][0]) / 4 / p[1];
+
+ // r_y r_z = (R_23 + R_32) / 4
+ p[2] = (m[1][2] + m[2][1]) / 4 / p[1];
+
+ // r_0 r_y = (R_13 - R_31) / 4
+ p[3] = (m[0][2] - m[2][0]) / 4 / p[1];
+
+ } else if (p[2] >= p[0] && p[2] >= p[1] && p[2] >= p[3]) {
+
+ p[2] = std::sqrt(p[2]);
+
+ // r_x r_z = (R_13 + R_31) / 4
+ p[0] = (m[0][2] + m[2][0]) / 4 / p[2];
+
+ // r_y r_z = (R_23 + R_32) / 4
+ p[1] = (m[1][2] + m[2][1]) / 4 / p[2];
+
+ // r_0 r_z = (R_21 - R_12) / 4
+ p[3] = (m[1][0] - m[0][1]) / 4 / p[2];
+
+ } else {
+
+ p[3] = std::sqrt(p[3]);
+
+ // r_0 r_x = (R_32 - R_23) / 4
+ p[0] = (m[2][1] - m[1][2]) / 4 / p[3];
+
+ // r_0 r_y = (R_13 - R_31) / 4
+ p[1] = (m[0][2] - m[2][0]) / 4 / p[3];
+
+ // r_0 r_z = (R_21 - R_12) / 4
+ p[2] = (m[1][0] - m[0][1]) / 4 / p[3];
+
+ }
+
+ }
};
#endif
--
GitLab