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