src/AIModel/AIEscort.cxx

   1 // FGAIEscort - FGAIShip-derived class creates an AI Ground Vehicle\r
   2 // by adding a ground following utility\r
   3 //\r
   4 // Written by Vivian Meazza, started August 2009.\r
   5 // - vivian.meazza at lineone.net\r
   6 //\r
   7 // This program is free software; you can redistribute it and/or\r
   8 // modify it under the terms of the GNU General Public License as\r
   9 // published by the Free Software Foundation; either version 2 of the\r
  10 // License, or (at your option) any later version.\r
  11 //\r
  12 // This program is distributed in the hope that it will be useful, but\r
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\r
  15 // General Public License for more details.\r
  16 //\r
  17 // You should have received a copy of the GNU General Public License\r
  18 // along with this program; if not, write to the Free Software\r
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\r
  20 \r
  21 #ifdef HAVE_CONFIG_H\r
  22 #  include <config.h>\r
  23 #endif\r
  24 \r
  25 #include <algorithm>\r
  26 #include <string>\r
  27 #include <vector>\r
  28 \r
  29 #include <simgear/sg_inlines.h>\r
  30 #include <simgear/math/SGMath.hxx>\r
  31 #include <simgear/math/sg_geodesy.hxx>\r
  32 \r
  33 #include <math.h>\r
  34 #include <Main/util.hxx>\r
  35 #include <Main/viewer.hxx>\r
  36 \r
  37 #include <Scenery/scenery.hxx>\r
  38 #include <Scenery/tilemgr.hxx>\r
  39 \r
  40 #include "AIEscort.hxx"\r
  41 \r
  42 using std::string;\r
  43 \r
  44 FGAIEscort::FGAIEscort() :\r
  45 FGAIShip(otEscort),\r
  46 \r
  47 _selected_ac(0),\r
  48 _relbrg (0),\r
  49 _stn_truebrg(0),\r
  50 _parent_speed(0),\r
  51 _stn_limit(0),\r
  52 _stn_angle_limit(0),\r
  53 _stn_speed(0),\r
  54 _stn_height(0),\r
  55 _max_speed(0),\r
  56 _interval(0),\r
  57 _MPControl(false),\r
  58 _patrol(false),\r
  59 _stn_deg_true(false),\r
  60 _parent("")\r
  61 \r
  62 {\r
  63     invisible = false;\r
  64 }\r
  65 \r
  66 FGAIEscort::~FGAIEscort() {}\r
  67 \r
  68 void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {\r
  69     if (!scFileNode)\r
  70         return;\r
  71 \r
  72     FGAIShip::readFromScenario(scFileNode);\r
  73 \r
  74     setName(scFileNode->getStringValue("name", "Escort"));\r
  75     setSMPath(scFileNode->getStringValue("submodel-path", ""));\r
  76     setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));\r
  77     setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));\r
  78     setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));\r
  79     setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));\r
  80     setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));\r
  81     setStnPatrol(scFileNode->getBoolValue("station/patrol", false));\r
  82     setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));\r
  83     setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));\r
  84     setParentName(scFileNode->getStringValue("station/parent", ""));\r
  85     setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));\r
  86     setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));\r
  87     setCallSign(scFileNode->getStringValue("callsign", ""));\r
  88 \r
  89     if(_patrol)\r
  90         sg_srandom_time();\r
  91 \r
  92 }\r
  93 \r
  94 void FGAIEscort::bind() {\r
  95     FGAIShip::bind();\r
  96 \r
  97     props->tie("station/rel-bearing-deg",\r
  98         SGRawValuePointer<double>(&_stn_relbrg));\r
  99     props->tie("station/true-bearing-deg",\r
 100         SGRawValuePointer<double>(&_stn_truebrg));\r
 101     props->tie("station/range-nm",\r
 102         SGRawValuePointer<double>(&_stn_range));\r
 103     props->tie("station/range-limit-nm",\r
 104         SGRawValuePointer<double>(&_stn_limit));\r
 105     props->tie("station/angle-limit-deg",\r
 106         SGRawValuePointer<double>(&_stn_angle_limit));\r
 107     props->tie("station/speed-kts",\r
 108         SGRawValuePointer<double>(&_stn_speed));\r
 109     props->tie("station/height-ft",\r
 110         SGRawValuePointer<double>(&_stn_height));\r
 111     props->tie("controls/update-interval-sec",\r
 112         SGRawValuePointer<double>(&_interval));\r
 113     props->tie("controls/parent-mp-control",\r
 114         SGRawValuePointer<bool>(&_MPControl));\r
 115     props->tie("station/target-range-nm",\r
 116         SGRawValuePointer<double>(&_tgtrange));\r
 117     props->tie("station/target-brg-deg-t",\r
 118         SGRawValuePointer<double>(&_tgtbrg));\r
 119     props->tie("station/patrol",\r
 120         SGRawValuePointer<bool>(&_patrol));\r
 121 }\r
 122 \r
 123 void FGAIEscort::unbind() {\r
 124     FGAIShip::unbind();\r
 125 \r
 126     props->untie("station/rel-bearing-deg");\r
 127     props->untie("station/true-bearing-deg");\r
 128     props->untie("station/range-nm");\r
 129     props->untie("station/range-limit-nm");\r
 130     props->untie("station/angle-limit-deg");\r
 131     props->untie("station/speed-kts");\r
 132     props->untie("station/height-ft");\r
 133     props->untie("controls/update-interval-sec");\r
 134 \r
 135 }\r
 136 \r
 137 bool FGAIEscort::init(bool search_in_AI_path) {\r
 138     if (!FGAIShip::init(search_in_AI_path))\r
 139         return false;\r
 140 \r
 141     invisible = false;\r
 142     no_roll = false;\r
 143 \r
 144     props->setStringValue("controls/parent-name", _parent.c_str());\r
 145     setParentNode();\r
 146     pos = _tgtpos;\r
 147     speed = _parent_speed;\r
 148     hdg = _parent_hdg;\r
 149 \r
 150     return true;\r
 151 }\r
 152 \r
 153 void FGAIEscort::update(double dt) {\r
 154     FGAIShip::update(dt);\r
 155 \r
 156     RunEscort(dt);\r
 157 }\r
 158 \r
 159 void FGAIEscort::setStnRange(double r) {\r
 160     _stn_range = r;\r
 161 }\r
 162 \r
 163 void FGAIEscort::setStnBrg(double b) {\r
 164     _stn_brg = b;\r
 165 }\r
 166 \r
 167 void FGAIEscort::setStnLimit(double l) {\r
 168     _stn_limit = l;\r
 169 }\r
 170 \r
 171 void FGAIEscort::setStnAngleLimit(double al) {\r
 172     _stn_angle_limit = al;\r
 173 }\r
 174 \r
 175 void FGAIEscort::setStnSpeed(double s) {\r
 176     _stn_speed = s;\r
 177 }\r
 178 \r
 179 void FGAIEscort::setStnHtFt(double h) {\r
 180     _stn_height = h;\r
 181 }\r
 182 \r
 183 void FGAIEscort::setStnDegTrue(bool t) {\r
 184     _stn_deg_true = t;\r
 185 }\r
 186 \r
 187 void FGAIEscort::setMaxSpeed(double m) {\r
 188     _max_speed = m;\r
 189 }\r
 190 \r
 191 void FGAIEscort::setUpdateInterval(double i) {\r
 192     _interval = i;\r
 193 }\r
 194 \r
 195 void FGAIEscort::setStnPatrol(bool p) {\r
 196     _patrol = p;\r
 197 }\r
 198 \r
 199 void FGAIEscort::setParentName(const string& p) {\r
 200     _parent = p;\r
 201 }\r
 202 \r
 203 bool FGAIEscort::getGroundElev(SGGeod inpos) {\r
 204 \r
 205     double height_m ;\r
 206 \r
 207     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){\r
 208         _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;\r
 209 \r
 210         if (_material) {\r
 211             const vector<string>& names = _material->get_names();\r
 212 \r
 213             _solid = _material->get_solid();\r
 214 \r
 215             if (!names.empty())\r
 216                 props->setStringValue("material/name", names[0].c_str());\r
 217             else\r
 218                 props->setStringValue("material/name", "");\r
 219 \r
 220             //cout << "material " << names[0].c_str()\r
 221             //    << " _elevation_m " << _elevation_m\r
 222             //    << " solid " << _solid\r
 223             //    << " load " << _load_resistance\r
 224             //    << " frictionFactor " << _frictionFactor\r
 225             //    << endl;\r
 226 \r
 227         }\r
 228 \r
 229         return true;\r
 230     } else {\r
 231         return false;\r
 232     }\r
 233 \r
 234 }\r
 235 \r
 236 void FGAIEscort::setParentNode() {\r
 237 \r
 238     const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);\r
 239 \r
 240     for (int i = ai->nChildren() - 1; i >= -1; i--) {\r
 241         SGPropertyNode_ptr model;\r
 242 \r
 243         if (i < 0) { // last iteration: selected model\r
 244             model = _selected_ac;\r
 245         } else {\r
 246             model = ai->getChild(i);\r
 247             string path = ai->getPath();\r
 248             const string name = model->getStringValue("name");\r
 249 \r
 250             if (!model->nChildren()){\r
 251                 continue;\r
 252             }\r
 253             if (name == _parent) {\r
 254                 _selected_ac = model;  // save selected model for last iteration\r
 255                 break;\r
 256             }\r
 257 \r
 258         }\r
 259         if (!model)\r
 260             continue;\r
 261 \r
 262     }// end for loop\r
 263 \r
 264     if (_selected_ac != 0){\r
 265         const string name = _selected_ac->getStringValue("name");\r
 266         setParent();\r
 267 \r
 268         //double lat = _selected_ac->getDoubleValue("position/latitude-deg");\r
 269         //double lon = _selected_ac->getDoubleValue("position/longitude-deg");\r
 270         //double elevation = _selected_ac->getDoubleValue("position/altitude-ft");\r
 271         //_MPControl = _selected_ac->getBoolValue("controls/mp-control");\r
 272 \r
 273         //_selectedpos.setLatitudeDeg(lat);\r
 274         //_selectedpos.setLongitudeDeg(lon);\r
 275         //_selectedpos.setElevationFt(elevation);\r
 276 \r
 277         //_parent_speed    = _selected_ac->getDoubleValue("velocities/speed-kts");\r
 278         //_parent_hdg      = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
 279 \r
 280         //if(!_stn_deg_true){\r
 281         //    _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);\r
 282         //    _stn_relbrg = _stn_brg;\r
 283         //    //cout << _name <<" set rel"<<endl;\r
 284         //} else {\r
 285         //    _stn_truebrg = _stn_brg;\r
 286         //    _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg); \r
 287         //    //cout << _name << " set true"<<endl;\r
 288         //}\r
 289 \r
 290         //double course2;\r
 291 \r
 292         //SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,\r
 293         //    _tgtpos, course2);\r
 294 \r
 295         //_tgtpos.setElevationFt(_stn_height);\r
 296 \r
 297         //calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),\r
 298         //    _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);\r
 299 \r
 300         //_relbrg = calcRelBearingDeg(_tgtbrg, hdg);\r
 301 \r
 302     } else {\r
 303         SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name\r
 304             << " parent not found: dying ");\r
 305         setDie(true);\r
 306     }\r
 307 \r
 308 }\r
 309 \r
 310 void FGAIEscort::setParent()\r
 311 {\r
 312     double lat = _selected_ac->getDoubleValue("position/latitude-deg");\r
 313     double lon = _selected_ac->getDoubleValue("position/longitude-deg");\r
 314     double elevation = _selected_ac->getDoubleValue("position/altitude-ft");\r
 315     _MPControl = _selected_ac->getBoolValue("controls/mp-control");\r
 316 \r
 317     _selectedpos.setLatitudeDeg(lat);\r
 318     _selectedpos.setLongitudeDeg(lon);\r
 319     _selectedpos.setElevationFt(elevation);\r
 320 \r
 321     _parent_speed    = _selected_ac->getDoubleValue("velocities/speed-kts");\r
 322     _parent_hdg      = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
 323 \r
 324     if(!_stn_deg_true){\r
 325         _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);\r
 326         _stn_relbrg = _stn_brg;\r
 327         //cout << _name <<" set rel"<<endl;\r
 328     } else {\r
 329         _stn_truebrg = _stn_brg;\r
 330         _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg); \r
 331         //cout << _name << " set true"<<endl;\r
 332     }\r
 333 \r
 334             double course2;\r
 335 \r
 336         SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,\r
 337             _tgtpos, course2);\r
 338 \r
 339         _tgtpos.setElevationFt(_stn_height);\r
 340 \r
 341         calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),\r
 342             _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);\r
 343 \r
 344         _relbrg = calcRelBearingDeg(_tgtbrg, hdg);\r
 345 \r
 346 }\r
 347 \r
 348 void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,\r
 349                                   double &range, double &bearing) const\r
 350 {\r
 351     // calculate the bearing and range of the second pos from the first\r
 352     double az2, distance;\r
 353     geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);\r
 354     range = distance * SG_METER_TO_NM;\r
 355 }\r
 356 \r
 357 double FGAIEscort::calcRelBearingDeg(double bearing, double heading)\r
 358 {\r
 359     double angle = bearing - heading;\r
 360     SG_NORMALIZE_RANGE(angle, -180.0, 180.0);\r
 361     return angle;\r
 362 }\r
 363 \r
 364 double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)\r
 365 {\r
 366     double angle = bearing + heading;\r
 367     SG_NORMALIZE_RANGE(angle, 0.0, 360.0);\r
 368     return angle;\r
 369 }\r
 370 \r
 371 double FGAIEscort::calcRecipBearingDeg(double bearing)\r
 372 {\r
 373     double angle = bearing - 180;\r
 374     SG_NORMALIZE_RANGE(angle, 0.0, 360.0);\r
 375     return angle;\r
 376 }\r
 377 \r
 378 SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {\r
 379     double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
 380     double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");\r
 381     double roll = _selected_ac->getDoubleValue("orientation/roll-deg");\r
 382 \r
 383     // Transform that one to the horizontal local coordinate system.\r
 384     SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);\r
 385 \r
 386     // and postrotate the orientation of the AIModel wrt the horizontal\r
 387     // local frame\r
 388     hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);\r
 389 \r
 390     // The offset converted to the usual body fixed coordinate system\r
 391     // rotated to the earth fiexed coordinates axis\r
 392     SGVec3d off = hlTrans.backTransform(_off);\r
 393 \r
 394     // Add the position offset of the AIModel to gain the earth centered position\r
 395     SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);\r
 396 \r
 397     return cartPos + off;\r
 398 }\r
 399 \r
 400 \r
 401 void FGAIEscort::setStationSpeed(){\r
 402 \r
 403     double speed = 0;\r
 404     double angle = 0;\r
 405 \r
 406     // these are the AI rules for the manoeuvring of escorts\r
 407 \r
 408     if (_MPControl && _tgtrange > 4 * _stn_limit){\r
 409         SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name\r
 410             << " re-aligning to MP pos");\r
 411         pos = _tgtpos;\r
 412         speed = 0;\r
 413         angle = 0;\r
 414     }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){\r
 415         angle =_relbrg;\r
 416 \r
 417         if(_tgtrange > 4 * _stn_limit)\r
 418             speed = 4 * -_stn_speed;\r
 419         else\r
 420             speed = -_stn_speed;\r
 421 \r
 422     }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){\r
 423         angle = _relbrg;\r
 424 \r
 425         if(_tgtrange > 4 * _stn_limit)\r
 426             speed = 4 * _stn_speed;\r
 427         else\r
 428             speed = _stn_speed;\r
 429 \r
 430     } else {\r
 431 \r
 432         if(_patrol){\r
 433             angle = 15 * sg_random();\r
 434             speed =  5 * sg_random();\r
 435         } else {\r
 436             angle = 0;\r
 437             speed = 0;\r
 438         }\r
 439 \r
 440     }\r
 441 \r
 442     double station_speed = _parent_speed + speed;\r
 443 \r
 444     SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);\r
 445     SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);\r
 446 \r
 447     AccelTo(station_speed);\r
 448     TurnTo(_parent_hdg + angle);\r
 449     ClimbTo(_stn_height);\r
 450 \r
 451 }\r
 452 \r
 453 void FGAIEscort::RunEscort(double dt){\r
 454 \r
 455     _dt_count += dt;\r
 456 \r
 457 \r
 458 \r
 459     ///////////////////////////////////////////////////////////////////////////\r
 460     // Check execution time (currently once every 0.05 sec or 20 fps)\r
 461     // Add a bit of randomization to prevent the execution of all flight plans\r
 462     // in synchrony, which can add significant periodic framerate flutter.\r
 463     // Randomization removed to get better appearance\r
 464     ///////////////////////////////////////////////////////////////////////////\r
 465 \r
 466     //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;\r
 467     if (_dt_count < _next_run)\r
 468         return;\r
 469     _next_run = _interval /*+ (0.015 * sg_random())*/;\r
 470 \r
 471     if(_parent == ""){\r
 472         return;\r
 473     }\r
 474 \r
 475     setParent();\r
 476     setStationSpeed();\r
 477     //getGroundElev(pos);\r
 478 \r
 479     _dt_count = 0;\r
 480 \r
 481 }\r
 482 \r
 483 // end AIGroundvehicle\r