X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIBallistic.cxx;h=16b15fe1d93f3aec36cb81a45ad6631c09a3398c;hb=438e365aadc321f70941a31a0b9462b4b0155fd6;hp=7b66be4bbb1824080448853cd5c1fbde7650b48b;hpb=030742fa4ae7d99facb517381da71fa404c4ee2b;p=flightgear.git

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index 7b66be4bb..16b15fe1d 100644
--- a/src/AIModel/AIBallistic.cxx
+++ b/src/AIModel/AIBallistic.cxx
@@ -29,39 +29,44 @@
 
 #include <Scenery/scenery.hxx>
 
-#include "AIModelData.hxx"
 #include "AIBallistic.hxx"
 
 #include <Main/util.hxx>
 
+using namespace simgear;
+
 const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
 const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
 
-using namespace simgear;
-
 FGAIBallistic::FGAIBallistic(object_type ot) :
 FGAIBase(ot),
-    _elevation(0),
-    _aero_stabilised(false),
-    _drag_area(0.007),
-    _life_timer(0.0),
+_height(0.0),
+_speed(0),
+_ht_agl_ft(0.0),
+_azimuth(0.0),
+_elevation(0.0),
+_rotation(0.0),
+hs(0),
+_elapsed_time(0),
+_aero_stabilised(false),
+_drag_area(0.007),
+_life_timer(0.0),
 _gravity(32.1740485564),
-    _buoyancy(0),
-    _random(false),
-    _ht_agl_ft(0),
-    _load_resistance(0),
-    _solid(false),
-    _report_collision(false),
-    _report_impact(false),
+_buoyancy(0),
 _wind(true),
-    _impact_report_node(fgGetNode("/ai/models/model-impact", true)),
-_external_force(false),
+_mass(0),
+_random(false),
+_load_resistance(0),
+_solid(false),
+_force_stabilised(false),
 _slave_to_ac(false),
 _slave_load_to_ac(false),
-_formate_to_ac(false),
 _contents_lb(0),
-_mass(0),
-_height(0),
+_report_collision(false),
+_report_impact(false),
+_external_force(false),
+_report_expiry(false),
+_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
 _old_height(0)
 
 {
@@ -78,23 +83,25 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
 
     FGAIBase::readFromScenario(scFileNode);
 
-    //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac"));
+    //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac")); 
+    setRandom(scFileNode->getBoolValue("random", false));
     setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
     setElevation(scFileNode->getDoubleValue("elevation", 0));
     setDragArea(scFileNode->getDoubleValue("eda", 0.007));
     setLife(scFileNode->getDoubleValue("life", 900.0));
     setBuoyancy(scFileNode->getDoubleValue("buoyancy", 0));
-    setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0));
-    setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0));
+    //setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0));
+    //setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0));
     setWind(scFileNode->getBoolValue("wind", false));
     setRoll(scFileNode->getDoubleValue("roll", 0.0));
     setCd(scFileNode->getDoubleValue("cd", 0.029));
     //setMass(scFileNode->getDoubleValue("mass", 0.007));
     setWeight(scFileNode->getDoubleValue("weight", 0.25));
-    setStabilisation(scFileNode->getBoolValue("aero_stabilized", false));
+    setStabilisation(scFileNode->getBoolValue("aero-stabilised", false));
     setNoRoll(scFileNode->getBoolValue("no-roll", false));
-    setRandom(scFileNode->getBoolValue("random", false));
     setImpact(scFileNode->getBoolValue("impact", false));
+    setExpiry(scFileNode->getBoolValue("expiry", false));
+    setCollision(scFileNode->getBoolValue("collision", false));
     setImpactReportNode(scFileNode->getStringValue("impact-reports"));
     setName(scFileNode->getStringValue("name", "Rocket"));
     setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
@@ -102,7 +109,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
     setSubID(scFileNode->getIntValue("SubID", 0));
     setExternalForce(scFileNode->getBoolValue("external-force", false));
     setForcePath(scFileNode->getStringValue("force-path", ""));
-    setForceStabilisation(scFileNode->getBoolValue("force_stabilized", false));
+    setForceStabilisation(scFileNode->getBoolValue("force-stabilised", false));
     setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
     setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
     setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
@@ -113,14 +120,8 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
     setLoadOffset(scFileNode->getDoubleValue("load-offset", 0.0));
     setSlaved(scFileNode->getBoolValue("slaved", false));
     setSlavedLoad(scFileNode->getBoolValue("slaved-load", false));
-    setContentsNode(scFileNode->getStringValue("contents"));
-    setRandom(scFileNode->getBoolValue("random", false));
-}
-
-osg::Node* FGAIBallistic::load3DModel(const string &path, SGPropertyNode *prop_root)
-{
-  model = SGModelLib::loadModel(path, prop_root, new FGAIModelData(this, prop_root));
-  return model.get();
+    setContentsPath(scFileNode->getStringValue("contents"));
+    setParentName(scFileNode->getStringValue("parent"));
 }
 
 bool FGAIBallistic::init(bool search_in_AI_path) {
@@ -128,13 +129,36 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
 
     _impact_reported = false;
     _collision_reported = false;
+    _expiry_reported = false;
+
+    _impact_lat = 0;
+    _impact_lon = 0;
+    _impact_elev = 0;
+    _impact_hdg = 0;
+    _impact_pitch = 0;
+    _impact_roll = 0;
+    _impact_speed = 0;
+
     invisible = false;
 
     _elapsed_time += (sg_random() * 100);
 
     props->setStringValue("material/name", "");
     props->setStringValue("name", _name.c_str());
-    props->setStringValue("submodels/path", _submodel.c_str());
+    props->setStringValue("submodels/path", _path.c_str());
+
+    if (_slave_to_ac){
+        props->setStringValue("force/path", _force_path.c_str());
+        props->setStringValue("contents/path", _contents_path.c_str());
+    }
+
+    //if(_parent != ""){
+    //    setParentNode();
+    //}
+
+    //setParentNodes(_selected_ac);
+
+    //props->setStringValue("vector/path", _vector_path.c_str());
 
     // start with high value so that animations don't trigger yet
     _ht_agl_ft = 1e10;
@@ -144,6 +168,12 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
 
     Transform();
 
+    if(_parent != ""){
+        setParentNode();
+    }
+
+    setParentNodes(_selected_ac);
+
     return true;
 }
 
@@ -153,22 +183,21 @@ void FGAIBallistic::bind() {
     props->tie("sim/time/elapsed-sec",
         SGRawValueMethods<FGAIBallistic,double>(*this,
         &FGAIBallistic::_getTime));
-    props->tie("mass-slug",
-        SGRawValueMethods<FGAIBallistic,double>(*this,
-        &FGAIBallistic::getMass));
-    props->tie("material/load-resistance",
-                SGRawValuePointer<double>(&_load_resistance));
+    //props->tie("mass-slug",
+    //    SGRawValueMethods<FGAIBallistic,double>(*this,
+    //    &FGAIBallistic::getMass));
+
     props->tie("material/solid",
-                SGRawValuePointer<bool>(&_solid));
+        SGRawValuePointer<bool>(&_solid));
     props->tie("altitude-agl-ft",
-                SGRawValuePointer<double>(&_ht_agl_ft));
+        SGRawValuePointer<double>(&_ht_agl_ft));
     props->tie("controls/slave-to-ac",
         SGRawValueMethods<FGAIBallistic,bool>
         (*this, &FGAIBallistic::getSlaved, &FGAIBallistic::setSlaved));
     props->tie("controls/invisible",
         SGRawValuePointer<bool>(&invisible));
 
-    if(_external_force){
+    if(_external_force || _slave_to_ac){
         props->tie("controls/force_stabilized",
             SGRawValuePointer<bool>(&_force_stabilised));
         props->tie("position/global-x", 
@@ -184,7 +213,7 @@ void FGAIBallistic::bind() {
         props->tie("velocities/horizontal-speed-fps",
             SGRawValuePointer<double>(&hs));
         props->tie("position/altitude-ft",
-            SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getAltitude, &FGAIBase::_setAltitude));
+            SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getElevationFt, &FGAIBase::_setAltitude));
         props->tie("position/latitude-deg", 
             SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLatitude, &FGAIBase::_setLatitude));
         props->tie("position/longitude-deg",
@@ -203,29 +232,30 @@ void FGAIBallistic::bind() {
             (*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
         props->tie("load/distance-to-hitch-ft",
             SGRawValueMethods<FGAIBallistic,double>
-            (*this, &FGAIBallistic::getDistanceLoadToHitch));
+            (*this, &FGAIBallistic::getDistanceToHitch));
         props->tie("load/elevation-to-hitch-deg",
             SGRawValueMethods<FGAIBallistic,double>
-            (*this, &FGAIBallistic::getElevLoadToHitch));
+            (*this, &FGAIBallistic::getElevToHitch));
         props->tie("load/bearing-to-hitch-deg",
             SGRawValueMethods<FGAIBallistic,double>
-            (*this, &FGAIBallistic::getBearingLoadToHitch));
+            (*this, &FGAIBallistic::getBearingToHitch));
+        props->tie("material/load-resistance",
+        SGRawValuePointer<double>(&_load_resistance));
     }
 
 }
 
 void FGAIBallistic::unbind() {
-    //    FGAIBase::unbind();
+//    FGAIBase::unbind();
 
     props->untie("sim/time/elapsed-sec");
     props->untie("mass-slug");
-    props->untie("material/load-resistance");
     props->untie("material/solid");
     props->untie("altitude-agl-ft");
     props->untie("controls/slave-to-ac");
     props->untie("controls/invisible");
 
-    if(_external_force){
+    if(_external_force || _slave_to_ac){
         props->untie("position/global-y");
         props->untie("position/global-x");
         props->untie("position/global-z");
@@ -244,6 +274,7 @@ void FGAIBallistic::unbind() {
         props->untie("load/distance-to-hitch-ft");
         props->untie("load/elevation-to-hitch-deg");
         props->untie("load/bearing-to-hitch-deg");
+        props->untie("material/load-resistance");
     }
 }
 
@@ -254,20 +285,21 @@ void FGAIBallistic::update(double dt) {
     if (_slave_to_ac){
         slaveToAC(dt);
         Transform();
-        setHitchVelocity(dt);
-    } else if (_formate_to_ac){
-        formateToAC(dt);
-        Transform();
-        setHitchVelocity(dt);
     } else if (!invisible){
-    Run(dt);
-    Transform();
-}
+        Run(dt);
+        Transform();
+    }
 
 }
 
 void FGAIBallistic::setAzimuth(double az) {
-    hdg = _azimuth = az;
+
+    if (_random)
+        hdg = _azimuth = (az - 5 ) + (10 * sg_random());
+    else 
+        hdg = _azimuth = az;
+
+    //cout << _name << " init hdg " << hdg << " random " << _random << endl;
 }
 
 void FGAIBallistic::setElevation(double el) {
@@ -295,7 +327,12 @@ void FGAIBallistic::setDragArea(double a) {
 }
 
 void FGAIBallistic::setLife(double seconds) {
-    life = seconds;
+
+    if (_random){
+        life = seconds * _randomness + (seconds * (1 -_randomness) * sg_random());
+        //cout << "life " << life << endl;
+    } else
+        life = seconds;
 }
 
 void FGAIBallistic::setBuoyancy(double fpss) {
@@ -325,6 +362,11 @@ void FGAIBallistic::setMass(double m) {
 void FGAIBallistic::setWeight(double w) {
     _weight_lb = w;
 }
+
+void FGAIBallistic::setRandomness(double r) {
+    _randomness = r;
+}
+
 void FGAIBallistic::setRandom(bool r) {
     _random = r;
 }
@@ -337,6 +379,10 @@ void FGAIBallistic::setCollision(bool c) {
     _report_collision = c;
 }
 
+void FGAIBallistic::setExpiry(bool e) {
+    _report_expiry = e;
+}
+
 void FGAIBallistic::setExternalForce(bool f) {
     _external_force = f;
 }
@@ -347,12 +393,9 @@ void FGAIBallistic::setImpactReportNode(const string& path) {
         _impact_report_node = fgGetNode(path.c_str(), true);
 }
 
-void FGAIBallistic::setName(const string& n) {
-    _name = n;
-}
-
 void FGAIBallistic::setSMPath(const string& s) {
-    _submodel = s;
+    _path = s;
+    //cout << "submodel path " << _path << endl;
 }
 
 void FGAIBallistic::setFuseRange(double f) {
@@ -383,27 +426,74 @@ void FGAIBallistic::setSlaved(bool s) {
     _slave_to_ac = s;
 }
 
-void FGAIBallistic::setFormate(bool f) {
-    _formate_to_ac = f;
-}
+void FGAIBallistic::setContentsPath(const string& path) {
+
+    _contents_path = path;
 
-void FGAIBallistic::setContentsNode(const string& path) {
     if (!path.empty()) {
         _contents_node = fgGetNode(path.c_str(), true);
     }
 }
 
+void FGAIBallistic::setContentsNode(SGPropertyNode_ptr node) {
+
+    if (node != 0) {
+        _contents_node = node;
+        _contents_path = _contents_node->getDisplayName();
+    }
+}
+
+void FGAIBallistic::setParentNodes(SGPropertyNode_ptr node) {
+
+    if (node != 0) {
+        _pnode = node;
+        _p_pos_node = _pnode->getChild("position", 0, true);
+        _p_lat_node = _p_pos_node->getChild("latitude-deg", 0, true);
+        _p_lon_node = _p_pos_node->getChild("longitude-deg", 0, true);
+        _p_alt_node = _p_pos_node->getChild("altitude-ft", 0, true);
+        _p_agl_node = _p_pos_node->getChild("altitude-agl-ft", 0, true);
+
+
+        _p_ori_node = _pnode->getChild("orientation", 0, true);
+        _p_pch_node = _p_ori_node->getChild("pitch-deg", 0, true);
+        _p_rll_node = _p_ori_node->getChild("roll-deg", 0, true);
+        _p_hdg_node = _p_ori_node->getChild("true-heading-deg",0, true);
+
+        _p_vel_node = _pnode->getChild("velocities", 0, true);
+        _p_spd_node = _p_vel_node->getChild("true-airspeed-kt", 0, true);
+    }
+
+}
+
+void FGAIBallistic::setParentPos() {
+
+    if (_pnode != 0) { 
+        //cout << "set parent pos" << endl;
+
+        double lat = _p_lat_node->getDoubleValue();
+        double lon = _p_lon_node->getDoubleValue();
+        double alt = _p_alt_node->getDoubleValue();
+
+        _parentpos.setLongitudeDeg(lon);
+        _parentpos.setLatitudeDeg(lat);
+        _parentpos.setElevationFt(alt);
+
+    }
+
+}
+
 bool FGAIBallistic::getSlaved() const {
     return _slave_to_ac;
-}  
+}
 
 double FGAIBallistic::getMass() const {
     return _mass;
 }
 
 double FGAIBallistic::getContents() {
-    if(_contents_node) 
+    if(_contents_node){
         _contents_lb = _contents_node->getChild("level-lbs",0,1)->getDoubleValue();
+    }
     return _contents_lb;
 }
 
@@ -430,27 +520,33 @@ void FGAIBallistic::setForcePath(const string& p) {
     }
 }
 
-bool FGAIBallistic::getHtAGL(){
+bool FGAIBallistic::getHtAGL(double start){
 
-    if (globals->get_scenery()->get_elevation_m(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
-        10000.0, _elevation_m, &_material)){
+    if (getGroundElevationM(SGGeod::fromGeodM(pos, start),
+        _elevation_m, &_material)) {
             _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
+
             if (_material) {
                 const vector<string>& names = _material->get_names();
-
                 _solid = _material->get_solid();
                 _load_resistance = _material->get_load_resistance();
                 _frictionFactor =_material->get_friction_factor();
+
                 if (!names.empty())
                     props->setStringValue("material/name", names[0].c_str());
                 else
                     props->setStringValue("material/name", "");
-                /*cout << "material " << mat_name 
-                << " solid " << _solid 
-                << " load " << _load_resistance
-                << " frictionFactor " << frictionFactor
-                << endl;*/
+
+                _mat_name = names[0];
+
+                //cout << "material " << _mat_name 
+                //<< " solid " << _solid 
+                //<< " load " << _load_resistance
+                //<< " frictionFactor " << _frictionFactor
+                //<< endl;
+
             }
+
             return true;
     } else {
         return false;
@@ -477,23 +573,32 @@ void FGAIBallistic::setBnk(double r, double dt, double coeff){
     roll = (r * c) + (roll * (1 - c));
 }
 
+void FGAIBallistic::setSpd(double s, double dt, double coeff){
+    double c = dt / (coeff + dt);
+    _speed = (s * c) + (_speed * (1 - c));
+}
+
 void FGAIBallistic::setHt(double h, double dt, double coeff){
     double c = dt / (coeff + dt);
     _height = (h * c) + (_height * (1 - c));
 }
 
-void FGAIBallistic::setHdg(double az, double dt, double coeff){
+int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff){
     double recip = getRecip(hdg);
     double c = dt / (coeff + dt);
     //we need to ensure that we turn the short way to the new hdg
-    if (az < recip && az < hdg && hdg > 180) {
-        hdg = ((az + 360) * c) + (hdg * (1 - c));
-    } else if (az > recip && az > hdg && hdg <= 180){
-        hdg = ((az - 360) * c) + (hdg * (1 - c));
+    if (tgt_hdg < recip && tgt_hdg < hdg && hdg > 180) {
+        hdg = ((tgt_hdg + 360) * c) + (hdg * (1 - c));
+//        cout << "case 1: right turn" << endl;
+    } else if (tgt_hdg > recip && tgt_hdg > hdg && hdg <= 180){
+        hdg = ((tgt_hdg - 360) * c) + (hdg * (1 - c));
+//        cout << "case 2: left turn" << endl;
     } else {
-        hdg = (az * c) + (hdg * (1 - c));
-    }
+        hdg = (tgt_hdg * c) + (hdg * (1 - c));
+//        cout << "case 4: left turn" << endl;
     }
+    return -1;
+}
 
 double  FGAIBallistic::getTgtXOffset() const {
     return _tgt_x_offset;
@@ -521,34 +626,63 @@ void FGAIBallistic::setTgtZOffset(double z){
 
 void FGAIBallistic::slaveToAC(double dt){
 
-    setHitchPos();
-    pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
-    pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
-    pos.setElevationFt(hitchpos.getElevationFt());
-    setHeading(manager->get_user_heading());
-    setPitch(manager->get_user_pitch() + _pitch_offset);
-    setBank(manager->get_user_roll() + _roll_offset);
-    setSpeed(manager->get_user_speed());
+    double hdg, pch, rll, agl = 0;
+
+    if (_pnode != 0) {
+        setParentPos();
+        hdg = _p_hdg_node->getDoubleValue();
+        pch = _p_pch_node->getDoubleValue();
+        rll = _p_rll_node->getDoubleValue();
+        agl = _p_agl_node->getDoubleValue();
+        setOffsetPos(_parentpos, hdg, pch, rll);
+        setSpeed(_p_spd_node->getDoubleValue());
+    }else {
+        hdg = manager->get_user_heading();
+        pch = manager->get_user_pitch();
+        rll = manager->get_user_roll();
+        agl = manager->get_user_agl();
+        setOffsetPos(userpos, hdg, pch, rll);
+        setSpeed(manager->get_user_speed());
+    }
+
+    pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
+    pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
+    pos.setElevationFt(_offsetpos.getElevationFt());
+    setHeading(hdg);
+    setPitch(pch + _pitch_offset);
+    setBank(rll + _roll_offset);
+    setOffsetVelocity(dt, pos);
+
     //update the mass (slugs)
     _mass = (_weight_lb + getContents()) / slugs_to_lbs;
 
-    /*cout <<"_mass "<<_mass <<" " << getContents() 
-    <<" " << getContents() / slugs_to_lbs << endl;*/
+    _impact_reported = false;
+
+    //cout << _name << " _mass "<<_mass <<" " << getContents() 
+    //<< " " << getContents() / slugs_to_lbs << " weight " << _weight_lb << endl;
+    //    cout << _name << " update hs " << hs << " vs " << vs << endl;
 }
 
 void FGAIBallistic::Run(double dt) {
     _life_timer += dt;
 
     // if life = -1 the object does not die
-    if (_life_timer > life && life != -1)
-        setDie(true);
+    if (_life_timer > life && life != -1){
+
+        if (_report_expiry && !_expiry_reported && !_impact_reported && !_collision_reported){
+            //cout<<"AIBallistic: expiry"<< endl;
+            handle_expiry();
+        } else
+            setDie(true);
+
+    }
 
     //set the contents in the appropriate tank or other property in the parent to zero
     setContents(0);
 
-    //randomise Cd by +- 5%
+    //randomise Cd by +- 10%
     if (_random)
-        _Cd = _Cd * 0.95 + (0.05 * sg_random());
+        _Cd = _Cd * 0.90 + (0.10 * sg_random());
 
     // Adjust Cd by Mach number. The equations are based on curves
     // for a conventional shell/bullet (no boat-tail).
@@ -561,7 +695,8 @@ void FGAIBallistic::Run(double dt) {
     else
         Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd;
 
-    //cout << "Mach " << Mach << " Cdm " << Cdm << "// ballistic speed kts "<< speed <<  endl;
+    //cout <<_name << " Mach " << Mach << " Cdm " << Cdm 
+    //    << " ballistic speed kts "<< speed <<  endl;
 
     // drag = Cd * 0.5 * rho * speed * speed * drag_area;
     // rho is adjusted for altitude in void FGAIBase::update,
@@ -605,7 +740,7 @@ void FGAIBallistic::Run(double dt) {
     //calculate velocity due to external force
     double force_speed_north_deg_sec = 0;
     double force_speed_east_deg_sec = 0;
-    double vs_force_fps = 0;
+//    double vs_force_fps = 0;
     double hs_force_fps = 0;
     double v_force_acc_fpss = 0;
     double force_speed_north_fps = 0;
@@ -622,18 +757,20 @@ void FGAIBallistic::Run(double dt) {
     double force_elevation_deg = 0;
 
     if (_external_force) {
+        //cout << _name << " external force" << endl;
+
         SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
         double force_lbs            = n->getChild("force-lb", 0, true)->getDoubleValue();
         force_elevation_deg         = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
         double force_azimuth_deg    = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
-
+        
         //resolve force into vertical and horizontal components:
         double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
         h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
 
         //ground interaction 
 
-        if (getHtAGL()){
+        if (getHtAGL(10000)){
             double deadzone = 0.1;
 
             if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
@@ -714,18 +851,22 @@ void FGAIBallistic::Run(double dt) {
 
     // set new position
     if(_slave_load_to_ac) {
-        setHitchPos();
-        pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
-        pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
-        pos.setElevationFt(hitchpos.getElevationFt());
-
-        if (getHtAGL()){
+        setOffsetPos(pos, 
+            manager->get_user_heading(),
+            manager->get_user_pitch(), 
+            manager->get_user_roll()
+            );
+        pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
+        pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
+        pos.setElevationFt(_offsetpos.getElevationFt());
+
+        if (getHtAGL(10000)){
             double deadzone = 0.1;
 
             if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
                 pos.setElevationFt(0 + _ground_offset);
             } else {
-                pos.setElevationFt(hitchpos.getElevationFt() + _load_offset);
+                pos.setElevationFt(_offsetpos.getElevationFt() + _load_offset);
             }
 
         }
@@ -739,6 +880,8 @@ void FGAIBallistic::Run(double dt) {
         pos.setElevationFt(pos.getElevationFt() + vs * dt);
     }
 
+//    cout << _name << " run hs " << hs << " vs " << vs << endl;
+
     // recalculate total speed
     if ( vs == 0 && hs == 0)
         speed = 0;
@@ -756,12 +899,15 @@ void FGAIBallistic::Run(double dt) {
         _azimuth += 360;
 
     if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
+        //cout<< "_aero_stabilised "<< endl;
         const double coeff = 0.9;
 
         // we assume a symetrical MI about the pitch and yaw axis
         setPch(_elevation, dt, coeff);
         setHdg(_azimuth, dt, coeff);
     } else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
+        //cout<< "_force_stabilised "<< endl;
+        
         const double coeff = 0.9;
         double ratio = h_force_lbs/(_mass * slugs_to_lbs);
 
@@ -798,7 +944,9 @@ double FGAIBallistic::_getTime() const {
 void FGAIBallistic::handle_impact() {
 
     // try terrain intersection
-    if(!getHtAGL()) 
+    double start = pos.getElevationM() + 10;
+
+    if(!getHtAGL(start)) 
         return;
 
     if (_ht_agl_ft <= 0) {
@@ -810,16 +958,30 @@ void FGAIBallistic::handle_impact() {
             invisible = true;
         } else if (_subID == 0)  // kill the AIObject if there is no subsubmodel
             setDie(true);
+    } 
+}
+
+void FGAIBallistic::handle_expiry() {
+
+    SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: handle_expiry " << pos.getElevationM());
+
+    report_impact(pos.getElevationM());
+    _expiry_reported = true;
+
+    if (life == -1){
+        invisible = true;
+    } else if (_subID == 0){  // kill the AIObject if there is no subsubmodel
+        setDie(true);
     }
+
 }
 
 void FGAIBallistic::handle_collision()
 {
     const FGAIBase *object = manager->calcCollision(pos.getElevationFt(),
-            pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
+        pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
 
     if (object) {
-        SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object hit");
         report_impact(pos.getElevationM(), object);
         _collision_reported = true;
     }
@@ -836,11 +998,14 @@ void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
     _impact_roll   = roll;
 
     SGPropertyNode *n = props->getNode("impact", true);
+
     if (object)
         n->setStringValue("type", object->getTypeString());
     else
         n->setStringValue("type", "terrain");
 
+    SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object impact" << _name << " lon " <<_impact_lon);
+
     n->setDoubleValue("longitude-deg", _impact_lon);
     n->setDoubleValue("latitude-deg", _impact_lat);
     n->setDoubleValue("elevation-m", _impact_elev);
@@ -860,16 +1025,16 @@ SGVec3d FGAIBallistic::getCartUserPos() const {
 SGVec3d FGAIBallistic::getCartHitchPos() const{
 
     // convert geodetic positions to geocentered
-    SGVec3d cartuserPos = getCartUserPos();
-    SGVec3d cartPos = getCartPos();
+    SGVec3d cartuserPos = SGVec3d::fromGeod(userpos);
+    //SGVec3d cartPos = getCartPos();
 
     // Transform to the right coordinate frame, configuration is done in
     // the x-forward, y-right, z-up coordinates (feet), computation
     // in the simulation usual body x-forward, y-right, z-down coordinates
     // (meters) )
     SGVec3d _off(_x_offset * SG_FEET_TO_METER,
-        _y_offset * SG_FEET_TO_METER,
-        -_z_offset * SG_FEET_TO_METER);
+            _y_offset * SG_FEET_TO_METER,
+            -_z_offset * SG_FEET_TO_METER);
 
     // Transform the user position to the horizontal local coordinate system.
     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
@@ -891,14 +1056,19 @@ SGVec3d FGAIBallistic::getCartHitchPos() const{
     return offsetPos;
 }
 
-void FGAIBallistic::setHitchPos(){
+void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, double roll){
     // convert the hitch geocentered position to geodetic
-    SGVec3d carthitchPos = getCartHitchPos();
 
-    SGGeodesy::SGCartToGeod(carthitchPos, hitchpos);
+    SGVec3d cartoffsetPos = getCartOffsetPos(inpos, heading, pitch, roll);
+
+    //SGVec3d cartoffsetPos = getCartHitchPos();
+
+    //SGGeodesy::SGCartToGeod(cartoffsetPos, hitchpos);
+    SGGeodesy::SGCartToGeod(cartoffsetPos, _offsetpos);
+
 }
 
-double FGAIBallistic::getDistanceLoadToHitch() const {
+double FGAIBallistic::getDistanceToHitch() const {
     //calculate the distance load to hitch 
     SGVec3d carthitchPos = getCartHitchPos();
     SGVec3d cartPos = getCartPos();
@@ -908,47 +1078,11 @@ double FGAIBallistic::getDistanceLoadToHitch() const {
     return distance * SG_METER_TO_FEET;
 }
 
-void FGAIBallistic::setHitchVelocity(double dt) {
-    //calculate the distance from the previous hitch position
-    SGVec3d carthitchPos = getCartHitchPos();
-    SGVec3d diff = carthitchPos - _oldcarthitchPos;
-
-    double distance = norm(diff);
-
-    //calculate speed knots
-    speed = (distance/dt) * SG_MPS_TO_KT;
-
-    //now calulate the angle between the old and current hitch positions (degrees)
-    double angle = 0;
-    double daltM = hitchpos.getElevationM() - oldhitchpos.getElevationM();
-
-    if (fabs(distance) < SGLimits<float>::min()) {
-        angle = 0;
-    } else {
-        double sAngle = daltM/distance;
-        sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
-        angle = SGMiscd::rad2deg(asin(sAngle));
-    }
-
-    _elevation = angle;
-
-    //calculate the bearing of the new hitch position from the old
-    double az1, az2, dist;
-
-    geo_inverse_wgs_84(oldhitchpos, hitchpos, &az1, &az2, &dist);
-
-    _azimuth = az1;
-
-    // and finally store the new values
-    _oldcarthitchPos = carthitchPos;
-    oldhitchpos = hitchpos;
-}
-
-double FGAIBallistic::getElevLoadToHitch() const {
+double FGAIBallistic::getElevToHitch() const {
     // now the angle, positive angles are upwards
-    double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER;
+    double distance = getDistanceToHitch() * SG_FEET_TO_METER;
     double angle = 0;
-    double daltM = hitchpos.getElevationM() - pos.getElevationM();
+    double daltM = _offsetpos.getElevationM() - pos.getElevationM();
 
     if (fabs(distance) < SGLimits<float>::min()) {
         angle = 0;
@@ -961,11 +1095,12 @@ double FGAIBallistic::getElevLoadToHitch() const {
     return angle;
 }
 
-double FGAIBallistic::getBearingLoadToHitch() const {
+double FGAIBallistic::getBearingToHitch() const {
     //calculate the bearing and range of the second pos from the first
-    double az1, az2, distance;
+    double distance = getDistanceToHitch() * SG_FEET_TO_METER;
+    double az1, az2;
 
-    geo_inverse_wgs_84(pos, hitchpos, &az1, &az2, &distance);
+    geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance);
 
     return az1;
 }
@@ -974,12 +1109,11 @@ double FGAIBallistic::getRelBrgHitchToUser() const {
     //calculate the relative bearing 
     double az1, az2, distance;
 
-    geo_inverse_wgs_84(hitchpos, userpos, &az1, &az2, &distance);
+    geo_inverse_wgs_84(_offsetpos, userpos, &az1, &az2, &distance);
 
     double rel_brg = az1 - hdg;
 
-    if (rel_brg > 180)
-        rel_brg -= 360;
+    SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0);
 
     return rel_brg;
 }
@@ -995,7 +1129,7 @@ double FGAIBallistic::getElevHitchToUser() const {
     double distance = norm(diff);
     double angle = 0;
 
-    double daltM = userpos.getElevationM() - hitchpos.getElevationM();
+    double daltM = userpos.getElevationM() - _offsetpos.getElevationM();
 
     // now the angle, positive angles are upwards
     if (fabs(distance) < SGLimits<float>::min()) {
@@ -1017,57 +1151,104 @@ void FGAIBallistic::setTgtOffsets(double dt, double coeff){
     _z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
 }
 
-void FGAIBallistic::formateToAC(double dt){
 
-    setTgtOffsets(dt, 25);
-    setHitchPos();
-    setHitchVelocity(dt);
+void FGAIBallistic::calcVSHS(){
+    // calculate vertical and horizontal speed components
+    double speed_fps = speed * SG_KT_TO_FPS;
+
+    if (speed == 0.0) {
+        hs = vs = 0.0;
+    } else {
+        vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
+        hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
+    }
+}
+
+void FGAIBallistic::calcNE(){
+    //resolve horizontal speed into north and east components:
+    _speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
+    _speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
+
+    // convert horizontal speed (fps) to degrees per second
+    speed_north_deg_sec = _speed_north_fps / ft_per_deg_lat;
+    speed_east_deg_sec  = _speed_east_fps / ft_per_deg_lon;
 
-    // elapsed time has a random initialisation so that each 
-    // wingman moves differently
-    _elapsed_time += dt;
+}
 
-    // we derive a sine based factor to give us smoothly 
-    // varying error between -1 and 1
-    double factor  = sin(SGMiscd::deg2rad(_elapsed_time * 10));
-    double r_angle = 5 * factor;
-    double p_angle = 2.5 * factor;
-    double h_angle = 5 * factor;
-    double h_feet  = 3 * factor;
+SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading, 
+                                        double user_pitch, double user_roll
+                                        ) const{
 
-    pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
-    pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
+    // convert geodetic positions to geocentered
+     SGVec3d cartuserPos = SGVec3d::fromGeod(inpos);
+    //SGVec3d cartuserPos = getCartUserPos();
+    //SGVec3d cartPos = getCartPos();
 
-    if (getHtAGL()){
+    // Transform to the right coordinate frame, configuration is done in
+    // the x-forward, y-right, z-up coordinates (feet), computation
+    // in the simulation usual body x-forward, y-right, z-down coordinates
+    // (meters) )
+    SGVec3d _off(_x_offset * SG_FEET_TO_METER,
+            _y_offset * SG_FEET_TO_METER,
+            -_z_offset * SG_FEET_TO_METER);
 
-        if(_ht_agl_ft <= 10) {
-            _height = userpos.getElevationFt();
-        } else if (_ht_agl_ft > 10 && _ht_agl_ft <= 150 ) {
-            setHt(userpos.getElevationFt(), dt, 1.0);
-        } else if (_ht_agl_ft > 150 && _ht_agl_ft <= 250) {
-            setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.75);
-        } else
-            setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.5);
+    // Transform the user position to the horizontal local coordinate system.
+    SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
 
-        pos.setElevationFt(_height);
-    }
+    // and postrotate the orientation of the user model wrt the horizontal
+    // local frame
+    hlTrans *= SGQuatd::fromYawPitchRollDeg(
+        user_heading,
+        user_pitch,
+        user_roll);
 
-    // these calculations are unreliable at slow speeds
-    if(speed >= 10) {
-        setHdg(_azimuth + h_angle, dt, 0.9);
-        setPch(_elevation + p_angle + _pitch_offset, dt, 0.9);
+    // The offset converted to the usual body fixed coordinate system
+    // rotated to the earth-fixed coordinates axis
+    SGVec3d off = hlTrans.backTransform(_off);
 
-        if (roll <= 115 && roll >= -115)
-            setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5);
-        else
-            roll = manager->get_user_roll() + r_angle + _roll_offset;
+    // Add the position offset of the user model to get the geocentered position
+    SGVec3d offsetPos = cartuserPos + off;
 
+    return offsetPos;
+}
+
+void FGAIBallistic::setOffsetVelocity(double dt, SGGeod offsetpos) {
+    //calculate the distance from the previous offset position
+    SGVec3d cartoffsetPos = SGVec3d::fromGeod(offsetpos);
+    SGVec3d diff = cartoffsetPos - _oldcartoffsetPos;
+
+    double distance = norm(diff);
+    //calculate speed knots
+    speed = (distance/dt) * SG_MPS_TO_KT;
+
+    //now calulate the angle between the old and current postion positions (degrees)
+    double angle = 0;
+    double daltM = offsetpos.getElevationM() - _oldoffsetpos.getElevationM();
+
+    if (fabs(distance) < SGLimits<float>::min()) {
+        angle = 0;
     } else {
-        setHdg(manager->get_user_heading(), dt, 0.9);
-        setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9);
-        setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9);
+        double sAngle = daltM/distance;
+        sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
+        angle = SGMiscd::rad2deg(asin(sAngle));
     }
 
-    setSpeed(speed);
+    _elevation = angle;
+
+    //calculate vertical and horizontal speed components
+    calcVSHS();
+
+    //calculate the bearing of the new offset position from the old
+    double az1, az2, dist;
+    geo_inverse_wgs_84(_oldoffsetpos, offsetpos, &az1, &az2, &dist);
+    _azimuth = az1;
+
+    //resolve horizontal speed into north and east components:
+    calcNE();
+
+    // and finally store the new values
+    _oldcartoffsetPos = cartoffsetPos;
+    _oldoffsetpos = offsetpos;
 }
+
 // end AIBallistic