geodesic distance

[[Imported from SVN: r4136]]

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