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