src/AIModel/AIBallistic.cxx

   1 // FGAIBallistic - FGAIBase-derived class creates a ballistic object
   2 //
   3 // Written by David Culp, started November 2003.
   4 // - davidculp2@comcast.net
   5 //
   6 // With major additions by Mathias Froehlich & Vivian Meazza 2004-2008
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <simgear/math/sg_random.h>
  27 #include <simgear/math/sg_geodesy.hxx>
  28 #include <simgear/scene/model/modellib.hxx>
  29
  30 #include <Scenery/scenery.hxx>
  31
  32 #include "AIBallistic.hxx"
  33
  34 #include <Main/util.hxx>
  35 #include <Environment/gravity.hxx>
  36
  37 using namespace simgear;
  38 using std::string;
  39
  40 const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
  41 const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
  42
  43 FGAIBallistic::FGAIBallistic(object_type ot) :
  44 FGAIBase(ot, false),
  45 _height(0.0),
  46 _speed(0),
  47 _ht_agl_ft(0.0),
  48 _azimuth(0.0),
  49 _elevation(0.0),
  50 _rotation(0.0),
  51 hs(0),
  52 _elapsed_time(0),
  53 _aero_stabilised(false),
  54 _drag_area(0.007),
  55 _cd(0.029),
  56 _cd_randomness(0.0),
  57 _life_timer(0.0),
  58 _buoyancy(0),
  59 _wind(true),
  60 _mass(0),
  61 _random(false),
  62 _life_randomness(0.0),
  63 _load_resistance(0),
  64 _solid(false),
  65 _force_stabilised(false),
  66 _slave_to_ac(false),
  67 _slave_load_to_ac(false),
  68 _contents_lb(0),
  69 _report_collision(false),
  70 _report_impact(false),
  71 _external_force(false),
  72 _report_expiry(false),
  73 _impact_report_node(fgGetNode("/ai/models/model-impact", true))
  74
  75 {
  76     no_roll = false;
  77 }
  78
  79 FGAIBallistic::~FGAIBallistic() {
  80 }
  81
  82 void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
  83     if (!scFileNode){
  84         return;
  85     }
  86
  87     FGAIBase::readFromScenario(scFileNode);
  88
  89     //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac"));
  90     setRandom(scFileNode->getBoolValue("random", false));
  91     setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
  92     setElevation(scFileNode->getDoubleValue("elevation", 0));
  93     setDragArea(scFileNode->getDoubleValue("eda", 0.007));
  94     setLife(scFileNode->getDoubleValue("life", 900.0));
  95     setBuoyancy(scFileNode->getDoubleValue("buoyancy", 0));
  96     //setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0));
  97     //setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0));
  98     setWind(scFileNode->getBoolValue("wind", false));
  99     setRoll(scFileNode->getDoubleValue("roll", 0.0));
 100     setCd(scFileNode->getDoubleValue("cd", 0.029));
 101     //setMass(scFileNode->getDoubleValue("mass", 0.007));
 102     setWeight(scFileNode->getDoubleValue("weight", 0.25));
 103     setStabilisation(scFileNode->getBoolValue("aero-stabilised", false));
 104     setNoRoll(scFileNode->getBoolValue("no-roll", false));
 105     setImpact(scFileNode->getBoolValue("impact", false));
 106     setExpiry(scFileNode->getBoolValue("expiry", false));
 107     setCollision(scFileNode->getBoolValue("collision", false));
 108     setImpactReportNode(scFileNode->getStringValue("impact-reports"));
 109     setName(scFileNode->getStringValue("name", "Rocket"));
 110     setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
 111     setSMPath(scFileNode->getStringValue("submodel-path", ""));
 112     setSubID(scFileNode->getIntValue("SubID", 0));
 113     setExternalForce(scFileNode->getBoolValue("external-force", false));
 114     setForcePath(scFileNode->getStringValue("force-path", ""));
 115     setForceStabilisation(scFileNode->getBoolValue("force-stabilised", false));
 116     setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
 117     setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
 118     setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
 119     setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
 120     setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
 121     setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
 122     setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
 123     setLoadOffset(scFileNode->getDoubleValue("load-offset", 0.0));
 124     setSlaved(scFileNode->getBoolValue("slaved", false));
 125     setSlavedLoad(scFileNode->getBoolValue("slaved-load", false));
 126     setContentsPath(scFileNode->getStringValue("contents"));
 127     setParentName(scFileNode->getStringValue("parent"));
 128 }
 129
 130 bool FGAIBallistic::init(bool search_in_AI_path) {
 131     FGAIBase::init(search_in_AI_path);
 132     reinit();
 133     return true;
 134 }
 135
 136 void FGAIBallistic::reinit() {
 137     _impact_reported = false;
 138     _collision_reported = false;
 139     _expiry_reported = false;
 140
 141     _impact_lat = 0;
 142     _impact_lon = 0;
 143     _impact_elev = 0;
 144     _impact_hdg = 0;
 145     _impact_pitch = 0;
 146     _impact_roll = 0;
 147     _impact_speed = 0;
 148
 149     invisible = false;
 150
 151     _elapsed_time += (sg_random() * 100);
 152
 153     _life_timer = 0;
 154
 155     props->setStringValue("material/name", "");
 156     props->setStringValue("name", _name.c_str());
 157     props->setStringValue("submodels/path", _path.c_str());
 158
 159     if (_slave_to_ac) {
 160         props->setStringValue("force/path", _force_path.c_str());
 161         props->setStringValue("contents/path", _contents_path.c_str());
 162     }
 163
 164     //cout << "init: name " << _name.c_str() << " _life_timer " << _life_timer
 165     //    << endl;
 166
 167     //if(_parent != ""){
 168     //    setParentNode();
 169     //}
 170
 171     //setParentNodes(_selected_ac);
 172
 173     //props->setStringValue("vector/path", _vector_path.c_str());
 174
 175     // start with high value so that animations don't trigger yet
 176     _ht_agl_ft = 1e10;
 177     hdg = _azimuth;
 178     pitch = _elevation;
 179     roll = _rotation;
 180
 181     Transform();
 182
 183     if (_parent != "") {
 184         setParentNode();
 185     }
 186
 187     setParentNodes(_selected_ac);
 188
 189     FGAIBase::reinit();
 190 }
 191
 192 void FGAIBallistic::bind() {
 193     //    FGAIBase::bind();
 194
 195     _tiedProperties.setRoot(props);
 196     tie("sim/time/elapsed-sec",
 197         SGRawValueMethods<FGAIBallistic,double>(*this,
 198         &FGAIBallistic::_getTime, &FGAIBallistic::setTime));
 199     //tie("mass-slug",
 200     //    SGRawValueMethods<FGAIBallistic,double>(*this,
 201     //    &FGAIBallistic::getMass));
 202
 203     tie("material/solid",
 204         SGRawValuePointer<bool>(&_solid));
 205     tie("altitude-agl-ft",
 206         SGRawValuePointer<double>(&_ht_agl_ft));
 207     tie("controls/slave-to-ac",
 208         SGRawValueMethods<FGAIBallistic,bool>
 209         (*this, &FGAIBallistic::getSlaved, &FGAIBallistic::setSlaved));
 210     tie("controls/invisible",
 211         SGRawValuePointer<bool>(&invisible));
 212
 213     if (_external_force || _slave_to_ac) {
 214         tie("controls/force_stabilized",
 215             SGRawValuePointer<bool>(&_force_stabilised));
 216         tie("position/global-x",
 217             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosX, 0));
 218         tie("position/global-y",
 219             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosY, 0));
 220         tie("position/global-z",
 221             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosZ, 0));
 222         tie("velocities/vertical-speed-fps",
 223             SGRawValuePointer<double>(&vs));
 224         tie("velocities/true-airspeed-kt",
 225             SGRawValuePointer<double>(&speed));
 226         tie("velocities/horizontal-speed-fps",
 227             SGRawValuePointer<double>(&hs));
 228         tie("position/altitude-ft",
 229             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getElevationFt, &FGAIBase::_setAltitude));
 230         tie("position/latitude-deg",
 231             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLatitude, &FGAIBase::_setLatitude));
 232         tie("position/longitude-deg",
 233             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLongitude, &FGAIBase::_setLongitude));
 234         tie("orientation/hdg-deg",
 235             SGRawValuePointer<double>(&hdg));
 236         tie("orientation/pitch-deg",
 237             SGRawValuePointer<double>(&pitch));
 238         tie("orientation/roll-deg",
 239             SGRawValuePointer<double>(&roll));
 240         tie("controls/slave-load-to-ac",
 241             SGRawValueMethods<FGAIBallistic,bool>
 242             (*this, &FGAIBallistic::getSlavedLoad, &FGAIBallistic::setSlavedLoad));
 243         tie("position/load-offset",
 244             SGRawValueMethods<FGAIBallistic,double>
 245             (*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
 246         tie("load/distance-to-hitch-ft",
 247             SGRawValueMethods<FGAIBallistic,double>
 248             (*this, &FGAIBallistic::getDistanceToHitch));
 249         tie("load/elevation-to-hitch-deg",
 250             SGRawValueMethods<FGAIBallistic,double>
 251             (*this, &FGAIBallistic::getElevToHitch));
 252         tie("load/bearing-to-hitch-deg",
 253             SGRawValueMethods<FGAIBallistic,double>
 254             (*this, &FGAIBallistic::getBearingToHitch));
 255         tie("material/load-resistance",
 256         SGRawValuePointer<double>(&_load_resistance));
 257     }
 258 }
 259
 260 void FGAIBallistic::update(double dt)
 261 {
 262     FGAIBase::update(dt);
 263
 264     if (_slave_to_ac) {
 265         slaveToAC(dt);
 266         Transform();
 267     }
 268     else if (!invisible) {
 269         Run(dt);
 270         Transform();
 271     }
 272
 273 }
 274
 275 void FGAIBallistic::setAzimuth(double az) {
 276     if (_random)
 277         hdg = _azimuth = (az - 5 ) + (10 * sg_random());
 278     else
 279         hdg = _azimuth = az;
 280 }
 281
 282 void FGAIBallistic::setElevation(double el) {
 283     pitch = _elevation = el;
 284 }
 285
 286 void FGAIBallistic::setRoll(double rl) {
 287     roll = _rotation = rl;
 288 }
 289
 290 void FGAIBallistic::setStabilisation(bool val) {
 291     _aero_stabilised = val;
 292 }
 293
 294 void FGAIBallistic::setForceStabilisation(bool val) {
 295     _force_stabilised = val;
 296 }
 297
 298 void FGAIBallistic::setNoRoll(bool nr) {
 299     no_roll = nr;
 300 }
 301
 302 void FGAIBallistic::setDragArea(double a) {
 303     _drag_area = a;
 304 }
 305
 306 void FGAIBallistic::setLife(double seconds) {
 307     if (_random)
 308         life = seconds * (1 - _life_randomness + 2 * _life_randomness * sg_random());
 309     else
 310         life = seconds;
 311 }
 312
 313 void FGAIBallistic::setBuoyancy(double fpss) {
 314     _buoyancy = fpss;
 315 }
 316
 317 void FGAIBallistic::setWind_from_east(double fps) {
 318     _wind_from_east = fps;
 319 }
 320
 321 void FGAIBallistic::setWind_from_north(double fps) {
 322     _wind_from_north = fps;
 323 }
 324
 325 void FGAIBallistic::setWind(bool val) {
 326     _wind = val;
 327 }
 328
 329 void FGAIBallistic::setCd(double cd) {
 330     _cd = cd;
 331 }
 332
 333 void FGAIBallistic::setCdRandomness(double randomness) {
 334     _cd_randomness = randomness;
 335 }
 336
 337 void FGAIBallistic::setMass(double m) {
 338     _mass = m;
 339 }
 340
 341 void FGAIBallistic::setWeight(double w) {
 342     _weight_lb = w;
 343 }
 344
 345 void FGAIBallistic::setLifeRandomness(double randomness) {
 346     _life_randomness = randomness;
 347 }
 348
 349 void FGAIBallistic::setRandom(bool r) {
 350     _random = r;
 351 }
 352
 353 void FGAIBallistic::setImpact(bool i) {
 354     _report_impact = i;
 355 }
 356
 357 void FGAIBallistic::setCollision(bool c) {
 358     _report_collision = c;
 359 }
 360
 361 void FGAIBallistic::setExpiry(bool e) {
 362     _report_expiry = e;
 363 }
 364
 365 void FGAIBallistic::setExternalForce(bool f) {
 366     _external_force = f;
 367 }
 368
 369 void FGAIBallistic::setImpactReportNode(const string& path) {
 370     if (!path.empty())
 371         _impact_report_node = fgGetNode(path.c_str(), true);
 372 }
 373
 374 void FGAIBallistic::setSMPath(const string& s) {
 375     _path = s;
 376     //cout << "submodel path " << _path << endl;
 377 }
 378
 379 void FGAIBallistic::setFuseRange(double f) {
 380     _fuse_range = f;
 381 }
 382
 383 void FGAIBallistic::setSubID(int i) {
 384     _subID = i;
 385 }
 386
 387 void FGAIBallistic::setSubmodel(const string& s) {
 388     _submodel = s;
 389 }
 390
 391 void FGAIBallistic::setGroundOffset(double g) {
 392     _ground_offset = g;
 393 }
 394
 395 void FGAIBallistic::setLoadOffset(double l) {
 396     _load_offset = l;
 397 }
 398
 399 double FGAIBallistic::getLoadOffset() const {
 400     return _load_offset;
 401 }
 402
 403 void FGAIBallistic::setSlaved(bool s) {
 404     _slave_to_ac = s;
 405 }
 406
 407 void FGAIBallistic::setContentsPath(const string& path) {
 408     _contents_path = path;
 409
 410     if (!path.empty()) {
 411         _contents_node = fgGetNode(path.c_str(), true);
 412     }
 413 }
 414
 415 void FGAIBallistic::setContentsNode(SGPropertyNode_ptr node) {
 416     if (node != 0) {
 417         _contents_node = node;
 418         _contents_path = _contents_node->getDisplayName();
 419     }
 420 }
 421
 422 void FGAIBallistic::setParentNodes(SGPropertyNode_ptr node) {
 423     if (node != 0) {
 424         _pnode = node;
 425         _p_pos_node = _pnode->getChild("position", 0, true);
 426         _p_lat_node = _p_pos_node->getChild("latitude-deg", 0, true);
 427         _p_lon_node = _p_pos_node->getChild("longitude-deg", 0, true);
 428         _p_alt_node = _p_pos_node->getChild("altitude-ft", 0, true);
 429         _p_agl_node = _p_pos_node->getChild("altitude-agl-ft", 0, true);
 430
 431
 432         _p_ori_node = _pnode->getChild("orientation", 0, true);
 433         _p_pch_node = _p_ori_node->getChild("pitch-deg", 0, true);
 434         _p_rll_node = _p_ori_node->getChild("roll-deg", 0, true);
 435         _p_hdg_node = _p_ori_node->getChild("true-heading-deg",0, true);
 436
 437         _p_vel_node = _pnode->getChild("velocities", 0, true);
 438         _p_spd_node = _p_vel_node->getChild("true-airspeed-kt", 0, true);
 439     }
 440 }
 441
 442 void FGAIBallistic::setParentPos() {
 443     if (_pnode != 0) {
 444         double lat = _p_lat_node->getDoubleValue();
 445         double lon = _p_lon_node->getDoubleValue();
 446         double alt = _p_alt_node->getDoubleValue();
 447
 448         _parentpos.setLongitudeDeg(lon);
 449         _parentpos.setLatitudeDeg(lat);
 450         _parentpos.setElevationFt(alt);
 451     }
 452 }
 453
 454 bool FGAIBallistic::getSlaved() const {
 455     return _slave_to_ac;
 456 }
 457
 458 double FGAIBallistic::getMass() const {
 459     return _mass;
 460 }
 461
 462 double FGAIBallistic::getContents() {
 463     if (_contents_node) {
 464         _contents_lb = _contents_node->getChild("level-lbs", 0, 1)->getDoubleValue();
 465     }
 466     return _contents_lb;
 467 }
 468
 469 void FGAIBallistic::setContents(double c) {
 470     if (_contents_node)
 471         _contents_lb = _contents_node->getChild("level-gal_us", 0, 1)->setDoubleValue(c);
 472 }
 473
 474 void FGAIBallistic::setSlavedLoad(bool l) {
 475     _slave_load_to_ac = l;
 476 }
 477
 478 bool FGAIBallistic::getSlavedLoad() const {
 479     return _slave_load_to_ac;
 480 }
 481
 482 void FGAIBallistic::setForcePath(const string& p) {
 483     _force_path = p;
 484     if (!_force_path.empty()) {
 485         SGPropertyNode *fnode = fgGetNode(_force_path.c_str(), 0, true );
 486         _force_node = fnode->getChild("force-lb", 0, true);
 487         _force_azimuth_node = fnode->getChild("force-azimuth-deg", 0, true);
 488         _force_elevation_node = fnode->getChild("force-elevation-deg", 0, true);
 489     }
 490 }
 491
 492 bool FGAIBallistic::getHtAGL(double start) {
 493     const simgear::BVHMaterial* mat = 0;
 494     if (getGroundElevationM(SGGeod::fromGeodM(pos, start),
 495         _elevation_m, &mat)) {
 496             const SGMaterial* material = dynamic_cast<const SGMaterial*>(mat);
 497             _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 498
 499             if (material) {
 500                 const std::vector<string>& names = material->get_names();
 501                 _solid = material->get_solid();
 502                 _load_resistance = material->get_load_resistance();
 503                 _frictionFactor = material->get_friction_factor();
 504
 505                 if (!names.empty())
 506                     props->setStringValue("material/name", names[0].c_str());
 507                 else
 508                     props->setStringValue("material/name", "");
 509
 510                 _mat_name = names[0];
 511
 512                 //cout << "material " << _mat_name
 513                 //<< " solid " << _solid
 514                 //<< " load " << _load_resistance
 515                 //<< " frictionFactor " << _frictionFactor
 516                 //<< endl;
 517             }
 518
 519             return true;
 520     }
 521     else {
 522         return false;
 523     }
 524 }
 525
 526 double FGAIBallistic::getRecip(double az) {
 527     // calculate the reciprocal of the input azimuth
 528     if (az - 180 < 0) {
 529         return az + 180;
 530     }
 531     else {
 532         return az - 180;
 533     }
 534 }
 535
 536 void FGAIBallistic::setPch(double e, double dt, double coeff) {
 537     double c = dt / (coeff + dt);
 538     pitch = (e * c) + (pitch * (1 - c));
 539 }
 540
 541 void FGAIBallistic::setBnk(double r, double dt, double coeff) {
 542     double c = dt / (coeff + dt);
 543     roll = (r * c) + (roll * (1 - c));
 544 }
 545
 546 void FGAIBallistic::setSpd(double s, double dt, double coeff) {
 547     double c = dt / (coeff + dt);
 548     _speed = (s * c) + (_speed * (1 - c));
 549 }
 550
 551 void FGAIBallistic::setHt(double h, double dt, double coeff) {
 552     double c = dt / (coeff + dt);
 553     _height = (h * c) + (_height * (1 - c));
 554 }
 555
 556 int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff) {
 557     double recip = getRecip(hdg);
 558     double c = dt / (coeff + dt);
 559     //cout << "set heading " << tgt_hdg << endl;
 560     //we need to ensure that we turn the short way to the new hdg
 561     if (tgt_hdg < recip && tgt_hdg < hdg && hdg > 180) {
 562         hdg = ((tgt_hdg + 360) * c) + (hdg * (1 - c));
 563 //        cout << "case 1: right turn" << endl;
 564     } else if (tgt_hdg > recip && tgt_hdg > hdg && hdg <= 180){
 565         hdg = ((tgt_hdg - 360) * c) + (hdg * (1 - c));
 566 //        cout << "case 2: left turn" << endl;
 567     } else {
 568         hdg = (tgt_hdg * c) + (hdg * (1 - c));
 569 //        cout << "case 4: left turn" << endl;
 570     }
 571     return -1;
 572 }
 573
 574 double  FGAIBallistic::getTgtXOffset() const {
 575     return _tgt_x_offset;
 576 }
 577
 578 double  FGAIBallistic::getTgtYOffset() const {
 579     return _tgt_y_offset;
 580 }
 581
 582 double  FGAIBallistic::getTgtZOffset() const {
 583     return _tgt_z_offset;
 584 }
 585
 586 void FGAIBallistic::setTgtXOffset(double x) {
 587     _tgt_x_offset = x;
 588 }
 589
 590 void FGAIBallistic::setTgtYOffset(double y) {
 591     _tgt_y_offset = y;
 592 }
 593
 594 void FGAIBallistic::setTgtZOffset(double z) {
 595     _tgt_z_offset = z;
 596 }
 597
 598 void FGAIBallistic::slaveToAC(double dt) {
 599     if (invisible)
 600         return;
 601
 602     double hdg, pch, rll;//, agl = 0;
 603
 604     if (_pnode != 0) {
 605         setParentPos();
 606         hdg = _p_hdg_node->getDoubleValue();
 607         pch = _p_pch_node->getDoubleValue();
 608         rll = _p_rll_node->getDoubleValue();
 609 //        agl = _p_agl_node->getDoubleValue();
 610         setOffsetPos(_parentpos, hdg, pch, rll);
 611         setSpeed(_p_spd_node->getDoubleValue());
 612     }
 613     else {
 614         hdg = manager->get_user_heading();
 615         pch = manager->get_user_pitch();
 616         rll = manager->get_user_roll();
 617 //        agl = manager->get_user_agl();
 618         setOffsetPos(globals->get_aircraft_position(), hdg, pch, rll);
 619         setSpeed(manager->get_user_speed());
 620     }
 621
 622     pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
 623     pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
 624     pos.setElevationFt(_offsetpos.getElevationFt());
 625     setHeading(hdg);
 626     setPitch(pch + _pitch_offset);
 627     setBank(rll + _roll_offset);
 628     setOffsetVelocity(dt, pos);
 629     setTime(0);
 630
 631     //update the mass (slugs)
 632     _mass = (_weight_lb + getContents()) / slugs_to_lbs;
 633
 634     _impact_reported = false;
 635
 636     //cout << _name << " _mass "<<_mass <<" " << getContents()
 637     //<< " " << getContents() / slugs_to_lbs << " weight " << _weight_lb << endl;
 638     //    cout << _name << " update hs " << hs << " vs " << vs << endl;
 639 }
 640
 641 void FGAIBallistic::Run(double dt) {
 642     _life_timer += dt;
 643
 644     //_pass += 1;
 645     //cout<<"AIBallistic run: name " << _name.c_str()
 646     //    << " dt " << dt <<  " _life_timer " << _life_timer << " pass " << _pass << endl;
 647
 648     // if life = -1 the object does not die
 649     if (_life_timer > life && life != -1) {
 650         if (_report_expiry && !_expiry_reported && !_impact_reported && !_collision_reported) {
 651             //cout<<"AIBallistic run: name " << _name.c_str() << " expiry "
 652                 //<< " _life_timer " << _life_timer<< endl;
 653             handle_expiry();
 654         }
 655         else {
 656             //cout<<"AIBallistic run: name " << _name.c_str()
 657             //    << " die " <<  " _life_timer " << _life_timer << endl;
 658             setDie(true);
 659         }
 660
 661         setTime(0);
 662     }
 663
 664     // Set the contents in the appropriate tank or other property in the parent to zero
 665     setContents(0);
 666
 667     // Randomize Cd by +- a certain percentage of the ideal Cd
 668     double Cd;
 669     if (_random)
 670         Cd = _cd * (1 - _cd_randomness + 2 * _cd_randomness * sg_random());
 671     else
 672         Cd = _cd;
 673
 674     // Adjust Cd by Mach number. The equations are based on curves
 675     // for a conventional shell/bullet (no boat-tail).
 676     double Cdm;
 677
 678     if (Mach < 0.7)
 679         Cdm = 0.0125 * Mach + Cd;
 680     else if (Mach < 1.2)
 681         Cdm = 0.3742 * pow(Mach, 2) - 0.252 * Mach + 0.0021 + Cd;
 682     else
 683         Cdm = 0.2965 * pow(Mach, -1.1506) + Cd;
 684
 685     //cout <<_name << " Mach " << Mach << " Cdm " << Cdm
 686     //    << " ballistic speed kts "<< speed <<  endl;
 687
 688     // drag = Cd * 0.5 * rho * speed * speed * drag_area;
 689     // rho is adjusted for altitude in void FGAIBase::update,
 690     // using Standard Atmosphere (sealevel temperature 15C)
 691     // acceleration = drag/mass;
 692     // adjust speed by drag
 693     speed -= (Cdm * 0.5 * rho * speed * speed * _drag_area/_mass) * dt;
 694
 695     // don't let speed become negative
 696     if (speed < 0.0)
 697         speed = 0.0;
 698
 699 //    double speed_fps = speed * SG_KT_TO_FPS;
 700
 701     // calculate vertical and horizontal speed components
 702     calcVSHS();
 703
 704     //resolve horizontal speed into north and east components:
 705     //and convert horizontal speed (fps) to degrees per second
 706     calcNE();
 707
 708     // If wind not required, set to zero
 709     if (!_wind) {
 710         _wind_from_north = 0;
 711         _wind_from_east = 0;
 712     }
 713     else {
 714         _wind_from_north = manager->get_wind_from_north();
 715         _wind_from_east = manager->get_wind_from_east();
 716     }
 717
 718     // Calculate velocity due to external force
 719     double force_speed_north_deg_sec = 0;
 720     double force_speed_east_deg_sec = 0;
 721     double hs_force_fps = 0;
 722     double v_force_acc_fpss = 0;
 723     double force_speed_north_fps = 0;
 724     double force_speed_east_fps = 0;
 725     double h_force_lbs = 0;
 726     double normal_force_lbs = 0;
 727     double normal_force_fpss = 0;
 728     double static_friction_force_lbs = 0;
 729     double dynamic_friction_force_lbs = 0;
 730     double friction_force_speed_north_fps = 0;
 731     double friction_force_speed_east_fps = 0;
 732     double friction_force_speed_north_deg_sec = 0;
 733     double friction_force_speed_east_deg_sec = 0;
 734     double force_elevation_deg = 0;
 735     double force_azimuth_deg  = 0;
 736     double force_lbs = 0;
 737
 738     if (_external_force) {
 739         //cout << _name << " external force " <<  hdg << " az " << _azimuth << endl;
 740
 741         SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
 742         force_lbs            = n->getChild("force-lb", 0, true)->getDoubleValue();
 743         force_elevation_deg  = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
 744         force_azimuth_deg    = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
 745
 746         // Resolve force into vertical and horizontal components:
 747         double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
 748         h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
 749
 750         // Perform ground interaction if impacts are not calculated
 751         if (!_report_impact && getHtAGL(10000)) {
 752             double deadzone = 0.1;
 753
 754             if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid) {
 755                 normal_force_lbs = (_mass * slugs_to_lbs) - v_force_lbs;
 756
 757                 if (normal_force_lbs < 0)
 758                     normal_force_lbs = 0;
 759
 760                 pos.setElevationFt(0 + _ground_offset);
 761                 if (vs < 0)
 762                     vs = -vs * 0.5;
 763
 764                 // Calculate friction. We assume a static coefficient of
 765                 // friction (mu) of 0.62 (wood on concrete)
 766                 double mu = 0.62;
 767
 768                 static_friction_force_lbs = mu * normal_force_lbs * _frictionFactor;
 769
 770                 // Adjust horizontal force. We assume that a speed of <= 5 fps is static
 771                 if (h_force_lbs <= static_friction_force_lbs && hs <= 5) {
 772                     h_force_lbs = hs = 0;
 773                     _speed_north_fps = _speed_east_fps = 0;
 774                 }
 775                 else
 776                     dynamic_friction_force_lbs = (static_friction_force_lbs * 0.95);
 777
 778                 // Ignore wind when on the ground for now
 779                 //TODO fix this
 780                 _wind_from_north = 0;
 781                 _wind_from_east = 0;
 782             }
 783         }
 784
 785         //acceleration = (force(lbsf)/mass(slugs))
 786         v_force_acc_fpss = v_force_lbs / _mass;
 787         normal_force_fpss = normal_force_lbs / _mass;
 788         double h_force_acc_fpss = h_force_lbs / _mass;
 789         double dynamic_friction_acc_fpss = dynamic_friction_force_lbs / _mass;
 790
 791         // velocity = acceleration * dt
 792         hs_force_fps = h_force_acc_fpss * dt;
 793         double friction_force_fps = dynamic_friction_acc_fpss * dt;
 794
 795         //resolve horizontal speeds into north and east components:
 796         force_speed_north_fps   = cos(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
 797         force_speed_east_fps    = sin(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
 798
 799         friction_force_speed_north_fps = cos(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
 800         friction_force_speed_east_fps  = sin(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
 801
 802         // convert horizontal speed (fps) to degrees per second
 803         force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat;
 804         force_speed_east_deg_sec  = force_speed_east_fps / ft_per_deg_lon;
 805
 806         friction_force_speed_north_deg_sec = friction_force_speed_north_fps / ft_per_deg_lat;
 807         friction_force_speed_east_deg_sec  = friction_force_speed_east_fps / ft_per_deg_lon;
 808     }
 809
 810     // convert wind speed (fps) to degrees lat/lon per second
 811     double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
 812     double wind_speed_from_east_deg_sec  = _wind_from_east / ft_per_deg_lon;
 813
 814     //recombine the horizontal velocity components
 815     hs = sqrt(((_speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps)
 816         * (_speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps))
 817         + ((_speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps)
 818         * (_speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps)));
 819
 820     if (hs <= 0.00001)
 821         hs = 0;
 822
 823     // adjust vertical speed for acceleration of gravity, buoyancy, and vertical force
 824     double gravity = SG_METER_TO_FEET * (Environment::Gravity::instance()->getGravity(pos));
 825     vs -= (gravity - _buoyancy - v_force_acc_fpss - normal_force_fpss) * dt;
 826
 827     if (vs <= 0.00001 && vs >= -0.00001)
 828         vs = 0;
 829
 830     // set new position
 831     if (_slave_load_to_ac) {
 832         setOffsetPos(pos,
 833             manager->get_user_heading(),
 834             manager->get_user_pitch(),
 835             manager->get_user_roll()
 836             );
 837         pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
 838         pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
 839         pos.setElevationFt(_offsetpos.getElevationFt());
 840
 841         if (getHtAGL(10000)) {
 842             double deadzone = 0.1;
 843
 844             if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid) {
 845                 pos.setElevationFt(0 + _ground_offset);
 846             }
 847             else {
 848                 pos.setElevationFt(_offsetpos.getElevationFt() + _load_offset);
 849             }
 850         }
 851     }
 852     else {
 853         pos.setLatitudeDeg( pos.getLatitudeDeg()
 854             + (speed_north_deg_sec - wind_speed_from_north_deg_sec
 855             + force_speed_north_deg_sec + friction_force_speed_north_deg_sec) * dt );
 856         pos.setLongitudeDeg( pos.getLongitudeDeg()
 857             + (speed_east_deg_sec - wind_speed_from_east_deg_sec
 858             + force_speed_east_deg_sec + friction_force_speed_east_deg_sec) * dt );
 859         pos.setElevationFt(pos.getElevationFt() + vs * dt);
 860     }
 861
 862 //    cout << _name << " run hs " << hs << " vs " << vs << endl;
 863
 864     // recalculate total speed
 865     if ( vs == 0 && hs == 0)
 866         speed = 0;
 867     else
 868         speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
 869
 870     // recalculate elevation and azimuth (velocity vectors)
 871     _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
 872     _azimuth =  atan2((_speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps),
 873         (_speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps))
 874         * SG_RADIANS_TO_DEGREES;
 875
 876     // rationalise azimuth
 877     if (_azimuth < 0)
 878         _azimuth += 360;
 879
 880     if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
 881         //cout<< "_aero_stabilised " << hdg << " az " << _azimuth << endl;
 882         const double coeff = 0.9;
 883
 884         // we assume a symetrical MI about the pitch and yaw axis
 885         setPch(_elevation, dt, coeff);
 886         setHdg(_azimuth, dt, coeff);
 887     }
 888     else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
 889         //cout<< "_force_stabilised "<< endl;
 890
 891         const double coeff = 0.9;
 892         double ratio = h_force_lbs/(_mass * slugs_to_lbs);
 893
 894         if (ratio >  1) ratio =  1;
 895         if (ratio < -1) ratio = -1;
 896
 897         double force_pitch = acos(ratio) * SG_RADIANS_TO_DEGREES;
 898
 899         if (force_pitch <= force_elevation_deg)
 900             force_pitch = force_elevation_deg;
 901
 902         // we assume a symetrical MI about the pitch and yaw axis
 903         setPch(force_pitch,dt, coeff);
 904         setHdg(_azimuth, dt, coeff);
 905     }
 906
 907     // Do impacts and collisions
 908     if (_report_impact && !_impact_reported)
 909         handle_impact();
 910
 911     if (_report_collision && !_collision_reported)
 912         handle_collision();
 913
 914     // Set destruction flag if altitude less than sea level -1000
 915     if (altitude_ft < -1000.0 && life != -1)
 916         setDie(true);
 917 }
 918
 919 double FGAIBallistic::_getTime() const {
 920     return _life_timer;
 921 }
 922
 923 void FGAIBallistic::setTime(double s) {
 924     _life_timer = s;
 925 }
 926
 927 void FGAIBallistic::handleEndOfLife(double elevation) {
 928     report_impact(elevation);
 929
 930     // Make the submodel invisible if the submodel is immortal, otherwise kill it if it has no subsubmodels
 931     if (life == -1) {
 932         invisible = true;
 933     }
 934     else if (_subID == 0) {
 935         // Kill the AIObject if there is no subsubmodel
 936         setDie(true);
 937     }
 938 }
 939
 940 void FGAIBallistic::handle_impact() {
 941     // Try terrain intersection
 942     double start = pos.getElevationM() + 100;
 943
 944     if (!getHtAGL(start))
 945         return;
 946
 947     if (_ht_agl_ft <= 0) {
 948         SG_LOG(SG_AI, SG_DEBUG, "AIBallistic: terrain impact material" << _mat_name);
 949         _impact_reported = true;
 950         handleEndOfLife(_elevation_m);
 951     }
 952 }
 953
 954 void FGAIBallistic::handle_expiry() {
 955     _expiry_reported = true;
 956     handleEndOfLife(pos.getElevationM());
 957 }
 958
 959 void FGAIBallistic::handle_collision()
 960 {
 961     const FGAIBase *object = manager->calcCollision(pos.getElevationFt(),
 962         pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
 963
 964     if (object) {
 965         report_impact(pos.getElevationM(), object);
 966         _collision_reported = true;
 967     }
 968 }
 969
 970 void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
 971 {
 972     _impact_lat    = pos.getLatitudeDeg();
 973     _impact_lon    = pos.getLongitudeDeg();
 974     _impact_elev   = elevation;
 975     _impact_speed  = speed * SG_KT_TO_MPS;
 976     _impact_hdg    = hdg;
 977     _impact_pitch  = pitch;
 978     _impact_roll   = roll;
 979
 980     SGPropertyNode *n = props->getNode("impact", true);
 981
 982     if (object)
 983         n->setStringValue("type", object->getTypeString());
 984     else
 985         n->setStringValue("type", "terrain");
 986
 987     SG_LOG(SG_AI, SG_DEBUG, "AIBallistic: object impact " << _name
 988         << " lon " <<_impact_lon << " lat " <<_impact_lat << " sec " << _life_timer);
 989
 990     n->setDoubleValue("longitude-deg", _impact_lon);
 991     n->setDoubleValue("latitude-deg", _impact_lat);
 992     n->setDoubleValue("elevation-m", _impact_elev);
 993     n->setDoubleValue("heading-deg", _impact_hdg);
 994     n->setDoubleValue("pitch-deg", _impact_pitch);
 995     n->setDoubleValue("roll-deg", _impact_roll);
 996     n->setDoubleValue("speed-mps", _impact_speed);
 997
 998     _impact_report_node->setStringValue(props->getPath());
 999 }
1000
1001 SGVec3d FGAIBallistic::getCartHitchPos() const {
1002     // convert geodetic positions to geocentered
1003     SGVec3d cartuserPos = globals->get_aircraft_position_cart();
1004
1005     //SGVec3d cartPos = getCartPos();
1006
1007     // Transform to the right coordinate frame, configuration is done in
1008     // the x-forward, y-right, z-up coordinates (feet), computation
1009     // in the simulation usual body x-forward, y-right, z-down coordinates
1010     // (meters) )
1011     SGVec3d _off(_x_offset * SG_FEET_TO_METER,
1012             _y_offset * SG_FEET_TO_METER,
1013             -_z_offset * SG_FEET_TO_METER);
1014
1015     // Transform the user position to the horizontal local coordinate system.
1016     SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
1017
1018     // and postrotate the orientation of the user model wrt the horizontal
1019     // local frame
1020     hlTrans *= SGQuatd::fromYawPitchRollDeg(
1021         manager->get_user_heading(),
1022         manager->get_user_pitch(),
1023         manager->get_user_roll());
1024
1025     // The offset converted to the usual body fixed coordinate system
1026     // rotated to the earth-fixed coordinates axis
1027     SGVec3d off = hlTrans.backTransform(_off);
1028
1029     // Add the position offset of the user model to get the geocentered position
1030     SGVec3d offsetPos = cartuserPos + off;
1031     return offsetPos;
1032 }
1033
1034 void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, double roll) {
1035     // Convert the hitch geocentered position to geodetic
1036     SGVec3d cartoffsetPos = getCartOffsetPos(inpos, heading, pitch, roll);
1037     SGGeodesy::SGCartToGeod(cartoffsetPos, _offsetpos);
1038 }
1039
1040 double FGAIBallistic::getDistanceToHitch() const {
1041     //calculate the distance load to hitch
1042     SGVec3d carthitchPos = getCartHitchPos();
1043     SGVec3d cartPos = getCartPos();
1044
1045     SGVec3d diff = carthitchPos - cartPos;
1046     double distance = norm(diff);
1047     return distance * SG_METER_TO_FEET;
1048 }
1049
1050 double FGAIBallistic::getElevToHitch() const {
1051     // now the angle, positive angles are upwards
1052     double distance = getDistanceToHitch() * SG_FEET_TO_METER;
1053     double angle = 0;
1054     double daltM = _offsetpos.getElevationM() - pos.getElevationM();
1055
1056     if (fabs(distance) < SGLimits<float>::min()) {
1057         angle = 0;
1058     } else {
1059         double sAngle = daltM/distance;
1060         sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
1061         angle = SGMiscd::rad2deg(asin(sAngle));
1062     }
1063
1064     return angle;
1065 }
1066
1067 double FGAIBallistic::getBearingToHitch() const {
1068     //calculate the bearing and range of the second pos from the first
1069     double distance = getDistanceToHitch() * SG_FEET_TO_METER;
1070     double az1, az2;
1071
1072     geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance);
1073
1074     return az1;
1075 }
1076
1077 double FGAIBallistic::getRelBrgHitchToUser() const {
1078     //calculate the relative bearing
1079     double az1, az2, distance;
1080
1081     geo_inverse_wgs_84(_offsetpos, globals->get_aircraft_position(), &az1, &az2, &distance);
1082
1083     double rel_brg = az1 - hdg;
1084
1085     SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0);
1086
1087     return rel_brg;
1088 }
1089
1090 double FGAIBallistic::getElevHitchToUser() const {
1091     // Calculate the distance from the user position
1092     SGVec3d carthitchPos = getCartHitchPos();
1093     SGVec3d cartuserPos = globals->get_aircraft_position_cart();
1094
1095     SGVec3d diff = cartuserPos - carthitchPos;
1096
1097     double distance = norm(diff);
1098     double angle = 0;
1099
1100     double daltM = globals->get_aircraft_position().getElevationM() - _offsetpos.getElevationM();
1101
1102     // Now the angle, positive angles are upwards
1103     if (fabs(distance) < SGLimits<float>::min()) {
1104         angle = 0;
1105     }
1106     else {
1107         double sAngle = daltM/distance;
1108         sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
1109         angle = SGMiscd::rad2deg(asin(sAngle));
1110     }
1111
1112     return angle;
1113 }
1114
1115 void FGAIBallistic::setTgtOffsets(double dt, double coeff) {
1116     double c = dt / (coeff + dt);
1117
1118     _x_offset = (_tgt_x_offset * c) + (_x_offset * (1 - c));
1119     _y_offset = (_tgt_y_offset * c) + (_y_offset * (1 - c));
1120     _z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
1121 }
1122
1123 void FGAIBallistic::calcVSHS() {
1124     // Calculate vertical and horizontal speed components
1125     double speed_fps = speed * SG_KT_TO_FPS;
1126
1127     if (speed == 0.0) {
1128         hs = vs = 0.0;
1129     }
1130     else {
1131         vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
1132         hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
1133     }
1134 }
1135
1136 void FGAIBallistic::calcNE() {
1137     // Resolve horizontal speed into north and east components:
1138     _speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
1139     _speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
1140
1141     // Convert horizontal speed (fps) to degrees per second
1142     speed_north_deg_sec = _speed_north_fps / ft_per_deg_lat;
1143     speed_east_deg_sec  = _speed_east_fps / ft_per_deg_lon;
1144 }
1145
1146 SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading,
1147                                         double user_pitch, double user_roll
1148                                         ) const {
1149     // Convert geodetic positions to geocentered
1150     SGVec3d cartuserPos = SGVec3d::fromGeod(inpos);
1151
1152     // Transform to the right coordinate frame, configuration is done in
1153     // the x-forward, y-right, z-up coordinates (feet), computation
1154     // in the simulation usual body x-forward, y-right, z-down coordinates
1155     // (meters) )
1156     SGVec3d _off(_x_offset * SG_FEET_TO_METER,
1157             _y_offset * SG_FEET_TO_METER,
1158             -_z_offset * SG_FEET_TO_METER);
1159
1160     // Transform the user position to the horizontal local coordinate system.
1161     SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
1162
1163     // And postrotate the orientation of the user model wrt the horizontal
1164     // local frame
1165     hlTrans *= SGQuatd::fromYawPitchRollDeg(
1166         user_heading,
1167         user_pitch,
1168         user_roll);
1169
1170     // The offset converted to the usual body fixed coordinate system
1171     // rotated to the earth-fixed coordinates axis
1172     SGVec3d off = hlTrans.backTransform(_off);
1173
1174     // Add the position offset of the user model to get the geocentered position
1175     SGVec3d offsetPos = cartuserPos + off;
1176
1177     return offsetPos;
1178 }
1179
1180 void FGAIBallistic::setOffsetVelocity(double dt, SGGeod offsetpos) {
1181     // Calculate the distance from the previous offset position
1182     SGVec3d cartoffsetPos = SGVec3d::fromGeod(offsetpos);
1183     SGVec3d diff = cartoffsetPos - _oldcartoffsetPos;
1184
1185     double distance = norm(diff);
1186     // Calculate speed knots
1187     speed = (distance / dt) * SG_MPS_TO_KT;
1188
1189     // Now calulate the angle between the old and current postion positions (degrees)
1190     double angle = 0;
1191     double daltM = offsetpos.getElevationM() - _oldoffsetpos.getElevationM();
1192
1193     if (fabs(distance) < SGLimits<float>::min()) {
1194         angle = 0;
1195     }
1196     else {
1197         double sAngle = daltM / distance;
1198         sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
1199         angle = SGMiscd::rad2deg(asin(sAngle));
1200     }
1201
1202     _elevation = angle;
1203
1204     // Calculate vertical and horizontal speed components
1205     calcVSHS();
1206
1207     // Calculate the bearing of the new offset position from the old
1208     // Don't do this if speed is low
1209     //cout << "speed " << speed << endl;
1210     if (speed > 0.1) {
1211         double az1, az2, dist;
1212         geo_inverse_wgs_84(_oldoffsetpos, offsetpos, &az1, &az2, &dist);
1213         _azimuth = az1;
1214         //cout << "offset az " << _azimuth << endl;
1215     }
1216     else {
1217         _azimuth = hdg;
1218         //cout << " slow offset az " << _azimuth << endl;
1219     }
1220
1221     // Resolve horizontal speed into north and east components
1222     calcNE();
1223
1224     // And finally store the new values
1225     _oldcartoffsetPos = cartoffsetPos;
1226     _oldoffsetpos = offsetpos;
1227 }