src/AIModel/AIShip.cxx

   1 // FGAIShip - FGAIBase-derived class creates an AI ship
   2 //
   3 // Written by David Culp, started October 2003.
   4 // with major amendments and additions by Vivian Meazza, 2004 - 2007
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19
  20 #ifdef HAVE_CONFIG_H
  21 #  include <config.h>
  22 #endif
  23
  24 #ifdef _MSC_VER
  25 #  include <float.h>
  26 #  define finite _finite
  27 #elif defined(__sun) || defined(sgi)
  28 #  include <ieeefp.h>
  29 #endif
  30
  31 #include <math.h>
  32 #include <simgear/math/sg_geodesy.hxx>
  33 #include <simgear/math/sg_random.h>
  34
  35 #include "AIShip.hxx"
  36
  37
  38 FGAIShip::FGAIShip(object_type ot) :
  39     FGAIBase(ot),
  40     _dt_count(0),
  41     _next_run(0)
  42 {
  43 }
  44
  45 FGAIShip::~FGAIShip() {
  46 }
  47
  48 void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
  49     if (!scFileNode)
  50         return;
  51
  52     FGAIBase::readFromScenario(scFileNode);
  53
  54     setRudder(scFileNode->getFloatValue("rudder", 0.0));
  55     setName(scFileNode->getStringValue("name", "Titanic"));
  56     setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000));
  57     std::string flightplan = scFileNode->getStringValue("flightplan");
  58     setRepeat(scFileNode->getBoolValue("repeat", false));
  59
  60     if (!flightplan.empty()) {
  61         FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
  62         setFlightPlan(fp);
  63     }
  64
  65 }
  66
  67 bool FGAIShip::init(bool search_in_AI_path) {
  68     prev = 0; // the one behind you
  69     curr = 0; // the one ahead
  70     next = 0; // the next plus 1
  71
  72     props->setStringValue("name", _name.c_str());
  73     props->setStringValue("position/waypoint-name-prev", _prev_name.c_str());
  74     props->setStringValue("position/waypoint-name-curr", _curr_name.c_str());
  75     props->setStringValue("position/waypoint-name-next", _next_name.c_str());
  76
  77     _hdg_lock = false;
  78     _rudder = 0.0;
  79     no_roll = false;
  80
  81     _rudder_constant = 0.5;
  82     _roll_constant = 0.001;
  83     _speed_constant = 0.05;
  84     _hdg_constant = 0.01;
  85
  86     _rd_turn_radius_ft = _sp_turn_radius_ft = turn_radius_ft;
  87
  88     _fp_init = false;
  89     _missed = false;
  90     _waiting = false;
  91     _new_waypoint = true;
  92
  93     _missed_count = 0;
  94     _wait_count = 0;
  95     _missed_time_sec = 30;
  96
  97     _wp_range = _old_range = 0;
  98     _range_rate = 0;
  99
 100     if (fp)
 101         _fp_init = initFlightPlan();
 102
 103     return FGAIBase::init(search_in_AI_path);
 104 }
 105
 106
 107 void FGAIShip::bind() {
 108     FGAIBase::bind();
 109
 110     props->tie("surface-positions/rudder-pos-deg",
 111         SGRawValuePointer<float>(&_rudder));
 112     props->tie("controls/heading-lock",
 113         SGRawValuePointer<bool>(&_hdg_lock));
 114     props->tie("controls/tgt-speed-kts",
 115         SGRawValuePointer<double>(&tgt_speed));
 116     props->tie("controls/tgt-heading-degs",
 117         SGRawValuePointer<double>(&tgt_heading));
 118     props->tie("controls/constants/rudder",
 119         SGRawValuePointer<double>(&_rudder_constant));
 120     props->tie("controls/constants/roll",
 121         SGRawValuePointer<double>(&_roll_constant));
 122     props->tie("controls/constants/rudder",
 123         SGRawValuePointer<double>(&_rudder_constant));
 124     props->tie("controls/constants/speed",
 125         SGRawValuePointer<double>(&_speed_constant));
 126     props->tie("position/waypoint-range-nm",
 127         SGRawValuePointer<double>(&_wp_range));
 128     props->tie("position/waypoint-range-old-nm",
 129         SGRawValuePointer<double>(&_old_range));
 130     props->tie("position/waypoint-range-rate-nm-sec",
 131         SGRawValuePointer<double>(&_range_rate));
 132     props->tie("position/waypoint-new",
 133         SGRawValuePointer<bool>(&_new_waypoint));
 134     props->tie("position/waypoint-missed",
 135         SGRawValuePointer<bool>(&_missed));
 136     props->tie("position/waypoint-missed-count",
 137         SGRawValuePointer<double>(&_missed_count));
 138     props->tie("position/waypoint-missed-time-sec",
 139         SGRawValuePointer<double>(&_missed_time_sec));
 140     props->tie("position/waypoint-wait-count",
 141         SGRawValuePointer<double>(&_wait_count));
 142     props->tie("position/waypoint-waiting",
 143         SGRawValuePointer<bool>(&_waiting));
 144 }
 145
 146 void FGAIShip::unbind() {
 147     FGAIBase::unbind();
 148     props->untie("surface-positions/rudder-pos-deg");
 149     props->untie("controls/heading-lock");
 150     props->untie("controls/tgt-speed-kts");
 151     props->untie("controls/tgt-heading-degs");
 152     props->untie("controls/constants/roll");
 153     props->untie("controls/constants/rudder");
 154     props->untie("controls/constants/speed");
 155     props->untie("position/waypoint-range-nm");
 156     props->untie("position/waypoint-range-old-nm");
 157     props->untie("position/waypoint-range-rate-nm-sec");
 158     props->untie("position/waypoint-new");
 159     props->untie("position/waypoint-missed");
 160     props->untie("position/waypoint-wait-count");
 161     props->untie("position/waypoint-waiting");
 162     props->untie("position/waypoint-missed-time-sec");
 163 }
 164
 165 void FGAIShip::update(double dt) {
 166     FGAIBase::update(dt);
 167     Run(dt);
 168     Transform();
 169 }
 170
 171 void FGAIShip::Run(double dt) {
 172
 173     if (_fp_init)
 174         ProcessFlightPlan(dt);
 175
 176     double speed_north_deg_sec;
 177     double speed_east_deg_sec;
 178     double alpha;
 179     double rudder_limit;
 180     double raw_roll;
 181
 182     // adjust speed
 183     double speed_diff = tgt_speed - speed;
 184
 185     if (fabs(speed_diff) > 0.1) {
 186
 187         if (speed_diff > 0.0)
 188             speed += _speed_constant * dt;
 189
 190         if (speed_diff < 0.0)
 191             speed -= _speed_constant * dt;
 192     }
 193
 194     // do not allow unreasonable ship speeds
 195     if (speed > 40)
 196         speed = 40;
 197
 198     // convert speed to degrees per second
 199     speed_north_deg_sec = cos(hdg / SGD_RADIANS_TO_DEGREES)
 200         * speed * 1.686 / ft_per_deg_lat;
 201     speed_east_deg_sec = sin(hdg / SGD_RADIANS_TO_DEGREES)
 202         * speed * 1.686 / ft_per_deg_lon;
 203
 204     // set new position
 205     pos.setLatitudeDeg(pos.getLatitudeDeg() + speed_north_deg_sec * dt);
 206     pos.setLongitudeDeg(pos.getLongitudeDeg() + speed_east_deg_sec * dt);
 207
 208     // adjust heading based on current _rudder angle
 209
 210     //cout << "turn_radius_ft " << turn_radius_ft ;
 211
 212     if (turn_radius_ft <= 0)
 213         turn_radius_ft = 0; // don't allow nonsense values
 214
 215     if (_rudder > 45)
 216         _rudder = 45;
 217
 218     if (_rudder < -45)
 219         _rudder = -45;
 220
 221     //we assume that at slow speed ships will manoeuvre using engines/bow thruster
 222     if (fabs(speed)<=5)
 223         _sp_turn_radius_ft = 500;
 224     else
 225         // adjust turn radius for speed. The equation is very approximate.
 226         // we need to allow for negative speeds
 227         _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
 228
 229     //cout << " speed turn radius " << _sp_turn_radius_ft ;
 230
 231     if (_rudder <= -0.25 || _rudder >= 0.25) {
 232         // adjust turn radius for _rudder angle. The equation is even more approximate.
 233         float a = 19;
 234         float b = -0.2485;
 235         float c = 0.543;
 236
 237         _rd_turn_radius_ft = (a * exp(b * fabs(_rudder)) + c) * _sp_turn_radius_ft;
 238
 239         //cout <<" _rudder turn radius " << _rd_turn_radius_ft << endl;
 240
 241         // calculate the angle, alpha, subtended by the arc traversed in time dt
 242         alpha = ((speed * 1.686 * dt) / _rd_turn_radius_ft) * SG_RADIANS_TO_DEGREES;
 243
 244         // make sure that alpha is applied in the right direction
 245         hdg += alpha * sign(_rudder);
 246
 247         if (hdg > 360.0)
 248             hdg -= 360.0;
 249
 250         if (hdg < 0.0)
 251             hdg += 360.0;
 252
 253         //adjust roll for _rudder angle and speed. Another bit of voodoo
 254         raw_roll = -0.0166667 * speed * _rudder;
 255     } else {
 256         // _rudder angle is 0
 257         raw_roll = 0;
 258     }
 259
 260     //low pass filter
 261     if (speed < 0)
 262         roll = -roll;
 263
 264     roll = (raw_roll * _roll_constant) + (roll * (1 - _roll_constant));
 265
 266     // adjust target _rudder angle if heading lock engaged
 267     if (_hdg_lock) {
 268         double rudder_sense = 0.0;
 269         double diff = fabs(hdg - tgt_heading);
 270         //cout << "_rudder diff" << diff << endl;
 271         if (diff > 180)
 272             diff = fabs(diff - 360);
 273
 274         double sum = hdg + diff;
 275
 276         if (sum > 360.0)
 277             sum -= 360.0;
 278
 279         if (fabs(sum - tgt_heading)< 1.0)
 280             rudder_sense = 1.0;
 281         else
 282             rudder_sense = -1.0;
 283
 284         if (speed < 0)
 285             rudder_sense = -rudder_sense;
 286
 287         if (diff < 15)
 288             _tgt_rudder = diff * rudder_sense;
 289         else
 290             _tgt_rudder = 45 * rudder_sense;
 291     }
 292
 293     // adjust _rudder angle
 294     double rudder_diff = _tgt_rudder - _rudder;
 295     // set the _rudder limit by speed
 296     if (speed <= 40)
 297         rudder_limit = (-0.825 * speed) + 35;
 298     else
 299         rudder_limit = 2;
 300
 301     if (fabs(rudder_diff)> 0.1) { // apply dead zone
 302
 303         if (rudder_diff > 0.0) {
 304             _rudder += _rudder_constant * dt;
 305
 306             if (_rudder > rudder_limit) // apply the _rudder limit
 307                 _rudder = rudder_limit;
 308
 309         } else if (rudder_diff < 0.0) {
 310             _rudder -= _rudder_constant * dt;
 311
 312             if (_rudder < -rudder_limit)
 313                 _rudder = -rudder_limit;
 314
 315         }
 316
 317     }
 318 }//end function
 319
 320 void FGAIShip::AccelTo(double speed) {
 321     tgt_speed = speed;
 322 }
 323
 324 void FGAIShip::PitchTo(double angle) {
 325     tgt_pitch = angle;
 326 }
 327
 328 void FGAIShip::RollTo(double angle) {
 329     tgt_roll = angle;
 330 }
 331
 332 void FGAIShip::YawTo(double angle) {
 333 }
 334
 335 void FGAIShip::ClimbTo(double altitude) {
 336 }
 337
 338
 339 void FGAIShip::TurnTo(double heading) {
 340     tgt_heading = heading;
 341     _hdg_lock = true;
 342 }
 343
 344 double FGAIShip::sign(double x) {
 345     if (x < 0.0)
 346         return -1.0;
 347     else
 348         return 1.0;
 349 }
 350
 351 void FGAIShip::setFlightPlan(FGAIFlightPlan* f) {
 352     fp = f;
 353 }
 354
 355 void FGAIShip::setName(const string& n) {
 356     _name = n;
 357 }
 358
 359 void FGAIShip::setCurrName(const string& c) {
 360     _curr_name = c;
 361     props->setStringValue("position/waypoint-name-curr", _curr_name.c_str());
 362 }
 363
 364 void FGAIShip::setNextName(const string& n) {
 365     _next_name = n;
 366     props->setStringValue("position/waypoint-name-next", _next_name.c_str());
 367 }
 368
 369 void FGAIShip::setPrevName(const string& p) {
 370     _prev_name = p;
 371     props->setStringValue("position/waypoint-name-prev", _prev_name.c_str());
 372 }
 373
 374 void FGAIShip::setRepeat(bool r) {
 375     _repeat = r;
 376 }
 377
 378 void FGAIShip::setMissed(bool m) {
 379     _missed = m;
 380     props->setBoolValue("position/waypoint-missed", _missed);
 381 }
 382
 383 void FGAIShip::ProcessFlightPlan(double dt) {
 384
 385     _missed = false;
 386     _dt_count += dt;
 387
 388     ///////////////////////////////////////////////////////////////////////////
 389     // Check Execution time (currently once every 1 sec)
 390     // Add a bit of randomization to prevent the execution of all flight plans
 391     // in synchrony, which can add significant periodic framerate flutter.
 392     ///////////////////////////////////////////////////////////////////////////
 393     if (_dt_count < _next_run)
 394         return;
 395
 396     _next_run = 1.0 + (0.5 * sg_random());
 397
 398     // check to see if we've reached the point for our next turn
 399     // if the range to the waypoint is less than the calculated turn
 400     // radius we can start the turn to the next leg
 401     _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
 402     _range_rate = (_wp_range - _old_range) / _dt_count;
 403     double sp_turn_radius_nm = _sp_turn_radius_ft / 6076.1155;
 404
 405     // we need to try to identify a _missed waypoint
 406
 407     // calculate the time needed to turn through an arc of 90 degrees, and allow an error of 30 secs
 408     if (speed != 0)
 409         _missed_time_sec = 30 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed)));
 410     else
 411         _missed_time_sec = 30;
 412
 413     if ((_range_rate > 0) && (_wp_range < 3 * sp_turn_radius_nm) && !_new_waypoint)
 414         _missed_count += _dt_count;
 415
 416
 417     if (_missed_count >= _missed_time_sec) {
 418         setMissed(true);
 419     } else {
 420         setMissed(false);
 421     }
 422
 423     _old_range = _wp_range;
 424
 425     if ((_wp_range < sp_turn_radius_nm) || _missed || _waiting && !_new_waypoint) {
 426
 427         if (_next_name == "END") {
 428
 429             if (_repeat) {
 430                 SG_LOG(SG_GENERAL, SG_INFO, "AIShip: Flightplan restarting ");
 431                 fp->restart();
 432                 prev = curr;
 433                 curr = fp->getCurrentWaypoint();
 434                 next = fp->getNextWaypoint();
 435                 setWPNames();
 436                 _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
 437                 _old_range = _wp_range;
 438                 _range_rate = 0;
 439                 _new_waypoint = true;
 440                 _missed_count = 0;
 441                 AccelTo(prev->speed);
 442             } else {
 443                 SG_LOG(SG_GENERAL, SG_INFO, "AIShip: Flightplan dieing ");
 444                 setDie(true);
 445                 _dt_count = 0;
 446                 return;
 447             }
 448
 449         } else if (_next_name == "WAIT") {
 450
 451             if (_wait_count < next->wait_time) {
 452                 SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " _waiting ");
 453                 setSpeed(0);
 454                 _waiting = true;
 455                 _wait_count += _dt_count;
 456                 _dt_count = 0;
 457                 return;
 458             } else {
 459                 SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " wait done: getting new waypoints ");
 460                 prev = curr;
 461                 fp->IncrementWaypoint(false);
 462                 fp->IncrementWaypoint(false);  // do it twice
 463                 curr = fp->getCurrentWaypoint();
 464                 next = fp->getNextWaypoint();
 465                 _waiting = false;
 466                 _wait_count = 0;
 467             }
 468
 469         } else {
 470             //now reorganise the waypoints, so that next becomes current and so on
 471             SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " getting new waypoints ");
 472             fp->IncrementWaypoint(false);
 473             prev = fp->getPreviousWaypoint(); //first waypoint
 474             curr = fp->getCurrentWaypoint();  //second waypoint
 475             next = fp->getNextWaypoint();     //third waypoint (might not exist!)
 476         }
 477
 478         setWPNames();
 479         _new_waypoint = true;
 480         _missed_count = 0;
 481         _range_rate = 0;
 482         _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
 483         _old_range = _wp_range;
 484         AccelTo(prev->speed);
 485     } else {
 486         _new_waypoint = false;
 487     }
 488
 489     //   now revise the required course for the next way point
 490     double course = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
 491
 492     if (finite(course))
 493         TurnTo(course);
 494     else
 495         SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: Bearing or Range is not a finite number");
 496
 497      _dt_count = 0;
 498 } // end Processing FlightPlan
 499
 500 void FGAIShip::setRudder(float r) {
 501     _rudder = r;
 502 }
 503
 504 void FGAIShip::setRoll(double rl) {
 505     roll = rl;
 506 }
 507
 508 double FGAIShip::getRange(double lat, double lon, double lat2, double lon2) const {
 509
 510     double course, distance, az2;
 511
 512     //calculate the bearing and range of the second pos from the first
 513     geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance);
 514     distance *= SG_METER_TO_NM;
 515     return distance;
 516 }
 517
 518 double FGAIShip::getCourse(double lat, double lon, double lat2, double lon2) const {
 519
 520     double course, distance, recip;
 521
 522     //calculate the bearing and range of the second pos from the first
 523     geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &recip, &distance);
 524     if (tgt_speed >= 0) {
 525         return course;
 526     } else {
 527         return recip;
 528     }
 529 }
 530
 531 bool FGAIShip::initFlightPlan() {
 532     SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " initialising waypoints ");
 533     fp->restart();
 534     fp->IncrementWaypoint(false);
 535
 536     prev = fp->getPreviousWaypoint(); //first waypoint
 537     curr = fp->getCurrentWaypoint();  //second waypoint
 538     next = fp->getNextWaypoint();     //third waypoint (might not exist!)
 539
 540     if (curr->name == "WAIT") {  // don't wait when initialising
 541         SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " re-initialising waypoints ");
 542         fp->IncrementWaypoint(false);
 543         curr = fp->getCurrentWaypoint();
 544         next = fp->getNextWaypoint();
 545     }
 546
 547     setWPNames();
 548     setLatitude(prev->latitude);
 549     setLongitude(prev->longitude);
 550     setSpeed(prev->speed);
 551     setHeading(getCourse(prev->latitude, prev->longitude, curr->latitude, curr->longitude));
 552     _hdg_lock = true;
 553     _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
 554     _old_range = _wp_range;
 555     _range_rate = 0;
 556     _missed = false;
 557     _missed_count = 0;
 558     _new_waypoint = true;
 559
 560     SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " done initialising waypoints ");
 561
 562     if (prev)
 563         return true;
 564     else
 565         return false;
 566
 567 } // end of initialization
 568
 569 void FGAIShip::setWPNames() {
 570
 571     if (prev != 0)
 572         setPrevName(prev->name);
 573     else
 574         setPrevName("");
 575
 576     setCurrName(curr->name);
 577
 578     if (next != 0)
 579         setNextName(next->name);
 580     else
 581         setNextName("");
 582
 583     SG_LOG(SG_GENERAL, SG_INFO, "AIShip: prev wp name " << prev->name);
 584     SG_LOG(SG_GENERAL, SG_INFO, "AIShip: current wp name " << curr->name);
 585     SG_LOG(SG_GENERAL, SG_INFO, "AIShip: next wp name " << next->name);
 586
 587 }