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