src/AIModel/AIMultiplayer.cxx

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