Vivian Meazza:

Testing revealed that the code was not reading y-offset - a
typo in the original code, and roll was in the wrong sense. All readily
fixable, and it now works.

diff --git a/src/Systems/submodel.cxx b/src/Systems/submodel.cxx
index 58f38190f8dc14270426a63d79dd0ac83889e132..f936b4d0648bcda7d767a79ac63069d196f0d746 100644 (file)
--- a/src/Systems/submodel.cxx
+++ b/src/Systems/submodel.cxx
@@ -15,10 +15,14 @@
 
 SubmodelSystem::SubmodelSystem ()
 {
+    
   x_offset = y_offset = 0.0;
   z_offset = -4.0;
   pitch_offset = 2.0;
   yaw_offset = 0.0;
+  
+  out[0] = out[1] = out[2] = 0;
+  in[3] = out[3] = 1;
 }
 
 SubmodelSystem::~SubmodelSystem ()
@@ -46,6 +50,8 @@ SubmodelSystem::init ()
     _user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
 
     ai = (FGAIManager*)globals->get_subsystem("ai_model");
+
+
 }
 
 void
@@ -88,8 +94,7 @@ SubmodelSystem::release (submodel* sm, double dt)
   if (sm->timer < sm->delay) return false;
   sm->timer = 0.0;
 
-  // calculate submodel's initial conditions in world-coordinates
-  transform(sm);
+  transform(sm);  // calculate submodel's initial conditions in world-coordinates
 
   //cout << "Creating a submodel." << endl; 
   FGAIModelEntity entity;
@@ -151,7 +156,7 @@ SubmodelSystem::load ()
      sm->count          = entry_node->getIntValue   ("count", 1); 
      sm->slaved         = entry_node->getBoolValue  ("slaved", false); 
      sm->x_offset       = entry_node->getDoubleValue("x-offset", 0.0); 
-     sm->y_offset       = entry_node->getDoubleValue("y_offset", 0.0); 
+     sm->y_offset       = entry_node->getDoubleValue("y-offset", 0.0); 
      sm->z_offset       = entry_node->getDoubleValue("z-offset", 0.0); 
      sm->yaw_offset     = entry_node->getDoubleValue("yaw-offset", 0.0); 
      sm->pitch_offset   = entry_node->getDoubleValue("pitch-offset", 0.0);
@@ -166,6 +171,11 @@ SubmodelSystem::load ()
      sm->prop = fgGetNode("/systems/submodels/submodel", i, true);
      sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
 
+//      in[0] = sm->x_offset;
+//      in[1] = sm->y_offset;
+//      in[2] = sm->z_offset; 
+ 
+
      submodels.push_back( sm );
    }
 
@@ -177,17 +187,87 @@ SubmodelSystem::load ()
 void
 SubmodelSystem::transform( submodel* sm) 
 {
-  IC.lat =             _user_lat_node->getDoubleValue();
-  IC.lon =             _user_lon_node->getDoubleValue();
-  IC.alt =             _user_alt_node->getDoubleValue();
-  IC.azimuth =         _user_heading_node->getDoubleValue() + sm->yaw_offset;
-  IC.elevation =       _user_pitch_node->getDoubleValue() + sm->pitch_offset;
+
+ // get initial conditions 
+    
+  IC.lat =        _user_lat_node->getDoubleValue();    
+  IC.lon =        _user_lon_node->getDoubleValue();       
+  IC.alt =        _user_alt_node->getDoubleValue();    
+  IC.roll =      - _user_roll_node->getDoubleValue();   // rotation about x axis
+  IC.elevation =  _user_pitch_node->getDoubleValue();  // rotation about y axis
+  IC.azimuth =    _user_heading_node->getDoubleValue(); // rotation about z axis
+  
   IC.speed =           _user_speed_node->getDoubleValue() + sm->speed;
   IC.wind_from_east =  _user_wind_from_east_node->getDoubleValue();
   IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
-//  IC.wind_from_east =  4;
-//   IC.wind_from_north = 5;
+  
+  in[0] = sm->x_offset;
+  in[1] = sm->y_offset;
+  in[2] = sm->z_offset; 
+  
+ 
+
+// 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);
+
+
+// set up the transform matrix
+
+    trans[0][0] =      cosRy * cosRz;
+    trans[0][1] =      -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
+    trans[0][2] =      sinRx * sinRz + cosRx * sinRy * cosRz;
+
+    trans[1][0] =      cosRy * sinRz;
+    trans[1][1] =      cosRx * cosRz + sinRx * sinRy * sinRz;
+    trans[1][2] =      -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
+
+    trans[2][0] =      -1 * sinRy;
+    trans[2][1] =      sinRx * cosRy;
+    trans[2][2] =   cosRx * cosRy;
+
+
+// multiply the input and transform matrices
 
+//     for( int i=0; i<4; i++) {
+//             for( int j=0; j<4; j++ ) {
+//                     out[i] += in[j] * trans[i][j];
+//             }
+//     }
+
+out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
+out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
+out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
+
+// convert ft to degrees of latitude
+
+    out[0] = out[0] /(366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
+
+// convert ft to degrees of longitude
+
+    out[1] = out[1] /(365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
+
+
+    IC.lat += out[0];
+    IC.lon += out[1];
+    IC.alt += out[2];
+    IC.elevation += sm->pitch_offset;
+    IC.azimuth   += sm->yaw_offset;
+
+
+}
+
+void 
+SubmodelSystem::updatelat(double lat) 
+{
+    double latitude = lat;
+    ft_per_deg_latitude = 366468.96 - 3717.12 * cos(latitude / SG_RADIANS_TO_DEGREES);
+    ft_per_deg_longitude = 365228.16 * cos(latitude / SG_RADIANS_TO_DEGREES);
 }
 
 // end of submodel.cxx

diff --git a/src/Systems/submodel.hxx b/src/Systems/submodel.hxx
index 56e3ac8549118a96f333531c6e8bb78244f379b0..8acb6b47ce207f6fb89e012e95a75b11ea4cc68b 100644 (file)
--- a/src/Systems/submodel.hxx
+++ b/src/Systems/submodel.hxx
@@ -25,9 +25,10 @@ class SubmodelSystem : public SGSubsystem
 
 public:
 
+
  typedef struct {
-  SGPropertyNode_ptr trigger;
-  SGPropertyNode_ptr prop;
+  SGPropertyNode* trigger;
+  SGPropertyNode* prop;
   string             name;
   string             model;
   double             speed;
@@ -52,6 +53,7 @@ public:
   double     lat;
   double     lon;
   double     alt;
+  double     roll;
   double     azimuth;
   double     elevation;
   double     speed;
@@ -70,6 +72,7 @@ public:
     void update (double dt);
     bool release (submodel* sm, double dt);
     void transform (submodel* sm);
+    void updatelat( double lat );
 
 private:
 
@@ -79,21 +82,35 @@ private:
     submodel_vector_type       submodels;
     submodel_vector_iterator   submodel_iterator;
 
+    float trans[3][3];
+    float in[3];
+    float out[3];
+
+    double Rx, Ry, Rz;
+    double Sx, Sy, Sz;
+    double Tx, Ty, Tz;
+
+    float cosRx, sinRx;
+    float cosRy, sinRy;
+    float cosRz, sinRz;
+
+    double ft_per_deg_longitude;
+    double ft_per_deg_latitude;
 
     double x_offset, y_offset, z_offset;
     double pitch_offset, yaw_offset;
 
-    SGPropertyNode_ptr _serviceable_node;
-    SGPropertyNode_ptr _user_lat_node;
-    SGPropertyNode_ptr _user_lon_node;
-    SGPropertyNode_ptr _user_heading_node;
-    SGPropertyNode_ptr _user_alt_node;
-    SGPropertyNode_ptr _user_pitch_node;
-    SGPropertyNode_ptr _user_roll_node;
-    SGPropertyNode_ptr _user_yaw_node;
-    SGPropertyNode_ptr _user_speed_node;
-       SGPropertyNode_ptr _user_wind_from_east_node;
-    SGPropertyNode_ptr _user_wind_from_north_node;
+    SGPropertyNode* _serviceable_node;
+    SGPropertyNode* _user_lat_node;
+    SGPropertyNode* _user_lon_node;
+    SGPropertyNode* _user_heading_node;
+    SGPropertyNode* _user_alt_node;
+    SGPropertyNode* _user_pitch_node;
+    SGPropertyNode* _user_roll_node;
+    SGPropertyNode* _user_yaw_node;
+    SGPropertyNode* _user_speed_node;
+    SGPropertyNode* _user_wind_from_east_node;
+    SGPropertyNode* _user_wind_from_north_node;
     FGAIManager* ai;
     IC_struct  IC;
 };