Introduce quaternion finite difference method.

This implements a function for the quaternion implementation
that computes the angular velocity that matches an explicit euler
step that propagates from a starting quaternion orientation to a
destination quaternion orientation.

diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx
index 415a4d59ecda925c8e51bd3f62a7033f0d4b83ff..ef1f50747953e244c3454a5e0731c3422a7887ce 100644 (file)
--- a/simgear/math/SGMathTest.cxx
+++ b/simgear/math/SGMathTest.cxx
@@ -23,6 +23,7 @@
 #include <iostream>
 
 #include "SGMath.hxx"
+#include "sg_random.h"
 
 template<typename T>
 bool
@@ -198,6 +199,34 @@ QuatTest(void)
   return true;
 }
 
+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;
+}
+
 template<typename T>
 bool
 MatrixTest(void)
@@ -273,17 +302,17 @@ GeodesyTest(void)
   // uses examples from Williams aviation formulary
   SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
   SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
-  
+
   double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
   std::cout << "distance is " << distNm << std::endl;
   if (0.5 < fabs(distNm - 2144)) // 2144 nm
        return false;
-       
+
   double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
   std::cout << "course is " << crsDeg << std::endl;
   if (0.5 < fabs(crsDeg - 66)) // 66 degrees
        return false;
-       
+
   SGGeoc adv;
   SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
   std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
@@ -298,6 +327,8 @@ GeodesyTest(void)
 int
 main(void)
 {
+  sg_srandom(17);
+
   // Do vector tests
   if (!Vec3Test<float>())
     return EXIT_FAILURE;
@@ -309,6 +340,10 @@ main(void)
     return EXIT_FAILURE;
   if (!QuatTest<double>())
     return EXIT_FAILURE;
+  if (!QuatDerivativeTest<float>())
+    return EXIT_FAILURE;
+  if (!QuatDerivativeTest<double>())
+    return EXIT_FAILURE;
 
   // Do matrix tests
   if (!MatrixTest<float>())

diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx
index 256ddb28a8b046b7c4f6dd6707779ba2fed415e1..9449c914cbf731aed0d7ac9178c113dd3a4cb33e 100644 (file)
--- a/simgear/math/SGQuat.hxx
+++ b/simgear/math/SGQuat.hxx
@@ -230,7 +230,7 @@ public:
     T absv1 = fabs(v(0));
     T absv2 = fabs(v(1));
     T absv3 = fabs(v(2));
-    
+
     SGVec3<T> axis;
     if (absv2 < absv1 && absv3 < absv1) {
       T quot = v(1)/v(0);
@@ -279,7 +279,7 @@ public:
       xRad = 0;
     else
       xRad = atan2(num, den);
-    
+
     T tmp = 2*(x()*z() - w()*y());
     if (tmp <= -1)
       yRad = T(0.5)*SGMisc<T>::pi();
@@ -287,8 +287,8 @@ public:
       yRad = -T(0.5)*SGMisc<T>::pi();
     else
       yRad = -asin(tmp);
-   
-    num = 2*(x()*y() + w()*z()); 
+
+    num = 2*(x()*y() + w()*z());
     den = sqrQW + sqrQX - sqrQY - sqrQZ;
     if (fabs(den) <= SGLimits<T>::min() &&
         fabs(num) <= SGLimits<T>::min())
@@ -323,7 +323,7 @@ public:
       T sAng = sin(angle);
       if (fabs(sAng) <= SGLimits<T>::min())
         axis = SGVec3<T>(1, 0, 0);
-      else 
+      else
         axis = (rNrm/sAng)*imag(*this);
       angle *= 2;
     }
@@ -449,10 +449,46 @@ public:
     deriv.x() = T(0.5)*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
     deriv.y() = T(0.5)*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
     deriv.z() = T(0.5)*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
-    
+
     return deriv;
   }
 
+  /// Return the angular velocity w that makes q0 translate to q1 using
+  /// an explicit euler step with stepsize h.
+  /// That is, look for an w where
+  /// q1 = normalize(q0 + h*q0.derivative(w))
+  static SGVec3<T>
+  forwardDifferenceVelocity(const SGQuat& q0, const SGQuat& q1, const T& h)
+  {
+    // Let D_q0*w = q0.derivative(w), D_q0 the above 4x3 matrix.
+    // Then D_q0^t*D_q0 = 0.25*Id and D_q0*q0 = 0.
+    // Let lambda be a nonzero normailzation factor, then
+    //  q1 = normalize(q0 + h*q0.derivative(w))
+    // can be rewritten
+    //  lambda*q1 = q0 + h*D_q0*w.
+    // Multiply left by the transpose D_q0^t and reorder gives
+    //  4*lambda/h*D_q0^t*q1 = w.
+    // Now compute lambda by substitution of w into the original
+    // equation
+    //  lambda*q1 = q0 + 4*lambda*D_q0*D_q0^t*q1,
+    // multiply by q1^t from the left
+    //  lambda*<q1,q1> = <q0,q1> + 4*lambda*<D_q0^t*q1,D_q0^t*q1>
+    // and solving for lambda gives
+    //  lambda = <q0,q1>/(1 - 4*<D_q0^t*q1,D_q0^t*q1>).
+
+    // The transpose of the derivative matrix
+    // the 0.5 factor is handled below
+    // also note that the initializer uses x, y, z, w instead of w, x, y, z
+    SGQuat d0(q0.w(), q0.z(), -q0.y(), -q0.x());
+    SGQuat d1(-q0.z(), q0.w(), q0.x(), -q0.y());
+    SGQuat d2(q0.y(), -q0.x(), q0.w(), -q0.z());
+    // 2*D_q0^t*q1
+    SGVec3<T> Dq(dot(d0, q1), dot(d1, q1), dot(d2, q1));
+    // Like above, but take into account that Dq = 2*D_q0^t*q1
+    T lambda = dot(q0, q1)/(T(1) - dot(Dq, Dq));
+    return (2*lambda/h)*Dq;
+  }
+
 private:
 
   // Private because it assumes normalized inputs.
@@ -478,7 +514,7 @@ private:
 
     // Now our assumption of angles <= 90 deg comes in play.
     // For that reason, we know that cos05ang is not zero.
-    // It is even more, we can see from the above formula that 
+    // It is even more, we can see from the above formula that
     // sqrt(0.5) < cos05ang.