From 8028a8123dbc8dcbcbb316cbffd76917fa6e3147 Mon Sep 17 00:00:00 2001
From: Jonathan Youett <youett@mi.fu-berlin.de>
Date: Thu, 3 Nov 2011 14:36:07 +0000
Subject: [PATCH] fix interpolation-bug when quaternions don't have the same
sign. Thanks to Oliver Sander for the patch ;-)
[[Imported from SVN: r8102]]
---
dune/gfe/rotation.hh | 18 ++++++++++++++++--
1 file changed, 16 insertions(+), 2 deletions(-)
diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index 75e4f14b..7d6c5135 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -454,7 +454,7 @@ public:
return APseudoInv;
}
- /** \brief The cayley mapping from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$.
+ /** \brief The cayley mapping from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$.
*
* The formula is taken from 'J.C.Simo, N.Tarnom, M.Doblare - Non-linear dynamics of
* three-dimensional rods:Exact energy and momentum conserving algorithms'
@@ -480,7 +480,7 @@ public:
return q;
}
- /** \brief The inverse of the Cayley mapping.
+ /** \brief The inverse of the Cayley mapping.
*
* The formula is taken from J.M.Selig - Cayley Maps for SE(3).
*/
@@ -529,6 +529,13 @@ public:
diff.invert();
diff = diff.mult(b);
+ T dist = 2*std::acos( std::min(diff[3],1.0) );
+
+ // Make sure we do the right thing if a and b are not in the same sheet
+ // of the double covering of the unit quaternions over SO(3)
+ if (dist>=M_PI)
+ diff *= -1;
+
// Compute the geodesical distance between a and b on SO(3)
// Due to numerical dirt, diff[3] may be larger than 1.
// In that case, use 1 instead of diff[3].
@@ -558,6 +565,13 @@ public:
T dist = 2*std::acos( std::min(diff[3],1.0) );
+ // Make sure we do the right thing if a and b are not in the same sheet
+ // of the double covering of the unit quaternions over SO(3)
+ if (dist>=M_PI) {
+ dist -= M_PI;
+ diff *= -1;
+ }
+
T invSinc = 1/sincHalf(dist);
// Compute difference on T_a SO(3)
--
GitLab