bugfix in interpolateDerivate and a unit test for the same method

[[Imported from SVN: r1118]]

src/quaternion.hh

@@ -235,6 +235,21 @@ public:
 
         result[0] = result[1] = result[2] = result[3] = 0;
         diffExp.umv(der,result);
+
+        result = a.mult(result);
+
+        double eps = 1e-6;
+        Quaternion<T> fdResult = interpolate(a,b, omega+eps);
+        fdResult              -= interpolate(a,b, omega-eps);
+        fdResult /= 2*eps;
+        fdResult /= intervallLength;
+
+        if ((result-fdResult).two_norm() > 1e-4) {
+            std::cout << "Wrong interpolation:\n";
+            std::cout << "Analytical: " << result << "        fd: " << fdResult << std::endl;
+            abort();
+        }
+            
 #endif
         //std::cout << result << std::endl;
         return result;