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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <string>
  29
  30 #include "AIMultiplayer.hxx"
  31
  32 #include <simgear/scene/util/SGNodeMasks.hxx>
  33
  34 // #define SG_DEBUG SG_ALERT
  35
  36 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
  37    no_roll = false;
  38
  39    mTimeOffsetSet = false;
  40    mAllowExtrapolation = true;
  41    mLagAdjustSystemSpeed = 10;
  42    mLastTimestamp = 0;
  43    aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT);
  44    lastUpdateTime = 0;
  45
  46 }
  47
  48 FGAIMultiplayer::~FGAIMultiplayer() {
  49 }
  50
  51 bool FGAIMultiplayer::init(bool search_in_AI_path) {
  52     props->setStringValue("sim/model/path", model_path.c_str());
  53     //refuel_node = fgGetNode("systems/refuel/contact", true);
  54     isTanker = false; // do this until this property is
  55                       // passed over the net
  56
  57     string str1 = _getCallsign();
  58     string str2 = "MOBIL";
  59
  60     string::size_type loc1= str1.find( str2, 0 );
  61     if ( (loc1 != string::npos && str2 != "") ){
  62         //         cout << " string found "     << str2 << " in " << str1 << endl;
  63         isTanker = true;
  64         //         cout << "isTanker " << isTanker << " " << mCallSign <<endl;
  65     }
  66
  67     // load model
  68     bool result = FGAIBase::init(search_in_AI_path);
  69     // propagate installation state (used by MP pilot list)
  70     props->setBoolValue("model-installed", _installed);
  71     return result;
  72 }
  73
  74 void FGAIMultiplayer::bind() {
  75     FGAIBase::bind();
  76
  77     props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));
  78     props->tie("tanker", SGRawValuePointer<bool>(&isTanker));
  79
  80     props->tie("controls/invisible",
  81         SGRawValuePointer<bool>(&invisible));
  82
  83 #define AIMPROProp(type, name) \
  84 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
  85
  86 #define AIMPRWProp(type, name) \
  87 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
  88       &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
  89
  90     //props->tie("callsign", AIMPROProp(const char *, CallSign));
  91
  92     props->tie("controls/allow-extrapolation",
  93                AIMPRWProp(bool, AllowExtrapolation));
  94     props->tie("controls/lag-adjust-system-speed",
  95                AIMPRWProp(double, LagAdjustSystemSpeed));
  96
  97
  98 #undef AIMPROProp
  99 #undef AIMPRWProp
 100 }
 101
 102 void FGAIMultiplayer::unbind() {
 103     FGAIBase::unbind();
 104
 105     //props->untie("callsign");
 106     props->untie("controls/allow-extrapolation");
 107     props->untie("controls/lag-adjust-system-speed");
 108     props->untie("controls/invisible");
 109     props->untie("refuel/contact");
 110     props->untie("tanker");
 111
 112 }
 113
 114 void FGAIMultiplayer::update(double dt)
 115 {
 116   using namespace simgear;
 117
 118   if (dt <= 0)
 119     return;
 120
 121   FGAIBase::update(dt);
 122
 123   // Check if we already got data
 124   if (mMotionInfo.empty())
 125     return;
 126
 127   // The current simulation time we need to update for,
 128   // note that the simulation time is updated before calling all the
 129   // update methods. Thus it contains the time intervals *end* time
 130   double curtime = globals->get_sim_time_sec();
 131
 132   // Get the last available time
 133   MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
 134   double curentPkgTime = it->second.time;
 135
 136   // Dynamically optimize the time offset between the feeder and the client
 137   // Well, 'dynamically' means that the dynamic of that update must be very
 138   // slow. You would otherwise notice huge jumps in the multiplayer models.
 139   // The reason is that we want to avoid huge extrapolation times since
 140   // extrapolation is highly error prone. For that we need something
 141   // approaching the average latency of the packets. This first order lag
 142   // component will provide this. We just take the error of the currently
 143   // requested time to the most recent available packet. This is the
 144   // target we want to reach in average.
 145   double lag = it->second.lag;
 146   if (!mTimeOffsetSet) {
 147     mTimeOffsetSet = true;
 148     mTimeOffset = curentPkgTime - curtime - lag;
 149   } else {
 150     double offset = curentPkgTime - curtime - lag;
 151     if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
 152         || (offset - 10 > mTimeOffset)) {
 153       mTimeOffset = offset;
 154       SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
 155              "avoid extrapolation: time offset = " << mTimeOffset);
 156     } else {
 157       // the error of the offset, respectively the negative error to avoid
 158       // a minus later ...
 159       double err = offset - mTimeOffset;
 160       // limit errors leading to shorter lag values somehow, that is late
 161       // arriving packets will pessimize the overall lag much more than
 162       // early packets will shorten the overall lag
 163       double sysSpeed;
 164       if (err < 0) {
 165         // Ok, we have some very late packets and nothing newer increase the
 166         // lag by the given speedadjust
 167         sysSpeed = mLagAdjustSystemSpeed*err;
 168       } else {
 169         // We have a too pessimistic display delay shorten that a small bit
 170         sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
 171       }
 172
 173       // simple euler integration for that first order system including some
 174       // overshooting guard to prevent to aggressive system speeds
 175       // (stiff systems) to explode the systems state
 176       double systemIncrement = dt*sysSpeed;
 177       if (fabs(err) < fabs(systemIncrement))
 178         systemIncrement = err;
 179       mTimeOffset += systemIncrement;
 180
 181       SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
 182              << mTimeOffset << ", expected longitudinal position error due to "
 183              " current adjustment of the offset: "
 184              << fabs(norm(it->second.linearVel)*systemIncrement));
 185     }
 186   }
 187
 188
 189   // Compute the time in the feeders time scale which fits the current time
 190   // we need to
 191   double tInterp = curtime + mTimeOffset;
 192
 193   SGVec3d ecPos;
 194   SGQuatf ecOrient;
 195
 196   if (tInterp <= curentPkgTime) {
 197     // Ok, we need a time prevous to the last available packet,
 198     // that is good ...
 199
 200     // Find the first packet before the target time
 201     MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
 202     if (nextIt == mMotionInfo.begin()) {
 203       SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
 204       // We have no packet before the target time, just use the first one
 205       MotionInfo::iterator firstIt = mMotionInfo.begin();
 206       ecPos = firstIt->second.position;
 207       ecOrient = firstIt->second.orientation;
 208       speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
 209
 210       std::vector<FGPropertyData*>::const_iterator firstPropIt;
 211       std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
 212       firstPropIt = firstIt->second.properties.begin();
 213       firstPropItEnd = firstIt->second.properties.end();
 214       while (firstPropIt != firstPropItEnd) {
 215         //cout << " Setting property..." << (*firstPropIt)->id;
 216         PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
 217         if (pIt != mPropertyMap.end())
 218         {
 219           //cout << "Found " << pIt->second->getPath() << ":";
 220           switch ((*firstPropIt)->type) {
 221             case props::INT:
 222             case props::BOOL:
 223             case props::LONG:
 224               pIt->second->setIntValue((*firstPropIt)->int_value);
 225               //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 226               break;
 227             case props::FLOAT:
 228             case props::DOUBLE:
 229               pIt->second->setFloatValue((*firstPropIt)->float_value);
 230               //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 231               break;
 232             case props::STRING:
 233             case props::UNSPECIFIED:
 234               pIt->second->setStringValue((*firstPropIt)->string_value);
 235               //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 236               break;
 237             default:
 238               // FIXME - currently defaults to float values
 239               pIt->second->setFloatValue((*firstPropIt)->float_value);
 240               //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
 241               break;
 242           }
 243         }
 244         else
 245         {
 246           SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 247         }
 248         ++firstPropIt;
 249       }
 250
 251     } else {
 252       // Ok, we have really found something where our target time is in between
 253       // do interpolation here
 254       MotionInfo::iterator prevIt = nextIt;
 255       --prevIt;
 256
 257       // Interpolation coefficient is between 0 and 1
 258       double intervalStart = prevIt->second.time;
 259       double intervalEnd = nextIt->second.time;
 260       double intervalLen = intervalEnd - intervalStart;
 261       double tau = (tInterp - intervalStart)/intervalLen;
 262
 263       SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
 264              << intervalStart << ", " << intervalEnd << "], intervalLen = "
 265              << intervalLen << ", interpolation parameter = " << tau);
 266
 267       // Here we do just linear interpolation on the position
 268       ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
 269       ecOrient = interpolate((float)tau, prevIt->second.orientation,
 270                              nextIt->second.orientation);
 271       speed = norm((1-tau)*prevIt->second.linearVel
 272                    + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
 273
 274       if (prevIt->second.properties.size()
 275           == nextIt->second.properties.size()) {
 276         std::vector<FGPropertyData*>::const_iterator prevPropIt;
 277         std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
 278         std::vector<FGPropertyData*>::const_iterator nextPropIt;
 279         std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
 280         prevPropIt = prevIt->second.properties.begin();
 281         prevPropItEnd = prevIt->second.properties.end();
 282         nextPropIt = nextIt->second.properties.begin();
 283         nextPropItEnd = nextIt->second.properties.end();
 284         while (prevPropIt != prevPropItEnd) {
 285           PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
 286           //cout << " Setting property..." << (*prevPropIt)->id;
 287
 288           if (pIt != mPropertyMap.end())
 289           {
 290             //cout << "Found " << pIt->second->getPath() << ":";
 291
 292             int ival;
 293             float val;
 294             switch ((*prevPropIt)->type) {
 295               case props::INT:
 296               case props::BOOL:
 297               case props::LONG:
 298                 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
 299                   tau*((double) (*nextPropIt)->int_value));
 300                 pIt->second->setIntValue(ival);
 301                 //cout << "Int: " << ival << "\n";
 302                 break;
 303               case props::FLOAT:
 304               case props::DOUBLE:
 305                 val = (1-tau)*(*prevPropIt)->float_value +
 306                   tau*(*nextPropIt)->float_value;
 307                 //cout << "Flo: " << val << "\n";
 308                 pIt->second->setFloatValue(val);
 309                 break;
 310               case props::STRING:
 311               case props::UNSPECIFIED:
 312                 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
 313                 pIt->second->setStringValue((*nextPropIt)->string_value);
 314                 break;
 315               default:
 316                 // FIXME - currently defaults to float values
 317                 val = (1-tau)*(*prevPropIt)->float_value +
 318                   tau*(*nextPropIt)->float_value;
 319                 //cout << "Unk: " << val << "\n";
 320                 pIt->second->setFloatValue(val);
 321                 break;
 322             }
 323           }
 324           else
 325           {
 326             SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
 327           }
 328
 329           ++prevPropIt;
 330           ++nextPropIt;
 331         }
 332       }
 333
 334       // Now throw away too old data
 335       if (prevIt != mMotionInfo.begin())
 336       {
 337         --prevIt;
 338         mMotionInfo.erase(mMotionInfo.begin(), prevIt);
 339       }
 340     }
 341   } else {
 342     // Ok, we need to predict the future, so, take the best data we can have
 343     // and do some eom computation to guess that for now.
 344     FGExternalMotionData& motionInfo = it->second;
 345
 346     // The time to predict, limit to 5 seconds
 347     double t = tInterp - motionInfo.time;
 348     t = SGMisc<double>::min(t, 5);
 349
 350     SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
 351            "extrapolation time = " << t);
 352
 353     // Do a few explicit euler steps with the constant acceleration's
 354     // This must be sufficient ...
 355     ecPos = motionInfo.position;
 356     ecOrient = motionInfo.orientation;
 357     SGVec3f linearVel = motionInfo.linearVel;
 358     SGVec3f angularVel = motionInfo.angularVel;
 359     while (0 < t) {
 360       double h = 1e-1;
 361       if (t < h)
 362         h = t;
 363
 364       SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
 365       ecPos += h*ecVel;
 366       ecOrient += h*ecOrient.derivative(angularVel);
 367
 368       linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
 369       angularVel += h*motionInfo.angularAccel;
 370
 371       t -= h;
 372     }
 373
 374     std::vector<FGPropertyData*>::const_iterator firstPropIt;
 375     std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
 376     speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
 377     firstPropIt = it->second.properties.begin();
 378     firstPropItEnd = it->second.properties.end();
 379     while (firstPropIt != firstPropItEnd) {
 380       PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
 381       //cout << " Setting property..." << (*firstPropIt)->id;
 382
 383       if (pIt != mPropertyMap.end())
 384       {
 385         switch ((*firstPropIt)->type) {
 386           case props::INT:
 387           case props::BOOL:
 388           case props::LONG:
 389             pIt->second->setIntValue((*firstPropIt)->int_value);
 390             //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 391             break;
 392           case props::FLOAT:
 393           case props::DOUBLE:
 394             pIt->second->setFloatValue((*firstPropIt)->float_value);
 395             //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 396             break;
 397           case props::STRING:
 398           case props::UNSPECIFIED:
 399             pIt->second->setStringValue((*firstPropIt)->string_value);
 400             //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 401             break;
 402           default:
 403             // FIXME - currently defaults to float values
 404             pIt->second->setFloatValue((*firstPropIt)->float_value);
 405             //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
 406             break;
 407         }
 408       }
 409       else
 410       {
 411         SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 412       }
 413
 414       ++firstPropIt;
 415     }
 416   }
 417
 418   // extract the position
 419   pos = SGGeod::fromCart(ecPos);
 420   double recent_alt_ft = altitude_ft;
 421   altitude_ft = pos.getElevationFt();
 422
 423   // expose a valid vertical speed
 424   if (lastUpdateTime != 0)
 425   {
 426       double dT = curtime - lastUpdateTime;
 427       double Weighting=1;
 428       if (dt < 1.0)
 429           Weighting = dt;
 430       // simple smoothing over 1 second
 431       vs = (1.0-Weighting)*vs +  Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
 432   }
 433   lastUpdateTime = curtime;
 434
 435   // The quaternion rotating from the earth centered frame to the
 436   // horizontal local frame
 437   SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
 438                                           (float)pos.getLatitudeRad());
 439   // The orientation wrt the horizontal local frame
 440   SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
 441   float hDeg, pDeg, rDeg;
 442   hlOr.getEulerDeg(hDeg, pDeg, rDeg);
 443   hdg = hDeg;
 444   roll = rDeg;
 445   pitch = pDeg;
 446
 447   SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
 448          << ecPos << ", " << hlOr);
 449
 450   //###########################//
 451   // do calculations for radar //
 452   //###########################//
 453     double range_ft2 = UpdateRadar(manager);
 454
 455     //************************************//
 456     // Tanker code                        //
 457     //************************************//
 458
 459
 460     if ( isTanker) {
 461         //cout << "IS tanker ";
 462         if ( (range_ft2 < 250.0 * 250.0) &&
 463             (y_shift > 0.0)    &&
 464             (elevation > 0.0) ){
 465                 // refuel_node->setBoolValue(true);
 466                  //cout << "in contact"  << endl;
 467             contact = true;
 468         } else {
 469             // refuel_node->setBoolValue(false);
 470             //cout << "not in contact"  << endl;
 471             contact = false;
 472         }
 473     } else {
 474         //cout << "NOT tanker " << endl;
 475         contact = false;
 476     }
 477
 478   Transform();
 479 }
 480
 481 void
 482 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
 483                                long stamp)
 484 {
 485   mLastTimestamp = stamp;
 486
 487   if (!mMotionInfo.empty()) {
 488     double diff = motionInfo.time - mMotionInfo.rbegin()->first;
 489
 490     // packet is very old -- MP has probably reset (incl. his timebase)
 491     if (diff < -10.0)
 492       mMotionInfo.clear();
 493
 494     // drop packets arriving out of order
 495     else if (diff < 0.0)
 496       return;
 497   }
 498   mMotionInfo[motionInfo.time] = motionInfo;
 499   // We just copied the property (pointer) list - they are ours now. Clear the
 500   // properties list in given/returned object, so former owner won't deallocate them.
 501   motionInfo.properties.clear();
 502 }
 503
 504 void
 505 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
 506 {
 507   SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
 508   pNode->setDoubleValue(val);
 509 }