src/AIModel/AIGroundVehicle.cxx

   1 // FGAIGroundVehicle - 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 <simgear/sg_inlines.h>
  26
  27 #include <Main/viewer.hxx>
  28 #include <Scenery/scenery.hxx>
  29 #include <Scenery/tilemgr.hxx>
  30 #include <Airports/dynamics.hxx>
  31
  32 #include "AIGroundVehicle.hxx"
  33
  34 FGAIGroundVehicle::FGAIGroundVehicle() :
  35 FGAIShip(otGroundVehicle),
  36
  37 _pitch(0),
  38 _pitch_deg(0),
  39 _speed_kt(0),
  40 _range_ft(0),
  41 _relbrg (0),
  42 _parent_speed(0),
  43 _parent_x_offset(0),
  44 _selected_ac(0)
  45 {
  46     invisible = false;
  47 }
  48
  49 FGAIGroundVehicle::~FGAIGroundVehicle() {}
  50
  51 void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
  52     if (!scFileNode)
  53         return;
  54
  55     FGAIShip::readFromScenario(scFileNode);
  56
  57     setNoRoll(scFileNode->getBoolValue("no-roll", true));
  58     setName(scFileNode->getStringValue("name", "groundvehicle"));
  59     setSMPath(scFileNode->getStringValue("submodel-path", ""));
  60     setContactX1offset(scFileNode->getDoubleValue("contact_x1_offset", 0.0));
  61     setContactX2offset(scFileNode->getDoubleValue("contact_x2_offset", 0.0));
  62     setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55));
  63     setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
  64     setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
  65     setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
  66     setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.5));
  67     setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.5));
  68     setParentName(scFileNode->getStringValue("parent", ""));
  69     setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0));
  70     setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
  71     //we may need these later for towed vehicles
  72     //    setSubID(scFileNode->getIntValue("SubID", 0));
  73     //    setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
  74     //    setFormate(scFileNode->getBoolValue("formate", true));
  75 }
  76
  77 void FGAIGroundVehicle::bind() {
  78     FGAIShip::bind();
  79
  80     props->tie("controls/constants/elevation-coeff",
  81         SGRawValuePointer<double>(&_elevation_coeff));
  82     props->tie("controls/constants/pitch-coeff",
  83         SGRawValuePointer<double>(&_pitch_coeff));
  84     props->tie("position/ht-AGL-ft",
  85         SGRawValuePointer<double>(&_ht_agl_ft));
  86     props->tie("hitch/rel-bearing-deg",
  87          SGRawValuePointer<double>(&_relbrg));
  88     props->tie("hitch/tow-angle-deg",
  89          SGRawValuePointer<double>(&_tow_angle));
  90     props->tie("hitch/range-ft",
  91         SGRawValuePointer<double>(&_range_ft));
  92     props->tie("hitch/x-offset-ft",
  93         SGRawValuePointer<double>(&_x_offset));
  94     props->tie("hitch/parent-x-offset-ft",
  95         SGRawValuePointer<double>(&_parent_x_offset));
  96     props->tie("controls/constants/tow-angle/gain",
  97         SGRawValuePointer<double>(&_tow_angle_gain));
  98     props->tie("controls/constants/tow-angle/limit-deg",
  99         SGRawValuePointer<double>(&_tow_angle_limit));
 100
 101
 102     //we may need these later for towed vehicles
 103
 104     //    (*this, &FGAIBallistic::getElevHitchToUser));
 105     //props->tie("position/x-offset",
 106     //    SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
 107     //props->tie("position/y-offset",
 108     //    SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
 109     //props->tie("position/z-offset",
 110     //    SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
 111     //props->tie("position/tgt-x-offset",
 112     //    SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset));
 113     //props->tie("position/tgt-y-offset",
 114     //    SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset));
 115     //props->tie("position/tgt-z-offset",
 116     //    SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset));
 117 }
 118
 119 void FGAIGroundVehicle::unbind() {
 120     FGAIShip::unbind();
 121
 122     props->untie("controls/constants/elevation-coeff");
 123     props->untie("position/ht-AGL-ft");
 124     props->untie("controls/constants/pitch-coeff");
 125     props->untie("hitch/rel-bearing-deg");
 126     props->untie("hitch/tow-angle-deg");
 127     props->untie("hitch/range-ft");
 128     props->untie("hitch/x-offset-ft");
 129     props->untie("hitch/parent-x-offset-ft");
 130     props->untie("controls/constants/tow-angle/gain");
 131     props->untie("controls/constants/tow-angle/limit-deg");
 132
 133     //we may need these later for towed vehicles
 134     //props->untie("load/rel-brg-to-user-deg");
 135     //props->untie("load/elev-to-user-deg");
 136     //props->untie("velocities/vertical-speed-fps");
 137     //props->untie("position/x-offset");
 138     //props->untie("position/y-offset");
 139     //props->untie("position/z-offset");
 140     //props->untie("position/tgt-x-offset");
 141     //props->untie("position/tgt-y-offset");
 142     //props->untie("position/tgt-z-offset");
 143 }
 144
 145 bool FGAIGroundVehicle::init(bool search_in_AI_path) {
 146     if (!FGAIShip::init(search_in_AI_path))
 147         return false;
 148
 149     invisible = false;
 150
 151     _limit = 200;
 152     no_roll = true;
 153
 154     return true;
 155 }
 156
 157 void FGAIGroundVehicle::update(double dt) {
 158     //    SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
 159
 160     if (getPitch()){
 161         setElevation(_elevation, dt, _elevation_coeff);
 162         ClimbTo(_elevation_ft);
 163         setPitch(_pitch, dt, _pitch_coeff);
 164         PitchTo(_pitch_deg);
 165     }
 166
 167     if(_parent !=""){
 168         setParent();
 169
 170         string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
 171         bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
 172         _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
 173
 174         if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
 175             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 176                 << " setting END: getting new waypoints ");
 177             AdvanceFP();
 178             setWPNames();
 179         /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
 180         } else if (parent_waiting && !_waiting){
 181             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 182                 << " setting WAIT/WAITUNTIL: getting new waypoints ");
 183             AdvanceFP();
 184             setWPNames();
 185             _waiting = true;
 186         } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
 187             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 188                 << " wait done: getting new waypoints ");
 189             _waiting = false;
 190             _wait_count = 0;
 191             fp->IncrementWaypoint(false);
 192             next = fp->getNextWaypoint();
 193
 194             if (next->name == "WAITUNTIL" || next->name == "WAIT"
 195                 || next->name == "END"){
 196             } else {
 197                 prev = curr;
 198                 fp->IncrementWaypoint(false);
 199                 curr = fp->getCurrentWaypoint();
 200                 next = fp->getNextWaypoint();
 201             }
 202
 203             setWPNames();
 204         } else if (_range_ft > 1000){
 205
 206              SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
 207                 << " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
 208
 209             setTowAngle(0, dt, 1);
 210             setSpeed(_parent_speed * 2);
 211
 212         } else if (_parent_speed > 1){
 213
 214             setTowSpeed();
 215             setTowAngle(_relbrg, dt, 1);
 216
 217         } else if (_parent_speed < -1){
 218
 219             setTowSpeed();
 220
 221             if (_relbrg < 0)
 222                 setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
 223             else
 224                 setTowAngle(-(180 - _relbrg), dt, 1);
 225
 226         } else
 227             setSpeed(_parent_speed);
 228     }
 229
 230     FGAIShip::update(dt);
 231 }
 232
 233 void FGAIGroundVehicle::setNoRoll(bool nr) {
 234     no_roll = nr;
 235 }
 236
 237 void FGAIGroundVehicle::setContactX1offset(double x1) {
 238     _contact_x1_offset = x1;
 239 }
 240
 241 void FGAIGroundVehicle::setContactX2offset(double x2) {
 242     _contact_x2_offset = x2;
 243 }
 244
 245 void FGAIGroundVehicle::setXOffset(double x) {
 246     _x_offset = x;
 247 }
 248
 249 void FGAIGroundVehicle::setPitchCoeff(double pc) {
 250     _pitch_coeff = pc;
 251 }
 252
 253 void FGAIGroundVehicle::setElevCoeff(double ec) {
 254     _elevation_coeff = ec;
 255 }
 256
 257 void FGAIGroundVehicle::setTowAngleGain(double g) {
 258     _tow_angle_gain = g;
 259 }
 260
 261 void FGAIGroundVehicle::setTowAngleLimit(double l) {
 262     _tow_angle_limit = l;
 263 }
 264
 265 void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){
 266     double c = dt / (coeff + dt);
 267     _elevation_ft = (h * c) + (_elevation_ft * (1 - c));
 268 }
 269
 270 void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){
 271     double c = dt / (coeff + dt);
 272     _pitch_deg = (p * c) + (_pitch_deg * (1 - c));
 273 }
 274
 275 void FGAIGroundVehicle::setParentName(const string& p) {
 276     _parent = p;
 277 }
 278
 279 void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
 280     ta *= _tow_angle_gain;
 281     //_tow_angle = pow(ta,2) * sign(ta);
 282     //if (_tow_angle > _tow_angle_limit) _tow_angle = _tow_angle_limit;
 283     //if (_tow_angle < -_tow_angle_limit) _tow_angle = -_tow_angle_limit;
 284     SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
 285 }
 286
 287 bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
 288
 289     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000),
 290         _elevation_m, &_material)){
 291             _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 292
 293             if (_material) {
 294                 const vector<string>& names = _material->get_names();
 295
 296                 _solid = _material->get_solid();
 297                 _load_resistance = _material->get_load_resistance();
 298                 _frictionFactor =_material->get_friction_factor();
 299                 _elevation = _elevation_m * SG_METER_TO_FEET;
 300
 301                 if (!names.empty())
 302                     props->setStringValue("material/name", names[0].c_str());
 303                 else
 304                     props->setStringValue("material/name", "");
 305
 306                 //cout << "material " << names[0].c_str()
 307                 //    << " _elevation_m " << _elevation_m
 308                 //    << " solid " << _solid
 309                 //    << " load " << _load_resistance
 310                 //    << " frictionFactor " << _frictionFactor
 311                 //    << endl;
 312
 313             }
 314
 315             return true;
 316     } else {
 317         return false;
 318     }
 319
 320 }
 321
 322 bool FGAIGroundVehicle::getPitch() {
 323
 324     double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
 325     double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
 326     double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
 327
 328     SGVec3d front(-contact_offset_x1_m, 0, 0);
 329     SGVec3d rear(-contact_offset_x2_m, 0, 0);
 330     SGVec3d Front = getCartPosAt(front);
 331     SGVec3d Rear = getCartPosAt(rear);
 332
 333     SGGeod geodFront = SGGeod::fromCart(Front);
 334     SGGeod geodRear = SGGeod::fromCart(Rear);
 335
 336     double front_elev_m = 0;
 337     double rear_elev_m = 0;
 338     double elev_front = 0;
 339     double elev_rear = 0;
 340
 341     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000),
 342         elev_front, &_material)){
 343
 344             front_elev_m = elev_front;
 345
 346             //if (_material) {
 347             //
 348             //}
 349
 350     } else
 351         return false;
 352
 353     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
 354         elev_rear, &_material)){
 355             rear_elev_m = elev_rear;
 356             //if (_material) {
 357             //    rear_elev_m = elev_rear;
 358             //}
 359
 360     } else
 361         return false;
 362
 363     if (vel >= 0){
 364         double diff = front_elev_m - rear_elev_m;
 365         _pitch = atan2 (diff,
 366             fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
 367         _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
 368     } else {
 369         double diff = rear_elev_m - front_elev_m;
 370         _pitch = atan2 (diff,
 371             fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
 372         _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
 373         _pitch = -_pitch;
 374     }
 375
 376     return true;
 377 }
 378
 379 void FGAIGroundVehicle::setParent() {
 380
 381     const SGPropertyNode *ai = fgGetNode("/ai/models", true);
 382
 383     for (int i = ai->nChildren() - 1; i >= -1; i--) {
 384         const SGPropertyNode *model;
 385
 386         if (i < 0) { // last iteration: selected model
 387             model = _selected_ac;
 388         } else {
 389             model = ai->getChild(i);
 390             const string name = model->getStringValue("name");
 391
 392             if (!model->nChildren()){
 393                 continue;
 394             }
 395             if (name == _parent) {
 396                 _selected_ac = model;  // save selected model for last iteration
 397                 break;
 398             }
 399
 400         }
 401         if (!model)
 402             continue;
 403
 404     }// end for loop
 405
 406     if (_selected_ac != 0){
 407         const string name = _selected_ac->getStringValue("name");
 408         double lat = _selected_ac->getDoubleValue("position/latitude-deg");
 409         double lon = _selected_ac->getDoubleValue("position/longitude-deg");
 410         double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
 411         double hitch_offset_m = _x_offset * SG_FEET_TO_METER;
 412         _selectedpos.setLatitudeDeg(lat);
 413         _selectedpos.setLongitudeDeg(lon);
 414         _selectedpos.setElevationFt(elevation);
 415
 416         SGVec3d rear_hitch(-hitch_offset_m, 0, 0);
 417         SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
 418
 419         SGGeod rearpos = SGGeod::fromCart(RearHitch);
 420
 421         double user_lat = rearpos.getLatitudeDeg();
 422         double user_lon = rearpos.getLongitudeDeg();
 423
 424         double range, bearing;
 425
 426         calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
 427             user_lat, user_lon, range, bearing);
 428         _range_ft = range * 6076.11549;
 429         _relbrg = calcRelBearingDeg(bearing, hdg);
 430     } else {
 431         SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
 432                 << " parent not found: dying ");
 433         setDie(true);
 434     }
 435
 436 }
 437
 438 void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2,
 439                             double &range, double &bearing) const
 440 {
 441     // calculate the bearing and range of the second pos from the first
 442     double az2, distance;
 443     geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
 444     range = distance *= SG_METER_TO_NM;
 445 }
 446
 447 double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
 448 {
 449     double angle = bearing - heading;
 450
 451     SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
 452
 453     return angle;
 454 }
 455
 456 SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const {
 457     double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
 458     double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
 459     double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
 460
 461     // Transform that one to the horizontal local coordinate system.
 462     SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
 463
 464     // and postrotate the orientation of the AIModel wrt the horizontal
 465     // local frame
 466     hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
 467
 468     // The offset converted to the usual body fixed coordinate system
 469     // rotated to the earth fiexed coordinates axis
 470     SGVec3d off = hlTrans.backTransform(_off);
 471
 472     // Add the position offset of the AIModel to gain the earth centered position
 473     SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
 474
 475     return cartPos + off;
 476 }
 477
 478 void FGAIGroundVehicle::AdvanceFP(){
 479
 480     double count = 0;
 481     string parent_next_name =_selected_ac->getStringValue("waypoint/name-next");
 482
 483     while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){
 484         SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 485             <<" advancing waypoint to: " << parent_next_name);
 486
 487         if (fp->getNextWaypoint()->name == parent_next_name){
 488             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 489                 << " not setting waypoint already at: " << fp->getNextWaypoint()->name);
 490             return;
 491         }
 492
 493         prev = curr;
 494         fp->IncrementWaypoint(false);
 495         curr = fp->getCurrentWaypoint();
 496         next = fp->getNextWaypoint();
 497
 498         if (fp->getNextWaypoint()->name == parent_next_name){
 499             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 500             << " waypoint set to: " << fp->getNextWaypoint()->name);
 501             return;
 502         }
 503
 504         count++;
 505
 506     }// end while loop
 507
 508     while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END"
 509         && count > -10){
 510             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 511             << " retreating waypoint to: " << parent_next_name
 512             << " at: " << fp->getNextWaypoint()->name);
 513
 514         if (fp->getNextWaypoint()->name == parent_next_name){
 515             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 516                 << " not setting waypoint already at:" << fp->getNextWaypoint()->name );
 517             return;
 518         }
 519
 520         prev = curr;
 521         fp->DecrementWaypoint(false);
 522         curr = fp->getCurrentWaypoint();
 523         next = fp->getNextWaypoint();
 524
 525         if (fp->getNextWaypoint()->name == parent_next_name){
 526             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 527             << " waypoint set to: " << fp->getNextWaypoint()->name);
 528             return;
 529         }
 530
 531         count--;
 532
 533     }// end while loop
 534 }
 535
 536 void FGAIGroundVehicle::setTowSpeed(){
 537
 538     _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
 539
 540     // double diff = _range_ft - _parent_x_offset;
 541     double  x = 0;
 542
 543     if (_range_ft > _x_offset * 3) x = 50;
 544
 545     if (_relbrg < -90 || _relbrg > 90){
 546         setSpeed(_parent_speed - 5 - x);
 547         //cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
 548     }else if (_range_ft > _parent_x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
 549         setSpeed(_parent_speed + 1 + x);
 550         //cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
 551         //    << diff << " range " << _range_ft << endl;
 552     } else if (_range_ft < _parent_x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
 553         setSpeed(_parent_speed - 1 - x);
 554         //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
 555         //    << diff << " " << _range_ft << endl;
 556     } else {
 557         setSpeed(_parent_speed);
 558         //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl;
 559     }
 560
 561 }
 562 // end AIGroundvehicle