src/AIModel/submodel.cxx

   1 // submodel.cxx - models a releasable submodel.
   2 // Written by Dave Culp, started Aug 2004
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #include "submodel.hxx"
   7
   8 #include <simgear/structure/exception.hxx>
   9 #include <simgear/misc/sg_path.hxx>
  10
  11 #include <Main/fg_props.hxx>
  12 #include <Main/util.hxx>
  13 #include <AIModel/AIManager.hxx>
  14
  15
  16 const double FGSubmodelMgr::lbs_to_slugs = 0.031080950172;
  17
  18 FGSubmodelMgr::FGSubmodelMgr ()
  19 {
  20
  21   x_offset = y_offset = 0.0;
  22   z_offset = -4.0;
  23   pitch_offset = 2.0;
  24   yaw_offset = 0.0;
  25
  26   out[0] = out[1] = out[2] = 0;
  27   in[3] = out[3] = 1;
  28   string contents_node;
  29 }
  30
  31 FGSubmodelMgr::~FGSubmodelMgr ()
  32 {
  33 }
  34
  35 void
  36 FGSubmodelMgr::init ()
  37 {
  38     load();
  39     _serviceable_node = fgGetNode("/sim/submodels/serviceable", true);
  40
  41     _user_lat_node = fgGetNode("/position/latitude-deg", true);
  42     _user_lon_node = fgGetNode("/position/longitude-deg", true);
  43     _user_alt_node = fgGetNode("/position/altitude-ft", true);
  44
  45     _user_heading_node = fgGetNode("/orientation/heading-deg", true);
  46     _user_pitch_node =   fgGetNode("/orientation/pitch-deg", true);
  47     _user_roll_node =    fgGetNode("/orientation/roll-deg", true);
  48     _user_yaw_node =     fgGetNode("/orientation/yaw-deg", true);
  49     _user_alpha_node =   fgGetNode("/orientation/alpha-deg", true);
  50
  51     _user_speed_node = fgGetNode("/velocities/uBody-fps", true);
  52
  53     _user_wind_from_east_node  = fgGetNode("/environment/wind-from-east-fps",true);
  54     _user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
  55
  56     _user_speed_down_fps_node   = fgGetNode("/velocities/speed-down-fps",true);
  57     _user_speed_east_fps_node   = fgGetNode("/velocities/speed-east-fps",true);
  58     _user_speed_north_fps_node  = fgGetNode("/velocities/speed-north-fps",true);
  59
  60     ai = (FGAIManager*)globals->get_subsystem("ai_model");
  61
  62
  63 }
  64
  65 void
  66 FGSubmodelMgr::bind ()
  67 {
  68 }
  69
  70 void
  71 FGSubmodelMgr::unbind ()
  72 {
  73   submodel_iterator = submodels.begin();
  74   while(submodel_iterator != submodels.end()) {
  75     (*submodel_iterator)->prop->untie("count");
  76     ++submodel_iterator;
  77   }
  78 }
  79
  80 void
  81 FGSubmodelMgr::update (double dt)
  82 {
  83   if (!(_serviceable_node->getBoolValue())) return;
  84   int i=-1;
  85   submodel_iterator = submodels.begin();
  86   while(submodel_iterator != submodels.end()) {
  87     i++;
  88     if ((*submodel_iterator)->trigger->getBoolValue()) {
  89         if ((*submodel_iterator)->count != 0) {
  90           release( (*submodel_iterator), dt);
  91         }
  92     } else {
  93       (*submodel_iterator)->first_time = true;
  94     }
  95     ++submodel_iterator;
  96   }
  97
  98 }
  99
 100 bool
 101 FGSubmodelMgr::release (submodel* sm, double dt)
 102 {
 103   sm->timer += dt;
 104   if (sm->timer < sm->delay) return false;
 105   sm->timer = 0.0;
 106
 107   if (sm->first_time) {
 108     dt = 0.0;
 109     sm->first_time = false;
 110   }
 111
 112   transform(sm);  // calculate submodel's initial conditions in world-coordinates
 113
 114   FGAIModelEntity entity;
 115
 116   entity.path = sm->model.c_str();
 117   entity.latitude = IC.lat;
 118   entity.longitude = IC.lon;
 119   entity.altitude = IC.alt;
 120   entity.azimuth = IC.azimuth;
 121   entity.elevation = IC.elevation;
 122   entity.roll = IC.roll;
 123   entity.speed = IC.speed;
 124   entity.eda = sm->drag_area;
 125   entity.life = sm->life;
 126   entity.buoyancy = sm->buoyancy;
 127   entity.wind_from_east = IC.wind_from_east;
 128   entity.wind_from_north = IC.wind_from_north;
 129   entity.wind = sm->wind;
 130   entity.cd = sm->cd;
 131   entity.mass = IC.mass;
 132   entity.aero_stabilised = sm->aero_stabilised;
 133   ai->createBallistic( &entity );
 134
 135   if (sm->count > 0) (sm->count)--;
 136
 137   return true;
 138 }
 139
 140 void
 141 FGSubmodelMgr::load ()
 142 {
 143
 144     int i;
 145     SGPropertyNode *path = fgGetNode("/sim/submodels/path");
 146     SGPropertyNode root;
 147
 148     if (path) {
 149       SGPath config( globals->get_fg_root() );
 150       config.append( path->getStringValue() );
 151
 152       try {
 153         readProperties(config.str(), &root);
 154       } catch (const sg_exception &e) {
 155         SG_LOG(SG_GENERAL, SG_ALERT,
 156         "Unable to read submodels file: ");
 157         cout << config.str() << endl;
 158         return;
 159       }
 160     }
 161
 162    int count = root.nChildren();
 163    for (i = 0; i < count; i++) {
 164      // cout << "Reading submodel " << i << endl;
 165      SGPropertyNode *prop;
 166      submodel* sm = new submodel;
 167      SGPropertyNode * entry_node = root.getChild(i);
 168      sm->trigger        = fgGetNode(entry_node->getStringValue("trigger", "none"), true);
 169      sm->name           = entry_node->getStringValue("name", "none_defined");
 170      sm->model          = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
 171      sm->speed          = entry_node->getDoubleValue("speed", 2329.4 );
 172      sm->repeat         = entry_node->getBoolValue  ("repeat", false);
 173      sm->delay          = entry_node->getDoubleValue("delay", 0.25);
 174      sm->count          = entry_node->getIntValue   ("count", 1);
 175      sm->slaved         = entry_node->getBoolValue  ("slaved", false);
 176      sm->x_offset       = entry_node->getDoubleValue("x-offset", 0.0);
 177      sm->y_offset       = entry_node->getDoubleValue("y-offset", 0.0);
 178      sm->z_offset       = entry_node->getDoubleValue("z-offset", 0.0);
 179      sm->yaw_offset     = entry_node->getDoubleValue("yaw-offset", 0.0);
 180      sm->pitch_offset   = entry_node->getDoubleValue("pitch-offset", 0.0);
 181      sm->drag_area      = entry_node->getDoubleValue("eda", 0.034);
 182      sm->life           = entry_node->getDoubleValue("life", 900.0);
 183      sm->buoyancy       = entry_node->getDoubleValue("buoyancy", 0);
 184      sm->wind           = entry_node->getBoolValue  ("wind", false);
 185      sm->first_time     = false;
 186      sm->cd             = entry_node->getDoubleValue("cd", 0.193);
 187      sm->weight         = entry_node->getDoubleValue("weight", 0.25);
 188      sm->aero_stabilised = entry_node->getBoolValue  ("aero-stabilised", true);
 189      sm->contents_node  = fgGetNode(entry_node->getStringValue("contents", "none"), true);
 190
 191      sm->trigger->setBoolValue(false);
 192      sm->timer = sm->delay;
 193
 194      sm->contents = sm->contents_node->getDoubleValue();
 195
 196      sm->prop = fgGetNode("/ai/submodels/submodel", i, true);
 197      sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
 198
 199 //   sm->prop->tie("contents", SGRawValuePointer<double>(&(sm->contents)));
 200 //   sm->prop->tie("contents path", SGRawValuePointer<const char *>(&(sm->contents_node)));
 201      submodels.push_back( sm );
 202    }
 203
 204   submodel_iterator = submodels.begin();
 205
 206 }
 207
 208
 209 void
 210 FGSubmodelMgr::transform( submodel* sm)
 211 {
 212
 213 // get initial conditions
 214
 215 // get the weight of the contents (lbs) and convert to mass (slugs)
 216   sm->contents = sm->contents_node->getDoubleValue();
 217
 218   IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;;
 219 //  cout << IC.mass << endl;
 220
 221 // set contents to 0 in the parent
 222    sm->contents_node->setDoubleValue(0);
 223
 224   IC.lat =        _user_lat_node->getDoubleValue();
 225   IC.lon =        _user_lon_node->getDoubleValue();
 226   IC.alt =        _user_alt_node->getDoubleValue();
 227   IC.roll =       _user_roll_node->getDoubleValue();    // rotation about x axis
 228   IC.elevation =  _user_pitch_node->getDoubleValue();   // rotation about y axis
 229   IC.azimuth =    _user_heading_node->getDoubleValue(); // rotation about z axis
 230
 231   IC.speed =           _user_speed_node->getDoubleValue();
 232   IC.wind_from_east =  _user_wind_from_east_node->getDoubleValue();
 233   IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
 234
 235   IC.speed_down_fps =   _user_speed_down_fps_node->getDoubleValue();
 236   IC.speed_east_fps =   _user_speed_east_fps_node->getDoubleValue();
 237   IC.speed_north_fps =  _user_speed_north_fps_node->getDoubleValue();
 238
 239
 240   in[0] = sm->x_offset;
 241   in[1] = sm->y_offset;
 242   in[2] = sm->z_offset;
 243
 244
 245 // pre-process the trig functions
 246
 247     cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
 248     sinRx = sin(-IC.roll * SG_DEGREES_TO_RADIANS);
 249     cosRy = cos(-IC.elevation * SG_DEGREES_TO_RADIANS);
 250     sinRy = sin(-IC.elevation * SG_DEGREES_TO_RADIANS);
 251     cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS);
 252     sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS);
 253
 254 // set up the transform matrix
 255
 256     trans[0][0] =  cosRy * cosRz;
 257     trans[0][1] =  -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
 258     trans[0][2] =  sinRx * sinRz + cosRx * sinRy * cosRz;
 259
 260     trans[1][0] =  cosRy * sinRz;
 261     trans[1][1] =  cosRx * cosRz + sinRx * sinRy * sinRz;
 262     trans[1][2] =  -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
 263
 264     trans[2][0] =  -1 * sinRy;
 265     trans[2][1] =  sinRx * cosRy;
 266     trans[2][2] =  cosRx * cosRy;
 267
 268
 269 // multiply the input and transform matrices
 270
 271    out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
 272    out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
 273    out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
 274
 275    // convert ft to degrees of latitude
 276    out[0] = out[0] /(366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
 277
 278    // convert ft to degrees of longitude
 279    out[1] = out[1] /(365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
 280
 281    // set submodel initial position
 282    IC.lat += out[0];
 283    IC.lon += out[1];
 284    IC.alt += out[2];
 285
 286    // get aircraft velocity vector angles in XZ and XY planes
 287     //double alpha = _user_alpha_node->getDoubleValue();
 288     //double velXZ = IC.elevation - alpha * cosRx;
 289     //double velXY = IC.azimuth - (IC.elevation - alpha * sinRx);
 290
 291    // Get submodel initial velocity vector angles in XZ and XY planes.
 292    // This needs to be fixed. This vector should be added to aircraft's vector.
 293    IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
 294    IC.azimuth   += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
 295
 296    // For now assume vector is close to airplane's vector.  This needs to be fixed.
 297    //IC.speed += ;
 298
 299    // calcuate the total speed north
 300
 301    IC.total_speed_north = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
 302                     cos(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
 303
 304    // calculate the total speed east
 305
 306    IC.total_speed_east = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
 307                      sin(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
 308
 309    // calculate the total speed down
 310
 311    IC.total_speed_down = sm->speed * -sin(IC.elevation*SG_DEGREES_TO_RADIANS) +
 312                         IC.speed_down_fps;
 313
 314   // re-calculate speed, elevation and azimuth
 315
 316    IC.speed = sqrt( IC.total_speed_north * IC.total_speed_north +
 317                     IC.total_speed_east * IC.total_speed_east +
 318                     IC.total_speed_down * IC.total_speed_down);
 319
 320    IC.azimuth = atan(IC.total_speed_east/IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
 321
 322    // rationalise the output
 323
 324    if (IC.total_speed_north <= 0){
 325     IC.azimuth = 180 + IC.azimuth;
 326    }
 327    else{
 328      if(IC.total_speed_east <= 0){
 329      IC.azimuth = 360 + IC.azimuth;
 330    }
 331   }
 332
 333     IC.elevation = -atan(IC.total_speed_down/sqrt(IC.total_speed_north *
 334                        IC.total_speed_north +
 335                        IC.total_speed_east * IC.total_speed_east)) * SG_RADIANS_TO_DEGREES;
 336
 337 }
 338
 339 void
 340 FGSubmodelMgr::updatelat(double lat)
 341 {
 342     double latitude = lat;
 343     ft_per_deg_latitude = 366468.96 - 3717.12 * cos(latitude / SG_RADIANS_TO_DEGREES);
 344     ft_per_deg_longitude = 365228.16 * cos(latitude / SG_RADIANS_TO_DEGREES);
 345 }
 346
 347 // end of submodel.cxx
 348
 349
 350
 351