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