From ddcafb5b6a130b3fdc732d306b0c7cb2c0b4db88 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Thu, 30 Apr 2009 15:50:43 +0000
Subject: [PATCH] geodesic distance
[[Imported from SVN: r4136]]
---
src/rotation.hh | 13 +++++++++++++
1 file changed, 13 insertions(+)
diff --git a/src/rotation.hh b/src/rotation.hh
index dd01e919..d05572f1 100644
--- a/src/rotation.hh
+++ b/src/rotation.hh
@@ -241,6 +241,19 @@ public:
return APseudoInv;
}
+ static T distance(const Rotation<3,T>& a, const Rotation<3,T>& b) {
+ Quaternion<T> diff = a;
+
+ diff.invert();
+ diff = diff.mult(b);
+
+ // 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 (diff[3] > 1.0)
+ ? 0
+ : 2*std::acos( std::min(diff[3],1.0) );
+ }
/** \brief Compute the vector in T_aSO(3) that is mapped by the exponential map
to the geodesic from a to b
--
GitLab