From 8285b225349aa97ee8de61cb35e1f750f0cc1660 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Wed, 11 Jan 2012 17:43:44 +0000
Subject: [PATCH] make the 'distance' method simpler and faster
[[Imported from SVN: r8366]]
---
dune/gfe/rotation.hh | 19 +++++++------------
1 file changed, 7 insertions(+), 12 deletions(-)
diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index 6ddf2b1c..6a52b7d7 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -544,22 +544,17 @@ public:
}
static T distance(const Rotation<T,3>& a, const Rotation<T,3>& b) {
- Quaternion<T> diff = a;
+
+ // Distance in the unit quaternions: 2*arccos( ((a^{-1) b)_3 )
+ // But note that (a^{-1}b)_3 is actually <a,b> (in R^4)
+ T sp = a.globalCoordinates() * b.globalCoordinates();
- diff.invert();
- diff = diff.mult(b);
+ // Scalar product may be larger than 1.0, due to numerical dirt
+ T dist = 2*std::acos( std::min(sp,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)
- T dist = 2*std::acos( std::min(diff[3],1.0) );
-
- if (dist>=M_PI)
- return 2*M_PI - dist;
-
- // 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].
- return 2*std::acos( std::min(diff[3],1.0) );
+ return (dist>=M_PI) ? (2*M_PI - dist) : dist;
}
/** \brief Compute the vector in T_aSO(3) that is mapped by the exponential map
--
GitLab