Make better use of SGMath

diff --git a/src/AIModel/AIMultiplayer.cxx b/src/AIModel/AIMultiplayer.cxx
index 000386b88408952dc310063d5b31f6b839b03af8..e7a3abd47eaad07591e9d7662dc27c75930c00ab 100755 (executable)
--- a/src/AIModel/AIMultiplayer.cxx
+++ b/src/AIModel/AIMultiplayer.cxx
@@ -172,7 +172,6 @@ void FGAIMultiplayer::update(double dt)
 
   SGVec3d ecPos;
   SGQuatf ecOrient;
-  SGVec3f myVel;
 
   if (tInterp <= curentPkgTime) {
     // Ok, we need a time prevous to the last available packet,
@@ -186,7 +185,7 @@ void FGAIMultiplayer::update(double dt)
       MotionInfo::iterator firstIt = mMotionInfo.begin();
       ecPos = firstIt->second.position;
       ecOrient = firstIt->second.orientation;
-      myVel = firstIt->second.linearVel;
+      speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
 
       std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
       std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
@@ -220,7 +219,8 @@ void FGAIMultiplayer::update(double dt)
       ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
       ecOrient = interpolate((float)tau, prevIt->second.orientation,
                              nextIt->second.orientation);
-      myVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel);
+      speed = norm((1-tau)*prevIt->second.linearVel
+                   + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
 
       if (prevIt->second.properties.size()
           == nextIt->second.properties.size()) {
@@ -266,7 +266,6 @@ void FGAIMultiplayer::update(double dt)
     ecOrient = motionInfo.orientation;
     SGVec3f linearVel = motionInfo.linearVel;
     SGVec3f angularVel = motionInfo.angularVel;
-    myVel = linearVel;
     while (0 < t) {
       double h = 1e-1;
       if (t < h)
@@ -277,12 +276,13 @@ void FGAIMultiplayer::update(double dt)
       ecOrient += h*ecOrient.derivative(angularVel);
 
       linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
-      myVel = linearVel;
       angularVel += h*motionInfo.angularAccel;
       
       t -= h;
     }
 
+    speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
+
     std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
     std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
     firstPropIt = it->second.properties.begin();
@@ -300,13 +300,6 @@ void FGAIMultiplayer::update(double dt)
   pos = SGGeod::fromCart(ecPos);
   altitude_ft = pos.getElevationFt();
 
-  // estimate speed (we care only about magnitude not direction/frame
-  // of reference here)
-  double vel_ms = sqrt( myVel[0]*myVel[0] + myVel[1]*myVel[1]
-                        + myVel[2]*myVel[2] );
-  double vel_kts = vel_ms * SG_METER_TO_NM * 3600.0;
-  speed = vel_kts;
-
   // The quaternion rotating from the earth centered frame to the
   // horizontal local frame
   SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),