Rename this to computeGeodesicDifference and add support for Rotation<3,double>

Todo: The target space should really be a template parameter!

[[Imported from SVN: r4028]]

src/geodesicdifference.hh

@@ -3,8 +3,8 @@
 
 #include "rigidbodymotion.hh"
 
-Dune::BlockVector<Dune::FieldVector<double,6> > computeRodDifference(const std::vector<RigidBodyMotion<3> >& a,
-                                                                     const std::vector<RigidBodyMotion<3> >& b)
+Dune::BlockVector<Dune::FieldVector<double,6> > computeGeodesicDifference(const std::vector<RigidBodyMotion<3> >& a,
+                                                                          const std::vector<RigidBodyMotion<3> >& b)
 {
     if (a.size() != b.size())
         DUNE_THROW(Dune::Exception, "a and b have to have the same length!");
@@ -29,4 +29,22 @@ Dune::BlockVector<Dune::FieldVector<double,6> > computeRodDifference(const std::
     return result;
 }
 
+Dune::BlockVector<Dune::FieldVector<double,3> > computeGeodesicDifference(const std::vector<Rotation<3,double> >& a,
+                                                                          const std::vector<Rotation<3,double> >& b)
+{
+    if (a.size() != b.size())
+        DUNE_THROW(Dune::Exception, "a and b have to have the same length!");
+
+    Dune::BlockVector<Dune::FieldVector<double,3> > result(a.size());
+
+    for (size_t i=0; i<result.size(); i++) {
+
+        // Subtract orientations on the tangent space of 'a'
+        result[i] = Rotation<3,double>::difference(a[i], b[i]);
+
+    }
+
+    return result;
+}
+
 #endif