src/AIModel/AIEscort.cxx

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