Experimental patch for applying force to ballistic objects

From Vivian Meazza

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index c42d0d775c3d936890efb7e3d71b7d2591c91b75..556f20fc6d333ddd9c28ee57dee69ade1e7a39fb 100644 (file)
--- a/src/AIModel/AIBallistic.cxx
+++ b/src/AIModel/AIBallistic.cxx
@@ -48,7 +48,9 @@ FGAIBallistic::FGAIBallistic() :
     _solid(false),
     _report_collision(false),
     _report_impact(false),
-    _impact_report_node(fgGetNode("/ai/models/model-impact", true))
+    _impact_report_node(fgGetNode("/ai/models/model-impact", true)),
+    _external_force(false)
+
 {
     no_roll = false;
 }
@@ -192,7 +194,12 @@ void FGAIBallistic::setCollision(bool c) {
     _report_collision = c;
 }
 
+void FGAIBallistic::setExternalForce(bool f) {
+    _external_force = f;
+}
+
 void FGAIBallistic::setImpactReportNode(const string& path) {
+
     if (!path.empty())
         _impact_report_node = fgGetNode(path.c_str(), true);
 }
@@ -218,6 +225,16 @@ void FGAIBallistic::setSubmodel(const string& s) {
     _submodel = s;
 }
 
+void FGAIBallistic::setForcePath(const string& p) {
+    _force_path = p;
+    if (!_force_path.empty()) {
+        SGPropertyNode *fnode = fgGetNode(_force_path.c_str(), 0, true );
+        _force_node = fnode->getChild("force-lb", 0, true);
+        _force_azimuth_node = fnode->getChild("force-azimuth-deg", 0, true);
+        _force_elevation_node = fnode->getChild("force-elevation-deg", 0, true);
+    }
+}
+
 void FGAIBallistic::Run(double dt) {
     _life_timer += dt;
      //cout << "life timer" <<_name <<" " << _life_timer <<  dt << endl;
@@ -263,9 +280,13 @@ void FGAIBallistic::Run(double dt) {
         hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
     }
 
+    //resolve horizontal speed into north and east components:
+    double speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
+    double speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
+
     // convert horizontal speed (fps) to degrees per second
-    double speed_north_deg_sec = cos(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lat;
-    double speed_east_deg_sec  = sin(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lon;
+    double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
+    double speed_east_deg_sec  = speed_east_fps / ft_per_deg_lon;
 
     // if wind not required, set to zero
     if (!_wind) {
@@ -273,38 +294,108 @@ void FGAIBallistic::Run(double dt) {
         _wind_from_east = 0;
     }
 
-    // convert wind speed (fps) to degrees per second
+    // convert wind speed (fps) to degrees lat/lon per second
     double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
     double wind_speed_from_east_deg_sec  = _wind_from_east / ft_per_deg_lon;
 
+    //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 hs_force_fps = 0;
+    double v_force_acc_fpss = 0;
+    double force_speed_north_fps = 0;
+    double force_speed_east_fps = 0;
+
+    if (_external_force) {
+        SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
+        double force_lbs            = n->getChild("force-lb", 0, true)->getDoubleValue();
+        double 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 );
+        double h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
+
+        //acceleration = (force(lbsf)/mass(slugs))
+        v_force_acc_fpss = (v_force_lbs/_mass);
+        double h_force_acc_fpss = (h_force_lbs/_mass);
+
+        // velocity = acceleration * dt
+        hs_force_fps = h_force_acc_fpss * dt;
+
+        //resolve horizontal speed into north and east components:
+        force_speed_north_fps   = cos(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
+        force_speed_east_fps    = sin(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
+
+        // convert horizontal speed (fps) to degrees per second
+        double force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat;
+        double force_speed_east_deg_sec  = force_speed_east_fps / ft_per_deg_lon;
+
+        //recombine the horizontal velocity components
+        hs = sqrt(((speed_north_fps + force_speed_north_fps) * (speed_north_fps + force_speed_north_fps))
+            + ((speed_east_fps + force_speed_east_fps) * (speed_east_fps + force_speed_east_fps)));
+
+        /*cout << "mass " << _mass 
+        << " force " << force_lbs 
+        << " elevation " << force_elevation_deg
+        << " azimuth " << force_azimuth_deg
+        << endl; */
+
+        //cout << " _hs_force_fps " << hs_force_fps 
+        //<< " force_speed_north_fps " << force_speed_north_fps 
+        //<< " force_speed_east_fps " << force_speed_east_fps 
+        //<< " speed_north_fps " << speed_north_fps 
+        //<< " speed_east_fps " << speed_east_fps
+        //<< endl;
+    }
+
     // set new position
     pos.setLatitudeDeg( pos.getLatitudeDeg()
-        + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt );
+        + (speed_north_deg_sec - wind_speed_from_north_deg_sec + force_speed_north_deg_sec) * dt );
     pos.setLongitudeDeg( pos.getLongitudeDeg()
-        + (speed_east_deg_sec - wind_speed_from_east_deg_sec) * dt );
+        + (speed_east_deg_sec - wind_speed_from_east_deg_sec + force_speed_east_deg_sec) * dt );
 
-    // adjust vertical speed for acceleration of gravity and buoyancy
-    vs -= (_gravity - _buoyancy) * dt;
+    // adjust vertical speed for acceleration of gravity, buoyancy, and vertical force
+    //v_force_acc_fpss = 0;
+    vs -= (_gravity - _buoyancy - v_force_acc_fpss) * dt;
 
-    // adjust altitude (feet)
+    // adjust altitude (feet) and set new elevation
     altitude_ft += vs * dt;
     pos.setElevationFt(altitude_ft);
 
-    // recalculate elevation (velocity vector) if aerostabilized
-    /*cout << _name << ": " << "aero_stabilised " << _aero_stabilised
-    << " pitch " << pitch <<" vs "  << vs <<endl ;*/
+    // recalculate elevation and azimuth (velocity vectors)
+    _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
+    _azimuth =  atan2((speed_east_fps + force_speed_east_fps), 
+        (speed_north_fps + force_speed_north_fps)) * SG_RADIANS_TO_DEGREES;
+
+    // rationalise azimuth
+    if (_azimuth < 0) _azimuth += 360;
 
     if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
         const double coeff = 0.9;
         double c = dt / (coeff + dt);
-        //cout << "c " << c << endl;
-        _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
+
         pitch = (_elevation * c) + (pitch * (1 - c));
+
+        if (_azimuth - hdg > 180) {
+            hdg = (_azimuth * c) - (hdg * (1 - c));
+        } else {
+            hdg = (_azimuth * c) + (hdg * (1 - c));
+        }
+
     }
 
     // recalculate total speed
     speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
 
+    /*cout << "_elevation " << _elevation
+    << " pitch " << pitch
+    <<" _yaw " << _yaw 
+    << " hdg " << hdg 
+    << " speed " << speed
+    << endl;*/
+
     //do impacts and collisions
     if (_report_impact && !_impact_reported)
         handle_impact();

diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx
index ca92b8b8921b654b769836c7d5b63e0ce2513ad0..abfa68582a206e147ec47df17f79d7754cc91671 100644 (file)
--- a/src/AIModel/AIBallistic.hxx
+++ b/src/AIModel/AIBallistic.hxx
@@ -68,13 +68,17 @@ public:
     void setSMPath(const string&);
     void setSubID(int i);
     void setSubmodel(const string&);
-
-
+    void setExternalForce( bool f );
+    void setForcePath(const string&);
     double _getTime() const;
 
     virtual const char* getTypeString(void) const { return "ballistic"; }
     static const double slugs_to_kgs; //conversion factor
 
+    SGPropertyNode_ptr _force_node;
+    SGPropertyNode_ptr _force_azimuth_node;
+    SGPropertyNode_ptr _force_elevation_node;
+
 private:
 
     double _azimuth;         // degrees true
@@ -94,9 +98,11 @@ private:
     double _ht_agl_ft;       // height above ground level
     double _load_resistance; // ground load resistanc N/m^2
     bool   _solid;           // if true ground is solid for FDMs
+    //double _hs_force_fps;    // horizontal speed due to external force
 
     bool   _report_collision;       // if true a collision point with AI Objects is calculated
     bool   _report_impact;          // if true an impact point on the terrain is calculated
+    bool   _external_force;         // if true then apply external force
 
     SGPropertyNode_ptr _impact_report_node;  // report node for impact and collision
 
@@ -108,6 +114,7 @@ private:
     string _name;
     string _path;
     string _submodel;
+    string _force_path;
 
     void Run(double dt);
     void handle_collision();

diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx
index 2c3947c143deb7e8f5f2fb880ac952a415f84aae..774820dc8343ffda2fb313b72d86c3ecfd78c1c4 100644 (file)
--- a/src/AIModel/submodel.cxx
+++ b/src/AIModel/submodel.cxx
@@ -184,11 +184,11 @@ void FGSubmodelMgr::update(double dt)
             sm_list_iterator end = sm_list.end();
 
             while (sm_list_itr != end) {
+                in_range = true;
 
                 if (id == 0) {
                     SG_LOG(SG_GENERAL, SG_DEBUG,
                             "Submodels: continuing: " << id << " name " << name );
-                    in_range = true;
                     ++sm_list_itr;
                     continue;
                 }
@@ -198,15 +198,16 @@ void FGSubmodelMgr::update(double dt)
                 if (parent_id == id) {
                     double parent_lat = (*sm_list_itr)->_getLatitude();
                     double parent_lon = (*sm_list_itr)->_getLongitude();
+                    string parent_name = (*sm_list_itr)->_getName();
                     double own_lat    = _user_lat_node->getDoubleValue();
                     double own_lon    = _user_lon_node->getDoubleValue();
                     double range_nm   = getRange(parent_lat, parent_lon, own_lat, own_lon);
-                    // cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl;
-                    //cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl;
+                    //cout << "parent name " << parent_name << ", "<< parent_id << ", "<< parent_lat << ", " << parent_lon << endl;
+                    //cout << "own name " << own_lat << ", " << own_lon << " range " << range_nm << endl;
 
                     if (range_nm > 15) {
                         SG_LOG(SG_GENERAL, SG_DEBUG,
-                                "Submodels: skipping release: " << id);
+                            "Submodels: skipping release, out of range: " << id);
                         in_range = false;
                     }
                 }
@@ -284,6 +285,8 @@ bool FGSubmodelMgr::release(submodel *sm, double dt)
     ballist->setFuseRange(sm->fuse_range);
     ballist->setSubmodel(sm->submodel.c_str());
     ballist->setSubID(sm->sub_id);
+    ballist->setExternalForce(sm->ext_force);
+    ballist->setForcePath(sm->force_path.c_str());
     ai->attach(ballist);
 
     if (sm->count > 0)
@@ -309,14 +312,18 @@ void FGSubmodelMgr::transform(submodel *sm)
     // set initial conditions
     if (sm->contents_node != 0) {
         // get the weight of the contents (lbs) and convert to mass (slugs)
-        sm->contents = sm->contents_node->getDoubleValue();
+        sm->contents = sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue();
+        //cout << "transform: contents " << sm->contents << endl;
         IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;
+        //cout << "mass inc contents"  << IC.mass << endl;
 
         // set contents to 0 in the parent
-        sm->contents_node->setDoubleValue(0);
-    } else {
+        sm->contents_node->getChild("level-gal_us",0,1)->setDoubleValue(0);
+        /*cout << "contents " << sm->contents_node->getChild("level-gal_us")->getDoubleValue()
+        << " " << sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue()
+        << endl;*/
+    } else
         IC.mass = sm->weight * lbs_to_slugs;
-    }
 
     // cout << "mass "  << IC.mass << endl;
 
@@ -592,7 +599,8 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
         sm->contents_node   = fgGetNode(entry_node->getStringValue("contents", "none"), false);
         sm->speed_node      = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
         sm->submodel        = entry_node->getStringValue("submodel-path", "");
-
+        sm->ext_force       = entry_node->getBoolValue("external-force", false);
+        sm->force_path      = entry_node->getStringValue("force-path", "");
         //cout <<  "sm->contents_node " << sm->contents_node << endl;
         if (sm->contents_node != 0)
             sm->contents = sm->contents_node->getDoubleValue();
@@ -606,6 +614,7 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
         }
 
         SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: trigger " << sm->trigger_node->getBoolValue() );
+
         if (sm->speed_node != 0)
             sm->speed = sm->speed_node->getDoubleValue();
 
@@ -629,6 +638,10 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
         sm->prop->setStringValue("submodel", submodel.c_str());
         //cout << " set submodel path " << submodel << endl;
 
+        string force_path = sm->force_path;
+        sm->prop->setStringValue("force_path", force_path.c_str());
+        //cout << "set force_path " << force_path << endl;
+
         if (sm->contents_node != 0)
             sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
 
@@ -691,6 +704,8 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable)
         sm->contents_node   = fgGetNode(entry_node->getStringValue("contents", "none"), false);
         sm->speed_node      = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
         sm->submodel        = entry_node->getStringValue("submodel-path", "");
+        sm->ext_force       = entry_node->getBoolValue("external-force", false);
+        sm->force_path      = entry_node->getStringValue("force-path", "");
 
         //cout <<  "sm->contents_node " << sm->contents_node << endl;
         if (sm->contents_node != 0)
@@ -726,6 +741,10 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable)
         sm->prop->setStringValue("submodel", submodel.c_str());
         // cout << " set submodel path " << submodel<< endl;
 
+        string force_path = sm->force_path;
+        sm->prop->setStringValue("force_path", force_path.c_str());
+        //cout << "set force_path " << force_path << endl;
+
         if (sm->contents_node != 0)
             sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
 
@@ -768,7 +787,7 @@ void FGSubmodelMgr::loadSubmodels()
 
     while (submodel_iterator != submodels.end()) {
         int id = (*submodel_iterator)->id;
-        SG_LOG(SG_GENERAL, SG_DEBUG,"after pusback "
+        SG_LOG(SG_GENERAL, SG_DEBUG,"after pushback "
                 << " id " << id
                 << " name " << (*submodel_iterator)->name
                 << " sub id " << (*submodel_iterator)->sub_id);

diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx
index 87beb5945493f0d5165db33f752e4e9f25315427..177741e4b9617721124d09ed79553d9e9746fa5b 100644 (file)
--- a/src/AIModel/submodel.hxx
+++ b/src/AIModel/submodel.hxx
@@ -68,6 +68,8 @@ public:
         double             fuse_range;
         string             submodel;
         int                sub_id;
+        bool               ext_force;
+        string             force_path;
     }   submodel;
 
     typedef struct {