src/AIModel/AIMultiplayer.cxx

   1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
   2 //
   3 // Based on FGAIAircraft
   4 // Written by David Culp, started October 2003.
   5 // Also by Gregor Richards, started December 2005.
   6 //
   7 // Copyright (C) 2003  David P. Culp - davidculp2@comcast.net
   8 // Copyright (C) 2005  Gregor Richards
   9 //
  10 // This program is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU General Public License as
  12 // published by the Free Software Foundation; either version 2 of the
  13 // License, or (at your option) any later version.
  14 //
  15 // This program is distributed in the hope that it will be useful, but
  16 // WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU General Public License
  21 // along with this program; if not, write to the Free Software
  22 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <string>
  29
  30 #include "AIMultiplayer.hxx"
  31
  32 // #define SG_DEBUG SG_ALERT
  33
  34 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
  35    no_roll = false;
  36
  37    mTimeOffsetSet = false;
  38    mAllowExtrapolation = true;
  39    mLagAdjustSystemSpeed = 10;
  40 }
  41
  42
  43 FGAIMultiplayer::~FGAIMultiplayer() {
  44 }
  45
  46 bool FGAIMultiplayer::init() {
  47    return FGAIBase::init();
  48 }
  49
  50 void FGAIMultiplayer::bind() {
  51     FGAIBase::bind();
  52
  53 #define AIMPROProp(type, name) \
  54 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
  55
  56 #define AIMPRWProp(type, name) \
  57 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
  58       &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
  59
  60     props->tie("callsign", AIMPROProp(const char *, CallSign));
  61
  62     props->tie("controls/allow-extrapolation",
  63                AIMPRWProp(bool, AllowExtrapolation));
  64     props->tie("controls/lag-adjust-system-speed",
  65                AIMPRWProp(double, LagAdjustSystemSpeed));
  66
  67 #undef AIMPROProp
  68 #undef AIMPRWProp
  69 }
  70
  71 void FGAIMultiplayer::unbind() {
  72     FGAIBase::unbind();
  73
  74     props->untie("callsign");
  75     props->untie("controls/allow-extrapolation");
  76     props->untie("controls/lag-adjust-system-speed");
  77 }
  78
  79 void FGAIMultiplayer::update(double dt)
  80 {
  81   if (dt <= 0)
  82     return;
  83
  84   FGAIBase::update(dt);
  85
  86   // Check if we already got data
  87   if (mMotionInfo.empty())
  88     return;
  89
  90   // The current simulation time we need to update for,
  91   // note that the simulation time is updated before calling all the
  92   // update methods. Thus it contains the time intervals *end* time
  93   double curtime = globals->get_sim_time_sec();
  94
  95   // Get the last available time
  96   MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
  97   double curentPkgTime = it->second.time;
  98
  99   // Dynamically optimize the time offset between the feeder and the client
 100   // Well, 'dynamically' means that the dynamic of that update must be very
 101   // slow. You would otherwise notice huge jumps in the multiplayer models.
 102   // The reason is that we want to avoid huge extrapolation times since
 103   // extrapolation is highly error prone. For that we need something
 104   // approaching the average latency of the packets. This first order lag
 105   // component will provide this. We just take the error of the currently
 106   // requested time to the most recent available packet. This is the
 107   // target we want to reach in average.
 108   double lag = it->second.lag;
 109   if (!mTimeOffsetSet) {
 110     mTimeOffsetSet = true;
 111     mTimeOffset = curentPkgTime - curtime - lag;
 112   } else {
 113     double offset = curentPkgTime - curtime - lag;
 114     if (!mAllowExtrapolation && offset + lag < mTimeOffset) {
 115       mTimeOffset = offset;
 116       SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
 117              "avoid extrapolation: time offset = " << mTimeOffset);
 118     } else {
 119       // the error of the offset, respectively the negative error to avoid
 120       // a minus later ...
 121       double err = offset - mTimeOffset;
 122       // limit errors leading to shorter lag values somehow, that is late
 123       // arriving packets will pessimize the overall lag much more than
 124       // early packets will shorten the overall lag
 125       double sysSpeed;
 126       if (err < 0) {
 127         // Ok, we have some very late packets and nothing newer increase the
 128         // lag by the given speedadjust
 129         sysSpeed = mLagAdjustSystemSpeed*err;
 130       } else {
 131         // We have a too pessimistic display delay shorten that a small bit
 132         sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
 133       }
 134
 135       // simple euler integration for that first order system including some
 136       // overshooting guard to prevent to aggressive system speeds
 137       // (stiff systems) to explode the systems state
 138       double systemIncrement = dt*sysSpeed;
 139       if (fabs(err) < fabs(systemIncrement))
 140         systemIncrement = err;
 141       mTimeOffset += systemIncrement;
 142
 143       SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
 144              << mTimeOffset << ", expected longitudinal position error due to "
 145              " current adjustment of the offset: "
 146              << fabs(norm(it->second.linearVel)*systemIncrement));
 147     }
 148   }
 149
 150
 151   // Compute the time in the feeders time scale which fits the current time
 152   // we need to
 153   double tInterp = curtime + mTimeOffset;
 154
 155   SGVec3d ecPos;
 156   SGQuatf ecOrient;
 157   if (tInterp <= curentPkgTime) {
 158     // Ok, we need a time prevous to the last available packet,
 159     // that is good ...
 160
 161     // Find the first packet before the target time
 162     MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
 163     if (nextIt == mMotionInfo.begin()) {
 164       SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
 165       // We have no packet before the target time, just use the first one
 166       MotionInfo::iterator firstIt = mMotionInfo.begin();
 167       ecPos = firstIt->second.position;
 168       ecOrient = firstIt->second.orientation;
 169
 170       std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
 171       std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
 172       firstPropIt = firstIt->second.properties.begin();
 173       firstPropItEnd = firstIt->second.properties.end();
 174       while (firstPropIt != firstPropItEnd) {
 175         float val = firstPropIt->value;
 176         PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
 177         if (pIt != mPropertyMap.end())
 178           pIt->second->setFloatValue(val);
 179         ++firstPropIt;
 180       }
 181
 182     } else {
 183       // Ok, we have really found something where our target time is in between
 184       // do interpolation here
 185       MotionInfo::iterator prevIt = nextIt;
 186       --prevIt;
 187
 188       // Interpolation coefficient is between 0 and 1
 189       double intervalStart = prevIt->second.time;
 190       double intervalEnd = nextIt->second.time;
 191       double intervalLen = intervalEnd - intervalStart;
 192       double tau = (tInterp - intervalStart)/intervalLen;
 193
 194       SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
 195              << intervalStart << ", " << intervalEnd << "], intervalLen = "
 196              << intervalLen << ", interpolation parameter = " << tau);
 197
 198       // Here we do just linear interpolation on the position
 199       ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
 200       ecOrient = interpolate((float)tau, prevIt->second.orientation,
 201                              nextIt->second.orientation);
 202
 203       if (prevIt->second.properties.size()
 204           == nextIt->second.properties.size()) {
 205         std::vector<FGFloatPropertyData>::const_iterator prevPropIt;
 206         std::vector<FGFloatPropertyData>::const_iterator prevPropItEnd;
 207         std::vector<FGFloatPropertyData>::const_iterator nextPropIt;
 208         std::vector<FGFloatPropertyData>::const_iterator nextPropItEnd;
 209         prevPropIt = prevIt->second.properties.begin();
 210         prevPropItEnd = prevIt->second.properties.end();
 211         nextPropIt = nextIt->second.properties.begin();
 212         nextPropItEnd = nextIt->second.properties.end();
 213         while (prevPropIt != prevPropItEnd) {
 214           float val = (1-tau)*prevPropIt->value + tau*nextPropIt->value;
 215           PropertyMap::iterator pIt = mPropertyMap.find(prevPropIt->id);
 216           if (pIt != mPropertyMap.end())
 217             pIt->second->setFloatValue(val);
 218           ++prevPropIt;
 219           ++nextPropIt;
 220         }
 221       }
 222
 223       // Now throw away too old data
 224       if (prevIt != mMotionInfo.begin()) {
 225         --prevIt;
 226         mMotionInfo.erase(mMotionInfo.begin(), prevIt);
 227       }
 228     }
 229   } else {
 230     // Ok, we need to predict the future, so, take the best data we can have
 231     // and do some eom computation to guess that for now.
 232     FGExternalMotionData motionInfo = it->second;
 233
 234     // The time to predict, limit to 5 seconds
 235     double t = tInterp - motionInfo.time;
 236     t = SGMisc<double>::min(t, 5);
 237
 238     SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
 239            "extrapolation time = " << t);
 240
 241     // Do a few explicit euler steps with the constant acceleration's
 242     // This must be sufficient ...
 243     ecPos = motionInfo.position;
 244     ecOrient = motionInfo.orientation;
 245     SGVec3f linearVel = motionInfo.linearVel;
 246     SGVec3f angularVel = motionInfo.angularVel;
 247     while (0 < t) {
 248       double h = 1e-1;
 249       if (t < h)
 250         h = t;
 251
 252       SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
 253       ecPos += h*ecVel;
 254       ecOrient += h*ecOrient.derivative(angularVel);
 255
 256       linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
 257       angularVel += h*motionInfo.angularAccel;
 258
 259       t -= h;
 260     }
 261
 262     std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
 263     std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
 264     firstPropIt = it->second.properties.begin();
 265     firstPropItEnd = it->second.properties.end();
 266     while (firstPropIt != firstPropItEnd) {
 267       float val = firstPropIt->value;
 268       PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
 269       if (pIt != mPropertyMap.end())
 270         pIt->second->setFloatValue(val);
 271       ++firstPropIt;
 272     }
 273   }
 274
 275   // extract the position
 276   SGGeod geod = ecPos;
 277   pos.setlat(geod.getLatitudeDeg());
 278   pos.setlon(geod.getLongitudeDeg());
 279   pos.setelev(geod.getElevationM());
 280
 281   // The quaternion rotating from the earth centered frame to the
 282   // horizontal local frame
 283   SGQuatf qEc2Hl = SGQuatf::fromLonLat((float)geod.getLongitudeRad(),
 284                                        (float)geod.getLatitudeRad());
 285   // The orientation wrt the horizontal local frame
 286   SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
 287   float hDeg, pDeg, rDeg;
 288   hlOr.getEulerDeg(hDeg, pDeg, rDeg);
 289   hdg = hDeg;
 290   roll = rDeg;
 291   pitch = pDeg;
 292
 293   SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
 294          << geod << ", " << hlOr);
 295
 296   //###########################//
 297   // do calculations for radar //
 298   //###########################//
 299   UpdateRadar(manager);
 300
 301   Transform();
 302 }
 303
 304 void
 305 FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
 306                                long stamp)
 307 {
 308   mLastTimestamp = stamp;
 309   // Drop packets arriving out of order
 310   if (!mMotionInfo.empty() && motionInfo.time < mMotionInfo.rbegin()->first)
 311     return;
 312   mMotionInfo[motionInfo.time] = motionInfo;
 313 }
 314
 315 void
 316 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
 317 {
 318   SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
 319   pNode->setDoubleValue(val);
 320 }
 321