From 5a73a46cd05f02c8c739aee9b148c8b258049303 Mon Sep 17 00:00:00 2001 From: mfranz Date: Thu, 7 Jun 2007 16:30:26 +0000 Subject: [PATCH] Vivian MEAZZA: - collision detection for AI objects - subsubmodels mf: various modifications; more general cleanup to do --- src/AIModel/AIBallistic.cxx | 111 +++++-- src/AIModel/AIBallistic.hxx | 32 +- src/AIModel/AIBase.cxx | 520 +++++++++++++++++--------------- src/AIModel/AIBase.hxx | 52 +++- src/AIModel/AIManager.cxx | 569 +++++++++++++++++++++--------------- src/AIModel/AIManager.hxx | 14 +- src/AIModel/submodel.cxx | 424 ++++++++++++++++++++++----- src/AIModel/submodel.hxx | 39 ++- 8 files changed, 1174 insertions(+), 587 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index fa5ea185b..9023e36c4 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -26,15 +26,12 @@ #include #include #include -#include -#include +#include #include #include "AIBallistic.hxx" -SG_USING_STD(vector); - const double FGAIBallistic::slugs_to_kgs = 14.5939029372; FGAIBallistic::FGAIBallistic() : @@ -43,11 +40,12 @@ FGAIBallistic::FGAIBallistic() : _drag_area(0.007), _life_timer(0.0), _gravity(32), - // _buoyancy(64), + _buoyancy(0), _ht_agl_ft(0), _load_resistance(0), _solid(false), - _impact_reported(false), + _report_collision(false), + _report_impact(false), _impact_report_node(fgGetNode("/ai/models/model-impact", true)), _mat_name("") { @@ -80,6 +78,9 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setImpact(scFileNode->getBoolValue("impact", false)); setImpactReportNode(scFileNode->getStringValue("impact-reports")); setName(scFileNode->getStringValue("name", "Bomb")); + setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0)); + setSMPath(scFileNode->getStringValue("submodel-path", "")); + setSubID(scFileNode->getIntValue("SubID", 0)); } bool FGAIBallistic::init(bool search_in_AI_path) { @@ -87,6 +88,7 @@ bool FGAIBallistic::init(bool search_in_AI_path) { props->setStringValue("material/name", _mat_name.c_str()); props->setStringValue("name", _name.c_str()); + props->setStringValue("submodels/path", _submodel.c_str()); // start with high value so that animations don't trigger yet _ht_agl_ft = 10000000; @@ -94,6 +96,7 @@ bool FGAIBallistic::init(bool search_in_AI_path) { pitch = _elevation; roll = _rotation; Transform(); + return true; } @@ -108,6 +111,8 @@ void FGAIBallistic::bind() { SGRawValuePointer(&_solid)); props->tie("altitude-agl-ft", SGRawValuePointer(&_ht_agl_ft)); + props->tie("sub-id", + SGRawValuePointer(&_subID)); } void FGAIBallistic::unbind() { @@ -116,6 +121,7 @@ void FGAIBallistic::unbind() { props->untie("material/load-resistance"); props->untie("material/solid"); props->untie("altitude-agl-ft"); + props->untie("sub-id"); } void FGAIBallistic::update(double dt) { @@ -184,6 +190,10 @@ void FGAIBallistic::setImpact(bool i) { _report_impact = i; } +void FGAIBallistic::setCollision(bool c) { + _report_collision = c; +} + void FGAIBallistic::setImpactReportNode(const string& path) { if (!path.empty()) _impact_report_node = fgGetNode(path.c_str(), true); @@ -193,9 +203,26 @@ void FGAIBallistic::setName(const string& n) { _name = n; } +void FGAIBallistic::setSMPath(const string& s) { + _submodel = s; +} + +void FGAIBallistic::setFuseRange(double f) { + _fuse_range = f; +} + +void FGAIBallistic::setSubID(int i) { + _subID = i; + //cout << "sub id " << _subID << " name " << _name << endl; +} + +void FGAIBallistic::setSubmodel(const string& s) { + _submodel = s; +} + void FGAIBallistic::Run(double dt) { _life_timer += dt; - // cout << "life timer 1" << _life_timer << dt << endl; + //cout << "life timer" <<_name <<" " << _life_timer << dt << endl; if (_life_timer > life) setDie(true); @@ -204,6 +231,7 @@ void FGAIBallistic::Run(double dt) { double wind_speed_from_north_deg_sec; double wind_speed_from_east_deg_sec; double Cdm; // Cd adjusted by Mach Number + double hs; //randomise Cd by +- 5% if (_random) @@ -234,8 +262,12 @@ void FGAIBallistic::Run(double dt) { double speed_fps = speed * SG_KT_TO_FPS; // calculate vertical and horizontal speed components - vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; - double hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; + if (speed == 0.0) { + hs = vs = 0.0; + } else { + vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; + hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; + } // convert horizontal speed (fps) to degrees per second speed_north_deg_sec = cos(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lat; @@ -274,9 +306,13 @@ void FGAIBallistic::Run(double dt) { // recalculate total speed speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS; + //do impacts and collisions if (_report_impact && !_impact_reported) handle_impact(); + if (_report_collision && !_collision_reported) + handle_collision(); + // set destruction flag if altitude less than sea level -1000 if (altitude_ft < -1000.0) setDie(true); @@ -311,25 +347,52 @@ void FGAIBallistic::handle_impact() { _ht_agl_ft = pos.getElevationFt() - elevation_m * SG_METER_TO_FEET; - // report impact by setting property-tied variables + // report impact by setting properties if (_ht_agl_ft <= 0) { + SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: terrain impact"); + report_impact(elevation_m); _impact_reported = true; - double speed_mps = speed * SG_KT_TO_MPS; - - SGPropertyNode *n = props->getNode("impact", true); - n->setDoubleValue("longitude-deg", pos.getLongitudeDeg()); - n->setDoubleValue("latitude-deg", pos.getLatitudeDeg()); - n->setDoubleValue("elevation-m", elevation_m); - n->setDoubleValue("heading-deg", hdg); - n->setDoubleValue("pitch-deg", pitch); - n->setDoubleValue("roll-deg", roll); - n->setDoubleValue("speed-mps", speed_mps); - n->setDoubleValue("energy-kJ", (_mass * slugs_to_kgs) - * speed_mps * speed_mps / (2 * 1000)); - - _impact_report_node->setStringValue(props->getPath()); } } +void FGAIBallistic::handle_collision() +{ + const FGAIBase *collision = manager->calcCollision(pos.getElevationFt(), + pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range); + + if (collision) { + SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: HIT!"); + report_impact(pos.getElevationM(), collision); + _collision_reported = true; + } +} + +void FGAIBallistic::report_impact(double elevation, const FGAIBase *object) +{ + _impact_lat = pos.getLatitudeDeg(); + _impact_lon = pos.getLongitudeDeg(); + _impact_elev = elevation; + _impact_speed = speed * SG_KT_TO_MPS; + _impact_hdg = hdg; + _impact_pitch = pitch; + _impact_roll = roll; + + SGPropertyNode *n = props->getNode("impact", true); + if (object) + n->setStringValue("type", object->getTypeString()); + else + n->setStringValue("type", "terrain"); + + n->setDoubleValue("longitude-deg", _impact_lon); + n->setDoubleValue("latitude-deg", _impact_lat); + n->setDoubleValue("elevation-m", _impact_elev); + n->setDoubleValue("heading-deg", _impact_hdg); + n->setDoubleValue("pitch-deg", _impact_pitch); + n->setDoubleValue("roll-deg", _impact_roll); + n->setDoubleValue("speed-mps", _impact_speed); + + _impact_report_node->setStringValue(props->getPath()); +} + // end AIBallistic diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx index 73dce5043..58b6f40f0 100644 --- a/src/AIModel/AIBallistic.hxx +++ b/src/AIModel/AIBallistic.hxx @@ -20,9 +20,16 @@ #ifndef _FG_AIBALLISTIC_HXX #define _FG_AIBALLISTIC_HXX +#include +#include +#include + + #include "AIManager.hxx" #include "AIBase.hxx" +SG_USING_STD(vector); +SG_USING_STD(list); class FGAIBallistic : public FGAIBase { @@ -54,8 +61,14 @@ public: void setNoRoll( bool nr ); void setRandom( bool r ); void setName(const string&); + void setCollision(bool c); void setImpact(bool i); void setImpactReportNode(const string&); + void setFuseRange(double f); + void setSMPath(const string&); + void setSubID(int i); + void setSubmodel(const string&); + double _getTime() const; @@ -81,17 +94,28 @@ private: double _ht_agl_ft; // height above ground level double _load_resistance; // ground load resistanc N/m^2 bool _solid; // if true ground is solid for FDMs - bool _report_impact; // if true an impact point on the terrain is calculated - bool _impact_reported; // if true impact data have been set - SGPropertyNode_ptr _impact_report_node; + + bool _report_collision; // if true a collision point with AI Objects is calculated + bool _report_impact; // if true an impact point on the terrain is calculated + + SGPropertyNode_ptr _impact_report_node; // report node for impact and collision + + double _fuse_range; + + double _dt_count; + double _next_run; string _mat_name; string _name; + string _path; + string _submodel; void Run(double dt); + void handle_collision(); void handle_impact(); - // FGAIBase* _ai; + void report_impact(double elevation, const FGAIBase *target = 0); + }; #endif // _FG_AIBALLISTIC_HXX diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index 4c8a24eaa..e375f606c 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -24,6 +24,7 @@ # include #endif + #include #include STL_STRING @@ -45,11 +46,9 @@ #include #include - #include "AIBase.hxx" #include "AIManager.hxx" - const double FGAIBase::e = 2.71828183; const double FGAIBase::lbs_to_slugs = 0.031080950172; //conversion factor @@ -59,6 +58,15 @@ FGAIBase::FGAIBase(object_type ot) : model_removed( fgGetNode("/ai/models/model-removed", true) ), manager( NULL ), fp( NULL ), + + _impact_lat(0), + _impact_lon(0), + _impact_elev(0), + _impact_hdg(0), + _impact_pitch(0), + _impact_roll(0), + _impact_speed(0), + _refID( _newAIModelID() ), _otype(ot) { @@ -67,10 +75,13 @@ FGAIBase::FGAIBase(object_type ot) : bearing = elevation = range = rdot = 0.0; x_shift = y_shift = rotation = 0.0; in_range = false; - invisible = true; + invisible = false; no_roll = true; life = 900; delete_me = false; + _impact_reported = false; + _collision_reported = false; + _subID = 0; } FGAIBase::~FGAIBase() { @@ -127,14 +138,14 @@ void FGAIBase::update(double dt) { void FGAIBase::Transform() { if (!invisible) { - aip.setPosition(pos); + aip.setPosition(pos); if (no_roll) - aip.setOrientation(0.0, pitch, hdg); + aip.setOrientation(0.0, pitch, hdg); else - aip.setOrientation(roll, pitch, hdg); + aip.setOrientation(roll, pitch, hdg); - aip.update(); + aip.update(); } } @@ -143,47 +154,44 @@ bool FGAIBase::init(bool search_in_AI_path) { if (!model_path.empty()) { - if ( (search_in_AI_path) - &&(model_path.substr(model_path.size() - 4, 4) == ".xml")) { - SGPath ai_path("AI"); - ai_path.append(model_path); - try { - model = load3DModel( globals->get_fg_root(), ai_path.str(), props, + if ( search_in_AI_path + && (model_path.substr(model_path.size() - 4, 4) == ".xml")) { + SGPath ai_path("AI"); + ai_path.append(model_path); + try { + model = load3DModel( globals->get_fg_root(), ai_path.str(), props, globals->get_sim_time_sec() ); - } catch (const sg_exception &e) { - model = NULL; - } - } else - model = NULL; - - if (!model) { - try { - model = load3DModel( globals->get_fg_root(), model_path, props, + } catch (const sg_exception &e) { + model = NULL; + } + } else + model = NULL; + + if (!model.get()) { + try { + model = load3DModel( globals->get_fg_root(), model_path, props, globals->get_sim_time_sec() ); - } catch (const sg_exception &e) { - model = NULL; - } - } - - } - - if (model.get()) { - aip.init( model.get() ); - aip.setVisible(true); - invisible = false; - globals->get_scenery()->get_scene_graph()->addChild(aip.getSceneGraph()); + } catch (const sg_exception &e) { + model = NULL; + } + } - fgSetString("/ai/models/model-added", props->getPath()); - } else { + } - if (!model_path.empty()) - SG_LOG(SG_INPUT, SG_WARN, "AIBase: Could not load model " << model_path); + if (model.get()) { + aip.init( model.get() ); + aip.setVisible(true); + invisible = false; + globals->get_scenery()->get_scene_graph()->addChild(aip.getSceneGraph()); + fgSetString("/ai/models/model-added", props->getPath()); - } + } else if (!model_path.empty()) { + SG_LOG(SG_INPUT, SG_WARN, "AIBase: Could not load model " << model_path); + } props->setStringValue("submodels/path", _path.c_str()); - setDie(false); - return true; + setDie(false); + return true; } @@ -199,77 +207,73 @@ osg::Node* FGAIBase::load3DModel(const string& fg_root, } bool FGAIBase::isa( object_type otype ) { - - if ( otype == _otype ) - return true; - else - return false; + return otype == _otype; } void FGAIBase::bind() { - props->tie("id", SGRawValueMethods(*this, - &FGAIBase::getID)); - props->tie("velocities/true-airspeed-kt", SGRawValuePointer(&speed)); - props->tie("velocities/vertical-speed-fps", - SGRawValueMethods(*this, - &FGAIBase::_getVS_fps, - &FGAIBase::_setVS_fps)); - - props->tie("position/altitude-ft", - SGRawValueMethods(*this, - &FGAIBase::_getAltitude, - &FGAIBase::_setAltitude)); - props->tie("position/latitude-deg", - SGRawValueMethods(*this, - &FGAIBase::_getLatitude, - &FGAIBase::_setLatitude)); - props->tie("position/longitude-deg", - SGRawValueMethods(*this, - &FGAIBase::_getLongitude, - &FGAIBase::_setLongitude)); - - props->tie("position/global-x", - SGRawValueMethods(*this, - &FGAIBase::_getCartPosX, - 0)); - props->tie("position/global-y", - SGRawValueMethods(*this, - &FGAIBase::_getCartPosY, - 0)); - props->tie("position/global-z", - SGRawValueMethods(*this, - &FGAIBase::_getCartPosZ, - 0)); - - props->tie("orientation/pitch-deg", SGRawValuePointer(&pitch)); - props->tie("orientation/roll-deg", SGRawValuePointer(&roll)); - props->tie("orientation/true-heading-deg", SGRawValuePointer(&hdg)); - - props->tie("radar/in-range", SGRawValuePointer(&in_range)); - props->tie("radar/bearing-deg", SGRawValuePointer(&bearing)); - props->tie("radar/elevation-deg", SGRawValuePointer(&elevation)); - props->tie("radar/range-nm", SGRawValuePointer(&range)); - props->tie("radar/h-offset", SGRawValuePointer(&horiz_offset)); - props->tie("radar/v-offset", SGRawValuePointer(&vert_offset)); - props->tie("radar/x-shift", SGRawValuePointer(&x_shift)); - props->tie("radar/y-shift", SGRawValuePointer(&y_shift)); - props->tie("radar/rotation", SGRawValuePointer(&rotation)); - props->tie("radar/ht-diff-ft", SGRawValuePointer(&ht_diff)); - - props->tie("controls/lighting/nav-lights", - SGRawValueFunctions(_isNight)); - props->setBoolValue("controls/lighting/beacon", true); - props->setBoolValue("controls/lighting/strobe", true); - props->setBoolValue("controls/glide-path", true); - - props->setStringValue("controls/flight/lateral-mode", "roll"); - props->setDoubleValue("controls/flight/target-hdg", hdg); - props->setDoubleValue("controls/flight/target-roll", roll); - - props->setStringValue("controls/flight/longitude-mode", "alt"); - props->setDoubleValue("controls/flight/target-alt", altitude_ft); - props->setDoubleValue("controls/flight/target-pitch", pitch); + props->tie("id", SGRawValueMethods(*this, + &FGAIBase::getID)); + props->tie("velocities/true-airspeed-kt", SGRawValuePointer(&speed)); + props->tie("velocities/vertical-speed-fps", + SGRawValueMethods(*this, + &FGAIBase::_getVS_fps, + &FGAIBase::_setVS_fps)); + + props->tie("position/altitude-ft", + SGRawValueMethods(*this, + &FGAIBase::_getAltitude, + &FGAIBase::_setAltitude)); + props->tie("position/latitude-deg", + SGRawValueMethods(*this, + &FGAIBase::_getLatitude, + &FGAIBase::_setLatitude)); + props->tie("position/longitude-deg", + SGRawValueMethods(*this, + &FGAIBase::_getLongitude, + &FGAIBase::_setLongitude)); + + props->tie("position/global-x", + SGRawValueMethods(*this, + &FGAIBase::_getCartPosX, + 0)); + props->tie("position/global-y", + SGRawValueMethods(*this, + &FGAIBase::_getCartPosY, + 0)); + props->tie("position/global-z", + SGRawValueMethods(*this, + &FGAIBase::_getCartPosZ, + 0)); + + props->tie("orientation/pitch-deg", SGRawValuePointer(&pitch)); + props->tie("orientation/roll-deg", SGRawValuePointer(&roll)); + props->tie("orientation/true-heading-deg", SGRawValuePointer(&hdg)); + + props->tie("radar/in-range", SGRawValuePointer(&in_range)); + props->tie("radar/bearing-deg", SGRawValuePointer(&bearing)); + props->tie("radar/elevation-deg", SGRawValuePointer(&elevation)); + props->tie("radar/range-nm", SGRawValuePointer(&range)); + props->tie("radar/h-offset", SGRawValuePointer(&horiz_offset)); + props->tie("radar/v-offset", SGRawValuePointer(&vert_offset)); + props->tie("radar/x-shift", SGRawValuePointer(&x_shift)); + props->tie("radar/y-shift", SGRawValuePointer(&y_shift)); + props->tie("radar/rotation", SGRawValuePointer(&rotation)); + props->tie("radar/ht-diff-ft", SGRawValuePointer(&ht_diff)); + props->tie("subID", SGRawValuePointer(&_subID)); + props->tie("controls/lighting/nav-lights", + SGRawValueFunctions(_isNight)); + props->setBoolValue("controls/lighting/beacon", true); + props->setBoolValue("controls/lighting/strobe", true); + props->setBoolValue("controls/glide-path", true); + + props->setStringValue("controls/flight/lateral-mode", "roll"); + props->setDoubleValue("controls/flight/target-hdg", hdg); + props->setDoubleValue("controls/flight/target-roll", roll); + + props->setStringValue("controls/flight/longitude-mode", "alt"); + props->setDoubleValue("controls/flight/target-alt", altitude_ft); + props->setDoubleValue("controls/flight/target-pitch", pitch); props->setDoubleValue("controls/flight/target-spd", speed); @@ -306,137 +310,136 @@ void FGAIBase::unbind() { } double FGAIBase::UpdateRadar(FGAIManager* manager) { - bool force_on = fgGetBool("/instrumentation/radar/debug-mode", false); - double radar_range_nm = fgGetDouble("/instrumentation/radar/range"); - double radar_range_ft2 = radar_range_nm; - radar_range_ft2 *= SG_NM_TO_METER * SG_METER_TO_FEET * 1.1; // + 10% - radar_range_ft2 *= radar_range_ft2; - - double user_latitude = manager->get_user_latitude(); - double user_longitude = manager->get_user_longitude(); - double lat_range = fabs(pos.getLatitudeDeg() - user_latitude) * ft_per_deg_lat; - double lon_range = fabs(pos.getLongitudeDeg() - user_longitude) * ft_per_deg_lon; - double range_ft2 = lat_range*lat_range + lon_range*lon_range; - - // - // Test whether the target is within radar range. - // - in_range = (range_ft2 && (range_ft2 <= radar_range_ft2)); - - if ( in_range || force_on ) { - props->setBoolValue("radar/in-range", true); - - // copy values from the AIManager - double user_altitude = manager->get_user_altitude(); - double user_heading = manager->get_user_heading(); - double user_pitch = manager->get_user_pitch(); - //double user_yaw = manager->get_user_yaw(); - //double user_speed = manager->get_user_speed(); - - // calculate range to target in feet and nautical miles - double range_ft = sqrt( range_ft2 ); - range = range_ft / 6076.11549; - - // calculate bearing to target - if (pos.getLatitudeDeg() >= user_latitude) { - bearing = atan2(lat_range, lon_range) * SG_RADIANS_TO_DEGREES; - if (pos.getLongitudeDeg() >= user_longitude) { - bearing = 90.0 - bearing; + double radar_range_ft2 = fgGetDouble("/instrumentation/radar/range"); + bool force_on = fgGetBool("/instrumentation/radar/debug-mode", false); + radar_range_ft2 *= SG_NM_TO_METER * SG_METER_TO_FEET * 1.1; // + 10% + radar_range_ft2 *= radar_range_ft2; + + double user_latitude = manager->get_user_latitude(); + double user_longitude = manager->get_user_longitude(); + double lat_range = fabs(pos.getLatitudeDeg() - user_latitude) * ft_per_deg_lat; + double lon_range = fabs(pos.getLongitudeDeg() - user_longitude) * ft_per_deg_lon; + double range_ft2 = lat_range*lat_range + lon_range*lon_range; + + // + // Test whether the target is within radar range. + // + in_range = (range_ft2 && (range_ft2 <= radar_range_ft2)); + + if ( in_range || force_on ) { + props->setBoolValue("radar/in-range", true); + + // copy values from the AIManager + double user_altitude = manager->get_user_altitude(); + double user_heading = manager->get_user_heading(); + double user_pitch = manager->get_user_pitch(); + //double user_yaw = manager->get_user_yaw(); + //double user_speed = manager->get_user_speed(); + + // calculate range to target in feet and nautical miles + double range_ft = sqrt( range_ft2 ); + range = range_ft / 6076.11549; + + // calculate bearing to target + if (pos.getLatitudeDeg() >= user_latitude) { + bearing = atan2(lat_range, lon_range) * SG_RADIANS_TO_DEGREES; + if (pos.getLongitudeDeg() >= user_longitude) { + bearing = 90.0 - bearing; + } else { + bearing = 270.0 + bearing; + } } else { - bearing = 270.0 + bearing; + bearing = atan2(lon_range, lat_range) * SG_RADIANS_TO_DEGREES; + if (pos.getLongitudeDeg() >= user_longitude) { + bearing = 180.0 - bearing; + } else { + bearing = 180.0 + bearing; + } } - } else { - bearing = atan2(lon_range, lat_range) * SG_RADIANS_TO_DEGREES; - if (pos.getLongitudeDeg() >= user_longitude) { - bearing = 180.0 - bearing; - } else { - bearing = 180.0 + bearing; - } - } - - // This is an alternate way to compute bearing and distance which - // agrees with the original scheme within about 0.1 degrees. - // - // Point3D start( user_longitude * SGD_DEGREES_TO_RADIANS, - // user_latitude * SGD_DEGREES_TO_RADIANS, 0 ); - // Point3D dest( pos.getLongitudeRad(), pos.getLatitudeRad(), 0 ); - // double gc_bearing, gc_range; - // calc_gc_course_dist( start, dest, &gc_bearing, &gc_range ); - // gc_range *= SG_METER_TO_NM; - // gc_bearing *= SGD_RADIANS_TO_DEGREES; - // printf("orig b = %.3f %.2f gc b= %.3f, %.2f\n", - // bearing, range, gc_bearing, gc_range); - - // calculate look left/right to target, without yaw correction - horiz_offset = bearing - user_heading; - if (horiz_offset > 180.0) horiz_offset -= 360.0; - if (horiz_offset < -180.0) horiz_offset += 360.0; - - // calculate elevation to target - elevation = atan2( altitude_ft - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES; - - // calculate look up/down to target - vert_offset = elevation - user_pitch; - - /* this calculation needs to be fixed, but it isn't important anyway - // calculate range rate - double recip_bearing = bearing + 180.0; - if (recip_bearing > 360.0) recip_bearing -= 360.0; - double my_horiz_offset = recip_bearing - hdg; - if (my_horiz_offset > 180.0) my_horiz_offset -= 360.0; - if (my_horiz_offset < -180.0) my_horiz_offset += 360.0; - rdot = (-user_speed * cos( horiz_offset * SG_DEGREES_TO_RADIANS )) - +(-speed * 1.686 * cos( my_horiz_offset * SG_DEGREES_TO_RADIANS )); -*/ - - // now correct look left/right for yaw - // horiz_offset += user_yaw; // FIXME: WHY WOULD WE WANT TO ADD IN SIDE-SLIP HERE? - // calculate values for radar display - y_shift = (range * cos( horiz_offset * SG_DEGREES_TO_RADIANS)) / radar_range_nm; - x_shift = (range * sin( horiz_offset * SG_DEGREES_TO_RADIANS)) / radar_range_nm; - rotation = hdg - user_heading; - if (rotation < 0.0) rotation += 360.0; - ht_diff = altitude_ft - user_altitude; + // This is an alternate way to compute bearing and distance which + // agrees with the original scheme within about 0.1 degrees. + // + // Point3D start( user_longitude * SGD_DEGREES_TO_RADIANS, + // user_latitude * SGD_DEGREES_TO_RADIANS, 0 ); + // Point3D dest( pos.getLongitudeRad(), pos.getLatitudeRad(), 0 ); + // double gc_bearing, gc_range; + // calc_gc_course_dist( start, dest, &gc_bearing, &gc_range ); + // gc_range *= SG_METER_TO_NM; + // gc_bearing *= SGD_RADIANS_TO_DEGREES; + // printf("orig b = %.3f %.2f gc b= %.3f, %.2f\n", + // bearing, range, gc_bearing, gc_range); + + // calculate look left/right to target, without yaw correction + horiz_offset = bearing - user_heading; + if (horiz_offset > 180.0) horiz_offset -= 360.0; + if (horiz_offset < -180.0) horiz_offset += 360.0; + + // calculate elevation to target + elevation = atan2( altitude_ft - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES; + + // calculate look up/down to target + vert_offset = elevation - user_pitch; + + /* this calculation needs to be fixed, but it isn't important anyway + // calculate range rate + double recip_bearing = bearing + 180.0; + if (recip_bearing > 360.0) recip_bearing -= 360.0; + double my_horiz_offset = recip_bearing - hdg; + if (my_horiz_offset > 180.0) my_horiz_offset -= 360.0; + if (my_horiz_offset < -180.0) my_horiz_offset += 360.0; + rdot = (-user_speed * cos( horiz_offset * SG_DEGREES_TO_RADIANS )) + +(-speed * 1.686 * cos( my_horiz_offset * SG_DEGREES_TO_RADIANS )); + */ + + // now correct look left/right for yaw + // horiz_offset += user_yaw; // FIXME: WHY WOULD WE WANT TO ADD IN SIDE-SLIP HERE? + + // calculate values for radar display + y_shift = range * cos( horiz_offset * SG_DEGREES_TO_RADIANS); + x_shift = range * sin( horiz_offset * SG_DEGREES_TO_RADIANS); + rotation = hdg - user_heading; + if (rotation < 0.0) rotation += 360.0; + ht_diff = altitude_ft - user_altitude; - } + } - return range_ft2; + return range_ft2; } /* * Getters and Setters */ - + SGVec3d FGAIBase::getCartPosAt(const SGVec3d& _off) const { // Transform that one to the horizontal local coordinate system. - SGQuatd hlTrans = SGQuatd::fromLonLat(pos); + SGQuatd hlTrans = SGQuatd::fromLonLat(pos); - // and postrotate the orientation of the AIModel wrt the horizontal - // local frame - hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); - // The offset converted to the usual body fixed coordinate system - // rotated to the earth fiexed coordinates axis - SGVec3d off = hlTrans.backTransform(_off); + // The offset converted to the usual body fixed coordinate system + // rotated to the earth fiexed coordinates axis + SGVec3d off = hlTrans.backTransform(_off); - // Add the position offset of the AIModel to gain the earth centered position - SGVec3d cartPos = SGVec3d::fromGeod(pos); + // Add the position offset of the AIModel to gain the earth centered position + SGVec3d cartPos = SGVec3d::fromGeod(pos); - return cartPos + off; + return cartPos + off; } SGVec3d FGAIBase::getCartPos() const { - // Transform that one to the horizontal local coordinate system. - SGQuatd hlTrans = SGQuatd::fromLonLat(pos); + // Transform that one to the horizontal local coordinate system. + SGQuatd hlTrans = SGQuatd::fromLonLat(pos); - // and postrotate the orientation of the AIModel wrt the horizontal - // local frame - hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); - SGVec3d cartPos = SGVec3d::fromGeod(pos); + SGVec3d cartPos = SGVec3d::fromGeod(pos); - return cartPos; + return cartPos; } double FGAIBase::_getCartPosX() const { @@ -462,14 +465,22 @@ void FGAIBase::_setLatitude ( double latitude ) { pos.setLatitudeDeg(latitude); } +void FGAIBase::_setSubID( int s ) { + _subID = s; +} + double FGAIBase::_getLongitude() const { return pos.getLongitudeDeg(); } -double FGAIBase::_getLatitude () const { +double FGAIBase::_getLatitude() const { return pos.getLatitudeDeg(); } +double FGAIBase::_getElevationFt () const { + return pos.getElevationFt(); +} + double FGAIBase::_getRdot() const { return rdot; } @@ -510,10 +521,50 @@ bool FGAIBase::_isNight() { return (fgGetFloat("/sim/time/sun-angle-rad") > 1.57); } +bool FGAIBase::_getCollisionData() { + return _collision_reported; +} + +bool FGAIBase::_getImpactData() { + return _impact_reported; +} + +double FGAIBase::_getImpactLat() const { + return _impact_lat; +} + +double FGAIBase::_getImpactLon() const { + return _impact_lon; +} + +double FGAIBase::_getImpactElevFt() const { + return _impact_elev * SG_METER_TO_FEET; +} + +double FGAIBase::_getImpactPitch() const { + return _impact_pitch; +} + +double FGAIBase::_getImpactRoll() const { + return _impact_roll; +} + +double FGAIBase::_getImpactHdg() const { + return _impact_hdg; +} + +double FGAIBase::_getImpactSpeed() const { + return _impact_speed; +} + int FGAIBase::getID() const { return _refID; } +int FGAIBase::_getSubID() const { + return _subID; +} + double FGAIBase::_getSpeed() const { return speed; } @@ -534,20 +585,33 @@ const char* FGAIBase::_getPath() { return _path.c_str(); } +const char* FGAIBase::_getName() { + return _name.c_str(); +} + +const char* FGAIBase::_getCallsign() { + return _callsign.c_str(); +} + +const char* FGAIBase::_getSubmodel() { + return _submodel.c_str(); +} + + void FGAIBase::CalculateMach() { // Calculate rho at altitude, using standard atmosphere // For the temperature T and the pressure p, double altitude = altitude_ft; if (altitude < 36152) { // curve fits for the troposphere - T = 59 - 0.00356 * altitude; - p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256); + T = 59 - 0.00356 * altitude; + p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256); } else if ( 36152 < altitude && altitude < 82345 ) { // lower stratosphere - T = -70; - p = 473.1 * pow( e , 1.73 - (0.000048 * altitude) ); + T = -70; + p = 473.1 * pow( e , 1.73 - (0.000048 * altitude) ); } else { // upper stratosphere - T = -205.05 + (0.00164 * altitude); - p = 51.97 * pow( ((T + 459.7) / 389.98) , -11.388); + T = -205.05 + (0.00164 * altitude); + p = 51.97 * pow( ((T + 459.7) / 389.98) , -11.388); } rho = p / (1718 * (T + 459.7)); @@ -563,15 +627,15 @@ void FGAIBase::CalculateMach() { // calculate Mach number Mach = speed/a; -// cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach << endl; + // cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach << endl; } int FGAIBase::_newAIModelID() { static int id = 0; - if (!++id) - id++; // id = 0 is not allowed. + if (!++id) + id++; // id = 0 is not allowed. - return id; + return id; } diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index 0d99103d1..01102b4e6 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -33,6 +33,7 @@ #include
+ SG_USING_STD(string); SG_USING_STD(list); @@ -73,8 +74,14 @@ public: void setZoffset( double z_offset ); void setServiceable ( bool serviceable ); void setDie( bool die ); + void setCollisionData( bool i, double lat, double lon, double elev ); + void setImpactData( bool d ); + void setImpactLat( double lat ); + void setImpactLon( double lon ); + void setImpactElev( double e ); int getID() const; + int _getSubID() const; bool getDie(); @@ -86,9 +93,14 @@ public: double _getCartPosZ() const; string _path; + string _callsign; + string _submodel; + string _name; protected: + SGPropertyNode_ptr props; + SGPropertyNode_ptr trigger_node; SGPropertyNode_ptr model_removed; // where to report model removal FGAIManager* manager; @@ -137,11 +149,23 @@ protected: bool invisible; bool no_roll; bool serviceable; + int _subID; double life; FGAIFlightPlan *fp; + bool _impact_reported; + bool _collision_reported; + + double _impact_lat; + double _impact_lon; + double _impact_elev; + double _impact_hdg; + double _impact_pitch; + double _impact_roll; + double _impact_speed; + void Transform(); void CalculateMach(); double UpdateRadar(FGAIManager* manager); @@ -149,7 +173,7 @@ protected: static int _newAIModelID(); private: - const int _refID; + int _refID; object_type _otype; public: @@ -163,13 +187,14 @@ public: void _setAltitude( double _alt ); void _setLongitude( double longitude ); void _setLatitude ( double latitude ); + void _setSubID( int s ); double _getVS_fps() const; double _getAltitude() const; double _getLongitude() const; - double _getLatitude () const; + double _getLatitude() const; double _getBearing() const; - double _getElevation() const; + double _getElevationFt() const; double _getRdot() const; double _getH_offset() const; double _getV_offset() const; @@ -182,12 +207,31 @@ public: double _getHeading() const; double _get_speed_east_fps() const; double _get_speed_north_fps() const; + double _get_SubPath() const; + double _getImpactLat() const; + double _getImpactLon() const; + double _getImpactElevFt() const; + double _getImpactHdg() const; + double _getImpactPitch() const; + double _getImpactRoll() const; + double _getImpactSpeed() const; + + //unsigned int _getCount() const; bool _getServiceable() const; + bool _getFirstTime() const; + bool _getImpact(); + bool _getImpactData(); + bool _getCollisionData(); + SGPropertyNode* _getProps() const; const char* _getPath(); const char* _getCallsign(); + const char* _getTriggerNode(); + const char* _getName(); + const char* _getSubmodel(); + // These are used in the Mach number calculations @@ -226,7 +270,6 @@ inline void FGAIBase::setServiceable(bool s) { serviceable = s; } - inline void FGAIBase::setSpeed( double speed_KTAS ) { speed = tgt_speed = speed_KTAS; } @@ -256,6 +299,7 @@ inline void FGAIBase::setPitch( double newpitch ) { inline void FGAIBase::setLongitude( double longitude ) { pos.setLongitudeDeg( longitude ); } + inline void FGAIBase::setLatitude ( double latitude ) { pos.setLatitudeDeg( latitude ); } diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index e1dce1a04..effbf63b6 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -33,302 +33,395 @@ #include "AIStatic.hxx" #include "AIMultiplayer.hxx" +#include + + FGAIManager::FGAIManager() { - _dt = 0.0; - mNumAiModels = 0; - for (unsigned i = 0; i < FGAIBase::MAX_OBJECTS; ++i) - mNumAiTypeModels[i] = 0; + _dt = 0.0; + mNumAiModels = 0; + + for (unsigned i = 0; i < FGAIBase::MAX_OBJECTS; ++i) + mNumAiTypeModels[i] = 0; } FGAIManager::~FGAIManager() { - ai_list_iterator ai_list_itr = ai_list.begin(); - while(ai_list_itr != ai_list.end()) { - (*ai_list_itr)->unbind(); - ++ai_list_itr; - } + ai_list_iterator ai_list_itr = ai_list.begin(); + + while(ai_list_itr != ai_list.end()) { + (*ai_list_itr)->unbind(); + ++ai_list_itr; + } } +void +FGAIManager::init() { + root = fgGetNode("sim/ai", true); -void FGAIManager::init() { - root = fgGetNode("sim/ai", true); + enabled = root->getNode("enabled", true)->getBoolValue(); - enabled = root->getNode("enabled", true)->getBoolValue(); - if (!enabled) - return; + if (!enabled) + return; - wind_from_down_node = fgGetNode("/environment/wind-from-down-fps", true); - wind_from_east_node = fgGetNode("/environment/wind-from-east-fps",true); - wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true); + wind_from_down_node = fgGetNode("/environment/wind-from-down-fps", true); + wind_from_east_node = fgGetNode("/environment/wind-from-east-fps",true); + wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true); - user_latitude_node = fgGetNode("/position/latitude-deg", true); - user_longitude_node = fgGetNode("/position/longitude-deg", true); - user_altitude_node = fgGetNode("/position/altitude-ft", true); - user_heading_node = fgGetNode("/orientation/heading-deg", true); - user_pitch_node = fgGetNode("/orientation/pitch-deg", true); - user_yaw_node = fgGetNode("/orientation/side-slip-deg", true); - user_speed_node = fgGetNode("/velocities/uBody-fps", true); + user_latitude_node = fgGetNode("/position/latitude-deg", true); + user_longitude_node = fgGetNode("/position/longitude-deg", true); + user_altitude_node = fgGetNode("/position/altitude-ft", true); + user_heading_node = fgGetNode("/orientation/heading-deg", true); + user_pitch_node = fgGetNode("/orientation/pitch-deg", true); + user_yaw_node = fgGetNode("/orientation/side-slip-deg", true); + user_speed_node = fgGetNode("/velocities/uBody-fps", true); } - -void FGAIManager::postinit() { - // postinit, so that it can access the Nasal subsystem - for(int i = 0 ; i < root->nChildren() ; i++) { - SGPropertyNode *aiEntry = root->getChild( i ); - if( !strcmp( aiEntry->getName(), "scenario" ) ) { - scenario_filename = aiEntry->getStringValue(); - if (!scenario_filename.empty()) - processScenario( scenario_filename ); +void +FGAIManager::postinit() { + // postinit, so that it can access the Nasal subsystem + for(int i = 0 ; i < root->nChildren() ; i++) { + SGPropertyNode *aiEntry = root->getChild( i ); + if( !strcmp( aiEntry->getName(), "scenario" ) ) { + scenario_filename = aiEntry->getStringValue(); + if (!scenario_filename.empty()) + processScenario( scenario_filename ); + } } - } } +void +FGAIManager::reinit() { + update(0.0); + ai_list_iterator ai_list_itr = ai_list.begin(); -void FGAIManager::reinit() { - update(0.0); - ai_list_iterator ai_list_itr = ai_list.begin(); - - while(ai_list_itr != ai_list.end()) { - (*ai_list_itr)->reinit(); - ++ai_list_itr; - } + while(ai_list_itr != ai_list.end()) { + (*ai_list_itr)->reinit(); + ++ai_list_itr; + } } - -void FGAIManager::bind() { - root = globals->get_props()->getNode("ai/models", true); - root->tie("count", SGRawValueMethods(*this, - &FGAIManager::getNumAiObjects)); +void +FGAIManager::bind() { + root = globals->get_props()->getNode("ai/models", true); + root->tie("count", SGRawValueMethods(*this, + &FGAIManager::getNumAiObjects)); } - -void FGAIManager::unbind() { +void +FGAIManager::unbind() { root->untie("count"); } - -void FGAIManager::update(double dt) { - // initialize these for finding nearest thermals - range_nearest = 10000.0; - strength = 0.0; - if (!enabled) - return; - - FGTrafficManager *tmgr = (FGTrafficManager*) globals->get_subsystem("Traffic Manager"); - _dt = dt; - - ai_list_iterator ai_list_itr = ai_list.begin(); - while(ai_list_itr != ai_list.end()) { - if ((*ai_list_itr)->getDie()) { - tmgr->release((*ai_list_itr)->getID()); - --mNumAiModels; - --(mNumAiTypeModels[(*ai_list_itr)->getType()]); - FGAIBase *base = *ai_list_itr; - SGPropertyNode *props = base->_getProps(); - - props->setBoolValue("valid", false); - base->unbind(); - - // for backward compatibility reset properties, so that aircraft, - // which don't know the property, keep working - // TODO: remove after a while - props->setIntValue("id", -1); - props->setBoolValue("radar/in-range", false); - props->setIntValue("refuel/tanker", false); - - ai_list_itr = ai_list.erase(ai_list_itr); - } else { - fetchUserState(); - if ((*ai_list_itr)->isa(FGAIBase::otThermal)) { - FGAIBase *base = *ai_list_itr; - processThermal((FGAIThermal*)base); - } else { - (*ai_list_itr)->update(_dt); - } - ++ai_list_itr; +void +FGAIManager::update(double dt) { + // initialize these for finding nearest thermals + range_nearest = 10000.0; + strength = 0.0; + + if (!enabled) + return; + + FGTrafficManager *tmgr = (FGTrafficManager*) globals->get_subsystem("Traffic Manager"); + _dt = dt; + + ai_list_iterator ai_list_itr = ai_list.begin(); + + while(ai_list_itr != ai_list.end()) { + + if ((*ai_list_itr)->getDie()) { + tmgr->release((*ai_list_itr)->getID()); + --mNumAiModels; + --(mNumAiTypeModels[(*ai_list_itr)->getType()]); + FGAIBase *base = *ai_list_itr; + SGPropertyNode *props = base->_getProps(); + + props->setBoolValue("valid", false); + base->unbind(); + + // for backward compatibility reset properties, so that aircraft, + // which don't know the property, keep working + // TODO: remove after a while + props->setIntValue("id", -1); + props->setBoolValue("radar/in-range", false); + props->setIntValue("refuel/tanker", false); + + ai_list_itr = ai_list.erase(ai_list_itr); + } else { + fetchUserState(); + if ((*ai_list_itr)->isa(FGAIBase::otThermal)) { + FGAIBase *base = *ai_list_itr; + processThermal((FGAIThermal*)base); + } else { + (*ai_list_itr)->update(_dt); + } + ++ai_list_itr; + } } - } - wind_from_down_node->setDoubleValue( strength ); // for thermals + + wind_from_down_node->setDoubleValue( strength ); // for thermals } void FGAIManager::attach(SGSharedPtr model) { - unsigned idx = mNumAiTypeModels[model->getType()]; - const char* typeString = model->getTypeString(); - SGPropertyNode* root = globals->get_props()->getNode("ai/models", true); - SGPropertyNode* p; - int i; - for (i=0;i<10000;i++) //find free index in the property tree, if we have - //more than 10000 mp-aircrafts in the property tree we should optimize the mp-server - { - p = root->getNode(typeString, i, false); - if (!p || !p->getBoolValue("valid", false)) - break; - if (p->getIntValue("id",-1)==model->getID()) { - p->setStringValue("callsign","***invalid node***"); //debug only, should never set! + //unsigned idx = mNumAiTypeModels[model->getType()]; + const char* typeString = model->getTypeString(); + SGPropertyNode* root = globals->get_props()->getNode("ai/models", true); + SGPropertyNode* p; + int i; + + // find free index in the property tree, if we have + // more than 10000 mp-aircrafts in the property tree we should optimize the mp-server + for (i = 0; i < 10000; i++) { + p = root->getNode(typeString, i, false); + + if (!p || !p->getBoolValue("valid", false)) + break; + + if (p->getIntValue("id",-1)==model->getID()) { + p->setStringValue("callsign","***invalid node***"); //debug only, should never set! + } } - } - p = root->getNode(typeString, i, true); - model->setManager(this, p); - ai_list.push_back(model); - ++mNumAiModels; - ++(mNumAiTypeModels[model->getType()]); - model->init(model->getType()==FGAIBase::otAircraft - || model->getType()==FGAIBase::otMultiplayer - || model->getType()==FGAIBase::otStatic); - model->bind(); - p->setBoolValue("valid", true); + + p = root->getNode(typeString, i, true); + model->setManager(this, p); + ai_list.push_back(model); + ++mNumAiModels; + ++(mNumAiTypeModels[model->getType()]); + model->init(model->getType()==FGAIBase::otAircraft + || model->getType()==FGAIBase::otMultiplayer + || model->getType()==FGAIBase::otStatic); + model->bind(); + p->setBoolValue("valid", true); } +void +FGAIManager::destroyObject( int ID ) { + ai_list_iterator ai_list_itr = ai_list.begin(); + + while(ai_list_itr != ai_list.end()) { + + if ((*ai_list_itr)->getID() == ID) { + --mNumAiModels; + --(mNumAiTypeModels[(*ai_list_itr)->getType()]); + (*ai_list_itr)->unbind(); + ai_list_itr = ai_list.erase(ai_list_itr); + } else + ++ai_list_itr; + } -void FGAIManager::destroyObject( int ID ) { - ai_list_iterator ai_list_itr = ai_list.begin(); - while(ai_list_itr != ai_list.end()) { - if ((*ai_list_itr)->getID() == ID) { - --mNumAiModels; - --(mNumAiTypeModels[(*ai_list_itr)->getType()]); - (*ai_list_itr)->unbind(); - ai_list_itr = ai_list.erase(ai_list_itr); - } else - ++ai_list_itr; - } } int FGAIManager::getNumAiObjects(void) const { - return mNumAiModels; + return mNumAiModels; } -void FGAIManager::fetchUserState( void ) { - user_latitude = user_latitude_node->getDoubleValue(); - user_longitude = user_longitude_node->getDoubleValue(); - user_altitude = user_altitude_node->getDoubleValue(); - user_heading = user_heading_node->getDoubleValue(); - user_pitch = user_pitch_node->getDoubleValue(); - user_yaw = user_yaw_node->getDoubleValue(); - user_speed = user_speed_node->getDoubleValue() * 0.592484; - wind_from_east = wind_from_east_node->getDoubleValue(); - wind_from_north = wind_from_north_node->getDoubleValue(); +void +FGAIManager::fetchUserState( void ) { + user_latitude = user_latitude_node->getDoubleValue(); + user_longitude = user_longitude_node->getDoubleValue(); + user_altitude = user_altitude_node->getDoubleValue(); + user_heading = user_heading_node->getDoubleValue(); + user_pitch = user_pitch_node->getDoubleValue(); + user_yaw = user_yaw_node->getDoubleValue(); + user_speed = user_speed_node->getDoubleValue() * 0.592484; + wind_from_east = wind_from_east_node->getDoubleValue(); + wind_from_north = wind_from_north_node->getDoubleValue(); } - // only keep the results from the nearest thermal -void FGAIManager::processThermal( FGAIThermal* thermal ) { - thermal->update(_dt); - if ( thermal->_getRange() < range_nearest ) { - range_nearest = thermal->_getRange(); - strength = thermal->getStrength(); - } +void +FGAIManager::processThermal( FGAIThermal* thermal ) { + thermal->update(_dt); + + if ( thermal->_getRange() < range_nearest ) { + range_nearest = thermal->_getRange(); + strength = thermal->getStrength(); + } + } +void +FGAIManager::processScenario( const string &filename ) { + + SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename); + + if (!scenarioTop) + return; + + SGPropertyNode* scenarios = scenarioTop->getChild("scenario"); + + if (!scenarios) + return; + + for (int i = 0; i < scenarios->nChildren(); i++) { + SGPropertyNode* scEntry = scenarios->getChild(i); + + if (strcmp(scEntry->getName(), "entry")) + continue; + std::string type = scEntry->getStringValue("type", "aircraft"); + + if (type == "aircraft") { + FGAIAircraft* aircraft = new FGAIAircraft; + aircraft->readFromScenario(scEntry); + attach(aircraft); + + } else if (type == "ship") { + FGAIShip* ship = new FGAIShip; + ship->readFromScenario(scEntry); + attach(ship); + + } else if (type == "carrier") { + FGAICarrier* carrier = new FGAICarrier; + carrier->readFromScenario(scEntry); + attach(carrier); + + } else if (type == "thunderstorm") { + FGAIStorm* storm = new FGAIStorm; + storm->readFromScenario(scEntry); + attach(storm); + + } else if (type == "thermal") { + FGAIThermal* thermal = new FGAIThermal; + thermal->readFromScenario(scEntry); + attach(thermal); + + } else if (type == "ballistic") { + FGAIBallistic* ballistic = new FGAIBallistic; + ballistic->readFromScenario(scEntry); + attach(ballistic); + + } else if (type == "static") { + FGAIStatic* aistatic = new FGAIStatic; + aistatic->readFromScenario(scEntry); + attach(aistatic); -void FGAIManager::processScenario( const string &filename ) { - - SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename); - if (!scenarioTop) - return; - SGPropertyNode* scenarios = scenarioTop->getChild("scenario"); - if (!scenarios) - return; - - for (int i = 0; i < scenarios->nChildren(); i++) { - SGPropertyNode* scEntry = scenarios->getChild(i); - if (strcmp(scEntry->getName(), "entry")) - continue; - std::string type = scEntry->getStringValue("type", "aircraft"); - - if (type == "aircraft") { - FGAIAircraft* aircraft = new FGAIAircraft; - aircraft->readFromScenario(scEntry); - attach(aircraft); - - } else if (type == "ship") { - FGAIShip* ship = new FGAIShip; - ship->readFromScenario(scEntry); - attach(ship); - - } else if (type == "carrier") { - FGAICarrier* carrier = new FGAICarrier; - carrier->readFromScenario(scEntry); - attach(carrier); - - } else if (type == "thunderstorm") { - FGAIStorm* storm = new FGAIStorm; - storm->readFromScenario(scEntry); - attach(storm); - - } else if (type == "thermal") { - FGAIThermal* thermal = new FGAIThermal; - thermal->readFromScenario(scEntry); - attach(thermal); - - } else if (type == "ballistic") { - FGAIBallistic* ballistic = new FGAIBallistic; - ballistic->readFromScenario(scEntry); - attach(ballistic); - - } else if (type == "static") { - FGAIStatic* aistatic = new FGAIStatic; - aistatic->readFromScenario(scEntry); - attach(aistatic); - + } } - } } SGPropertyNode_ptr FGAIManager::loadScenarioFile(const std::string& filename) { - SGPath path(globals->get_fg_root()); - path.append("AI/" + filename + ".xml"); - try { - SGPropertyNode_ptr root = new SGPropertyNode; - readProperties(path.str(), root); - return root; - } catch (const sg_exception &e) { - SG_LOG(SG_GENERAL, SG_ALERT, "Incorrect path specified for AI " - "scenario: \"" << path.str() << "\""); - return 0; - } + SGPath path(globals->get_fg_root()); + path.append("AI/" + filename + ".xml"); + try { + SGPropertyNode_ptr root = new SGPropertyNode; + readProperties(path.str(), root); + return root; + } catch (const sg_exception &e) { + SG_LOG(SG_GENERAL, SG_DEBUG, "Incorrect path specified for AI " + "scenario: \"" << path.str() << "\""); + return 0; + } } -bool FGAIManager::getStartPosition(const string& id, const string& pid, - SGGeod& geodPos, double& hdng, SGVec3d& uvw) +bool +FGAIManager::getStartPosition(const string& id, const string& pid, + SGGeod& geodPos, double& hdng, SGVec3d& uvw) { - bool found = false; - SGPropertyNode* root = fgGetNode("sim/ai", true); - if (!root->getNode("enabled", true)->getBoolValue()) - return found; - - for(int i = 0 ; (!found) && i < root->nChildren() ; i++) { - SGPropertyNode *aiEntry = root->getChild( i ); - if( !strcmp( aiEntry->getName(), "scenario" ) ) { - string filename = aiEntry->getStringValue(); - SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename); - if (scenarioTop) { - SGPropertyNode* scenarios = scenarioTop->getChild("scenario"); - if (scenarios) { - for (int i = 0; i < scenarios->nChildren(); i++) { - SGPropertyNode* scEntry = scenarios->getChild(i); - std::string type = scEntry->getStringValue("type"); - std::string pnumber = scEntry->getStringValue("pennant-number"); - std::string name = scEntry->getStringValue("name"); - if (type == "carrier" && (pnumber == id || name == id)) { - SGSharedPtr carrier = new FGAICarrier; - carrier->readFromScenario(scEntry); - - if (carrier->getParkPosition(pid, geodPos, hdng, uvw)) { - found = true; - break; - } + bool found = false; + SGPropertyNode* root = fgGetNode("sim/ai", true); + if (!root->getNode("enabled", true)->getBoolValue()) + return found; + + for (int i = 0 ; (!found) && i < root->nChildren() ; i++) { + SGPropertyNode *aiEntry = root->getChild( i ); + if ( !strcmp( aiEntry->getName(), "scenario" ) ) { + string filename = aiEntry->getStringValue(); + SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename); + if (scenarioTop) { + SGPropertyNode* scenarios = scenarioTop->getChild("scenario"); + if (scenarios) { + for (int i = 0; i < scenarios->nChildren(); i++) { + SGPropertyNode* scEntry = scenarios->getChild(i); + std::string type = scEntry->getStringValue("type"); + std::string pnumber = scEntry->getStringValue("pennant-number"); + std::string name = scEntry->getStringValue("name"); + if (type == "carrier" && (pnumber == id || name == id)) { + SGSharedPtr carrier = new FGAICarrier; + carrier->readFromScenario(scEntry); + + if (carrier->getParkPosition(pid, geodPos, hdng, uvw)) { + found = true; + break; + } + } + } + } } - } } - } } - } - return found; + return found; +} + +const FGAIBase * +FGAIManager::calcCollision(double alt, double lat, double lon, double fuse_range) +{ + // we specify tgt extent (ft) according to the AIObject type + double tgt_ht[] = {0, 50 ,100, 250, 0, 100, 0, 0, 50, 50}; + double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100}; + + ai_list_iterator ai_list_itr = ai_list.begin(); + ai_list_iterator end = ai_list.end(); + + while (ai_list_itr != end) { + double tgt_alt = (*ai_list_itr)->_getAltitude(); + int type = (*ai_list_itr)->getType(); + tgt_ht[type] += fuse_range; + + if (fabs(tgt_alt - alt) > tgt_ht[type] || type == FGAIBase::otBallistic + || type == FGAIBase::otStorm || type == FGAIBase::otThermal) { + SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: skipping " + << fabs(tgt_alt - alt) + << " " + << type + ); + ++ai_list_itr; + continue; + } + + double tgt_lat = (*ai_list_itr)->_getLatitude(); + double tgt_lon = (*ai_list_itr)->_getLongitude(); + int id = (*ai_list_itr)->getID(); + + double range = calcRange(lat, lon, tgt_lat, tgt_lon); + + SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: AI list size " + << ai_list.size() + << " type " << type + << " ID " << id + << " range " << range + //<< " bearing " << bearing + << " alt " << tgt_alt + ); + + tgt_length[type] += fuse_range; + + if (range < tgt_length[type]){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: HIT! " + << " type " << type + << " ID " << id + << " range " << range + << " alt " << tgt_alt + ); + return *ai_list_itr; + } + ++ai_list_itr; + } + return 0; +} + +double +FGAIManager::calcRange(double lat, double lon, double lat2, double lon2) const +{ + double course, az2, distance; + + //calculate the bearing and range of the second pos from the first + geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance); + distance *= SG_METER_TO_FEET; + return distance; } //end AIManager.cxx diff --git a/src/AIModel/AIManager.hxx b/src/AIModel/AIManager.hxx index 4f4392044..a327421ef 100644 --- a/src/AIModel/AIManager.hxx +++ b/src/AIModel/AIManager.hxx @@ -52,7 +52,10 @@ public: ai_list_type ai_list; - inline const list >& get_ai_list() const { return ai_list; } + inline const list >& get_ai_list() const { + SG_LOG(SG_GENERAL, SG_DEBUG, "AI Manager: AI model return list size " << ai_list.size()); + return ai_list; + } FGAIManager(); ~FGAIManager(); @@ -63,10 +66,10 @@ public: void bind(); void unbind(); void update(double dt); - void attach(SGSharedPtr model); void destroyObject( int ID ); + const FGAIBase *calcCollision(double alt, double lat, double lon, double fuse_range); inline double get_user_latitude() const { return user_latitude; } inline double get_user_longitude() const { return user_longitude; } @@ -93,6 +96,8 @@ private: int mNumAiTypeModels[FGAIBase::MAX_OBJECTS]; int mNumAiModels; + double calcRange(double lat, double lon, double lat2, double lon2)const; + SGPropertyNode_ptr root; SGPropertyNode_ptr wind_from_down_node; SGPropertyNode_ptr user_latitude_node; @@ -102,8 +107,8 @@ private: SGPropertyNode_ptr user_pitch_node; SGPropertyNode_ptr user_yaw_node; SGPropertyNode_ptr user_speed_node; - SGPropertyNode_ptr wind_from_east_node ; - SGPropertyNode_ptr wind_from_north_node ; + SGPropertyNode_ptr wind_from_east_node; + SGPropertyNode_ptr wind_from_north_node; string scenario_filename; @@ -117,6 +122,7 @@ private: double wind_from_east; double wind_from_north; double _dt; + void fetchUserState( void ); // used by thermals diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index 20e335281..035ced0d6 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -17,6 +17,7 @@ #include
#include
+ #include "AIBase.hxx" #include "AIManager.hxx" #include "AIBallistic.hxx" @@ -35,10 +36,12 @@ FGSubmodelMgr::FGSubmodelMgr() in[3] = out[3] = 1; string contents_node; contrail_altitude = 30000; + _count = 0; } FGSubmodelMgr::~FGSubmodelMgr() -{} +{ +} void FGSubmodelMgr::init() { @@ -79,6 +82,10 @@ void FGSubmodelMgr::init() void FGSubmodelMgr::postinit() { // postinit, so that the AI list is populated loadAI(); + loadSubmodels(); + + //TODO reload submodels if an MP ac joins + } void FGSubmodelMgr::bind() @@ -87,7 +94,6 @@ void FGSubmodelMgr::bind() void FGSubmodelMgr::unbind() { submodel_iterator = submodels.begin(); - while (submodel_iterator != submodels.end()) { (*submodel_iterator)->prop->untie("count"); ++submodel_iterator; @@ -97,31 +103,89 @@ void FGSubmodelMgr::unbind() void FGSubmodelMgr::update(double dt) { - if (!(_serviceable_node->getBoolValue())) + if (!_serviceable_node->getBoolValue()) return; int i = -1; bool in_range = true; bool trigger = false; + _impact = false; + _hit = false; + + sm_list = ai->get_ai_list(); + + sm_list_iterator sm_list_itr = sm_list.begin(); + sm_list_iterator end = sm_list.end(); + + submodel_iterator = submodels.begin(); + + while (sm_list_itr != end) { + _impact = (*sm_list_itr)->_getImpactData(); + _hit = (*sm_list_itr)->_getCollisionData(); + int parent_subID = (*sm_list_itr)->_getSubID(); + + if ( parent_subID == 0) { // this entry in the list has no associated submodel + ++sm_list_itr; // so we can continue + continue; + } + + if (_impact || _hit) { + SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit ); + + submodel_iterator = submodels.begin(); + + while (submodel_iterator != submodels.end()) { + int child_ID = (*submodel_iterator)->id; + cout << "Impact: parent SubID " << parent_subID << " child_ID " << child_ID << endl; + + if ( parent_subID == child_ID ) { + _parent_lat = (*sm_list_itr)->_getImpactLat(); + _parent_lon = (*sm_list_itr)->_getImpactLon(); + _parent_elev = (*sm_list_itr)->_getImpactElevFt(); + _parent_hdg = (*sm_list_itr)->_getImpactHdg(); + _parent_pitch = (*sm_list_itr)->_getImpactPitch(); + _parent_roll = (*sm_list_itr)->_getImpactRoll(); + _parent_speed = (*sm_list_itr)->_getImpactSpeed(); + (*submodel_iterator)->first_time = true; + + if (release(submodel_iterator, dt)) { + (*sm_list_itr)->setDie(true); + } + + } + + ++submodel_iterator; + } + } + + ++sm_list_itr; + } _contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude); submodel_iterator = submodels.begin(); - while (submodel_iterator != submodels.end()) { + while (submodel_iterator != submodels.end()) { i++; + in_range = true; if ((*submodel_iterator)->trigger_node != 0) { - trigger = (*submodel_iterator)->trigger_node->getBoolValue(); - //cout << (*submodel_iterator)->name << "trigger node found" << trigger << endl; + _trigger_node = (*submodel_iterator)->trigger_node; + trigger = _trigger_node->getBoolValue(); + //cout << "trigger node found " << trigger << endl; } else { trigger = true; - //cout << (*submodel_iterator)->name << "trigger node not found" << trigger << endl; + //cout << (*submodel_iterator)->name << "trigger node not found " << trigger << endl; } + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels: " << (*submodel_iterator)->id + << " name " << (*submodel_iterator)->name + << " in range " << in_range + ); if (trigger) { int id = (*submodel_iterator)->id; - + string name = (*submodel_iterator)->name; // don't release submodels from AI Objects if they are // too far away to be seen. id 0 is not an AI model, // so we can skip the whole process @@ -132,12 +196,13 @@ void FGSubmodelMgr::update(double dt) if (id == 0) { SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels: continuing: " << id); + "Submodels: continuing: " << id << " name " << name ); + in_range = true; ++sm_list_itr; continue; } - int parent_id = (*sm_list_itr)->getID(); + int parent_id = (*submodel_iterator)->id; if (parent_id == id) { double parent_lat = (*sm_list_itr)->_getLatitude(); @@ -145,8 +210,8 @@ void FGSubmodelMgr::update(double dt) double own_lat = _user_lat_node->getDoubleValue(); double own_lon = _user_lon_node->getDoubleValue(); double range_nm = getRange(parent_lat, parent_lon, own_lat, own_lon); - /* cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl; - cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl;*/ + // cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl; + //cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl; if (range_nm > 15) { SG_LOG(SG_GENERAL, SG_DEBUG, @@ -159,8 +224,15 @@ void FGSubmodelMgr::update(double dt) ++sm_list_itr; } // end while + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels end: " << (*submodel_iterator)->id + << " name " << (*submodel_iterator)->name + << " count " << (*submodel_iterator)->count + << " in range " << in_range + ); + if ((*submodel_iterator)->count != 0 && in_range) - release((*submodel_iterator), dt); + release((submodel_iterator), dt); } else (*submodel_iterator)->first_time = true; @@ -170,28 +242,36 @@ void FGSubmodelMgr::update(double dt) } -bool FGSubmodelMgr::release(submodel* sm, double dt) +bool FGSubmodelMgr::release(submodel_vector_iterator sm, double dt) { + //cout << "release id " << (*sm)->id << " name " << (*sm)->name + //<< " first time " << (*sm)->first_time << " repeat " << (*sm)->repeat << + // endl; + // only run if first time or repeat is set to true - if (!sm->first_time && !sm->repeat) + if (!(*sm)->first_time && !(*sm)->repeat){ + //cout<< "not first time " << (*sm)->first_time<< " repeat " << (*sm)->repeat <timer += dt; + (*sm)->timer += dt; - if (sm->timer < sm->delay) + if ((*sm)->timer < (*sm)->delay){ + //cout << "not yet: timer" << (*sm)->timer << " delay " << (*sm)->delay<< endl; return false; + } - sm->timer = 0.0; + (*sm)->timer = 0.0; - if (sm->first_time) { + if ((*sm)->first_time) { dt = 0.0; - sm->first_time = false; + (*sm)->first_time = false; } transform(sm); // calculate submodel's initial conditions in world-coordinates FGAIBallistic* ballist = new FGAIBallistic; - ballist->setPath(sm->model.c_str()); + ballist->setPath((*sm)->model.c_str()); ballist->setLatitude(IC.lat); ballist->setLongitude(IC.lon); ballist->setAltitude(IC.alt); @@ -202,20 +282,24 @@ bool FGSubmodelMgr::release(submodel* sm, double dt) ballist->setWind_from_east(IC.wind_from_east); ballist->setWind_from_north(IC.wind_from_north); ballist->setMass(IC.mass); - ballist->setDragArea(sm->drag_area); - ballist->setLife(sm->life); - ballist->setBuoyancy(sm->buoyancy); - ballist->setWind(sm->wind); - ballist->setCd(sm->cd); - ballist->setStabilisation(sm->aero_stabilised); - ballist->setNoRoll(sm->no_roll); - ballist->setName(sm->name); - ballist->setImpact(sm->impact); - ballist->setImpactReportNode(sm->impact_reports); + ballist->setDragArea((*sm)->drag_area); + ballist->setLife((*sm)->life); + ballist->setBuoyancy((*sm)->buoyancy); + ballist->setWind((*sm)->wind); + ballist->setCd((*sm)->cd); + ballist->setStabilisation((*sm)->aero_stabilised); + ballist->setNoRoll((*sm)->no_roll); + ballist->setName((*sm)->name); + ballist->setCollision((*sm)->collision); + ballist->setImpact((*sm)->impact); + ballist->setImpactReportNode((*sm)->impact_report); + ballist->setFuseRange((*sm)->fuse_range); + ballist->setSubmodel((*sm)->submodel.c_str()); + ballist->setSubID((*sm)->sub_id); ai->attach(ballist); - if (sm->count > 0) - (sm->count)--; + if ((*sm)->count > 0) + (*sm)->count--; return true; } @@ -235,28 +319,58 @@ void FGSubmodelMgr::load() } -void FGSubmodelMgr::transform(submodel* sm) +void FGSubmodelMgr::transform(submodel_vector_iterator submodel) { - // get initial conditions - if (sm->contents_node != 0) { + // set initial conditions + if ((*submodel)->contents_node != 0) { // get the weight of the contents (lbs) and convert to mass (slugs) - sm->contents = sm->contents_node->getDoubleValue(); - IC.mass = (sm->weight + sm->contents) * lbs_to_slugs; + (*submodel)->contents = (*submodel)->contents_node->getDoubleValue(); + IC.mass = ((*submodel)->weight + (*submodel)->contents) * lbs_to_slugs; // set contents to 0 in the parent - sm->contents_node->setDoubleValue(0); + (*submodel)->contents_node->setDoubleValue(0); } else - IC.mass = sm->weight * lbs_to_slugs; - - //cout << "mass " << IC.mass << endl; - - if (sm->speed_node != 0) - sm->speed = sm->speed_node->getDoubleValue(); - - int ind = sm->id; - - if (ind == 0) { - // set the data for a submodel tied to the main model + IC.mass = (*submodel)->weight * lbs_to_slugs; + + // cout << "mass " << IC.mass << endl; + + if ((*submodel)->speed_node != 0) + (*submodel)->speed = (*submodel)->speed_node->getDoubleValue(); + + int id = (*submodel)->id; + //int sub_id = (*submodel)->sub_id; + string name = (*submodel)->name; + + //cout << " name " << name << " id " << id << " sub id" << sub_id << endl; + + if (_impact || _hit){ + // set the data for a submodel tied to a submodel + _count++; + //cout << "Submodels: release sub sub " << _count<< endl; + //cout << " id " << (*submodel)->id + // << " lat " << _parent_lat + // << " lon " << _parent_lon + // << " elev " << _parent_elev + // << " name " << (*submodel)->name + // << endl; + + IC.lat = _parent_lat; + IC.lon = _parent_lon; + IC.alt = _parent_elev; + IC.roll = _parent_roll; // rotation about x axis + IC.elevation = _parent_pitch; // rotation about y axis + IC.azimuth = _parent_hdg; // rotation about z axis + IC.speed = _parent_speed; + IC.speed_down_fps = 0; + IC.speed_east_fps = 0; + IC.speed_north_fps = 0; + + } else if (id == 0) { + //set the data for a submodel tied to the main model + /*cout << "Submodels: release main sub " << endl; + cout << " name " << (*submodel)->name + << " id" << (*submodel)->id + << endl;*/ IC.lat = _user_lat_node->getDoubleValue(); IC.lon = _user_lon_node->getDoubleValue(); IC.alt = _user_alt_node->getDoubleValue(); @@ -274,9 +388,9 @@ void FGSubmodelMgr::transform(submodel* sm) sm_list_iterator end = sm_list.end(); while (sm_list_itr != end) { - int id = (*sm_list_itr)->getID(); + int parent_id = (*sm_list_itr)->getID(); - if (ind != id) { + if (id != parent_id) { ++sm_list_itr; continue; } @@ -303,14 +417,14 @@ void FGSubmodelMgr::transform(submodel* sm) 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 << "parent speed fps in" << IC.speed << "sm speed in " << (*submodel).speed << endl ;*/ IC.wind_from_east = _user_wind_from_east_node->getDoubleValue(); IC.wind_from_north = _user_wind_from_north_node->getDoubleValue(); - in[0] = sm->x_offset; - in[1] = sm->y_offset; - in[2] = sm->z_offset; + in[0] = (*submodel)->x_offset; + in[1] = (*submodel)->y_offset; + in[2] = (*submodel)->z_offset; // pre-process the trig functions cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS); @@ -357,43 +471,44 @@ void FGSubmodelMgr::transform(submodel* sm) // Get submodel initial velocity vector angles in XZ and XY planes. // This needs to be fixed. This vector should be added to aircraft's vector. - IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx); - IC.azimuth += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx); + IC.elevation += ((*submodel)->yaw_offset * sinRx) + ((*submodel)->pitch_offset * cosRx); + IC.azimuth += ((*submodel)->yaw_offset * cosRx) - ((*submodel)->pitch_offset * sinRx); - // calcuate the total speed north - IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) - * cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps; + // calculate the total speed north + IC.total_speed_north = (*submodel)->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) + * cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps; // calculate the total speed east - IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) - * sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps; + IC.total_speed_east = (*submodel)->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) + * sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps; // calculate the total speed down - IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS) - + IC.speed_down_fps; + IC.total_speed_down = (*submodel)->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS) + + IC.speed_down_fps; // re-calculate speed, elevation and azimuth IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north + IC.total_speed_east * IC.total_speed_east + IC.total_speed_down * IC.total_speed_down); - //cout << " speed fps out" << IC.speed << endl ; - IC.azimuth = atan(IC.total_speed_east / IC.total_speed_north) * SG_RADIANS_TO_DEGREES; - - // rationalise the output - if (IC.total_speed_north <= 0) { - IC.azimuth = 180 + IC.azimuth; - } else { + // 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<= 360) + IC.azimuth -= 360; } - 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 << "azimuth2 " << IC.azimuth<get_fg_root()); config.append(path); - + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels: path " << path); try { SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Trying to read AI submodels file: " << config.str()); @@ -489,10 +604,13 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->weight = entry_node->getDoubleValue("weight", 0.25); sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true); sm->no_roll = entry_node->getBoolValue("no-roll", false); + sm->collision = entry_node->getBoolValue("collision", false); sm->impact = entry_node->getBoolValue("impact", false); - sm->impact_reports = entry_node->getStringValue("impact-reports"); + sm->impact_report = entry_node->getStringValue("impact-reports"); + sm->fuse_range = entry_node->getDoubleValue("fuse-range", 0.0); sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false); sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false); + sm->submodel = entry_node->getStringValue("submodel-path", ""); //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) @@ -513,15 +631,22 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->id = id; sm->first_time = false; sm->serviceable = serviceable; + sm->sub_id = 0; sm->prop = fgGetNode("/ai/submodels/submodel", index, true); sm->prop->tie("count", SGRawValuePointer(&(sm->count))); sm->prop->tie("repeat", SGRawValuePointer(&(sm->repeat))); sm->prop->tie("id", SGRawValuePointer(&(sm->id))); + sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); + sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); string name = sm->name; sm->prop->setStringValue("name", name.c_str()); + string submodel = sm->submodel; + sm->prop->setStringValue("submodel", submodel.c_str()); + //cout << " set submodel path " << submodel << endl; + if (sm->contents_node != 0) sm->prop->tie("contents-lbs", SGRawValuePointer(&(sm->contents))); @@ -529,8 +654,147 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) submodels.push_back(sm); } +} + +void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) +{ + SGPropertyNode root; + + SGPath config(globals->get_fg_root()); + config.append(path); + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels: path " << path); + try { + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels: Trying to read AI submodels file: " << config.str()); + readProperties(config.str(), &root); + + } catch (const sg_exception &e) { + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels: Unable to read AI submodels file: " << config.str()); + return; + } + + vector children = root.getChildren("submodel"); + vector::iterator it = children.begin(); + vector::iterator end = children.end(); + + for (int i = 0; it != end; ++it, i++) { + //cout << "Reading AI submodel " << (*it)->getPath() << endl; + submodel* sm = new submodel; + SGPropertyNode * entry_node = *it; + sm->name = entry_node->getStringValue("name", "none_defined"); + sm->model = entry_node->getStringValue("model", "Models/Geometry/rocket.ac"); + sm->speed = entry_node->getDoubleValue("speed", 2329.4); + sm->repeat = entry_node->getBoolValue("repeat", false); + sm->delay = entry_node->getDoubleValue("delay", 0.25); + 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->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); + sm->drag_area = entry_node->getDoubleValue("eda", 0.034); + sm->life = entry_node->getDoubleValue("life", 900.0); + sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0); + sm->wind = entry_node->getBoolValue("wind", false); + sm->cd = entry_node->getDoubleValue("cd", 0.193); + sm->weight = entry_node->getDoubleValue("weight", 0.25); + sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true); + sm->no_roll = entry_node->getBoolValue("no-roll", false); + sm->collision = entry_node->getBoolValue("collision", false); + sm->impact = entry_node->getBoolValue("impact", false); + sm->impact_report = entry_node->getStringValue("impact-reports"); + sm->fuse_range = entry_node->getDoubleValue("fuse-range", 0.0); + sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false); + sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false); + sm->submodel = entry_node->getStringValue("submodel-path", ""); + + //cout << "sm->contents_node " << sm->contents_node << endl; + if (sm->contents_node != 0) + sm->contents = sm->contents_node->getDoubleValue(); + + const char *trigger_path = entry_node->getStringValue("trigger", 0); + if (trigger_path) { + sm->trigger_node = fgGetNode(trigger_path, true); + sm->trigger_node->setBoolValue(sm->trigger_node->getBoolValue()); + } else { + sm->trigger_node = 0; + } + + if (sm->speed_node != 0) + sm->speed = sm->speed_node->getDoubleValue(); + + sm->timer = sm->delay; + sm->id = index; + sm->first_time = false; + sm->serviceable = serviceable; + sm->sub_id = 0; + + sm->prop = fgGetNode("/ai/submodels/subsubmodel", index, true); + sm->prop->tie("count", SGRawValuePointer(&(sm->count))); + sm->prop->tie("repeat", SGRawValuePointer(&(sm->repeat))); + sm->prop->tie("id", SGRawValuePointer(&(sm->id))); + sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); + sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); + string name = sm->name; + sm->prop->setStringValue("name", name.c_str()); + + string submodel = sm->submodel; + sm->prop->setStringValue("submodel", submodel.c_str()); + // cout << " set submodel path " << submodel<< endl; + + if (sm->contents_node != 0) + sm->prop->tie("contents-lbs", SGRawValuePointer(&(sm->contents))); + + index++; + subsubmodels.push_back(sm); + } + +} + +void FGSubmodelMgr::loadSubmodels() +{ + SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Loading sub submodels"); + submodel_iterator = submodels.begin(); + while (submodel_iterator != submodels.end()) { + string submodel = (*submodel_iterator)->submodel; + if (!submodel.empty()) { + //int id = (*submodel_iterator)->id; + bool serviceable = true; + SG_LOG(SG_GENERAL, SG_ALERT, "found path sub sub " + << submodel + << " index " << index + << "name " << (*submodel_iterator)->name); + + (*submodel_iterator)->sub_id = index; + setSubData(index, submodel, serviceable); + } + + ++submodel_iterator; + } + + subsubmodel_iterator = subsubmodels.begin(); + + while (subsubmodel_iterator != subsubmodels.end()) { + submodels.push_back(*subsubmodel_iterator); + ++subsubmodel_iterator; + } + + submodel_iterator = submodels.begin(); + + while (submodel_iterator != submodels.end()) { + int id = (*submodel_iterator)->id; + SG_LOG(SG_GENERAL, SG_DEBUG,"after pusback " + << " id " << id + << " name " << (*submodel_iterator)->name + << " sub id " << (*submodel_iterator)->sub_id); + + ++submodel_iterator; + } } // end of submodel.cxx diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index 7b28b176b..bbb3eaa02 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -16,8 +16,12 @@ #include #include #include + +#include
+ SG_USING_STD(vector); SG_USING_STD(string); +SG_USING_STD(list); class FGAIBase; @@ -59,8 +63,12 @@ public: int id; bool no_roll; bool serviceable; + bool collision; bool impact; - string impact_reports; + string impact_report; + double fuse_range; + string submodel; + int sub_id; } submodel; @@ -84,7 +92,6 @@ public: double mass; int id; bool no_roll; - bool impact; } IC_struct; @@ -97,8 +104,6 @@ public: void bind(); void unbind(); void update(double dt); - bool release(submodel* sm, double dt); - void transform(submodel* sm); void updatelat(double lat); private: @@ -107,7 +112,8 @@ private: typedef submodel_vector_type::const_iterator submodel_vector_iterator; submodel_vector_type submodels; - submodel_vector_iterator submodel_iterator; + submodel_vector_type subsubmodels; + submodel_vector_iterator submodel_iterator, subsubmodel_iterator; float trans[3][3]; float in[3]; @@ -129,10 +135,21 @@ private: double x_offset, y_offset, z_offset; double pitch_offset, yaw_offset; + double _parent_lat; + double _parent_lon; + double _parent_elev; + double _parent_hdg; + double _parent_pitch; + double _parent_roll; + double _parent_speed; + static const double lbs_to_slugs; //conversion factor double contrail_altitude; + bool _impact; + bool _hit; + SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _user_lat_node; SGPropertyNode_ptr _user_lon_node; @@ -150,6 +167,9 @@ private: SGPropertyNode_ptr _user_speed_north_fps_node; SGPropertyNode_ptr _contrail_altitude_node; SGPropertyNode_ptr _contrail_trigger; + SGPropertyNode_ptr _count_node; + SGPropertyNode_ptr _trigger_node; + SGPropertyNode_ptr props; FGAIManager* ai; IC_struct IC; @@ -161,11 +181,20 @@ private: sm_list_type sm_list; + void loadAI(); void loadSubmodels(); void setData(int id, string& path, bool serviceable); + void setSubData(int id, string& path, bool serviceable); + void valueChanged (SGPropertyNode *); + void transform(submodel_vector_iterator); + + bool release(submodel_vector_iterator, double dt); + double getRange(double lat, double lon, double lat2, double lon2) const; + int _count; + }; #endif // __SYSTEMS_SUBMODEL_HXX -- 2.39.5