diff --git a/src/geodesicdifference.hh b/src/geodesicdifference.hh
index c3bbf8e0880d568a65847de4e5c37237c2efd61f..7961c5614d4d7d4b60a6bfd8a0dfc66cd23abb05 100644
--- a/src/geodesicdifference.hh
+++ b/src/geodesicdifference.hh
@@ -1,46 +1,19 @@
-#ifndef ROD_DIFFERENCE_HH
-#define ROD_DIFFERENCE_HH
+#ifndef GEODESIC_DIFFERENCE_HH
+#define GEODESIC_DIFFERENCE_HH
 
-#include "rigidbodymotion.hh"
-
-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!");
-
-    Dune::BlockVector<Dune::FieldVector<double,6> > result(a.size());
-
-    for (size_t i=0; i<result.size(); i++) {
-
-        // Subtract centerline position
-        for (int j=0; j<3; j++)
-            result[i][j] = a[i].r[j] - b[i].r[j];
-        
-        // Subtract orientations on the tangent space of 'a'
-        Dune::FieldVector<double,3> v = Rotation<3,double>::difference(a[i].q, b[i].q);
-
-        // Compute difference on T_a SO(3)
-        for (int j=0; j<3; j++)
-            result[i][j+3] = v[j];
-
-    }
-
-    return result;
-}
-
-Dune::BlockVector<Dune::FieldVector<double,3> > computeGeodesicDifference(const std::vector<Rotation<3,double> >& a,
-                                                                          const std::vector<Rotation<3,double> >& b)
+template <class TargetSpace>
+Dune::BlockVector<typename TargetSpace::TangentVector> computeGeodesicDifference(const std::vector<TargetSpace>& a,
+                                                                                 const std::vector<TargetSpace>& 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());
+    Dune::BlockVector<typename TargetSpace::TangentVector> 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]);
+        result[i] = TargetSpace::difference(a[i], b[i]);
 
     }