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