math: Move lerp function into SGMisc.

[simgear.git] / simgear / math / SGQuat.hxx

diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx
index a48cbf1ab08a17c2deb8a0c35316c271248c2c00..9449c914cbf731aed0d7ac9178c113dd3a4cb33e 100644 (file)
--- a/simgear/math/SGQuat.hxx
+++ b/simgear/math/SGQuat.hxx
@@ -150,6 +150,17 @@ public:
     return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
   }
 
+  /// Create a normalized quaternion just from the imaginary part.
+  /// The imaginary part should point into that axis direction that results in
+  /// a quaternion with a positive real part.
+  /// This is the smallest numerically stable representation of an orientation
+  /// in space. See getPositiveRealImag()
+  static SGQuat fromPositiveRealImag(const SGVec3<T>& imag)
+  {
+    T r = sqrt(SGMisc<T>::max(T(0), T(1) - dot(imag, imag)));
+    return fromRealImag(r, imag);
+  }
+
   /// Return a quaternion that rotates the from vector onto the to vector.
   static SGQuat fromRotateTo(const SGVec3<T>& from, const SGVec3<T>& to)
   {
@@ -219,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);
@@ -268,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();
@@ -276,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())
@@ -312,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;
     }
@@ -326,6 +337,19 @@ public:
     axis *= angle;
   }
 
+  /// Get the imaginary part of the quaternion.
+  /// The imaginary part should point into that axis direction that results in
+  /// a quaternion with a positive real part.
+  /// This is the smallest numerically stable representation of an orientation
+  /// in space. See fromPositiveRealImag()
+  SGVec3<T> getPositiveRealImag() const
+  {
+    if (real(*this) < T(0))
+      return (T(-1)/norm(*this))*imag(*this);
+    else
+      return (T(1)/norm(*this))*imag(*this);
+  }
+
   /// Access by index, the index is unchecked
   const T& operator()(unsigned i) const
   { return data()[i]; }
@@ -425,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.
@@ -454,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.