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