submodel: Fix incorrect yaw and pitch offsets

Signed-off-by: onox <denkpadje@gmail.com>

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index 78ed4cdf0a479809ff6eddb3ac44ebf13ac78241..a8e5a16b795f47afe433e801466c9f89763bbc61 100644 (file)
--- a/src/AIModel/AIBallistic.cxx
+++ b/src/AIModel/AIBallistic.cxx
@@ -48,9 +48,9 @@ _ht_agl_ft(0.0),
 _azimuth(0.0),
 _elevation(0.0),
 _rotation(0.0),
+hs(0),
 _az_random_error(0.0),
 _el_random_error(0.0),
-hs(0),
 _elapsed_time(0),
 _aero_stabilised(false),
 _drag_area(0.007),

diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx
index 5b18a88eaaefb3fa21960e23ab3339b059b97049..eb7938b201355576375570ca802e2ce4f3ae2588 100644 (file)
--- a/src/AIModel/AIBallistic.hxx
+++ b/src/AIModel/AIBallistic.hxx
@@ -176,11 +176,11 @@ private:
     bool   _aero_stabilised; // if true, object will align with trajectory
     double _drag_area;       // equivalent drag area in ft2
     double _buoyancy;        // fps^2
-    bool   _wind;            // if true, local wind will be applied to object
     double _cd;              // current drag coefficient
     double _init_cd;         // initial drag coefficient
     double _cd_randomness;   // randomness of Cd. 1.0 means +- 100%, 0.0 means no randomness
     double _life_timer;      // seconds
+    bool   _wind;            // if true, local wind will be applied to object
     double _mass;            // slugs
     bool   _random;          // modifier for Cd, life, az
     double _life_randomness; // dimension for _random, only applies to life at present

diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx
index cb07fd80417c7684db53387e8cc8429194b99699..6638e398686a5980849b8f2693e326975ba2c28a 100644 (file)
--- a/src/AIModel/submodel.cxx
+++ b/src/AIModel/submodel.cxx
@@ -345,11 +345,6 @@ void FGSubmodelMgr::transform(submodel *sm)
     if (sm->speed_node != 0)
         sm->speed = sm->speed_node->getDoubleValue();
 
-    double yaw_offset = sm->yaw_offset->get_value();
-    double pitch_offset = sm->pitch_offset->get_value();
-
-    //cout << " name " << name << " id " << id << " sub id" << sub_id << endl;
-
     // set the Initial Conditions for the types of submodel parent 
 
     if (_impact || _hit || _expiry) {
@@ -385,21 +380,10 @@ void FGSubmodelMgr::transform(submodel *sm)
         setParentNode(id);
     }
 
-    //cout << "Submodel: setting IC "<< name << endl;
-    //cout << "heading " << IC.azimuth << endl ;
-    //cout << "speed down " << IC.speed_down_fps << endl ;
-    //cout << "speed east " << IC.speed_east_fps << endl ;
-    //cout << "speed north " << IC.speed_north_fps << endl ;
-    //cout << "parent speed fps in " << IC.speed << "sm speed in " << sm->speed << endl ;
-    //cout << "lat " << IC.lat;
-    //cout << "alt " << IC.alt <<  endl ;
-
     // Set the Initial Conditions that are common to all types of parent
     IC.wind_from_east =  _user_wind_from_east_node->getDoubleValue();
     IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
 
-    //cout << "wind e " << IC.wind_from_east << " n " << IC.wind_from_north << endl;
-
     userpos.setLatitudeDeg(IC.lat);
     userpos.setLongitudeDeg(IC.lon);
     userpos.setElevationFt(IC.alt);
@@ -410,18 +394,15 @@ void FGSubmodelMgr::transform(submodel *sm)
 
     setOffsetPos();
 
-    // Pre-process the trig functions
-    cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
-    sinRx = sin(-IC.roll * SG_DEGREES_TO_RADIANS);
-    cosRy = cos(-IC.elevation * SG_DEGREES_TO_RADIANS);
-    sinRy = sin(-IC.elevation * SG_DEGREES_TO_RADIANS);
-    cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS);
-    sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS);
+    double yaw_offset = sm->yaw_offset->get_value();
+    double pitch_offset = sm->pitch_offset->get_value();
+
+    // Compute azimuth and elevation given the yaw and pitch offsets
+    SGQuatd ic_quat = SGQuatd::fromYawPitchRollDeg(IC.azimuth, IC.elevation, IC.roll);
+    ic_quat *= SGQuatd::fromYawPitchRollDeg(yaw_offset, pitch_offset, 0.0);
 
-    // Get submodel initial velocity vector angles in XZ and XY planes.
-    // This vector should be added to aircraft's vector.
-    IC.elevation += (yaw_offset * sinRx) + (pitch_offset * cosRx);
-    IC.azimuth   += (yaw_offset * cosRx) - (pitch_offset * sinRx);
+    double ic_roll;
+    ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll);
 
     // Calculate the total speed north
     IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
@@ -440,23 +421,25 @@ void FGSubmodelMgr::transform(submodel *sm)
             + IC.total_speed_east * IC.total_speed_east
             + IC.total_speed_down * IC.total_speed_down);
 
+    cout << "sm speed: " << sm->speed << " IC speed: " << IC.speed << endl;
+    cout << "az1: " << IC.azimuth << " el1: " << IC.elevation << endl;
+
     // If speeds are low this calculation can become unreliable
     if (IC.speed > 1) {
         IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
-        //        cout << "azimuth1 " << IC.azimuth<<endl;
 
         // Rationalize the output
         if (IC.azimuth < 0)
             IC.azimuth += 360;
         else if (IC.azimuth >= 360)
             IC.azimuth -= 360;
-        // cout << "azimuth2 " << IC.azimuth<<endl;
 
         IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
             * IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
             * SG_RADIANS_TO_DEGREES;
     }
-    //cout << "IC.speed " << IC.speed / SG_KT_TO_FPS << endl;
+
+    cout << "az2: " << IC.azimuth << " el2: " << IC.elevation << endl;
 }
 
 void FGSubmodelMgr::updatelat(double lat)

diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx
index 4c080526ef30c36e76459065e959f8f061079367..04b45d7326a1c95b43b199bc5bba99d8d49447f4 100644 (file)
--- a/src/AIModel/submodel.hxx
+++ b/src/AIModel/submodel.hxx
@@ -120,14 +120,6 @@ private:
     submodel_vector_type       subsubmodels;
     submodel_vector_iterator   submodel_iterator, subsubmodel_iterator;
 
-    //double Rx, Ry, Rz;
-    //double Sx, Sy, Sz;
-    //double Tx, Ty, Tz;
-
-    float cosRx, sinRx;
-    float cosRy, sinRy;
-    float cosRz, sinRz;
-
     int index;
 
     double ft_per_deg_longitude;
@@ -143,14 +135,13 @@ private:
     double _parent_pitch;
     double _parent_roll;
     double _parent_speed;
-    //double _parent_ID;
 
     double _x_offset;
     double _y_offset;
     double _z_offset;
 
-
-    static const double lbs_to_slugs; //conversion factor
+    // Conversion factor
+    static const double lbs_to_slugs;
 
     double contrail_altitude;
 
@@ -184,22 +175,18 @@ private:
     SGPropertyNode_ptr _selected_ac;
 
     IC_struct  IC;
-    
-    /**
-     * Helper to retrieve the AI manager, if it currently exists
-     */
+
+    // Helper to retrieve the AI manager, if it currently exists
     FGAIManager* aiManager();
-    
+
     void loadAI();
     void loadSubmodels();
     void setData(int id, const std::string& path, bool serviceable, const std::string& property_path, submodel_vector_type& models);
     void valueChanged (SGPropertyNode *);
     void transform(submodel *);
     void setParentNode(int parent_id);
-
     bool release(submodel *, double dt);
 
-
     int _count;
 
     SGGeod userpos;