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/sg_geodesy.hxx>
  31
  32 #include <math.h>
  33 #include <Main/util.hxx>
  34 #include <Viewer/viewer.hxx>
  35
  36 #include <Scenery/scenery.hxx>
  37
  38 #include "AIEscort.hxx"
  39
  40 using std::string;
  41
  42 FGAIEscort::FGAIEscort() :
  43 FGAIShip(otEscort),
  44
  45 _relbrg (0),
  46 _parent_speed(0),
  47 _interval(0),
  48 _stn_truebrg(0),
  49 _stn_height(0),
  50 _stn_speed(0),
  51 _stn_angle_limit(0),
  52 _stn_limit(0),
  53 _max_speed(0),
  54 _MPControl(false),
  55 _patrol(false),
  56 _stn_deg_true(false)
  57
  58 {
  59     invisible = false;
  60 }
  61
  62 FGAIEscort::~FGAIEscort() {}
  63
  64 void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {
  65     if (!scFileNode)
  66         return;
  67
  68     FGAIShip::readFromScenario(scFileNode);
  69
  70     setName(scFileNode->getStringValue("name", "Escort"));
  71     setSMPath(scFileNode->getStringValue("submodel-path", ""));
  72     setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));
  73     setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));
  74     setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));
  75     setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));
  76     setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));
  77     setStnPatrol(scFileNode->getBoolValue("station/patrol", false));
  78     setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));
  79     setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));
  80     setParentName(scFileNode->getStringValue("station/parent", ""));
  81     setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));
  82     setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));
  83     setCallSign(scFileNode->getStringValue("callsign", ""));
  84
  85     if(_patrol)
  86         sg_srandom_time();
  87
  88 }
  89
  90 void FGAIEscort::bind() {
  91     FGAIShip::bind();
  92
  93     tie("station/rel-bearing-deg",
  94         SGRawValuePointer<double>(&_stn_relbrg));
  95     tie("station/true-bearing-deg",
  96         SGRawValuePointer<double>(&_stn_truebrg));
  97     tie("station/range-nm",
  98         SGRawValuePointer<double>(&_stn_range));
  99     tie("station/range-limit-nm",
 100         SGRawValuePointer<double>(&_stn_limit));
 101     tie("station/angle-limit-deg",
 102         SGRawValuePointer<double>(&_stn_angle_limit));
 103     tie("station/speed-kts",
 104         SGRawValuePointer<double>(&_stn_speed));
 105     tie("station/height-ft",
 106         SGRawValuePointer<double>(&_stn_height));
 107     tie("controls/update-interval-sec",
 108         SGRawValuePointer<double>(&_interval));
 109     tie("controls/parent-mp-control",
 110         SGRawValuePointer<bool>(&_MPControl));
 111     tie("station/target-range-nm",
 112         SGRawValuePointer<double>(&_tgtrange));
 113     tie("station/target-brg-deg-t",
 114         SGRawValuePointer<double>(&_tgtbrg));
 115     tie("station/patrol",
 116         SGRawValuePointer<bool>(&_patrol));
 117 }
 118
 119 bool FGAIEscort::init(bool search_in_AI_path) {
 120     if (!FGAIShip::init(search_in_AI_path))
 121         return false;
 122     reinit();
 123     return true;
 124 }
 125
 126 void FGAIEscort::reinit() {
 127     invisible = false;
 128     no_roll = false;
 129
 130     props->setStringValue("controls/parent-name", _parent.c_str());
 131
 132     if (setParentNode()){
 133         setParent();
 134         pos = _tgtpos;
 135         speed = _parent_speed;
 136         hdg = _parent_hdg;
 137     }
 138
 139     FGAIShip::reinit();
 140 }
 141
 142 void FGAIEscort::update(double dt) {
 143     FGAIShip::update(dt);
 144
 145     RunEscort(dt);
 146 }
 147
 148 void FGAIEscort::setStnRange(double r) {
 149     _stn_range = r;
 150 }
 151
 152 void FGAIEscort::setStnBrg(double b) {
 153     _stn_brg = b;
 154 }
 155
 156 void FGAIEscort::setStnLimit(double l) {
 157     _stn_limit = l;
 158 }
 159
 160 void FGAIEscort::setStnAngleLimit(double al) {
 161     _stn_angle_limit = al;
 162 }
 163
 164 void FGAIEscort::setStnSpeed(double s) {
 165     _stn_speed = s;
 166 }
 167
 168 void FGAIEscort::setStnHtFt(double h) {
 169     _stn_height = h;
 170 }
 171
 172 void FGAIEscort::setStnDegTrue(bool t) {
 173     _stn_deg_true = t;
 174 }
 175
 176 void FGAIEscort::setMaxSpeed(double m) {
 177     _max_speed = m;
 178 }
 179
 180 void FGAIEscort::setUpdateInterval(double i) {
 181     _interval = i;
 182 }
 183
 184 void FGAIEscort::setStnPatrol(bool p) {
 185     _patrol = p;
 186 }
 187
 188 bool FGAIEscort::getGroundElev(SGGeod inpos) {
 189
 190     double height_m ;
 191
 192     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
 193         _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
 194
 195         if (_material) {
 196             const vector<string>& names = _material->get_names();
 197
 198             _solid = _material->get_solid();
 199
 200             if (!names.empty())
 201                 props->setStringValue("material/name", names[0].c_str());
 202             else
 203                 props->setStringValue("material/name", "");
 204
 205             //cout << "material " << names[0].c_str()
 206             //    << " _elevation_m " << _elevation_m
 207             //    << " solid " << _solid
 208             //    << " load " << _load_resistance
 209             //    << " frictionFactor " << _frictionFactor
 210             //    << endl;
 211
 212         }
 213
 214         return true;
 215     } else {
 216         return false;
 217     }
 218
 219 }
 220
 221 void FGAIEscort::setParent()
 222 {
 223     double lat = _selected_ac->getDoubleValue("position/latitude-deg");
 224     double lon = _selected_ac->getDoubleValue("position/longitude-deg");
 225     double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
 226     _MPControl = _selected_ac->getBoolValue("controls/mp-control");
 227
 228     _selectedpos.setLatitudeDeg(lat);
 229     _selectedpos.setLongitudeDeg(lon);
 230     _selectedpos.setElevationFt(elevation);
 231
 232     _parent_speed    = _selected_ac->getDoubleValue("velocities/speed-kts");
 233     _parent_hdg      = _selected_ac->getDoubleValue("orientation/true-heading-deg");
 234
 235     if(!_stn_deg_true){
 236         _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);
 237         _stn_relbrg = _stn_brg;
 238         //cout << _name <<" set rel"<<endl;
 239     } else {
 240         _stn_truebrg = _stn_brg;
 241         _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg);
 242         //cout << _name << " set true"<<endl;
 243     }
 244
 245     double course2;
 246
 247     SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,
 248         _tgtpos, course2);
 249
 250     _tgtpos.setElevationFt(_stn_height);
 251
 252     calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
 253         _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);
 254
 255     _relbrg = calcRelBearingDeg(_tgtbrg, hdg);
 256
 257 }
 258
 259 void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,
 260                                   double &range, double &bearing) const
 261 {
 262     // calculate the bearing and range of the second pos from the first
 263     double az2, distance;
 264     geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
 265     range = distance * SG_METER_TO_NM;
 266 }
 267
 268 double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)
 269 {
 270     double angle = bearing + heading;
 271     SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
 272     return angle;
 273 }
 274
 275 SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {
 276     double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
 277     double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
 278     double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
 279
 280     // Transform that one to the horizontal local coordinate system.
 281     SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
 282
 283     // and postrotate the orientation of the AIModel wrt the horizontal
 284     // local frame
 285     hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
 286
 287     // The offset converted to the usual body fixed coordinate system
 288     // rotated to the earth fiexed coordinates axis
 289     SGVec3d off = hlTrans.backTransform(_off);
 290
 291     // Add the position offset of the AIModel to gain the earth centered position
 292     SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
 293
 294     return cartPos + off;
 295 }
 296
 297
 298 void FGAIEscort::setStationSpeed(){
 299
 300     double speed = 0;
 301     double angle = 0;
 302
 303     // these are the AI rules for the manoeuvring of escorts
 304
 305     if (_MPControl && _tgtrange > 4 * _stn_limit){
 306         SG_LOG(SG_AI, SG_ALERT, "AIEscort: " << _name
 307             << " re-aligning to MP pos");
 308         pos = _tgtpos;
 309         speed = 0;
 310         angle = 0;
 311     }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){
 312         angle =_relbrg;
 313
 314         if(_tgtrange > 4 * _stn_limit)
 315             speed = 4 * -_stn_speed;
 316         else
 317             speed = -_stn_speed;
 318
 319     }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){
 320         angle = _relbrg;
 321
 322         if(_tgtrange > 4 * _stn_limit)
 323             speed = 4 * _stn_speed;
 324         else
 325             speed = _stn_speed;
 326
 327     } else {
 328
 329         if(_patrol){
 330             angle = 15 * sg_random();
 331             speed =  5 * sg_random();
 332         } else {
 333             angle = 0;
 334             speed = 0;
 335         }
 336
 337     }
 338
 339     double station_speed = _parent_speed + speed;
 340
 341     SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);
 342     SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);
 343
 344     AccelTo(station_speed);
 345     TurnTo(_parent_hdg + angle);
 346     ClimbTo(_stn_height);
 347
 348 }
 349
 350 void FGAIEscort::RunEscort(double dt){
 351
 352     _dt_count += dt;
 353
 354
 355
 356     ///////////////////////////////////////////////////////////////////////////
 357     // Check execution time (currently once every 0.05 sec or 20 fps)
 358     // Add a bit of randomization to prevent the execution of all flight plans
 359     // in synchrony, which can add significant periodic framerate flutter.
 360     // Randomization removed to get better appearance
 361     ///////////////////////////////////////////////////////////////////////////
 362
 363     //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
 364     if (_dt_count < _next_run)
 365         return;
 366     _next_run = _interval /*+ (0.015 * sg_random())*/;
 367
 368     if(_parent == ""){
 369         return;
 370     }
 371
 372     setParent();
 373     setStationSpeed();
 374
 375     _dt_count = 0;
 376
 377 }
 378
 379 // end AIEscort