diff --git a/src/quaternion.hh b/src/quaternion.hh index 00fb41d0f2ef6b1f21e548385094ee25d2b6e37b..ddd55033ad836801bee2129e1685eaadb0406e1d 100644 --- a/src/quaternion.hh +++ b/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;