X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIBallistic.cxx;h=16b15fe1d93f3aec36cb81a45ad6631c09a3398c;hb=38226af24ec01e8f0a20d7fd73ef838a69f6ef25;hp=2f5bb4f6f7db07dd5d1cb364069ebcfc0663c392;hpb=fdbfd6fd0b789eecf2c532d048a75d15f8d35ad8;p=flightgear.git

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index 2f5bb4f6f..16b15fe1d 100644
--- a/src/AIModel/AIBallistic.cxx
+++ b/src/AIModel/AIBallistic.cxx
@@ -41,11 +41,13 @@ const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
 FGAIBallistic::FGAIBallistic(object_type ot) :
 FGAIBase(ot),
 _height(0.0),
+_speed(0),
 _ht_agl_ft(0.0),
 _azimuth(0.0),
 _elevation(0.0),
 _rotation(0.0),
-_formate_to_ac(false),
+hs(0),
+_elapsed_time(0),
 _aero_stabilised(false),
 _drag_area(0.007),
 _life_timer(0.0),
@@ -61,13 +63,11 @@ _slave_to_ac(false),
 _slave_load_to_ac(false),
 _contents_lb(0),
 _report_collision(false),
-_report_expiry(false),
 _report_impact(false),
 _external_force(false),
+_report_expiry(false),
 _impact_report_node(fgGetNode("/ai/models/model-impact", true)),
-_old_height(0),
-_elapsed_time(0),
-hs(0)
+_old_height(0)
 
 {
     no_roll = false;
@@ -121,6 +121,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
     setSlaved(scFileNode->getBoolValue("slaved", false));
     setSlavedLoad(scFileNode->getBoolValue("slaved-load", false));
     setContentsPath(scFileNode->getStringValue("contents"));
+    setParentName(scFileNode->getStringValue("parent"));
 }
 
 bool FGAIBallistic::init(bool search_in_AI_path) {
@@ -151,6 +152,12 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
         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
@@ -161,7 +168,11 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
 
     Transform();
 
-    //cout << _name << " speed init: " << speed << endl;
+    if(_parent != ""){
+        setParentNode();
+    }
+
+    setParentNodes(_selected_ac);
 
     return true;
 }
@@ -221,13 +232,13 @@ 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));
     }
@@ -271,10 +282,7 @@ void FGAIBallistic::update(double dt) {
     FGAIBase::update(dt);
     _setUserPos();
 
-    if (_formate_to_ac){
-        formateToAC(dt);
-        Transform();
-    } else if (_slave_to_ac){
+    if (_slave_to_ac){
         slaveToAC(dt);
         Transform();
     } else if (!invisible){
@@ -385,10 +393,6 @@ 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) {
     _path = s;
     //cout << "submodel path " << _path << endl;
@@ -422,10 +426,6 @@ 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;
@@ -443,13 +443,16 @@ void FGAIBallistic::setContentsNode(SGPropertyNode_ptr node) {
     }
 }
 
-void FGAIBallistic::setParentNode(SGPropertyNode_ptr node) {
+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);
@@ -458,12 +461,15 @@ void FGAIBallistic::setParentNode(SGPropertyNode_ptr node) {
 
         _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) {
+
+    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();
@@ -473,16 +479,13 @@ void FGAIBallistic::setParentPos() {
         _parentpos.setElevationFt(alt);
 
     }
+
 }
 
 bool FGAIBallistic::getSlaved() const {
     return _slave_to_ac;
 }
 
-bool FGAIBallistic::getFormate() const {
-    return _formate_to_ac;
-}
-
 double FGAIBallistic::getMass() const {
     return _mass;
 }
@@ -570,22 +573,31 @@ 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 {
@@ -614,19 +626,21 @@ void FGAIBallistic::setTgtZOffset(double z){
 
 void FGAIBallistic::slaveToAC(double dt){
 
-    double hdg, pch, rll = 0;
+    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());
     }
@@ -1054,7 +1068,7 @@ void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, dou
 
 }
 
-double FGAIBallistic::getDistanceLoadToHitch() const {
+double FGAIBallistic::getDistanceToHitch() const {
     //calculate the distance load to hitch 
     SGVec3d carthitchPos = getCartHitchPos();
     SGVec3d cartPos = getCartPos();
@@ -1064,10 +1078,9 @@ double FGAIBallistic::getDistanceLoadToHitch() const {
     return distance * SG_METER_TO_FEET;
 }
 
-
-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 = _offsetpos.getElevationM() - pos.getElevationM();
 
@@ -1082,9 +1095,10 @@ 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, _offsetpos, &az1, &az2, &distance);
 
@@ -1099,8 +1113,7 @@ double FGAIBallistic::getRelBrgHitchToUser() const {
 
     double rel_brg = az1 - hdg;
 
-    if (rel_brg > 180)
-        rel_brg -= 360;
+    SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0);
 
     return rel_brg;
 }
@@ -1138,62 +1151,7 @@ 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);
-    setOffsetPos(userpos,
-            manager->get_user_heading(),
-            manager->get_user_pitch(), 
-            manager->get_user_roll()
-            );
-
-    // 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;
-
-    pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
-    pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
 
-    if (getHtAGL(10000)){
-
-        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(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
-        } else
-            setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
-
-        pos.setElevationFt(_height);
-    }
-
-    // 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);
-
-        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;
-
-    } 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);
-    }
-
-        setOffsetVelocity(dt, pos);
-}
 void FGAIBallistic::calcVSHS(){
     // calculate vertical and horizontal speed components
     double speed_fps = speed * SG_KT_TO_FPS;
@@ -1235,7 +1193,7 @@ SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading,
             -_z_offset * SG_FEET_TO_METER);
 
     // Transform the user position to the horizontal local coordinate system.
-    SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
+    SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
 
     // and postrotate the orientation of the user model wrt the horizontal
     // local frame