src/AIModel/submodel.cxx

   1 //// submodel.cxx - models a releasable submodel.
   2 // Written by Dave Culp, started Aug 2004
   3 // With major additions by Vivian Meaaza 2004 - 2007
   4 //
   5 // This file is in the Public Domain and comes with no warranty.
   6
   7 #ifdef HAVE_CONFIG_H
   8 #  include "config.h"
   9 #endif
  10
  11 #include "submodel.hxx"
  12
  13 #include <simgear/structure/exception.hxx>
  14 #include <simgear/misc/sg_path.hxx>
  15 #include <simgear/math/sg_geodesy.hxx>
  16 #include <simgear/props/props_io.hxx>
  17
  18 #include <Main/fg_props.hxx>
  19 #include <Main/util.hxx>
  20
  21
  22 #include "AIBase.hxx"
  23 #include "AIManager.hxx"
  24 #include "AIBallistic.hxx"
  25
  26 using std::cout;
  27 using std::endl;
  28
  29 const double FGSubmodelMgr::lbs_to_slugs = 0.031080950172;
  30
  31 FGSubmodelMgr::FGSubmodelMgr()
  32 {
  33     x_offset = y_offset = z_offset = 0.0;
  34     pitch_offset = 0.0;
  35     yaw_offset = 0.0;
  36
  37     //out[0] = out[1] = out[2] = 0;
  38     //string contents_node;
  39     contrail_altitude = 30000;
  40     _count = 0;
  41     _found_sub = true;
  42 }
  43
  44 FGSubmodelMgr::~FGSubmodelMgr()
  45 {
  46 }
  47
  48 FGAIManager* FGSubmodelMgr::aiManager()
  49 {
  50    return (FGAIManager*)globals->get_subsystem("ai-model");
  51 }
  52
  53 void FGSubmodelMgr::init()
  54 {
  55     index = 0;
  56
  57     _serviceable_node = fgGetNode("/sim/submodels/serviceable", true);
  58     _serviceable_node->setBoolValue(true);
  59
  60     _user_lat_node = fgGetNode("/position/latitude-deg", true);
  61     _user_lon_node = fgGetNode("/position/longitude-deg", true);
  62     _user_alt_node = fgGetNode("/position/altitude-ft", true);
  63
  64     _user_heading_node = fgGetNode("/orientation/heading-deg", true);
  65     _user_pitch_node =   fgGetNode("/orientation/pitch-deg", true);
  66     _user_roll_node =    fgGetNode("/orientation/roll-deg", true);
  67     _user_yaw_node =     fgGetNode("/orientation/yaw-deg", true);
  68     _user_alpha_node =   fgGetNode("/orientation/alpha-deg", true);
  69
  70     _user_speed_node = fgGetNode("/velocities/uBody-fps", true);
  71
  72     _user_wind_from_east_node  = fgGetNode("/environment/wind-from-east-fps", true);
  73     _user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps", true);
  74
  75     _user_speed_down_fps_node   = fgGetNode("/velocities/speed-down-fps", true);
  76     _user_speed_east_fps_node   = fgGetNode("/velocities/speed-east-fps", true);
  77     _user_speed_north_fps_node  = fgGetNode("/velocities/speed-north-fps", true);
  78
  79     _contrail_altitude_node = fgGetNode("/environment/params/contrail-altitude", true);
  80     contrail_altitude       = _contrail_altitude_node->getDoubleValue();
  81     _contrail_trigger       = fgGetNode("ai/submodels/contrails", true);
  82     _contrail_trigger->setBoolValue(false);
  83
  84     load();
  85
  86 }
  87
  88 void FGSubmodelMgr::postinit() {
  89     // postinit, so that the AI list is populated
  90
  91         loadAI();
  92
  93     while (_found_sub)
  94         loadSubmodels();
  95
  96     //TODO reload submodels if an MP ac joins
  97
  98     //_model_added_node = fgGetNode("ai/models/model-added", true);
  99     //_model_added_node->addChangeListener(this, false);
 100 }
 101
 102 void FGSubmodelMgr::bind()
 103 {}
 104
 105 void FGSubmodelMgr::unbind()
 106 {
 107     submodel_iterator = submodels.begin();
 108     while (submodel_iterator != submodels.end()) {
 109         (*submodel_iterator)->prop->untie("count");
 110         ++submodel_iterator;
 111     }
 112 }
 113
 114 void FGSubmodelMgr::update(double dt)
 115 {
 116     if (!_serviceable_node->getBoolValue())
 117         return;
 118
 119     _impact = false;
 120     _hit = false;
 121     _expiry = false;
 122
 123     // check if the submodel hit an object or terrain
 124     FGAIManager::ai_list_type sm_list(aiManager()->get_ai_list());
 125     FGAIManager::ai_list_iterator sm_list_itr = sm_list.begin(),
 126       end = sm_list.end();
 127
 128     for (; sm_list_itr != end; ++sm_list_itr) {
 129         FGAIBase::object_type object_type =(*sm_list_itr)->getType();
 130
 131         if (object_type != FGAIBase::otBallistic){// only work on ballistic objects
 132             continue; // so continue
 133         }
 134
 135         int parent_subID = (*sm_list_itr)->_getSubID();
 136         int id = (*sm_list_itr)->getID();
 137
 138         if ( parent_subID == 0 || id == -1) // this entry in the list has no associated submodel
 139             continue;                       // or is invalid so we can continue
 140
 141         //SG_LOG(SG_AI, SG_DEBUG, "Submodel: Impact " << _impact << " hit! "
 142         //        << _hit <<" parent_subID " << parent_subID);
 143
 144         _hit = (*sm_list_itr)->_getCollisionData();
 145         _impact = (*sm_list_itr)->_getImpactData();
 146         _expiry = (*sm_list_itr)->_getExpiryData();
 147
 148         //SG_LOG(SG_AI, SG_ALERT, "Submodel: " << (*sm_list_itr)->_getName()
 149         //    << " Impact " << _impact << " hit! " << _hit
 150         //    << " exipiry :-( " << _expiry );
 151
 152         if (_impact || _hit || _expiry) {
 153     //        SG_LOG(SG_AI, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit
 154                 //<< " exipiry :-( " << _expiry );
 155
 156             submodel_iterator = submodels.begin();
 157
 158             while (submodel_iterator != submodels.end()) {
 159                 int child_ID = (*submodel_iterator)->id;
 160                 //cout << "Impact: parent SubID " << parent_subID << " child_ID " << child_ID << endl;
 161
 162                 if ( parent_subID == child_ID ) {
 163                     _parent_lat = (*sm_list_itr)->_getImpactLat();
 164                     _parent_lon = (*sm_list_itr)->_getImpactLon();
 165                     _parent_elev = (*sm_list_itr)->_getImpactElevFt();
 166                     _parent_hdg = (*sm_list_itr)->_getImpactHdg();
 167                     _parent_pitch = (*sm_list_itr)->_getImpactPitch();
 168                     _parent_roll = (*sm_list_itr)->_getImpactRoll();
 169                     _parent_speed = (*sm_list_itr)->_getImpactSpeed();
 170                     (*submodel_iterator)->first_time = true;
 171                     //cout << "Impact: parent SubID = child_ID elev " << _parent_elev << endl;
 172
 173                     if (release(*submodel_iterator, dt)){
 174                         (*sm_list_itr)->setDie(true);
 175                         //cout << "Impact: set die" << (*sm_list_itr)->_getName() << endl;
 176                     }
 177
 178                 }
 179
 180                 ++submodel_iterator;
 181             }
 182         }
 183     }
 184
 185     _contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude);
 186
 187
 188 //    bool in_range = true;
 189     bool trigger = false;
 190     int i = -1;
 191
 192     submodel_iterator = submodels.begin();
 193     while (submodel_iterator != submodels.end())  {
 194         i++;
 195
 196         /*SG_LOG(SG_AI, SG_DEBUG,
 197                 "Submodels:  " << (*submodel_iterator)->id
 198                 << " name " << (*submodel_iterator)->name
 199                 );*/
 200
 201         if ((*submodel_iterator)->trigger_node != 0) {
 202             _trigger_node = (*submodel_iterator)->trigger_node;
 203             trigger = _trigger_node->getBoolValue();
 204             //cout << (*submodel_iterator)->name << "trigger node found " <<  trigger << endl;
 205         } else {
 206             trigger = false;
 207             //cout << (*submodel_iterator)->name << " trigger node not found " << trigger << endl;
 208         }
 209
 210         if (trigger && (*submodel_iterator)->count != 0) {
 211
 212             //int id = (*submodel_iterator)->id;
 213             //const string& name = (*submodel_iterator)->name;
 214
 215             SG_LOG(SG_AI, SG_DEBUG,
 216             "Submodels release:  " << (*submodel_iterator)->id
 217             << " name " << (*submodel_iterator)->name
 218             << " count " << (*submodel_iterator)->count
 219             << " slaved " << (*submodel_iterator)->slaved
 220             );
 221
 222             release(*submodel_iterator, dt);
 223         } else
 224             (*submodel_iterator)->first_time = true;
 225
 226         ++submodel_iterator;
 227     } // end while
 228 }
 229
 230 bool FGSubmodelMgr::release(submodel *sm, double dt)
 231 {
 232     //cout << "release id " << sm->id
 233     //    << " name " << sm->name
 234     //    << " first time " << sm->first_time
 235     //    << " repeat " << sm->repeat
 236     //    << " slaved " << sm->slaved
 237     //    << endl;
 238
 239     // only run if first time or repeat is set to true
 240     if (!sm->first_time && !sm->repeat) {
 241         //cout<< "returning: "<< sm->name
 242         //    << " not first time " << sm->first_time
 243         //    << " repeat " << sm->repeat
 244         //    << " slaved " << sm->slaved
 245         //    << endl;
 246         return false;
 247     }
 248
 249     sm->timer += dt;
 250
 251     if (sm->timer < sm->delay) {
 252         //cout << "not yet: timer " << sm->timer << " delay " << sm->delay << endl;
 253         return false;
 254     }
 255
 256     //cout << "released timer: " << sm->timer << " delay " << sm->delay << endl;
 257
 258     sm->timer = 0.0;
 259
 260     if (sm->first_time) {
 261         dt = 0.0;
 262         sm->first_time = false;
 263     }
 264
 265     transform(sm);  // calculate submodel's initial conditions in world-coordinates
 266
 267     FGAIBallistic* ballist = new FGAIBallistic;
 268     ballist->setPath(sm->model.c_str());
 269     ballist->setName(sm->name);
 270     ballist->setSlaved(sm->slaved);
 271     ballist->setRandom(sm->random);
 272     ballist->setRandomness(sm->randomness);
 273     ballist->setLatitude(offsetpos.getLatitudeDeg());
 274     ballist->setLongitude(offsetpos.getLongitudeDeg());
 275     ballist->setAltitude(offsetpos.getElevationFt());
 276     ballist->setAzimuth(IC.azimuth);
 277     ballist->setElevation(IC.elevation);
 278     ballist->setRoll(IC.roll);
 279     ballist->setSpeed(IC.speed / SG_KT_TO_FPS);
 280     ballist->setWind_from_east(IC.wind_from_east);
 281     ballist->setWind_from_north(IC.wind_from_north);
 282     ballist->setMass(IC.mass);
 283     ballist->setDragArea(sm->drag_area);
 284     ballist->setLife(sm->life);
 285     ballist->setBuoyancy(sm->buoyancy);
 286     ballist->setWind(sm->wind);
 287     ballist->setCd(sm->cd);
 288     ballist->setStabilisation(sm->aero_stabilised);
 289     ballist->setNoRoll(sm->no_roll);
 290     ballist->setCollision(sm->collision);
 291     ballist->setExpiry(sm->expiry);
 292     ballist->setImpact(sm->impact);
 293     ballist->setImpactReportNode(sm->impact_report);
 294     ballist->setFuseRange(sm->fuse_range);
 295     ballist->setSubmodel(sm->submodel.c_str());
 296     ballist->setSubID(sm->sub_id);
 297     ballist->setForceStabilisation(sm->force_stabilised);
 298     ballist->setExternalForce(sm->ext_force);
 299     ballist->setForcePath(sm->force_path.c_str());
 300     ballist->setXoffset(sm->x_offset);
 301     ballist->setYoffset(sm->y_offset);
 302     ballist->setZoffset(sm->z_offset);
 303     ballist->setPitchoffset(sm->pitch_offset);
 304     ballist->setYawoffset(sm->yaw_offset);
 305     ballist->setParentNodes(_selected_ac);
 306     ballist->setContentsNode(sm->contents_node);
 307     ballist->setWeight(sm->weight);
 308
 309     aiManager()->attach(ballist);
 310
 311     if (sm->count > 0)
 312         sm->count--;
 313     return true;
 314 }
 315
 316 void FGSubmodelMgr::load()
 317 {
 318     SGPropertyNode *path = fgGetNode("/sim/submodels/path");
 319
 320     if (path) {
 321         const int id = 0;
 322         const string& Path = path->getStringValue();
 323         bool Seviceable =_serviceable_node->getBoolValue();
 324         setData(id, Path, Seviceable);
 325     }
 326 }
 327
 328 void FGSubmodelMgr::transform(submodel *sm)
 329 {
 330     // set initial conditions
 331     if (sm->contents_node != 0 && !sm->slaved) {
 332         // get the weight of the contents (lbs) and convert to mass (slugs)
 333         sm->contents = sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue();
 334         //cout << "transform: contents " << sm->contents << endl;
 335         IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;
 336         //cout << "mass inc contents"  << IC.mass << endl;
 337
 338         // set contents to 0 in the parent
 339         sm->contents_node->getChild("level-gal_us",0,1)->setDoubleValue(0);
 340         /*cout << "contents " << sm->contents_node->getChild("level-gal_us")->getDoubleValue()
 341         << " " << sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue()
 342         << endl;*/
 343     } else
 344         IC.mass = sm->weight * lbs_to_slugs;
 345
 346     int id = sm->id;
 347     //int sub_id = sm->sub_id;
 348     //const string& name = sm->name;
 349
 350
 351     if (sm->speed_node != 0)
 352         sm->speed = sm->speed_node->getDoubleValue();
 353
 354
 355     //cout << " name " << name << " id " << id << " sub id" << sub_id << endl;
 356
 357     // set the Initial Conditions for the types of submodel parent
 358
 359     if (_impact || _hit || _expiry) {
 360         // set the data for a submodel tied to a submodel
 361
 362         _count++;
 363
 364         IC.lat             = _parent_lat;
 365         IC.lon             = _parent_lon;
 366         IC.alt             = _parent_elev;
 367         IC.roll            = _parent_roll;    // rotation about x axis
 368         IC.elevation       = _parent_pitch;   // rotation about y axis
 369         IC.azimuth         = _parent_hdg;     // rotation about z axis
 370         IC.speed           = _parent_speed;
 371         IC.speed_down_fps  = 0;
 372         IC.speed_east_fps  = 0;
 373         IC.speed_north_fps = 0;
 374
 375     } else if (id == 0) {
 376         //set the data for a submodel tied to the main model
 377
 378         IC.lat             = _user_lat_node->getDoubleValue();
 379         IC.lon             = _user_lon_node->getDoubleValue();
 380         IC.alt             = _user_alt_node->getDoubleValue();
 381         IC.roll            = _user_roll_node->getDoubleValue();    // rotation about x axis
 382         IC.elevation       = _user_pitch_node->getDoubleValue();   // rotation about y axis
 383         IC.azimuth         = _user_heading_node->getDoubleValue(); // rotation about z axis
 384         IC.speed           = _user_speed_node->getDoubleValue();
 385         IC.speed_down_fps  = _user_speed_down_fps_node->getDoubleValue();
 386         IC.speed_east_fps  = _user_speed_east_fps_node->getDoubleValue();
 387         IC.speed_north_fps = _user_speed_north_fps_node->getDoubleValue();
 388
 389     } else {
 390         // set the data for a submodel tied to an AI Object
 391         //cout << " set the data for a submodel tied to an AI Object " << id << endl;
 392         setParentNode(id);
 393     }
 394
 395     //cout << "Submodel: setting IC "<< name << endl;
 396     //cout << "heading " << IC.azimuth << endl ;
 397     //cout << "speed down " << IC.speed_down_fps << endl ;
 398     //cout << "speed east " << IC.speed_east_fps << endl ;
 399     //cout << "speed north " << IC.speed_north_fps << endl ;
 400     //cout << "parent speed fps in " << IC.speed << "sm speed in " << sm->speed << endl ;
 401     //cout << "lat " << IC.lat;
 402     //cout << "alt " << IC.alt <<  endl ;
 403
 404
 405     // Set the Initial Conditions that are common to all types of parent
 406     IC.wind_from_east =  _user_wind_from_east_node->getDoubleValue();
 407     IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
 408
 409 //cout << "wind e " << IC.wind_from_east << " n " << IC.wind_from_north << endl;
 410
 411     userpos.setLatitudeDeg(IC.lat);
 412     userpos.setLongitudeDeg(IC.lon);
 413     userpos.setElevationFt(IC.alt);
 414
 415     _x_offset = sm->x_offset;
 416     _y_offset = sm->y_offset;
 417     _z_offset = sm->z_offset;
 418
 419     setOffsetPos();
 420
 421     //IC.elevation += sm->pitch_offset;
 422     //IC.azimuth   += sm->yaw_offset ;
 423
 424     // pre-process the trig functions
 425     cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
 426     sinRx = sin(-IC.roll * SG_DEGREES_TO_RADIANS);
 427     cosRy = cos(-IC.elevation * SG_DEGREES_TO_RADIANS);
 428     sinRy = sin(-IC.elevation * SG_DEGREES_TO_RADIANS);
 429     cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS);
 430     sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS);
 431
 432
 433     // Get submodel initial velocity vector angles in XZ and XY planes.
 434     // This vector should be added to aircraft's vector.
 435     IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
 436     IC.azimuth   += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
 437
 438     // calculate the total speed north
 439     IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
 440             * cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
 441
 442     // calculate the total speed east
 443     IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
 444             * sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
 445
 446     // calculate the total speed down
 447     IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS)
 448             + IC.speed_down_fps;
 449
 450     // re-calculate speed, elevation and azimuth
 451     IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north
 452             + IC.total_speed_east * IC.total_speed_east
 453             + IC.total_speed_down * IC.total_speed_down);
 454
 455     // if speeds are low this calculation can become unreliable
 456     if (IC.speed > 1) {
 457         IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
 458         //        cout << "azimuth1 " << IC.azimuth<<endl;
 459
 460         // rationalise the output
 461         if (IC.azimuth < 0)
 462             IC.azimuth += 360;
 463         else if (IC.azimuth >= 360)
 464             IC.azimuth -= 360;
 465         // cout << "azimuth2 " << IC.azimuth<<endl;
 466
 467         IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
 468             * IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
 469             * SG_RADIANS_TO_DEGREES;
 470     }
 471     //cout << "IC.speed " << IC.speed / SG_KT_TO_FPS << endl;
 472 }
 473
 474 void FGSubmodelMgr::updatelat(double lat)
 475 {
 476     ft_per_deg_latitude = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
 477     ft_per_deg_longitude = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
 478 }
 479
 480 void FGSubmodelMgr::loadAI()
 481 {
 482     SG_LOG(SG_AI, SG_DEBUG, "Submodels: Loading AI submodels ");
 483
 484     FGAIManager::ai_list_type sm_list(aiManager()->get_ai_list());
 485
 486     if (sm_list.empty()) {
 487         SG_LOG(SG_AI, SG_ALERT, "Submodels: Unable to read AI submodel list");
 488         return;
 489     }
 490
 491     FGAIManager::ai_list_iterator sm_list_itr = sm_list.begin(),
 492       end = sm_list.end();
 493
 494     while (sm_list_itr != end) {
 495         string path = (*sm_list_itr)->_getSMPath();
 496
 497         if (path.empty()) {
 498             ++sm_list_itr;
 499             continue;
 500         }
 501
 502         int id = (*sm_list_itr)->getID();
 503         bool serviceable = (*sm_list_itr)->_getServiceable();
 504
 505         //string type = (*sm_list_itr)->getTypeString();
 506         //cout << "loadAI: type " << type << " path "<< path << " serviceable " << serviceable << endl;
 507
 508         setData(id, path, serviceable);
 509         ++sm_list_itr;
 510     }
 511 }
 512
 513
 514
 515 void FGSubmodelMgr::setData(int id, const string& path, bool serviceable)
 516 {
 517     SGPropertyNode root;
 518
 519     SGPath config = globals->resolve_aircraft_path(path);
 520     try {
 521         SG_LOG(SG_AI, SG_DEBUG,
 522                 "Submodels: Trying to read AI submodels file: " << config.str());
 523         readProperties(config.str(), &root);
 524     } catch (const sg_exception &) {
 525         SG_LOG(SG_AI, SG_ALERT,
 526                 "Submodels: Unable to read AI submodels file: " << config.str());
 527         return;
 528     }
 529
 530     vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
 531     vector<SGPropertyNode_ptr>::iterator it = children.begin();
 532     vector<SGPropertyNode_ptr>::iterator end = children.end();
 533
 534     for (int i = 0; it != end; ++it, i++) {
 535         //cout << "Reading AI submodel " << (*it)->getPath() << endl;
 536         submodel* sm = new submodel;
 537         SGPropertyNode * entry_node = *it;
 538         sm->name            = entry_node->getStringValue("name", "none_defined");
 539         sm->model           = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
 540         sm->speed           = entry_node->getDoubleValue("speed", 2329.4);
 541         sm->repeat          = entry_node->getBoolValue("repeat", false);
 542         sm->delay           = entry_node->getDoubleValue("delay", 0.25);
 543         sm->count           = entry_node->getIntValue("count", 1);
 544         sm->slaved          = entry_node->getBoolValue("slaved", false);
 545         sm->x_offset        = entry_node->getDoubleValue("x-offset", 0.0);
 546         sm->y_offset        = entry_node->getDoubleValue("y-offset", 0.0);
 547         sm->z_offset        = entry_node->getDoubleValue("z-offset", 0.0);
 548         sm->yaw_offset      = entry_node->getDoubleValue("yaw-offset", 0.0);
 549         sm->pitch_offset    = entry_node->getDoubleValue("pitch-offset", 0.0);
 550         sm->drag_area       = entry_node->getDoubleValue("eda", 0.034);
 551         sm->life            = entry_node->getDoubleValue("life", 900.0);
 552         sm->buoyancy        = entry_node->getDoubleValue("buoyancy", 0);
 553         sm->wind            = entry_node->getBoolValue("wind", false);
 554         sm->cd              = entry_node->getDoubleValue("cd", 0.193);
 555         sm->weight          = entry_node->getDoubleValue("weight", 0.25);
 556         sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
 557         sm->no_roll         = entry_node->getBoolValue("no-roll", false);
 558         sm->collision       = entry_node->getBoolValue("collision", false);
 559         sm->expiry                      = entry_node->getBoolValue("expiry", false);
 560         sm->impact          = entry_node->getBoolValue("impact", false);
 561         sm->impact_report   = entry_node->getStringValue("impact-reports");
 562         sm->fuse_range      = entry_node->getDoubleValue("fuse-range", 0.0);
 563         sm->contents_node   = fgGetNode(entry_node->getStringValue("contents", "none"), false);
 564         sm->speed_node      = fgGetNode(entry_node->getStringValue("speed-prop", "none"), false);
 565         sm->submodel        = entry_node->getStringValue("submodel-path", "");
 566         sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false);
 567         sm->ext_force       = entry_node->getBoolValue("external-force", false);
 568         sm->force_path      = entry_node->getStringValue("force-path", "");
 569         sm->random                      = entry_node->getBoolValue("random", false);
 570         sm->randomness          = entry_node->getDoubleValue("randomness", 0.5);
 571
 572         if (sm->contents_node != 0)
 573             sm->contents = sm->contents_node->getDoubleValue();
 574
 575         const char *trigger_path = entry_node->getStringValue("trigger", 0);
 576         if (trigger_path) {
 577             sm->trigger_node = fgGetNode(trigger_path, true);
 578             sm->trigger_node->setBoolValue(sm->trigger_node->getBoolValue());
 579         } else {
 580             sm->trigger_node = 0;
 581         }
 582
 583         if (sm->speed_node != 0)
 584             sm->speed = sm->speed_node->getDoubleValue();
 585
 586         sm->timer = sm->delay;
 587         sm->id = id;
 588         sm->first_time = false;
 589         sm->serviceable = serviceable;
 590         sm->sub_id = 0;
 591
 592         sm->prop = fgGetNode("/ai/submodels/submodel", index, true);
 593         sm->prop->tie("delay", SGRawValuePointer<double>(&(sm->delay)));
 594         sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
 595         sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
 596         sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
 597         sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
 598         sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
 599         sm->prop->tie("random", SGRawValuePointer<bool>(&(sm->random)));
 600         sm->prop->tie("slaved", SGRawValuePointer<bool>(&(sm->slaved)));
 601         const string& name = sm->name;
 602         sm->prop->setStringValue("name", name.c_str());
 603
 604         const string& submodel = sm->submodel;
 605         sm->prop->setStringValue("submodel", submodel.c_str());
 606
 607         const string& force_path = sm->force_path;
 608         sm->prop->setStringValue("force_path", force_path.c_str());
 609         //cout << "set force_path Sub " << force_path << endl;
 610
 611         if (sm->contents_node != 0)
 612             sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
 613
 614         index++;
 615         submodels.push_back(sm);
 616     }
 617 }
 618
 619 void FGSubmodelMgr::setSubData(int id, const string& path, bool serviceable)
 620 {
 621     SGPropertyNode root;
 622     SGPath config = globals->resolve_aircraft_path(path);
 623
 624     try {
 625         SG_LOG(SG_AI, SG_DEBUG,
 626                 "Submodels: Trying to read AI submodels file: " << config.str());
 627         readProperties(config.str(), &root);
 628
 629     } catch (const sg_exception &) {
 630         SG_LOG(SG_AI, SG_ALERT,
 631                 "Submodels: Unable to read AI submodels file: " << config.str());
 632         return;
 633     }
 634
 635     vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
 636     vector<SGPropertyNode_ptr>::iterator it = children.begin();
 637     vector<SGPropertyNode_ptr>::iterator end = children.end();
 638
 639     for (int i = 0; it != end; ++it, i++) {
 640         //cout << "Reading AI submodel " << (*it)->getPath() << endl;
 641         submodel* sm = new submodel;
 642         SGPropertyNode * entry_node = *it;
 643         sm->name            = entry_node->getStringValue("name", "none_defined");
 644         sm->model           = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
 645         sm->speed           = entry_node->getDoubleValue("speed", 2329.4);
 646         sm->repeat          = entry_node->getBoolValue("repeat", false);
 647         sm->delay           = entry_node->getDoubleValue("delay", 0.25);
 648         sm->count           = entry_node->getIntValue("count", 1);
 649         sm->slaved          = entry_node->getBoolValue("slaved", false);
 650         sm->x_offset        = entry_node->getDoubleValue("x-offset", 0.0);
 651         sm->y_offset        = entry_node->getDoubleValue("y-offset", 0.0);
 652         sm->z_offset        = entry_node->getDoubleValue("z-offset", 0.0);
 653         sm->yaw_offset      = entry_node->getDoubleValue("yaw-offset", 0.0);
 654         sm->pitch_offset    = entry_node->getDoubleValue("pitch-offset", 0.0);
 655         sm->drag_area       = entry_node->getDoubleValue("eda", 0.034);
 656         sm->life            = entry_node->getDoubleValue("life", 900.0);
 657         sm->buoyancy        = entry_node->getDoubleValue("buoyancy", 0);
 658         sm->wind            = entry_node->getBoolValue("wind", false);
 659         sm->cd              = entry_node->getDoubleValue("cd", 0.193);
 660         sm->weight          = entry_node->getDoubleValue("weight", 0.25);
 661         sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
 662         sm->no_roll         = entry_node->getBoolValue("no-roll", false);
 663         sm->collision       = entry_node->getBoolValue("collision", false);
 664         sm->expiry          = entry_node->getBoolValue("expiry", false);
 665         sm->impact          = entry_node->getBoolValue("impact", false);
 666         sm->impact_report   = entry_node->getStringValue("impact-reports");
 667         sm->fuse_range      = entry_node->getDoubleValue("fuse-range", 0.0);
 668         sm->contents_node   = fgGetNode(entry_node->getStringValue("contents", "none"), false);
 669         sm->speed_node      = fgGetNode(entry_node->getStringValue("speed-prop", "none"), false);
 670         sm->submodel        = entry_node->getStringValue("submodel-path", "");
 671         sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false);
 672         sm->ext_force       = entry_node->getBoolValue("external-force", false);
 673         sm->force_path      = entry_node->getStringValue("force-path", "");
 674         sm->random          = entry_node->getBoolValue("random", false);
 675         sm->randomness      = entry_node->getDoubleValue("randomness", 0.5);
 676
 677         if (sm->contents_node != 0)
 678             sm->contents = sm->contents_node->getDoubleValue();
 679
 680         const char *trigger_path = entry_node->getStringValue("trigger", 0);
 681         if (trigger_path) {
 682             sm->trigger_node = fgGetNode(trigger_path, true);
 683             sm->trigger_node->setBoolValue(sm->trigger_node->getBoolValue());
 684         } else {
 685             sm->trigger_node = 0;
 686         }
 687
 688         if (sm->speed_node != 0)
 689             sm->speed = sm->speed_node->getDoubleValue();
 690
 691         sm->timer = sm->delay;
 692         sm->id = index;
 693         sm->first_time = false;
 694         sm->serviceable = serviceable;
 695         sm->sub_id = 0;
 696
 697         sm->prop = fgGetNode("/ai/submodels/subsubmodel", index, true);
 698         sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
 699         sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
 700         sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
 701         sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
 702         sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
 703         sm->prop->tie("random", SGRawValuePointer<bool>(&(sm->random)));
 704         sm->prop->tie("slaved", SGRawValuePointer<bool>(&(sm->slaved)));
 705
 706         const string& name = sm->name;
 707         sm->prop->setStringValue("name", name.c_str());
 708
 709         const string& submodel = sm->submodel;
 710         sm->prop->setStringValue("submodel-path", submodel.c_str());
 711         // cout << " set submodel path AI" << submodel<< endl;
 712
 713         const string& force_path = sm->force_path;
 714         sm->prop->setStringValue("force_path", force_path.c_str());
 715         //cout << "set force_path  AI" << force_path << endl;
 716
 717         if (sm->contents_node != 0)
 718             sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
 719
 720         index++;
 721         subsubmodels.push_back(sm);
 722     }
 723 }
 724
 725 void FGSubmodelMgr::loadSubmodels()
 726 {
 727     SG_LOG(SG_AI, SG_DEBUG, "Submodels: Loading sub submodels");
 728
 729     _found_sub = false;
 730
 731     submodel_iterator = submodels.begin();
 732
 733     while (submodel_iterator != submodels.end()) {
 734         const string& submodel  = (*submodel_iterator)->submodel;
 735         if (!submodel.empty()) {
 736             //int id = (*submodel_iterator)->id;
 737             bool serviceable = true;
 738             SG_LOG(SG_AI, SG_DEBUG, "found path sub sub "
 739                     << submodel
 740                     << " index " << index
 741                     << " name " << (*submodel_iterator)->name);
 742
 743             if ((*submodel_iterator)->sub_id == 0){
 744                 (*submodel_iterator)->sub_id = index;
 745                 _found_sub = true;
 746                 setSubData(index, submodel, serviceable);
 747             }
 748         }
 749
 750         ++submodel_iterator;
 751     } // end while
 752
 753     subsubmodel_iterator = subsubmodels.begin();
 754
 755     while (subsubmodel_iterator != subsubmodels.end()) {
 756
 757         submodels.push_back(*subsubmodel_iterator);
 758         ++subsubmodel_iterator;
 759     } // end while
 760
 761     subsubmodels.clear();
 762
 763     //submodel_iterator = submodels.begin();
 764
 765     //int subcount = 0;
 766
 767     //while (submodel_iterator != submodels.end()) {
 768     //    int id = (*submodel_iterator)->id;
 769     //    subcount++;
 770
 771     //    SG_LOG(SG_AI, SG_ALERT,"after pushback "
 772     //            << " parent id " << id
 773     //            << " name " << (*submodel_iterator)->name
 774     //            << " sub id " << (*submodel_iterator)->sub_id
 775     //            << " subcount "<< subcount);
 776
 777     //    ++submodel_iterator;
 778     //}
 779 }
 780
 781 SGVec3d FGSubmodelMgr::getCartOffsetPos() const{
 782
 783     // convert geodetic positions to geocentered
 784     SGVec3d cartuserPos = SGVec3d::fromGeod(userpos);
 785     // Transform to the right coordinate frame, configuration is done in
 786     // the x-forward, y-right, z-up coordinates (feet), computation
 787     // in the simulation usual body x-forward, y-right, z-down coordinates
 788     // (meters) )
 789
 790     SGVec3d _off(_x_offset * SG_FEET_TO_METER,
 791         _y_offset * SG_FEET_TO_METER,
 792         -_z_offset * SG_FEET_TO_METER);
 793
 794     // Transform the user position to the horizontal local coordinate system.
 795     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
 796
 797     // and postrotate the orientation of the user model wrt the horizontal
 798     // local frame
 799     hlTrans *= SGQuatd::fromYawPitchRollDeg(
 800        IC.azimuth,
 801        IC.elevation,
 802        IC.roll);
 803
 804     // The offset converted to the usual body fixed coordinate system
 805     // rotated to the earth-fixed coordinates axis
 806     SGVec3d off = hlTrans.backTransform(_off);
 807
 808     // Add the position offset of the user model to get the geocentered position
 809     SGVec3d offsetPos = cartuserPos + off;
 810     return offsetPos;
 811 }
 812
 813 void FGSubmodelMgr::setOffsetPos(){
 814     // convert the offset geocentered position to geodetic
 815     SGVec3d cartoffsetPos = getCartOffsetPos();
 816
 817     SGGeodesy::SGCartToGeod(cartoffsetPos, offsetpos);
 818
 819     //cout << "OFFSET POS" << offsetpos.getElevationFt();
 820
 821 }
 822
 823 void FGSubmodelMgr::valueChanged(SGPropertyNode *prop)
 824 {
 825     return; // this isn't working atm
 826
 827     const char* _model_added = _model_added_node->getStringValue();
 828
 829     std::basic_string <char>::size_type indexCh2b;
 830
 831     string str2 = _model_added;
 832     const char *cstr2b = "multiplayer";
 833     indexCh2b = str2.find( cstr2b, 0 );
 834
 835     if (indexCh2b != string::npos ){        // we will ignore Ballistic Objects - there are potentially too many
 836
 837         //cout << "Submodels: model added - " << str2 <<" read path "<< endl;
 838         //return;
 839         SGPropertyNode *a_node = fgGetNode(_model_added, true);
 840         SGPropertyNode *sub_node = a_node->getChild("sim", 0, true);
 841         SGPropertyNode_ptr path_node = sub_node->getChild("path", 0, true);
 842         SGPropertyNode_ptr callsign_node = a_node->getChild("callsign", 0, true);
 843
 844         //const string& callsign = callsign_node->getStringValue();
 845         //cout << "Submodels: model added - " << callsign <<" read callsign "<< endl;
 846             return;
 847
 848         } else {
 849             cout << "model added - " << str2 <<" returning "<< endl;
 850         return;
 851         }
 852
 853 }
 854
 855 void FGSubmodelMgr::setParentNode(int id) {
 856
 857     const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);
 858
 859     for (int i = ai->nChildren() - 1; i >= -1; i--) {
 860         SGPropertyNode_ptr model;
 861
 862         if (i < 0) { // last iteration: selected model
 863             model = _selected_ac;
 864         } else {
 865             model = ai->getChild(i);
 866             //const string& path = ai->getPath();
 867             //const string& name = model->getStringValue("name");
 868             int parent_id = model->getIntValue("id");
 869             if (!model->nChildren()){
 870                 continue;
 871             }
 872             if (parent_id == id) {
 873                 _selected_ac = model;  // save selected model for last iteration
 874                 break;
 875             }
 876
 877         }
 878         if (!model)
 879             continue;
 880
 881     }// end for loop
 882
 883     if (_selected_ac != 0){
 884
 885         //cout << " parent node found"<< endl;
 886
 887         const string name  = _selected_ac->getStringValue("name");
 888         IC.lat             = _selected_ac->getDoubleValue("position/latitude-deg");
 889         IC.lon             = _selected_ac->getDoubleValue("position/longitude-deg");
 890         IC.alt             = _selected_ac->getDoubleValue("position/altitude-ft");
 891         IC.roll            = _selected_ac->getDoubleValue("orientation/roll-deg");
 892         IC.elevation       = _selected_ac->getDoubleValue("orientation/pitch-deg");
 893         IC.azimuth         = _selected_ac->getDoubleValue("orientation/true-heading-deg");
 894         IC.speed           = _selected_ac->getDoubleValue("velocities/true-airspeed-kt") * SG_KT_TO_FPS;
 895         IC.speed_down_fps  = -_selected_ac->getDoubleValue("velocities/vertical-speed-fps");
 896         IC.speed_east_fps  = _selected_ac->getDoubleValue("velocities/speed-east-fps");
 897         IC.speed_north_fps = _selected_ac->getDoubleValue("velocities/speed-north-fps");
 898
 899         //cout << name << " IC.speed " << IC.speed << endl;
 900
 901     } else {
 902         SG_LOG(SG_AI, SG_ALERT, "AISubmodel: parent node not found ");
 903     }
 904
 905 }
 906 // end of submodel.cxx