diff --git a/src/rotation.hh b/src/rotation.hh index dd01e91963054460fd373dee2fe5dbfd1a7ff774..d05572f1672bd4a2d2547ddde7689d9749f59f71 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