add the distance function and its derivatives to Rotation<2>

[[Imported from SVN: r5784]]

src/rotation.hh

@@ -33,6 +33,10 @@ public:
         : angle_(0)
     {}
 
+    Rotation(const T& angle)
+        : angle_(angle)
+    {}
+
     /** \brief Return the identity element */
     static Rotation<2,T> identity() {
         // Default constructor creates an identity
@@ -40,6 +44,16 @@ public:
         return id;
     }
 
+    static T distance(const Rotation<2,T>& a, const Rotation<2,T>& b) {
+        T dist = a.angle_ - b.angle_;
+        while (dist < 0)
+            dist += 2*M_PI;
+        while (dist > 2*M_PI)
+            dist -= 2*M_PI;
+
+        return (dist <= M_PI) ? dist : 2*M_PI - dist;
+    }
+
     /** \brief The exponential map from a given point $p \in SO(3)$. */
     static Rotation<2,T> exp(const Rotation<2,T>& p, const TangentVector& v) {
         Rotation<2,T> result = p;
@@ -55,6 +69,16 @@ public:
         return result;
     }
 
+    static TangentVector derivativeOfDistanceSquaredWRTSecondArgument(const Rotation<2,T>& a, 
+                                                                      const Rotation<2,T>& b) {
+        return -2 * distance(a,b);
+    }
+
+    static TangentVector secondDerivativeOfDistanceSquaredWRTSecondArgument(const Rotation<2,T>& a, 
+                                                                      const Rotation<2,T>& b) {
+        return 2;
+    }
+
     /** \brief Right multiplication */
     Rotation<2,T> mult(const Rotation<2,T>& other) const {
         Rotation<2,T> q = *this;