+template<typename T>
+bool
+QuatDerivativeTest(void)
+{
+ for (unsigned i = 0; i < 100; ++i) {
+ // Generate the test case:
+ // Give a lower bound to the distance, so avoid testing cancelation
+ T dt = T(0.01) + sg_random();
+ // Start with orientation o0, angular velocity av and a random stepsize
+ SGQuat<T> o0 = SGQuat<T>::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random());
+ SGVec3<T> av(sg_random(), sg_random(), sg_random());
+ // Do one euler step and renormalize
+ SGQuat<T> o1 = normalize(o0 + dt*o0.derivative(av));
+
+ // Check if we can restore the angular velocity
+ SGVec3<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
+ if (!equivalent(av, av2))
+ return false;
+
+ // Test with the equivalent orientation
+ o1 = -o1;
+ av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
+ if (!equivalent(av, av2))
+ return false;
+ }
+ return true;
+}
+