Make yasim accept the launchbar and hook properties. They are not tied to anything...

[flightgear.git] / src / FDM / JSBSim / FGQuaternion.cpp

diff --git a/src/FDM/JSBSim/FGQuaternion.cpp b/src/FDM/JSBSim/FGQuaternion.cpp
index 8c62dd500975ec9ba54bc97d2c1e7f7854382eda..55edcf88ef81a0f499ca27feb5830a09f8c8d3da 100644 (file)
--- a/src/FDM/JSBSim/FGQuaternion.cpp
+++ b/src/FDM/JSBSim/FGQuaternion.cpp
@@ -130,12 +130,17 @@ FGQuaternion::FGQuaternion(double phi, double tht, double psi)
    angular velocities PQR.
 */
 FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
+  double norm = Magnitude();
+  if (norm == 0.0)
+    return FGQuaternion::zero();
+  double rnorm = 1.0/norm;
+
   FGQuaternion QDot;
   QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
   QDot(2) =  0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
   QDot(3) =  0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
   QDot(4) =  0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
-  return QDot;
+  return rnorm*QDot;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%