From 72c5a314e6daa5052165310a600c4a9a4c20e43f Mon Sep 17 00:00:00 2001 From: Vivian Meazza Date: Sat, 14 Aug 2010 13:39:30 +0100 Subject: [PATCH 1/7] Add aubmodels to AI objects Signed-off-by: Vivian Meazza --- src/AIModel/AIBallistic.cxx | 177 ++++++++++----------- src/AIModel/AIBase.cxx | 14 +- src/AIModel/AIBase.hxx | 19 ++- src/AIModel/AIGroundVehicle.cxx | 2 +- src/AIModel/AIWingman.cxx | 43 +++++- src/AIModel/submodel.cxx | 228 ++++++++++++++++++---------- src/AIModel/submodel.hxx | 27 ++-- src/Environment/environment_mgr.cxx | 16 +- 8 files changed, 315 insertions(+), 211 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index 76e412c0c..e94844eaf 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -39,34 +39,34 @@ const double FGAIBallistic::slugs_to_kgs = 14.5939029372; const double FGAIBallistic::slugs_to_lbs = 32.1740485564; FGAIBallistic::FGAIBallistic(object_type ot) : - FGAIBase(ot), - _height(0.0), - _ht_agl_ft(0.0), - _azimuth(0.0), - _elevation(0.0), - _rotation(0.0), - _formate_to_ac(false), - _aero_stabilised(false), - _drag_area(0.007), - _life_timer(0.0), - _gravity(32.1740485564), - _buoyancy(0), - _wind(true), - _mass(0), - _random(false), - _load_resistance(0), - _solid(false), - _force_stabilised(false), - _slave_to_ac(false), - _slave_load_to_ac(false), - _contents_lb(0), - _report_collision(false), - _report_expiry(false), - _report_impact(false), - _external_force(false), - _impact_report_node(fgGetNode("/ai/models/model-impact", true)), - _old_height(0), - _elapsed_time(0) +FGAIBase(ot), +_height(0.0), +_ht_agl_ft(0.0), +_azimuth(0.0), +_elevation(0.0), +_rotation(0.0), +_formate_to_ac(false), +_aero_stabilised(false), +_drag_area(0.007), +_life_timer(0.0), +_gravity(32.1740485564), +_buoyancy(0), +_wind(true), +_mass(0), +_random(false), +_load_resistance(0), +_solid(false), +_force_stabilised(false), +_slave_to_ac(false), +_slave_load_to_ac(false), +_contents_lb(0), +_report_collision(false), +_report_expiry(false), +_report_impact(false), +_external_force(false), +_impact_report_node(fgGetNode("/ai/models/model-impact", true)), +_old_height(0), +_elapsed_time(0) { no_roll = false; @@ -83,7 +83,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { FGAIBase::readFromScenario(scFileNode); //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac")); - setRandom(scFileNode->getBoolValue("random", false)); + setRandom(scFileNode->getBoolValue("random", false)); setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0)); setElevation(scFileNode->getDoubleValue("elevation", 0)); setDragArea(scFileNode->getDoubleValue("eda", 0.007)); @@ -99,8 +99,8 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setStabilisation(scFileNode->getBoolValue("aero-stabilised", false)); setNoRoll(scFileNode->getBoolValue("no-roll", false)); setImpact(scFileNode->getBoolValue("impact", false)); - setExpiry(scFileNode->getBoolValue("expiry", false)); - setCollision(scFileNode->getBoolValue("collision", false)); + setExpiry(scFileNode->getBoolValue("expiry", false)); + setCollision(scFileNode->getBoolValue("collision", false)); setImpactReportNode(scFileNode->getStringValue("impact-reports")); setName(scFileNode->getStringValue("name", "Rocket")); setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0)); @@ -127,9 +127,9 @@ bool FGAIBallistic::init(bool search_in_AI_path) { _impact_reported = false; _collision_reported = false; - _expiry_reported = false; + _expiry_reported = false; - _impact_lat = 0; + _impact_lat = 0; _impact_lon = 0; _impact_elev = 0; _impact_hdg = 0; @@ -144,8 +144,8 @@ bool FGAIBallistic::init(bool search_in_AI_path) { props->setStringValue("material/name", ""); props->setStringValue("name", _name.c_str()); props->setStringValue("submodels/path", _submodel.c_str()); - props->setStringValue("force/path", _force_path.c_str()); - //props->setStringValue("vector/path", _vector_path.c_str()); + props->setStringValue("force/path", _force_path.c_str()); + //props->setStringValue("vector/path", _vector_path.c_str()); // start with high value so that animations don't trigger yet _ht_agl_ft = 1e10; @@ -168,11 +168,11 @@ void FGAIBallistic::bind() { SGRawValueMethods(*this, &FGAIBallistic::getMass)); props->tie("material/load-resistance", - SGRawValuePointer(&_load_resistance)); + SGRawValuePointer(&_load_resistance)); props->tie("material/solid", - SGRawValuePointer(&_solid)); + SGRawValuePointer(&_solid)); props->tie("altitude-agl-ft", - SGRawValuePointer(&_ht_agl_ft)); + SGRawValuePointer(&_ht_agl_ft)); props->tie("controls/slave-to-ac", SGRawValueMethods (*this, &FGAIBallistic::getSlaved, &FGAIBallistic::setSlaved)); @@ -271,20 +271,20 @@ void FGAIBallistic::update(double dt) { Transform(); setHitchVelocity(dt); } else if (!invisible){ - Run(dt); - Transform(); -} + Run(dt); + Transform(); + } } void FGAIBallistic::setAzimuth(double az) { - - if (_random) - hdg = _azimuth = (az - 5 ) + (10 * sg_random()); - else - hdg = _azimuth = az; - //cout << _name << " init hdg " << hdg << " random " << _random << endl; + if (_random) + hdg = _azimuth = (az - 5 ) + (10 * sg_random()); + else + hdg = _azimuth = az; + + //cout << _name << " init hdg " << hdg << " random " << _random << endl; } void FGAIBallistic::setElevation(double el) { @@ -313,11 +313,11 @@ void FGAIBallistic::setDragArea(double a) { void FGAIBallistic::setLife(double seconds) { - if (_random){ - life = seconds * _randomness + (seconds * (1 -_randomness) * sg_random()); - //cout << "life " << life << endl; - } else - life = seconds; + if (_random){ + life = seconds * _randomness + (seconds * (1 -_randomness) * sg_random()); + //cout << "life " << life << endl; + } else + life = seconds; } void FGAIBallistic::setBuoyancy(double fpss) { @@ -366,7 +366,7 @@ void FGAIBallistic::setCollision(bool c) { void FGAIBallistic::setExpiry(bool e) { _report_expiry = e; - //cout << "_report_expiry " << _report_expiry << endl; + //cout << "_report_expiry " << _report_expiry << endl; } void FGAIBallistic::setExternalForce(bool f) { @@ -465,7 +465,7 @@ void FGAIBallistic::setForcePath(const string& p) { bool FGAIBallistic::getHtAGL(){ if (getGroundElevationM(SGGeod::fromGeodM(pos, 10000), - _elevation_m, &_material)) { + _elevation_m, &_material)) { _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET; if (_material) { const vector& names = _material->get_names(); @@ -515,16 +515,16 @@ void FGAIBallistic::setHt(double h, double dt, double coeff){ } void FGAIBallistic::setHdg(double az, double dt, double coeff){ - double recip = getRecip(hdg); - double c = dt / (coeff + dt); - //we need to ensure that we turn the short way to the new hdg - if (az < recip && az < hdg && hdg > 180) { - hdg = ((az + 360) * c) + (hdg * (1 - c)); - } else if (az > recip && az > hdg && hdg <= 180){ - hdg = ((az - 360) * c) + (hdg * (1 - c)); - } else { - hdg = (az * c) + (hdg * (1 - c)); - } + double recip = getRecip(hdg); + double c = dt / (coeff + dt); + //we need to ensure that we turn the short way to the new hdg + if (az < recip && az < hdg && hdg > 180) { + hdg = ((az + 360) * c) + (hdg * (1 - c)); + } else if (az > recip && az > hdg && hdg <= 180){ + hdg = ((az - 360) * c) + (hdg * (1 - c)); + } else { + hdg = (az * c) + (hdg * (1 - c)); + } } double FGAIBallistic::getTgtXOffset() const { @@ -541,6 +541,7 @@ double FGAIBallistic::getTgtZOffset() const { void FGAIBallistic::setTgtXOffset(double x){ _tgt_x_offset = x; + cout <<"setTgtXOffset " <<_tgt_x_offset << endl; } void FGAIBallistic::setTgtYOffset(double y){ @@ -572,15 +573,15 @@ void FGAIBallistic::Run(double dt) { _life_timer += dt; // if life = -1 the object does not die - if (_life_timer > life && life != -1){ + if (_life_timer > life && life != -1){ - if (_report_expiry && !_expiry_reported){ - //cout<<"AIBallistic: expiry"<< endl; - handle_expiry(); - } else - setDie(true); + if (_report_expiry && !_expiry_reported){ + //cout<<"AIBallistic: expiry"<< endl; + handle_expiry(); + } else + setDie(true); - } + } //set the contents in the appropriate tank or other property in the parent to zero setContents(0); @@ -644,7 +645,7 @@ void FGAIBallistic::Run(double dt) { //calculate velocity due to external force double force_speed_north_deg_sec = 0; double force_speed_east_deg_sec = 0; -// double vs_force_fps = 0; + // double vs_force_fps = 0; double hs_force_fps = 0; double v_force_acc_fpss = 0; double force_speed_north_fps = 0; @@ -660,13 +661,13 @@ void FGAIBallistic::Run(double dt) { double friction_force_speed_east_deg_sec = 0; double force_elevation_deg = 0; - if (_external_force) { + if (_external_force) { SGPropertyNode *n = fgGetNode(_force_path.c_str(), true); double force_lbs = n->getChild("force-lb", 0, true)->getDoubleValue(); force_elevation_deg = n->getChild("force-elevation-deg", 0, true)->getDoubleValue(); double force_azimuth_deg = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue(); - + //resolve force into vertical and horizontal components: double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS ); h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS ); @@ -795,10 +796,10 @@ void FGAIBallistic::Run(double dt) { if (_azimuth < 0) _azimuth += 360; - //cout << "_azimuth " << _azimuth << " hdg "<< hdg << endl; + //cout << "_azimuth " << _azimuth << " hdg "<< hdg << endl; if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter - //cout<< "_aero_stabilised "<< endl; + //cout<< "_aero_stabilised "<< endl; const double coeff = 0.9; // we assume a symetrical MI about the pitch and yaw axis @@ -806,8 +807,8 @@ void FGAIBallistic::Run(double dt) { setHdg(_azimuth, dt, coeff); } else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter //cout<< "_force_stabilised "<< endl; - - const double coeff = 0.9; + + const double coeff = 0.9; double ratio = h_force_lbs/(_mass * slugs_to_lbs); if (ratio > 1) ratio = 1; @@ -859,23 +860,23 @@ void FGAIBallistic::handle_impact() { } void FGAIBallistic::handle_expiry() { - - SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: handle_expiry " << pos.getElevationM()); - report_impact(pos.getElevationM()); - _expiry_reported = true; + SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: handle_expiry " << pos.getElevationM()); + + report_impact(pos.getElevationM()); + _expiry_reported = true; + + //if (life == -1){ + // invisible = true; + //} else if (_subID == 0) // kill the AIObject if there is no subsubmodel + // setDie(true); - //if (life == -1){ - // invisible = true; - //} else if (_subID == 0) // kill the AIObject if there is no subsubmodel - // setDie(true); - } void FGAIBallistic::handle_collision() { const FGAIBase *object = manager->calcCollision(pos.getElevationFt(), - pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range); + pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range); if (object) { SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object hit"); diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index d143296a1..460ccc2e0 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -154,6 +154,9 @@ void FGAIBase::readFromScenario(SGPropertyNode* scFileNode) SGPropertyNode* submodels = scFileNode->getChild("submodels"); if (submodels) { + //cout << "IN submodels path " << submodels->getStringValue("path") + // << "IN serviceable " << submodels->getBoolValue("serviceable") + // << endl; setServiceable(submodels->getBoolValue("serviceable", false)); setSMPath(submodels->getStringValue("path", "")); } @@ -227,14 +230,17 @@ bool FGAIBase::init(bool search_in_AI_path) { void FGAIBase::initModel(osg::Node *node) { if (model.valid()) { - + if( _path != ""){ + props->setStringValue("submodels/path", _path.c_str()); + //props->setStringValue("submodel/path", _path.c_str()); + SG_LOG(SG_INPUT, SG_ALERT, "AIBase: submodels/path " << _path); + } fgSetString("/ai/models/model-added", props->getPath().c_str()); - } 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()); + //props->setStringValue("submodels/path", _path.c_str()); setDie(false); } @@ -533,7 +539,7 @@ double FGAIBase::_getLatitude() const { return pos.getLatitudeDeg(); } -double FGAIBase::_getElevationFt () const { +double FGAIBase::_getElevationFt() const { return pos.getElevationFt(); } diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index e47f778d0..68127fe4d 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -44,9 +44,9 @@ class FGAIBase : public SGReferenced { public: enum object_type { otNull = 0, otAircraft, otShip, otCarrier, otBallistic, - otRocket, otStorm, otThermal, otStatic, otWingman, otGroundVehicle, - otEscort, otMultiplayer, - MAX_OBJECTS }; // Needs to be last!!! + otRocket, otStorm, otThermal, otStatic, otWingman, otGroundVehicle, + otEscort, otMultiplayer, + MAX_OBJECTS }; // Needs to be last!!! FGAIBase(object_type ot); virtual ~FGAIBase(); @@ -95,7 +95,7 @@ public: SGVec3d getCartPos() const; bool getGroundElevationM(const SGGeod& pos, double& elev, - const SGMaterial** material) const; + const SGMaterial** material) const; double _getCartPosX() const; double _getCartPosY() const; @@ -177,7 +177,7 @@ protected: bool _impact_reported; bool _collision_reported; - bool _expiry_reported; + bool _expiry_reported; double _impact_lat; double _impact_lon; @@ -247,7 +247,7 @@ public: bool _getImpact(); bool _getImpactData(); bool _getCollisionData(); - bool _getExpiryData(); + bool _getExpiryData(); SGPropertyNode* _getProps() const; @@ -274,8 +274,8 @@ public: inline double _getBearing() { return bearing; }; virtual osg::Node* load3DModel(const string &path, - SGPropertyNode *prop_root); - + SGPropertyNode *prop_root); + static bool _isNight(); }; @@ -289,6 +289,7 @@ inline void FGAIBase::setPath(const char* model ) { } inline void FGAIBase::setSMPath(const string& p) { + cout << "setSMPath " << p <altitude; prev_alt = prev->altitude; - cout << "prev_alt" <altitude; distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude), diff --git a/src/AIModel/AIWingman.cxx b/src/AIModel/AIWingman.cxx index 4a081ee11..82874017b 100644 --- a/src/AIModel/AIWingman.cxx +++ b/src/AIModel/AIWingman.cxx @@ -42,7 +42,7 @@ void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) { setLife(scFileNode->getDoubleValue("life", -1)); setNoRoll(scFileNode->getBoolValue("no-roll", false)); setName(scFileNode->getStringValue("name", "Wingman")); - setSMPath(scFileNode->getStringValue("submodel-path", "")); + //setSMPath(scFileNode->getStringValue("submodel-path", "")); setSubID(scFileNode->getIntValue("SubID", 0)); setXoffset(scFileNode->getDoubleValue("x-offset", 0.0)); setYoffset(scFileNode->getDoubleValue("y-offset", 0.0)); @@ -57,6 +57,28 @@ void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) { void FGAIWingman::bind() { FGAIBallistic::bind(); + props->tie("id", SGRawValueMethods(*this, + &FGAIBase::getID)); + props->tie("subID", SGRawValueMethods(*this, + &FGAIBase::_getSubID)); + props->tie("position/altitude-ft", + SGRawValueMethods(*this, + &FGAIBase::_getElevationFt, + &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("orientation/pitch-deg", SGRawValuePointer(&pitch)); + props->tie("orientation/roll-deg", SGRawValuePointer(&roll)); + props->tie("orientation/true-heading-deg", SGRawValuePointer(&hdg)); + + props->tie("load/rel-brg-to-user-deg", SGRawValueMethods (*this, &FGAIBallistic::getRelBrgHitchToUser)); @@ -64,7 +86,7 @@ void FGAIWingman::bind() { SGRawValueMethods (*this, &FGAIBallistic::getElevHitchToUser)); props->tie("velocities/vertical-speed-fps", - SGRawValuePointer(&vs)); + SGRawValuePointer(&vs)); props->tie("position/x-offset", SGRawValueMethods(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset)); props->tie("position/y-offset", @@ -72,16 +94,27 @@ void FGAIWingman::bind() { props->tie("position/z-offset", SGRawValueMethods(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset)); props->tie("position/tgt-x-offset", - SGRawValueMethods(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset)); + SGRawValueMethods(*this, &FGAIBallistic::getTgtXOffset, &FGAIBallistic::setTgtXOffset)); props->tie("position/tgt-y-offset", - SGRawValueMethods(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset)); + SGRawValueMethods(*this, &FGAIBallistic::getTgtYOffset, &FGAIBallistic::setTgtYOffset)); props->tie("position/tgt-z-offset", - SGRawValueMethods(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset)); + SGRawValueMethods(*this, &FGAIBallistic::getTgtZOffset, &FGAIBallistic::setTgtZOffset)); } void FGAIWingman::unbind() { FGAIBallistic::unbind(); + props->untie("id"); + props->untie("SubID"); + + props->untie("position/altitude-ft"); + props->untie("position/latitude-deg"); + props->untie("position/longitude-deg"); + + props->untie("orientation/pitch-deg"); + props->untie("orientation/roll-deg"); + props->untie("orientation/true-heading-deg"); + props->untie("load/rel-brg-to-user-deg"); props->untie("load/elev-to-user-deg"); props->untie("velocities/vertical-speed-fps"); diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index 0958923c1..733bff3cb 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -37,7 +37,7 @@ FGSubmodelMgr::FGSubmodelMgr() string contents_node; contrail_altitude = 30000; _count = 0; - _found_sub = true; + _found_sub = true; } FGSubmodelMgr::~FGSubmodelMgr() @@ -78,16 +78,23 @@ void FGSubmodelMgr::init() ai = (FGAIManager*)globals->get_subsystem("ai_model"); load(); + +// _model_added_node = fgGetNode("ai/models/model-added", true); + //_model_added_node->addChangeListener(this, false); + } void FGSubmodelMgr::postinit() { // postinit, so that the AI list is populated loadAI(); - while (_found_sub) - loadSubmodels(); + while (_found_sub) + loadSubmodels(); //TODO reload submodels if an MP ac joins + + _model_added_node = fgGetNode("ai/models/model-added", true); + _model_added_node->addChangeListener(this, false); } void FGSubmodelMgr::bind() @@ -109,7 +116,7 @@ void FGSubmodelMgr::update(double dt) _impact = false; _hit = false; - _expiry = false; + _expiry = false; // check if the submodel hit an object or terrain sm_list = ai->get_ai_list(); @@ -119,7 +126,8 @@ void FGSubmodelMgr::update(double dt) for (; sm_list_itr != end; ++sm_list_itr) { _impact = (*sm_list_itr)->_getImpactData(); _hit = (*sm_list_itr)->_getCollisionData(); - _expiry = (*sm_list_itr)->_getExpiryData(); + _expiry = (*sm_list_itr)->_getExpiryData(); + int parent_subID = (*sm_list_itr)->_getSubID(); //SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " @@ -130,7 +138,7 @@ void FGSubmodelMgr::update(double dt) if (_impact || _hit || _expiry) { // SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit - //<< " exipiry :-( " << _expiry ); + //<< " exipiry :-( " << _expiry ); submodel_iterator = submodels.begin(); @@ -147,9 +155,9 @@ void FGSubmodelMgr::update(double dt) _parent_roll = (*sm_list_itr)->_getImpactRoll(); _parent_speed = (*sm_list_itr)->_getImpactSpeed(); (*submodel_iterator)->first_time = true; - //cout << "Impact: parent SubID = child_ID elev " << _parent_elev << endl; + //cout << "Impact: parent SubID = child_ID elev " << _parent_elev << endl; - if (release(*submodel_iterator, dt)) + if (release(*submodel_iterator, dt)) (*sm_list_itr)->setDie(true); } @@ -162,19 +170,18 @@ void FGSubmodelMgr::update(double dt) _contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude); - bool in_range = true; +// bool in_range = true; bool trigger = false; int i = -1; submodel_iterator = submodels.begin(); while (submodel_iterator != submodels.end()) { i++; - in_range = true; /*SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: " << (*submodel_iterator)->id << " name " << (*submodel_iterator)->name - << " in range " << in_range);*/ + );*/ if ((*submodel_iterator)->trigger_node != 0) { _trigger_node = (*submodel_iterator)->trigger_node; @@ -185,20 +192,20 @@ void FGSubmodelMgr::update(double dt) //cout << (*submodel_iterator)->name << "trigger node not found " << trigger << endl; } - if (trigger && (*submodel_iterator)->count != 0) { + if (trigger && (*submodel_iterator)->count != 0) { - int id = (*submodel_iterator)->id; - string name = (*submodel_iterator)->name; - - /*SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels end: " << (*submodel_iterator)->id - << " name " << (*submodel_iterator)->name - << " count " << (*submodel_iterator)->count - << " in range " << in_range);*/ + int id = (*submodel_iterator)->id; + string name = (*submodel_iterator)->name; + + SG_LOG(SG_GENERAL, SG_DEBUG, + "Submodels release: " << (*submodel_iterator)->id + << " name " << (*submodel_iterator)->name + << " count " << (*submodel_iterator)->count + ); - release(*submodel_iterator, dt); - } else - (*submodel_iterator)->first_time = true; + release(*submodel_iterator, dt); + } else + (*submodel_iterator)->first_time = true; ++submodel_iterator; } // end while @@ -222,8 +229,8 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) //cout << "not yet: timer " << sm->timer << " delay " << sm->delay << endl; return false; } - - //cout << "released timer: " << sm->timer << " delay " << sm->delay << endl; + + //cout << "released timer: " << sm->timer << " delay " << sm->delay << endl; sm->timer = 0.0; @@ -237,11 +244,11 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) FGAIBallistic* ballist = new FGAIBallistic; ballist->setPath(sm->model.c_str()); ballist->setName(sm->name); - ballist->setRandom(sm->random); - ballist->setRandomness(sm->randomness); - ballist->setLatitude(offsetpos.getLatitudeDeg()); - ballist->setLongitude(offsetpos.getLongitudeDeg()); - ballist->setAltitude(offsetpos.getElevationFt()); + ballist->setRandom(sm->random); + ballist->setRandomness(sm->randomness); + ballist->setLatitude(offsetpos.getLatitudeDeg()); + ballist->setLongitude(offsetpos.getLongitudeDeg()); + ballist->setAltitude(offsetpos.getElevationFt()); ballist->setAzimuth(IC.azimuth); ballist->setElevation(IC.elevation); ballist->setRoll(IC.roll); @@ -257,7 +264,7 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) ballist->setStabilisation(sm->aero_stabilised); ballist->setNoRoll(sm->no_roll); ballist->setCollision(sm->collision); - ballist->setExpiry(sm->expiry); + ballist->setExpiry(sm->expiry); ballist->setImpact(sm->impact); ballist->setImpactReportNode(sm->impact_report); ballist->setFuseRange(sm->fuse_range); @@ -309,12 +316,12 @@ void FGSubmodelMgr::transform(submodel *sm) sm->speed = sm->speed_node->getDoubleValue(); int id = sm->id; - //int sub_id = (*submodel)->sub_id; + int sub_id = sm->sub_id; string name = sm->name; //cout << " name " << name << " id " << id << " sub id" << sub_id << endl; - // set the Initial Conditions for the types of submodel parent + // set the Initial Conditions for the types of submodel parent if (_impact || _hit || _expiry) { // set the data for a submodel tied to a submodel @@ -347,6 +354,7 @@ void FGSubmodelMgr::transform(submodel *sm) } else { // set the data for a submodel tied to an AI Object + //cout << " set the data for a submodel tied to an AI Object " << id << endl; sm_list_iterator sm_list_itr = sm_list.begin(); sm_list_iterator end = sm_list.end(); @@ -358,19 +366,20 @@ void FGSubmodelMgr::transform(submodel *sm) continue; } - //cout << "found id " << id << endl; + //cout << " AI found id " << id << " alt " << (*sm_list_itr)->_getElevationFt()<< endl; IC.lat = (*sm_list_itr)->_getLatitude(); IC.lon = (*sm_list_itr)->_getLongitude(); - IC.alt = (*sm_list_itr)->_getAltitude(); + IC.alt = (*sm_list_itr)->_getElevationFt(); IC.roll = (*sm_list_itr)->_getRoll(); IC.elevation = (*sm_list_itr)->_getPitch(); IC.azimuth = (*sm_list_itr)->_getHeading(); - IC.alt = (*sm_list_itr)->_getAltitude(); IC.speed = (*sm_list_itr)->_getSpeed() * SG_KT_TO_FPS; IC.speed_down_fps = -(*sm_list_itr)->_getVS_fps(); IC.speed_east_fps = (*sm_list_itr)->_get_speed_east_fps(); IC.speed_north_fps = (*sm_list_itr)->_get_speed_north_fps(); + break; + ++sm_list_itr; } } @@ -381,22 +390,22 @@ void FGSubmodelMgr::transform(submodel *sm) cout << "speed north " << IC.speed_north_fps << endl ; cout << "parent speed fps in" << IC.speed << "sm speed in " << sm->speed << endl ;*/ - // Set the Initial Conditions that are common to all types of parent + // Set the Initial Conditions that are common to all types of parent IC.wind_from_east = _user_wind_from_east_node->getDoubleValue(); IC.wind_from_north = _user_wind_from_north_node->getDoubleValue(); - userpos.setLatitudeDeg(IC.lat); - userpos.setLongitudeDeg(IC.lon); - userpos.setElevationFt(IC.alt); + userpos.setLatitudeDeg(IC.lat); + userpos.setLongitudeDeg(IC.lon); + userpos.setElevationFt(IC.alt); _x_offset = sm->x_offset; _y_offset = sm->y_offset; _z_offset = sm->z_offset; - setOffsetPos(); + setOffsetPos(); - //IC.elevation += sm->pitch_offset; - //IC.azimuth += sm->yaw_offset ; + //IC.elevation += sm->pitch_offset; + //IC.azimuth += sm->yaw_offset ; // pre-process the trig functions cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS); @@ -460,7 +469,7 @@ void FGSubmodelMgr::loadAI() sm_list = ai->get_ai_list(); if (sm_list.empty()) { - SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Unable to read AI submodel list"); + SG_LOG(SG_GENERAL, SG_ALERT, "Submodels: Unable to read AI submodel list"); return; } @@ -476,7 +485,11 @@ void FGSubmodelMgr::loadAI() } int id = (*sm_list_itr)->getID(); + string type = (*sm_list_itr)->getTypeString(); bool serviceable = (*sm_list_itr)->_getServiceable(); + + //cout << "loadAI: type " << type << " path "<< path << " serviceable " << serviceable << endl; + setData(id, path, serviceable); ++sm_list_itr; } @@ -490,13 +503,13 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) SGPath config(globals->get_fg_root()); config.append(path); - SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: path " << path); + SG_LOG(SG_GENERAL, SG_DEBUG, "setData: 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 &) { - SG_LOG(SG_GENERAL, SG_DEBUG, + SG_LOG(SG_GENERAL, SG_ALERT, "Submodels: Unable to read AI submodels file: " << config.str()); return; } @@ -530,18 +543,18 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) 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->expiry = entry_node->getBoolValue("expiry", false); + sm->expiry = entry_node->getBoolValue("expiry", 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->speed_node = fgGetNode(entry_node->getStringValue("speed-prop", "none"), false); sm->submodel = entry_node->getStringValue("submodel-path", ""); sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false); sm->ext_force = entry_node->getBoolValue("external-force", false); sm->force_path = entry_node->getStringValue("force-path", ""); - sm->random = entry_node->getBoolValue("random", false); - sm->randomness = entry_node->getDoubleValue("randomness", 0.5); + sm->random = entry_node->getBoolValue("random", false); + sm->randomness = entry_node->getDoubleValue("randomness", 0.5); //cout << "sm->contents_node " << sm->contents_node << endl; @@ -566,7 +579,7 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->sub_id = 0; sm->prop = fgGetNode("/ai/submodels/submodel", index, true); - sm->prop->tie("delay", SGRawValuePointer(&(sm->delay))); + sm->prop->tie("delay", SGRawValuePointer(&(sm->delay))); sm->prop->tie("count", SGRawValuePointer(&(sm->count))); sm->prop->tie("repeat", SGRawValuePointer(&(sm->repeat))); sm->prop->tie("id", SGRawValuePointer(&(sm->id))); @@ -598,10 +611,10 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) SGPath config(globals->get_fg_root()); config.append(path); - SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels: path " << path); + SG_LOG(SG_GENERAL, SG_ALERT, "setSubData: path " << path); + try { - SG_LOG(SG_GENERAL, SG_DEBUG, + SG_LOG(SG_GENERAL, SG_ALERT, "Submodels: Trying to read AI submodels file: " << config.str()); readProperties(config.str(), &root); @@ -640,18 +653,18 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) 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->expiry = entry_node->getBoolValue("expiry", false); + sm->expiry = entry_node->getBoolValue("expiry", 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->speed_node = fgGetNode(entry_node->getStringValue("speed-prop", "none"), false); sm->submodel = entry_node->getStringValue("submodel-path", ""); - sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false); + sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false); sm->ext_force = entry_node->getBoolValue("external-force", false); sm->force_path = entry_node->getStringValue("force-path", ""); - sm->random = entry_node->getBoolValue("random", false); - sm->randomness = entry_node->getDoubleValue("randomness", 0.5); + sm->random = entry_node->getBoolValue("random", false); + sm->randomness = entry_node->getDoubleValue("randomness", 0.5); //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) @@ -680,7 +693,7 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) sm->prop->tie("id", SGRawValuePointer(&(sm->id))); sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); - sm->prop->tie("random", SGRawValuePointer(&(sm->random))); + sm->prop->tie("random", SGRawValuePointer(&(sm->random))); string name = sm->name; sm->prop->setStringValue("name", name.c_str()); @@ -704,29 +717,29 @@ void FGSubmodelMgr::loadSubmodels() { SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Loading sub submodels"); - _found_sub = false; + _found_sub = false; 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_DEBUG, "found path sub sub " - // << submodel - // << " index " << index - // << "name " << (*submodel_iterator)->name); + 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_DEBUG, "found path sub sub " + // << submodel + // << " index " << index + // << "name " << (*submodel_iterator)->name); - if ((*submodel_iterator)->sub_id == 0){ - (*submodel_iterator)->sub_id = index; - _found_sub = true; - setSubData(index, submodel, serviceable); - } - } + if ((*submodel_iterator)->sub_id == 0){ + (*submodel_iterator)->sub_id = index; + _found_sub = true; + setSubData(index, submodel, serviceable); + } + } - ++submodel_iterator; - } // end while + ++submodel_iterator; + } // end while subsubmodel_iterator = subsubmodels.begin(); @@ -735,7 +748,7 @@ void FGSubmodelMgr::loadSubmodels() ++subsubmodel_iterator; } // end while - subsubmodels.clear(); + subsubmodels.clear(); //submodel_iterator = submodels.begin(); @@ -760,7 +773,7 @@ SGVec3d FGSubmodelMgr::getCartOffsetPos() const{ // in the simulation usual body x-forward, y-right, z-down coordinates // (meters) ) - SGVec3d _off(_x_offset * SG_FEET_TO_METER, + SGVec3d _off(_x_offset * SG_FEET_TO_METER, _y_offset * SG_FEET_TO_METER, -_z_offset * SG_FEET_TO_METER); @@ -789,5 +802,60 @@ void FGSubmodelMgr::setOffsetPos(){ SGVec3d cartoffsetPos = getCartOffsetPos(); SGGeodesy::SGCartToGeod(cartoffsetPos, offsetpos); +} + +void FGSubmodelMgr::valueChanged(SGPropertyNode *prop) +{ + + const char* _model_added = _model_added_node->getStringValue(); + + basic_string ::size_type indexCh2b; + + string str2 = _model_added; + const char *cstr2b = "ballistic"; + indexCh2b = str2.find( cstr2b, 0 ); + + if (indexCh2b != string::npos ){ // we will ignore Ballistic Objects - there are potentially too many + return; + } else { + //cout << "model added - " << str2 <<" now do something "<< endl; + + SGPropertyNode *a_node = fgGetNode(_model_added, true ); + SGPropertyNode *sub_node = a_node->getChild("submodels", 0, true); + SGPropertyNode_ptr path_node = sub_node->getChild("path", 0, true); + + string path = path_node->getStringValue(); + + if (path.empty()){ + // nothing to do - return + //cout << "subpath empty - return" << endl << endl; + return; + } else { + //cout << "subpath not empty: " << path << endl << endl; + SGPropertyNode_ptr ident_node = a_node->getChild("id", 0, true); + int id = ident_node->getIntValue(); + + setData(id, path, true); + + _found_sub = true; + + while (_found_sub) + loadSubmodels(); + + } + + + + + } + + + + + + + + + } // end of submodel.cxx diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index cd645f64e..95e62f6b7 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -25,7 +25,7 @@ using std::list; class FGAIBase; -class FGSubmodelMgr : public SGSubsystem +class FGSubmodelMgr : public SGSubsystem, public SGPropertyChangeListener { public: @@ -53,7 +53,7 @@ public: double drag_area; double life; double buoyancy; - double randomness; + double randomness; bool wind; bool first_time; double cd; @@ -63,9 +63,9 @@ public: int id; bool no_roll; bool serviceable; - bool random; + bool random; bool collision; - bool expiry; + bool expiry; bool impact; string impact_report; double fuse_range; @@ -144,7 +144,7 @@ private: double _parent_pitch; double _parent_roll; double _parent_speed; - double _x_offset; + double _x_offset; double _y_offset; double _z_offset; @@ -155,8 +155,8 @@ private: bool _impact; bool _hit; - bool _expiry; - bool _found_sub; + bool _expiry; + bool _found_sub; SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _user_lat_node; @@ -178,6 +178,9 @@ private: SGPropertyNode_ptr _count_node; SGPropertyNode_ptr _trigger_node; SGPropertyNode_ptr props; + SGPropertyNode_ptr _model_added_node; + SGPropertyNode_ptr _path_node; + FGAIManager* ai; IC_struct IC; @@ -201,11 +204,11 @@ private: int _count; - - SGGeod userpos; - SGGeod offsetpos; - SGVec3d getCartOffsetPos() const; - void setOffsetPos(); + + SGGeod userpos; + SGGeod offsetpos; + SGVec3d getCartOffsetPos() const; + void setOffsetPos(); }; diff --git a/src/Environment/environment_mgr.cxx b/src/Environment/environment_mgr.cxx index 4a3fc9606..98184acf9 100644 --- a/src/Environment/environment_mgr.cxx +++ b/src/Environment/environment_mgr.cxx @@ -297,19 +297,9 @@ FGEnvironmentMgr::update (double dt) osg::Vec3 windVec(_environment->get_wind_from_north_fps(), -_environment->get_wind_from_east_fps(), 0); - // SG_LOG(SG_GENERAL, SG_ALERT, "-_environment->get_wind_from_north_mps() " << - //_environment->get_wind_from_north_fps() * SG_FEET_TO_METER - //<< " -_environment->get_wind_from_east_mps() " - //<< -_environment->get_wind_from_east_fps() * SG_FEET_TO_METER - //); - - // simgear::Particles::setWindVector(windVec * SG_FEET_TO_METER); - simgear::Particles::setWindFrom( _environment->get_wind_from_heading_deg(), - _environment->get_wind_speed_kt() ); - //double wind_true_deg = _environment->get_wind_from_heading_deg(); - //simgear::Particles::setWindFrom( wind_true_deg, - // _environment->get_wind_speed_kt() ); - + simgear::Particles::setWindVector(windVec * SG_FEET_TO_METER); + //simgear::Particles::setWindFrom( _environment->get_wind_from_heading_deg(), + // _environment->get_wind_speed_kt() ); } FGEnvironment From dff8099994e9205d0790aaf91e3a6a9795150e74 Mon Sep 17 00:00:00 2001 From: Vivian Meazza Date: Sun, 29 Aug 2010 00:00:09 +0100 Subject: [PATCH 2/7] Major update of Ballistic objects and submodels. Introduce submodels slaved to AI Objects. Remove deprecated code. Try to increase framerate. Use quaternion calculations for Initial Conditions. Signed-off-by: Vivian Meazza --- src/AIModel/AIBallistic.cxx | 397 +++++++++++++++++++++++++----------- src/AIModel/AIBallistic.hxx | 54 +++-- src/AIModel/AIBase.cxx | 10 +- src/AIModel/AIBase.hxx | 3 - src/AIModel/AIManager.cxx | 3 +- src/AIModel/AIWingman.cxx | 29 ++- src/AIModel/submodel.cxx | 229 +++++++++++++-------- src/AIModel/submodel.hxx | 6 + 8 files changed, 496 insertions(+), 235 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index e94844eaf..5f67d88e4 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -66,7 +66,8 @@ _report_impact(false), _external_force(false), _impact_report_node(fgGetNode("/ai/models/model-impact", true)), _old_height(0), -_elapsed_time(0) +_elapsed_time(0), +hs(0) { no_roll = false; @@ -89,8 +90,8 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setDragArea(scFileNode->getDoubleValue("eda", 0.007)); setLife(scFileNode->getDoubleValue("life", 900.0)); setBuoyancy(scFileNode->getDoubleValue("buoyancy", 0)); - setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0)); - setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0)); + //setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0)); + //setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0)); setWind(scFileNode->getBoolValue("wind", false)); setRoll(scFileNode->getDoubleValue("roll", 0.0)); setCd(scFileNode->getDoubleValue("cd", 0.029)); @@ -119,7 +120,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setLoadOffset(scFileNode->getDoubleValue("load-offset", 0.0)); setSlaved(scFileNode->getBoolValue("slaved", false)); setSlavedLoad(scFileNode->getBoolValue("slaved-load", false)); - setContentsNode(scFileNode->getStringValue("contents")); + setContentsPath(scFileNode->getStringValue("contents")); } bool FGAIBallistic::init(bool search_in_AI_path) { @@ -143,8 +144,13 @@ bool FGAIBallistic::init(bool search_in_AI_path) { props->setStringValue("material/name", ""); props->setStringValue("name", _name.c_str()); - props->setStringValue("submodels/path", _submodel.c_str()); - props->setStringValue("force/path", _force_path.c_str()); + props->setStringValue("submodels/path", _path.c_str()); + + if (_slave_to_ac){ + props->setStringValue("force/path", _force_path.c_str()); + props->setStringValue("contents/path", _contents_path.c_str()); + } + //props->setStringValue("vector/path", _vector_path.c_str()); // start with high value so that animations don't trigger yet @@ -155,6 +161,8 @@ bool FGAIBallistic::init(bool search_in_AI_path) { Transform(); + //cout << _name << " speed init: " << speed << endl; + return true; } @@ -164,11 +172,10 @@ void FGAIBallistic::bind() { props->tie("sim/time/elapsed-sec", SGRawValueMethods(*this, &FGAIBallistic::_getTime)); - props->tie("mass-slug", - SGRawValueMethods(*this, - &FGAIBallistic::getMass)); - props->tie("material/load-resistance", - SGRawValuePointer(&_load_resistance)); + //props->tie("mass-slug", + // SGRawValueMethods(*this, + // &FGAIBallistic::getMass)); + props->tie("material/solid", SGRawValuePointer(&_solid)); props->tie("altitude-agl-ft", @@ -179,7 +186,7 @@ void FGAIBallistic::bind() { props->tie("controls/invisible", SGRawValuePointer(&invisible)); - if(_external_force){ + if(_external_force || _slave_to_ac){ props->tie("controls/force_stabilized", SGRawValuePointer(&_force_stabilised)); props->tie("position/global-x", @@ -195,7 +202,7 @@ void FGAIBallistic::bind() { props->tie("velocities/horizontal-speed-fps", SGRawValuePointer(&hs)); props->tie("position/altitude-ft", - SGRawValueMethods(*this, &FGAIBase::_getAltitude, &FGAIBase::_setAltitude)); + SGRawValueMethods(*this, &FGAIBase::_getElevationFt, &FGAIBase::_setAltitude)); props->tie("position/latitude-deg", SGRawValueMethods(*this, &FGAIBase::_getLatitude, &FGAIBase::_setLatitude)); props->tie("position/longitude-deg", @@ -221,22 +228,23 @@ void FGAIBallistic::bind() { props->tie("load/bearing-to-hitch-deg", SGRawValueMethods (*this, &FGAIBallistic::getBearingLoadToHitch)); + props->tie("material/load-resistance", + SGRawValuePointer(&_load_resistance)); } } void FGAIBallistic::unbind() { - // FGAIBase::unbind(); +// FGAIBase::unbind(); props->untie("sim/time/elapsed-sec"); props->untie("mass-slug"); - props->untie("material/load-resistance"); props->untie("material/solid"); props->untie("altitude-agl-ft"); props->untie("controls/slave-to-ac"); props->untie("controls/invisible"); - if(_external_force){ + if(_external_force || _slave_to_ac){ props->untie("position/global-y"); props->untie("position/global-x"); props->untie("position/global-z"); @@ -255,6 +263,7 @@ void FGAIBallistic::unbind() { props->untie("load/distance-to-hitch-ft"); props->untie("load/elevation-to-hitch-deg"); props->untie("load/bearing-to-hitch-deg"); + props->untie("material/load-resistance"); } } @@ -262,14 +271,12 @@ void FGAIBallistic::update(double dt) { FGAIBase::update(dt); _setUserPos(); - if (_slave_to_ac){ - slaveToAC(dt); - Transform(); - setHitchVelocity(dt); - } else if (_formate_to_ac){ + if (_formate_to_ac){ formateToAC(dt); Transform(); - setHitchVelocity(dt); + } else if (_slave_to_ac){ + slaveToAC(dt); + Transform(); } else if (!invisible){ Run(dt); Transform(); @@ -366,7 +373,6 @@ void FGAIBallistic::setCollision(bool c) { void FGAIBallistic::setExpiry(bool e) { _report_expiry = e; - //cout << "_report_expiry " << _report_expiry << endl; } void FGAIBallistic::setExternalForce(bool f) { @@ -384,7 +390,8 @@ void FGAIBallistic::setName(const string& n) { } void FGAIBallistic::setSMPath(const string& s) { - _submodel = s; + _path = s; + //cout << "submodel path " << _path << endl; } void FGAIBallistic::setFuseRange(double f) { @@ -419,23 +426,67 @@ void FGAIBallistic::setFormate(bool f) { _formate_to_ac = f; } -void FGAIBallistic::setContentsNode(const string& path) { +void FGAIBallistic::setContentsPath(const string& path) { + + _contents_path = path; + if (!path.empty()) { _contents_node = fgGetNode(path.c_str(), true); } } +void FGAIBallistic::setContentsNode(SGPropertyNode_ptr node) { + + if (node != 0) { + _contents_node = node; + _contents_path = _contents_node->getDisplayName(); + } +} + +void FGAIBallistic::setParentNode(SGPropertyNode_ptr node) { + if (node != 0) { + _pnode = node; + _p_pos_node = _pnode->getChild("position", 0, true); + _p_lat_node = _p_pos_node->getChild("latitude-deg", 0, true); + _p_lon_node = _p_pos_node->getChild("longitude-deg", 0, true); + _p_alt_node = _p_pos_node->getChild("altitude-ft", 0, true); + + _p_ori_node = _pnode->getChild("orientation", 0, true); + _p_pch_node = _p_ori_node->getChild("pitch-deg", 0, true); + _p_rll_node = _p_ori_node->getChild("roll-deg", 0, true); + _p_hdg_node = _p_ori_node->getChild("true-heading-deg",0, true); + + _p_vel_node = _pnode->getChild("velocities", 0, true); + _p_spd_node = _p_vel_node->getChild("true-airspeed-kt", 0, true); + + } +} + +void FGAIBallistic::setParentPos() { + if (_pnode != 0) { + double lat = _p_lat_node->getDoubleValue(); + double lon = _p_lon_node->getDoubleValue(); + double alt = _p_alt_node->getDoubleValue(); + + _parentpos.setLongitudeDeg(lon); + _parentpos.setLatitudeDeg(lat); + _parentpos.setElevationFt(alt); + + } +} + bool FGAIBallistic::getSlaved() const { return _slave_to_ac; -} +} double FGAIBallistic::getMass() const { return _mass; } double FGAIBallistic::getContents() { - if(_contents_node) + if(_contents_node){ _contents_lb = _contents_node->getChild("level-lbs",0,1)->getDoubleValue(); + } return _contents_lb; } @@ -462,27 +513,33 @@ void FGAIBallistic::setForcePath(const string& p) { } } -bool FGAIBallistic::getHtAGL(){ +bool FGAIBallistic::getHtAGL(double start){ - if (getGroundElevationM(SGGeod::fromGeodM(pos, 10000), + if (getGroundElevationM(SGGeod::fromGeodM(pos, start), _elevation_m, &_material)) { _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET; + if (_material) { const vector& names = _material->get_names(); - _solid = _material->get_solid(); _load_resistance = _material->get_load_resistance(); _frictionFactor =_material->get_friction_factor(); + if (!names.empty()) props->setStringValue("material/name", names[0].c_str()); else props->setStringValue("material/name", ""); - /*cout << "material " << mat_name - << " solid " << _solid - << " load " << _load_resistance - << " frictionFactor " << frictionFactor - << endl;*/ + + _mat_name = names[0]; + + //cout << "material " << _mat_name + //<< " solid " << _solid + //<< " load " << _load_resistance + //<< " frictionFactor " << _frictionFactor + //<< endl; + } + return true; } else { return false; @@ -541,7 +598,6 @@ double FGAIBallistic::getTgtZOffset() const { void FGAIBallistic::setTgtXOffset(double x){ _tgt_x_offset = x; - cout <<"setTgtXOffset " <<_tgt_x_offset << endl; } void FGAIBallistic::setTgtYOffset(double y){ @@ -554,19 +610,39 @@ void FGAIBallistic::setTgtZOffset(double z){ void FGAIBallistic::slaveToAC(double dt){ - setHitchPos(); - pos.setLatitudeDeg(hitchpos.getLatitudeDeg()); - pos.setLongitudeDeg(hitchpos.getLongitudeDeg()); - pos.setElevationFt(hitchpos.getElevationFt()); - setHeading(manager->get_user_heading()); - setPitch(manager->get_user_pitch() + _pitch_offset); - setBank(manager->get_user_roll() + _roll_offset); - setSpeed(manager->get_user_speed()); + double hdg, pch, rll = 0; + + if (_pnode != 0) { + setParentPos(); + hdg = _p_hdg_node->getDoubleValue(); + pch = _p_pch_node->getDoubleValue(); + rll = _p_rll_node->getDoubleValue(); + setOffsetPos(_parentpos, hdg, pch, rll); + setSpeed(_p_spd_node->getDoubleValue()); + }else { + hdg = manager->get_user_heading(); + pch = manager->get_user_pitch(); + rll = manager->get_user_roll(); + setOffsetPos(userpos, hdg, pch, rll); + setSpeed(manager->get_user_speed()); + } + + pos.setLatitudeDeg(_offsetpos.getLatitudeDeg()); + pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); + pos.setElevationFt(_offsetpos.getElevationFt()); + setHeading(hdg); + setPitch(pch + _pitch_offset); + setBank(rll + _roll_offset); + setOffsetVelocity(dt, pos); + //update the mass (slugs) _mass = (_weight_lb + getContents()) / slugs_to_lbs; - /*cout <<"_mass "<<_mass <<" " << getContents() - <<" " << getContents() / slugs_to_lbs << endl;*/ + _impact_reported = false; + + //cout << _name << " _mass "<<_mass <<" " << getContents() + //<< " " << getContents() / slugs_to_lbs << " weight " << _weight_lb << endl; + // cout << _name << " update hs " << hs << " vs " << vs << endl; } void FGAIBallistic::Run(double dt) { @@ -575,7 +651,7 @@ void FGAIBallistic::Run(double dt) { // if life = -1 the object does not die if (_life_timer > life && life != -1){ - if (_report_expiry && !_expiry_reported){ + if (_report_expiry && !_expiry_reported && !_impact_reported && !_collision_reported){ //cout<<"AIBallistic: expiry"<< endl; handle_expiry(); } else @@ -601,7 +677,8 @@ void FGAIBallistic::Run(double dt) { else Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd; - //cout << "Mach " << Mach << " Cdm " << Cdm << "// ballistic speed kts "<< speed << endl; + //cout <<_name << " Mach " << Mach << " Cdm " << Cdm + // << " ballistic speed kts "<< speed << endl; // drag = Cd * 0.5 * rho * speed * speed * drag_area; // rho is adjusted for altitude in void FGAIBase::update, @@ -645,7 +722,7 @@ void FGAIBallistic::Run(double dt) { //calculate velocity due to external force double force_speed_north_deg_sec = 0; double force_speed_east_deg_sec = 0; - // double vs_force_fps = 0; +// double vs_force_fps = 0; double hs_force_fps = 0; double v_force_acc_fpss = 0; double force_speed_north_fps = 0; @@ -662,19 +739,20 @@ void FGAIBallistic::Run(double dt) { double force_elevation_deg = 0; if (_external_force) { + //cout << _name << " external force" << endl; SGPropertyNode *n = fgGetNode(_force_path.c_str(), true); double force_lbs = n->getChild("force-lb", 0, true)->getDoubleValue(); force_elevation_deg = n->getChild("force-elevation-deg", 0, true)->getDoubleValue(); double force_azimuth_deg = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue(); - + //resolve force into vertical and horizontal components: double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS ); h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS ); //ground interaction - if (getHtAGL()){ + if (getHtAGL(10000)){ double deadzone = 0.1; if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){ @@ -755,18 +833,22 @@ void FGAIBallistic::Run(double dt) { // set new position if(_slave_load_to_ac) { - setHitchPos(); - pos.setLatitudeDeg(hitchpos.getLatitudeDeg()); - pos.setLongitudeDeg(hitchpos.getLongitudeDeg()); - pos.setElevationFt(hitchpos.getElevationFt()); + setOffsetPos(pos, + manager->get_user_heading(), + manager->get_user_pitch(), + manager->get_user_roll() + ); + pos.setLatitudeDeg(_offsetpos.getLatitudeDeg()); + pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); + pos.setElevationFt(_offsetpos.getElevationFt()); - if (getHtAGL()){ + if (getHtAGL(10000)){ double deadzone = 0.1; if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){ pos.setElevationFt(0 + _ground_offset); } else { - pos.setElevationFt(hitchpos.getElevationFt() + _load_offset); + pos.setElevationFt(_offsetpos.getElevationFt() + _load_offset); } } @@ -780,6 +862,8 @@ void FGAIBallistic::Run(double dt) { pos.setElevationFt(pos.getElevationFt() + vs * dt); } +// cout << _name << " run hs " << hs << " vs " << vs << endl; + // recalculate total speed if ( vs == 0 && hs == 0) speed = 0; @@ -796,8 +880,6 @@ void FGAIBallistic::Run(double dt) { if (_azimuth < 0) _azimuth += 360; - //cout << "_azimuth " << _azimuth << " hdg "<< hdg << endl; - if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter //cout<< "_aero_stabilised "<< endl; const double coeff = 0.9; @@ -807,7 +889,7 @@ void FGAIBallistic::Run(double dt) { setHdg(_azimuth, dt, coeff); } else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter //cout<< "_force_stabilised "<< endl; - + const double coeff = 0.9; double ratio = h_force_lbs/(_mass * slugs_to_lbs); @@ -844,7 +926,9 @@ double FGAIBallistic::_getTime() const { void FGAIBallistic::handle_impact() { // try terrain intersection - if(!getHtAGL()) + double start = pos.getElevationM() + 10; + + if(!getHtAGL(start)) return; if (_ht_agl_ft <= 0) { @@ -866,10 +950,11 @@ void FGAIBallistic::handle_expiry() { report_impact(pos.getElevationM()); _expiry_reported = true; - //if (life == -1){ - // invisible = true; - //} else if (_subID == 0) // kill the AIObject if there is no subsubmodel - // setDie(true); + if (life == -1){ + invisible = true; + } else if (_subID == 0){ // kill the AIObject if there is no subsubmodel + setDie(true); + } } @@ -879,7 +964,6 @@ void FGAIBallistic::handle_collision() pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range); if (object) { - SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object hit"); report_impact(pos.getElevationM(), object); _collision_reported = true; } @@ -896,11 +980,14 @@ void FGAIBallistic::report_impact(double elevation, const FGAIBase *object) _impact_roll = roll; SGPropertyNode *n = props->getNode("impact", true); + if (object) n->setStringValue("type", object->getTypeString()); else n->setStringValue("type", "terrain"); + SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object impact" << _name << " lon " <<_impact_lon); + n->setDoubleValue("longitude-deg", _impact_lon); n->setDoubleValue("latitude-deg", _impact_lat); n->setDoubleValue("elevation-m", _impact_elev); @@ -920,7 +1007,7 @@ SGVec3d FGAIBallistic::getCartUserPos() const { SGVec3d FGAIBallistic::getCartHitchPos() const{ // convert geodetic positions to geocentered - SGVec3d cartuserPos = getCartUserPos(); + SGVec3d cartuserPos = SGVec3d::fromGeod(userpos); //SGVec3d cartPos = getCartPos(); // Transform to the right coordinate frame, configuration is done in @@ -928,8 +1015,8 @@ SGVec3d FGAIBallistic::getCartHitchPos() const{ // in the simulation usual body x-forward, y-right, z-down coordinates // (meters) ) SGVec3d _off(_x_offset * SG_FEET_TO_METER, - _y_offset * SG_FEET_TO_METER, - -_z_offset * SG_FEET_TO_METER); + _y_offset * SG_FEET_TO_METER, + -_z_offset * SG_FEET_TO_METER); // Transform the user position to the horizontal local coordinate system. SGQuatd hlTrans = SGQuatd::fromLonLat(userpos); @@ -951,11 +1038,16 @@ SGVec3d FGAIBallistic::getCartHitchPos() const{ return offsetPos; } -void FGAIBallistic::setHitchPos(){ +void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, double roll){ // convert the hitch geocentered position to geodetic - SGVec3d carthitchPos = getCartHitchPos(); - SGGeodesy::SGCartToGeod(carthitchPos, hitchpos); + SGVec3d cartoffsetPos = getCartOffsetPos(inpos, heading, pitch, roll); + + //SGVec3d cartoffsetPos = getCartHitchPos(); + + //SGGeodesy::SGCartToGeod(cartoffsetPos, hitchpos); + SGGeodesy::SGCartToGeod(cartoffsetPos, _offsetpos); + } double FGAIBallistic::getDistanceLoadToHitch() const { @@ -968,47 +1060,12 @@ double FGAIBallistic::getDistanceLoadToHitch() const { return distance * SG_METER_TO_FEET; } -void FGAIBallistic::setHitchVelocity(double dt) { - //calculate the distance from the previous hitch position - SGVec3d carthitchPos = getCartHitchPos(); - SGVec3d diff = carthitchPos - _oldcarthitchPos; - - double distance = norm(diff); - - //calculate speed knots - speed = (distance/dt) * SG_MPS_TO_KT; - - //now calulate the angle between the old and current hitch positions (degrees) - double angle = 0; - double daltM = hitchpos.getElevationM() - oldhitchpos.getElevationM(); - - if (fabs(distance) < SGLimits::min()) { - angle = 0; - } else { - double sAngle = daltM/distance; - sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle)); - angle = SGMiscd::rad2deg(asin(sAngle)); - } - - _elevation = angle; - - //calculate the bearing of the new hitch position from the old - double az1, az2, dist; - - geo_inverse_wgs_84(oldhitchpos, hitchpos, &az1, &az2, &dist); - - _azimuth = az1; - - // and finally store the new values - _oldcarthitchPos = carthitchPos; - oldhitchpos = hitchpos; -} double FGAIBallistic::getElevLoadToHitch() const { // now the angle, positive angles are upwards double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER; double angle = 0; - double daltM = hitchpos.getElevationM() - pos.getElevationM(); + double daltM = _offsetpos.getElevationM() - pos.getElevationM(); if (fabs(distance) < SGLimits::min()) { angle = 0; @@ -1025,7 +1082,7 @@ double FGAIBallistic::getBearingLoadToHitch() const { //calculate the bearing and range of the second pos from the first double az1, az2, distance; - geo_inverse_wgs_84(pos, hitchpos, &az1, &az2, &distance); + geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance); return az1; } @@ -1034,7 +1091,7 @@ double FGAIBallistic::getRelBrgHitchToUser() const { //calculate the relative bearing double az1, az2, distance; - geo_inverse_wgs_84(hitchpos, userpos, &az1, &az2, &distance); + geo_inverse_wgs_84(_offsetpos, userpos, &az1, &az2, &distance); double rel_brg = az1 - hdg; @@ -1055,7 +1112,7 @@ double FGAIBallistic::getElevHitchToUser() const { double distance = norm(diff); double angle = 0; - double daltM = userpos.getElevationM() - hitchpos.getElevationM(); + double daltM = userpos.getElevationM() - _offsetpos.getElevationM(); // now the angle, positive angles are upwards if (fabs(distance) < SGLimits::min()) { @@ -1080,8 +1137,11 @@ void FGAIBallistic::setTgtOffsets(double dt, double coeff){ void FGAIBallistic::formateToAC(double dt){ setTgtOffsets(dt, 25); - setHitchPos(); - setHitchVelocity(dt); + setOffsetPos(userpos, + manager->get_user_heading(), + manager->get_user_pitch(), + manager->get_user_roll() + ); // elapsed time has a random initialisation so that each // wingman moves differently @@ -1095,19 +1155,19 @@ void FGAIBallistic::formateToAC(double dt){ double h_angle = 5 * factor; double h_feet = 3 * factor; - pos.setLatitudeDeg(hitchpos.getLatitudeDeg()); - pos.setLongitudeDeg(hitchpos.getLongitudeDeg()); + pos.setLatitudeDeg(_offsetpos.getLatitudeDeg()); + pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); - if (getHtAGL()){ + if (getHtAGL(10000)){ if(_ht_agl_ft <= 10) { _height = userpos.getElevationFt(); } else if (_ht_agl_ft > 10 && _ht_agl_ft <= 150 ) { setHt(userpos.getElevationFt(), dt, 1.0); } else if (_ht_agl_ft > 150 && _ht_agl_ft <= 250) { - setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.75); + setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75); } else - setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.5); + setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5); pos.setElevationFt(_height); } @@ -1128,6 +1188,105 @@ void FGAIBallistic::formateToAC(double dt){ setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9); } - setSpeed(speed); + setOffsetVelocity(dt, pos); } +void FGAIBallistic::calcVSHS(){ + // calculate vertical and horizontal speed components + double speed_fps = speed * SG_KT_TO_FPS; + + if (speed == 0.0) { + hs = vs = 0.0; + } else { + vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps; + hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps; + } +} + +void FGAIBallistic::calcNE(){ + //resolve horizontal speed into north and east components: + _speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs; + _speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs; + + // convert horizontal speed (fps) to degrees per second + speed_north_deg_sec = _speed_north_fps / ft_per_deg_lat; + speed_east_deg_sec = _speed_east_fps / ft_per_deg_lon; + +} + +SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading, + double user_pitch, double user_roll + ) const{ + + // convert geodetic positions to geocentered + SGVec3d cartuserPos = SGVec3d::fromGeod(inpos); + //SGVec3d cartuserPos = getCartUserPos(); + //SGVec3d cartPos = getCartPos(); + + // Transform to the right coordinate frame, configuration is done in + // the x-forward, y-right, z-up coordinates (feet), computation + // in the simulation usual body x-forward, y-right, z-down coordinates + // (meters) ) + SGVec3d _off(_x_offset * SG_FEET_TO_METER, + _y_offset * SG_FEET_TO_METER, + -_z_offset * SG_FEET_TO_METER); + + // Transform the user position to the horizontal local coordinate system. + SGQuatd hlTrans = SGQuatd::fromLonLat(userpos); + + // and postrotate the orientation of the user model wrt the horizontal + // local frame + hlTrans *= SGQuatd::fromYawPitchRollDeg( + user_heading, + user_pitch, + user_roll); + + // The offset converted to the usual body fixed coordinate system + // rotated to the earth-fixed coordinates axis + SGVec3d off = hlTrans.backTransform(_off); + + // Add the position offset of the user model to get the geocentered position + SGVec3d offsetPos = cartuserPos + off; + + return offsetPos; +} + +void FGAIBallistic::setOffsetVelocity(double dt, SGGeod offsetpos) { + //calculate the distance from the previous offset position + SGVec3d cartoffsetPos = SGVec3d::fromGeod(offsetpos); + SGVec3d diff = cartoffsetPos - _oldcartoffsetPos; + + double distance = norm(diff); + //calculate speed knots + speed = (distance/dt) * SG_MPS_TO_KT; + + //now calulate the angle between the old and current postion positions (degrees) + double angle = 0; + double daltM = offsetpos.getElevationM() - _oldoffsetpos.getElevationM(); + + if (fabs(distance) < SGLimits::min()) { + angle = 0; + } else { + double sAngle = daltM/distance; + sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle)); + angle = SGMiscd::rad2deg(asin(sAngle)); + } + + _elevation = angle; + + //calculate vertical and horizontal speed components + calcVSHS(); + + //calculate the bearing of the new offset position from the old + double az1, az2, dist; + geo_inverse_wgs_84(_oldoffsetpos, offsetpos, &az1, &az2, &dist); + _azimuth = az1; + + //resolve horizontal speed into north and east components: + calcNE(); + + // and finally store the new values + _oldcartoffsetPos = cartoffsetPos; + _oldoffsetpos = offsetpos; +} + // end AIBallistic diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx index 09efbd6e1..6ce9c13d8 100644 --- a/src/AIModel/AIBallistic.hxx +++ b/src/AIModel/AIBallistic.hxx @@ -68,31 +68,32 @@ public: void setWeight( double w ); void setNoRoll( bool nr ); void setRandom( bool r ); - void setRandomness( double r ); + void setRandomness( double r ); void setName(const string&); void setCollision(bool c); - void setExpiry(bool e); + void setExpiry(bool e); void setImpact(bool i); void setImpactReportNode(const string&); - void setContentsNode(const string&); + void setContentsNode(const SGPropertyNode_ptr); void setFuseRange(double f); void setSMPath(const string&); void setSubID(int i); void setSubmodel(const string&); void setExternalForce( bool f ); void setForcePath(const string&); + void setContentsPath(const string&); void setForceStabilisation( bool val ); void setGroundOffset(double g); void setLoadOffset(double l); void setSlaved(bool s); void setSlavedLoad(bool s); - void setHitchPos(); void setPch (double e, double dt, double c); void setHdg (double az, double dt, double c); void setBnk(double r, double dt, double c); void setHt(double h, double dt, double c); - void setHitchVelocity(double dt); void setFormate(bool f); + void setParentNode(const SGPropertyNode_ptr); + void setParentPos(); double _getTime() const; double getRelBrgHitchToUser() const; @@ -102,7 +103,7 @@ public: SGVec3d getCartHitchPos() const; - bool getHtAGL(); + bool getHtAGL(double start); bool getSlaved() const; bool getSlavedLoad() const; @@ -114,13 +115,13 @@ public: SGPropertyNode_ptr _force_azimuth_node; SGPropertyNode_ptr _force_elevation_node; - SGGeod hitchpos; - double _height; double _ht_agl_ft; // height above ground level double _azimuth; // degrees true double _elevation; // degrees double _rotation; // degrees + double _speed_north_fps; + double _speed_east_fps; bool _formate_to_ac; @@ -153,7 +154,7 @@ private: double _Cd; // drag coefficient double _mass; // slugs bool _random; // modifier for Cd, life, az - double _randomness; // dimension for _random, only applies to life at present + double _randomness; // dimension for _random, only applies to life at present double _load_resistance; // ground load resistanc N/m^2 double _frictionFactor; // dimensionless modifier for Coefficient of Friction bool _solid; // if true ground is solid for FDMs @@ -163,41 +164,56 @@ private: bool _slave_load_to_ac;// if true, object will be slaved to the parent ac pos double _contents_lb; // contents of the object double _weight_lb; // weight of the object (no contents if appropriate) (lbs) + string _mat_name; 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 bool _external_force; // if true then apply external force - bool _report_expiry; + bool _report_expiry; SGPropertyNode_ptr _impact_report_node; // report node for impact and collision - SGPropertyNode_ptr _contents_node; // report node for impact and collision + SGPropertyNode_ptr _contents_node; // node for droptank etc. contents + SGPropertyNode_ptr _pnode; // node for parent model + SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters + SGPropertyNode_ptr _p_lat_node; + SGPropertyNode_ptr _p_lon_node; + SGPropertyNode_ptr _p_alt_node; + SGPropertyNode_ptr _p_ori_node; + SGPropertyNode_ptr _p_pch_node; + SGPropertyNode_ptr _p_rll_node; + SGPropertyNode_ptr _p_hdg_node; + SGPropertyNode_ptr _p_vel_node; + SGPropertyNode_ptr _p_spd_node; double _fuse_range; double _distance; double _dt_count; double _next_run; - string _name; - string _path; string _submodel; string _force_path; + string _contents_path; const SGMaterial* _material; void handle_collision(); - void handle_expiry(); + void handle_expiry(); void handle_impact(); void report_impact(double elevation, const FGAIBase *target = 0); void slaveToAC(double dt); void setContents(double c); void formateToAC(double dt); + void calcVSHS(); + void calcNE(); + void setOffsetPos(SGGeod pos, double heading, double pitch, double roll); + void setOffsetVelocity(double dt, SGGeod pos); SGVec3d getCartUserPos() const; + SGVec3d getCartOffsetPos(SGGeod pos, double heading, double pitch, double roll) const; double getDistanceLoadToHitch() const; double getElevLoadToHitch() const; double getBearingLoadToHitch() const; - double getRecip(double az); double getMass() const; @@ -206,9 +222,13 @@ private: double _load_offset; double _old_height; - SGVec3d _oldcarthitchPos; + SGVec3d _oldcartoffsetPos; + SGVec3d _oldcartPos; - SGGeod oldhitchpos; + SGGeod _parentpos; + SGGeod _oldpos; + SGGeod _offsetpos; + SGGeod _oldoffsetpos; }; diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index 460ccc2e0..ac8c4fb57 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -81,7 +81,7 @@ FGAIBase::FGAIBase(object_type ot) : delete_me = false; _impact_reported = false; _collision_reported = false; - _expiry_reported = false; + _expiry_reported = false; _subID = 0; @@ -154,9 +154,6 @@ void FGAIBase::readFromScenario(SGPropertyNode* scFileNode) SGPropertyNode* submodels = scFileNode->getChild("submodels"); if (submodels) { - //cout << "IN submodels path " << submodels->getStringValue("path") - // << "IN serviceable " << submodels->getBoolValue("serviceable") - // << endl; setServiceable(submodels->getBoolValue("serviceable", false)); setSMPath(submodels->getStringValue("path", "")); } @@ -232,8 +229,7 @@ void FGAIBase::initModel(osg::Node *node) if (model.valid()) { if( _path != ""){ props->setStringValue("submodels/path", _path.c_str()); - //props->setStringValue("submodel/path", _path.c_str()); - SG_LOG(SG_INPUT, SG_ALERT, "AIBase: submodels/path " << _path); + SG_LOG(SG_INPUT, SG_DEBUG, "AIBase: submodels/path " << _path); } fgSetString("/ai/models/model-added", props->getPath().c_str()); } else if (!model_path.empty()) { @@ -478,7 +474,7 @@ SGVec3d FGAIBase::getCartPosAt(const SGVec3d& _off) const { hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); // The offset converted to the usual body fixed coordinate system - // rotated to the earth fiexed coordinates axis + // rotated to the earth fixed coordinates axis SGVec3d off = hlTrans.backTransform(_off); // Add the position offset of the AIModel to gain the earth centered position diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index 68127fe4d..20bca1d9c 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -289,7 +289,6 @@ inline void FGAIBase::setPath(const char* model ) { } inline void FGAIBase::setSMPath(const string& p) { - cout << "setSMPath " << p <tie("orientation/roll-deg", SGRawValuePointer(&roll)); props->tie("orientation/true-heading-deg", SGRawValuePointer(&hdg)); + props->tie("submodels/serviceable", SGRawValuePointer(&serviceable)); props->tie("load/rel-brg-to-user-deg", SGRawValueMethods @@ -85,8 +87,17 @@ void FGAIWingman::bind() { props->tie("load/elev-to-user-deg", SGRawValueMethods (*this, &FGAIBallistic::getElevHitchToUser)); + props->tie("velocities/vertical-speed-fps", SGRawValuePointer(&vs)); + props->tie("velocities/true-airspeed-kt", + SGRawValuePointer(&speed)); + props->tie("velocities/speed-east-fps", + SGRawValuePointer(&_speed_east_fps)); + props->tie("velocities/speed-north-fps", + SGRawValuePointer(&_speed_north_fps)); + + props->tie("position/x-offset", SGRawValueMethods(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset)); props->tie("position/y-offset", @@ -107,17 +118,23 @@ void FGAIWingman::unbind() { props->untie("id"); props->untie("SubID"); - props->untie("position/altitude-ft"); - props->untie("position/latitude-deg"); - props->untie("position/longitude-deg"); - props->untie("orientation/pitch-deg"); props->untie("orientation/roll-deg"); props->untie("orientation/true-heading-deg"); + props->untie("submodels/serviceable"); + + props->untie("velocities/true-airspeed-kt"); + props->untie("velocities/vertical-speed-fps"); + props->untie("velocities/speed_east_fps"); + props->untie("velocities/speed_north_fps"); + props->untie("load/rel-brg-to-user-deg"); props->untie("load/elev-to-user-deg"); - props->untie("velocities/vertical-speed-fps"); + + props->untie("position/altitude-ft"); + props->untie("position/latitude-deg"); + props->untie("position/longitude-deg"); props->untie("position/x-offset"); props->untie("position/y-offset"); props->untie("position/z-offset"); @@ -141,11 +158,13 @@ bool FGAIWingman::init(bool search_in_AI_path) { roll = _rotation; _ht_agl_ft = 1e10; + props->setStringValue("submodels/path", _path.c_str()); return true; } void FGAIWingman::update(double dt) { FGAIBallistic::update(dt); +// cout << FGAIBase::_getName() << " update speed " << FGAIBase::_getSpeed() << endl; } // end AIWingman diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index 733bff3cb..7f9f80c02 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -28,12 +28,11 @@ const double FGSubmodelMgr::lbs_to_slugs = 0.031080950172; FGSubmodelMgr::FGSubmodelMgr() { - x_offset = y_offset = 0.0; - z_offset = -4.0; - pitch_offset = 2.0; + x_offset = y_offset = z_offset = 0.0; + pitch_offset = 0.0; yaw_offset = 0.0; - out[0] = out[1] = out[2] = 0; + //out[0] = out[1] = out[2] = 0; string contents_node; contrail_altitude = 30000; _count = 0; @@ -79,22 +78,23 @@ void FGSubmodelMgr::init() load(); -// _model_added_node = fgGetNode("ai/models/model-added", true); + //_model_added_node = fgGetNode("ai/models/model-added", true); //_model_added_node->addChangeListener(this, false); } void FGSubmodelMgr::postinit() { // postinit, so that the AI list is populated - loadAI(); + + loadAI(); while (_found_sub) loadSubmodels(); //TODO reload submodels if an MP ac joins - _model_added_node = fgGetNode("ai/models/model-added", true); - _model_added_node->addChangeListener(this, false); + //_model_added_node = fgGetNode("ai/models/model-added", true); + //_model_added_node->addChangeListener(this, false); } void FGSubmodelMgr::bind() @@ -124,17 +124,28 @@ void FGSubmodelMgr::update(double dt) sm_list_iterator end = sm_list.end(); for (; sm_list_itr != end; ++sm_list_itr) { - _impact = (*sm_list_itr)->_getImpactData(); - _hit = (*sm_list_itr)->_getCollisionData(); - _expiry = (*sm_list_itr)->_getExpiryData(); + FGAIBase::object_type object_type =(*sm_list_itr)->getType(); + + if (object_type != FGAIBase::otBallistic){// only work on ballistic objects + continue; // so continue + } int parent_subID = (*sm_list_itr)->_getSubID(); + int id = (*sm_list_itr)->getID(); + + if ( parent_subID == 0 || id == -1) // this entry in the list has no associated submodel + continue; // or is invalid so we can continue //SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " // << _hit <<" parent_subID " << parent_subID); - if ( parent_subID == 0) // this entry in the list has no associated submodel - continue; // so we can continue + _hit = (*sm_list_itr)->_getCollisionData(); + _impact = (*sm_list_itr)->_getImpactData(); + _expiry = (*sm_list_itr)->_getExpiryData(); + + //SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: " << (*sm_list_itr)->_getName() + // << " Impact " << _impact << " hit! " << _hit + // << " exipiry :-( " << _expiry ); if (_impact || _hit || _expiry) { // SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit @@ -157,8 +168,10 @@ void FGSubmodelMgr::update(double dt) (*submodel_iterator)->first_time = true; //cout << "Impact: parent SubID = child_ID elev " << _parent_elev << endl; - if (release(*submodel_iterator, dt)) + if (release(*submodel_iterator, dt)){ (*sm_list_itr)->setDie(true); + //cout << "Impact: set die" << (*sm_list_itr)->_getName() << endl; + } } @@ -188,8 +201,8 @@ void FGSubmodelMgr::update(double dt) trigger = _trigger_node->getBoolValue(); //cout << (*submodel_iterator)->name << "trigger node found " << trigger << endl; } else { - trigger = true; - //cout << (*submodel_iterator)->name << "trigger node not found " << trigger << endl; + trigger = false; + //cout << (*submodel_iterator)->name << " trigger node not found " << trigger << endl; } if (trigger && (*submodel_iterator)->count != 0) { @@ -201,6 +214,7 @@ void FGSubmodelMgr::update(double dt) "Submodels release: " << (*submodel_iterator)->id << " name " << (*submodel_iterator)->name << " count " << (*submodel_iterator)->count + << " slaved " << (*submodel_iterator)->slaved ); release(*submodel_iterator, dt); @@ -213,13 +227,20 @@ void FGSubmodelMgr::update(double dt) bool FGSubmodelMgr::release(submodel *sm, double dt) { - //cout << "release id " << sm->id << " name " << sm->name - //<< " first time " << sm->first_time << " repeat " << sm->repeat << - // endl; + //cout << "release id " << sm->id + // << " name " << sm->name + // << " first time " << sm->first_time + // << " repeat " << sm->repeat + // << " slaved " << sm->slaved + // << endl; // only run if first time or repeat is set to true if (!sm->first_time && !sm->repeat) { - //cout<< "not first time " << sm->first_time<< " repeat " << sm->repeat <name + // << " not first time " << sm->first_time + // << " repeat " << sm->repeat + // << " slaved " << sm->slaved + // << endl; return false; } @@ -244,6 +265,7 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) FGAIBallistic* ballist = new FGAIBallistic; ballist->setPath(sm->model.c_str()); ballist->setName(sm->name); + ballist->setSlaved(sm->slaved); ballist->setRandom(sm->random); ballist->setRandomness(sm->randomness); ballist->setLatitude(offsetpos.getLatitudeDeg()); @@ -273,6 +295,14 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) ballist->setForceStabilisation(sm->force_stabilised); ballist->setExternalForce(sm->ext_force); ballist->setForcePath(sm->force_path.c_str()); + ballist->setXoffset(sm->x_offset); + ballist->setYoffset(sm->y_offset); + ballist->setZoffset(sm->z_offset); + ballist->setPitchoffset(sm->pitch_offset); + ballist->setYawoffset(sm->yaw_offset); + ballist->setParentNode(_selected_ac); + ballist->setContentsNode(sm->contents_node); + ballist->setWeight(sm->weight); ai->attach(ballist); if (sm->count > 0) @@ -295,7 +325,7 @@ void FGSubmodelMgr::load() void FGSubmodelMgr::transform(submodel *sm) { // set initial conditions - if (sm->contents_node != 0) { + if (sm->contents_node != 0 && !sm->slaved) { // get the weight of the contents (lbs) and convert to mass (slugs) sm->contents = sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue(); //cout << "transform: contents " << sm->contents << endl; @@ -310,14 +340,14 @@ void FGSubmodelMgr::transform(submodel *sm) } else IC.mass = sm->weight * lbs_to_slugs; - // cout << "mass " << IC.mass << endl; + int id = sm->id; + int sub_id = sm->sub_id; + string name = sm->name; + if (sm->speed_node != 0) sm->speed = sm->speed_node->getDoubleValue(); - int id = sm->id; - int sub_id = sm->sub_id; - string name = sm->name; //cout << " name " << name << " id " << id << " sub id" << sub_id << endl; @@ -325,6 +355,7 @@ void FGSubmodelMgr::transform(submodel *sm) if (_impact || _hit || _expiry) { // set the data for a submodel tied to a submodel + _count++; IC.lat = _parent_lat; @@ -357,43 +388,25 @@ void FGSubmodelMgr::transform(submodel *sm) //cout << " set the data for a submodel tied to an AI Object " << id << endl; sm_list_iterator sm_list_itr = sm_list.begin(); sm_list_iterator end = sm_list.end(); - - while (sm_list_itr != end) { - int parent_id = (*sm_list_itr)->getID(); - - if (id != parent_id) { - ++sm_list_itr; - continue; - } - - //cout << " AI found id " << id << " alt " << (*sm_list_itr)->_getElevationFt()<< endl; - IC.lat = (*sm_list_itr)->_getLatitude(); - IC.lon = (*sm_list_itr)->_getLongitude(); - IC.alt = (*sm_list_itr)->_getElevationFt(); - IC.roll = (*sm_list_itr)->_getRoll(); - IC.elevation = (*sm_list_itr)->_getPitch(); - IC.azimuth = (*sm_list_itr)->_getHeading(); - IC.speed = (*sm_list_itr)->_getSpeed() * SG_KT_TO_FPS; - IC.speed_down_fps = -(*sm_list_itr)->_getVS_fps(); - IC.speed_east_fps = (*sm_list_itr)->_get_speed_east_fps(); - IC.speed_north_fps = (*sm_list_itr)->_get_speed_north_fps(); - - break; - - ++sm_list_itr; - } + setParentNode(id); } - /*cout << "heading " << IC.azimuth << endl ; - 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 << "Submodel: setting IC "<< name << endl; + //cout << "heading " << IC.azimuth << endl ; + //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 << "lat " << IC.lat; + //cout << "alt " << IC.alt << endl ; + // Set the Initial Conditions that are common to all types of parent IC.wind_from_east = _user_wind_from_east_node->getDoubleValue(); IC.wind_from_north = _user_wind_from_north_node->getDoubleValue(); +//cout << "wind e " << IC.wind_from_east << " n " << IC.wind_from_north << endl; + userpos.setLatitudeDeg(IC.lat); userpos.setLongitudeDeg(IC.lon); userpos.setElevationFt(IC.alt); @@ -440,7 +453,7 @@ void FGSubmodelMgr::transform(submodel *sm) // 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; + IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES; // cout << "azimuth1 " << IC.azimuth<random = entry_node->getBoolValue("random", false); sm->randomness = entry_node->getDoubleValue("randomness", 0.5); - - //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) sm->contents = sm->contents_node->getDoubleValue(); @@ -586,12 +598,12 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); sm->prop->tie("random", SGRawValuePointer(&(sm->random))); + sm->prop->tie("slaved", SGRawValuePointer(&(sm->slaved))); 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; string force_path = sm->force_path; sm->prop->setStringValue("force_path", force_path.c_str()); @@ -611,15 +623,15 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) SGPath config(globals->get_fg_root()); config.append(path); - SG_LOG(SG_GENERAL, SG_ALERT, "setSubData: path " << path); + SG_LOG(SG_GENERAL, SG_DEBUG, "setSubData: path " << path); try { - SG_LOG(SG_GENERAL, SG_ALERT, + SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Trying to read AI submodels file: " << config.str()); readProperties(config.str(), &root); } catch (const sg_exception &) { - SG_LOG(SG_GENERAL, SG_DEBUG, + SG_LOG(SG_GENERAL, SG_ALERT, "Submodels: Unable to read AI submodels file: " << config.str()); return; } @@ -666,7 +678,6 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) sm->random = entry_node->getBoolValue("random", false); sm->randomness = entry_node->getDoubleValue("randomness", 0.5); - //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) sm->contents = sm->contents_node->getDoubleValue(); @@ -694,11 +705,13 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); sm->prop->tie("random", SGRawValuePointer(&(sm->random))); + sm->prop->tie("slaved", SGRawValuePointer(&(sm->slaved))); + string name = sm->name; sm->prop->setStringValue("name", name.c_str()); string submodel = sm->submodel; - sm->prop->setStringValue("submodel", submodel.c_str()); + sm->prop->setStringValue("submodel-path", submodel.c_str()); // cout << " set submodel path AI" << submodel<< endl; string force_path = sm->force_path; @@ -726,10 +739,10 @@ void FGSubmodelMgr::loadSubmodels() if (!submodel.empty()) { //int id = (*submodel_iterator)->id; bool serviceable = true; - //SG_LOG(SG_GENERAL, SG_DEBUG, "found path sub sub " - // << submodel - // << " index " << index - // << "name " << (*submodel_iterator)->name); + SG_LOG(SG_GENERAL, SG_DEBUG, "found path sub sub " + << submodel + << " index " << index + << " name " << (*submodel_iterator)->name); if ((*submodel_iterator)->sub_id == 0){ (*submodel_iterator)->sub_id = index; @@ -744,6 +757,7 @@ void FGSubmodelMgr::loadSubmodels() subsubmodel_iterator = subsubmodels.begin(); while (subsubmodel_iterator != subsubmodels.end()) { + submodels.push_back(*subsubmodel_iterator); ++subsubmodel_iterator; } // end while @@ -752,14 +766,19 @@ void FGSubmodelMgr::loadSubmodels() //submodel_iterator = submodels.begin(); - //while (submodel_iterator != submodels.end()) { - //int id = (*submodel_iterator)->id; - //SG_LOG(SG_GENERAL, SG_DEBUG,"after pushback " - // << " id " << id - // << " name " << (*submodel_iterator)->name - // << " sub id " << (*submodel_iterator)->sub_id); + //int subcount = 0; - //++submodel_iterator; + //while (submodel_iterator != submodels.end()) { + // int id = (*submodel_iterator)->id; + // subcount++; + + // SG_LOG(SG_GENERAL, SG_ALERT,"after pushback " + // << " parent id " << id + // << " name " << (*submodel_iterator)->name + // << " sub id " << (*submodel_iterator)->sub_id + // << " subcount "<< subcount); + + // ++submodel_iterator; //} } @@ -767,7 +786,6 @@ SGVec3d FGSubmodelMgr::getCartOffsetPos() const{ // convert geodetic positions to geocentered SGVec3d cartuserPos = SGVec3d::fromGeod(userpos); - // Transform to the right coordinate frame, configuration is done in // the x-forward, y-right, z-up coordinates (feet), computation // in the simulation usual body x-forward, y-right, z-down coordinates @@ -793,7 +811,6 @@ SGVec3d FGSubmodelMgr::getCartOffsetPos() const{ // Add the position offset of the user model to get the geocentered position SGVec3d offsetPos = cartuserPos + off; - return offsetPos; } @@ -802,10 +819,14 @@ void FGSubmodelMgr::setOffsetPos(){ SGVec3d cartoffsetPos = getCartOffsetPos(); SGGeodesy::SGCartToGeod(cartoffsetPos, offsetpos); + + //cout << "OFFSET POS" << offsetpos.getElevationFt(); + } void FGSubmodelMgr::valueChanged(SGPropertyNode *prop) { +// cout << "LISTENER: " << endl; const char* _model_added = _model_added_node->getStringValue(); @@ -815,11 +836,12 @@ void FGSubmodelMgr::valueChanged(SGPropertyNode *prop) const char *cstr2b = "ballistic"; indexCh2b = str2.find( cstr2b, 0 ); +// cout << "model added - " << str2 <<" now do something "<< endl; + if (indexCh2b != string::npos ){ // we will ignore Ballistic Objects - there are potentially too many return; } else { - //cout << "model added - " << str2 <<" now do something "<< endl; - + SGPropertyNode *a_node = fgGetNode(_model_added, true ); SGPropertyNode *sub_node = a_node->getChild("submodels", 0, true); SGPropertyNode_ptr path_node = sub_node->getChild("path", 0, true); @@ -831,7 +853,7 @@ void FGSubmodelMgr::valueChanged(SGPropertyNode *prop) //cout << "subpath empty - return" << endl << endl; return; } else { - //cout << "subpath not empty: " << path << endl << endl; + //cout << "subpath found - loading" << endl << endl; SGPropertyNode_ptr ident_node = a_node->getChild("id", 0, true); int id = ident_node->getIntValue(); @@ -844,18 +866,59 @@ void FGSubmodelMgr::valueChanged(SGPropertyNode *prop) } - - - } +} +void FGSubmodelMgr::setParentNode(int id) { + const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true); + for (int i = ai->nChildren() - 1; i >= -1; i--) { + SGPropertyNode_ptr model; + if (i < 0) { // last iteration: selected model + model = _selected_ac; + } else { + model = ai->getChild(i); + string path = ai->getPath(); + const string name = model->getStringValue("name"); + int parent_id = model->getIntValue("id"); + if (!model->nChildren()){ + continue; + } + if (parent_id == id) { + _selected_ac = model; // save selected model for last iteration + break; + } + } + if (!model) + continue; + }// end for loop + if (_selected_ac != 0){ + + //cout << " parent node found"<< endl; + + const string name = _selected_ac->getStringValue("name"); + IC.lat = _selected_ac->getDoubleValue("position/latitude-deg"); + IC.lon = _selected_ac->getDoubleValue("position/longitude-deg"); + IC.alt = _selected_ac->getDoubleValue("position/altitude-ft"); + IC.roll = _selected_ac->getDoubleValue("orientation/roll-deg"); + IC.elevation = _selected_ac->getDoubleValue("orientation/pitch-deg"); + IC.azimuth = _selected_ac->getDoubleValue("orientation/true-heading-deg"); + IC.speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt") * SG_KT_TO_FPS; + IC.speed_down_fps = -_selected_ac->getDoubleValue("velocities/vertical-speed-fps"); + IC.speed_east_fps = _selected_ac->getDoubleValue("velocities/speed-east-fps"); + IC.speed_north_fps = _selected_ac->getDoubleValue("velocities/speed-north-fps"); + + //cout << name << " IC.speed " << IC.speed << endl; + + } else { + SG_LOG(SG_GENERAL, SG_ALERT, "AISubmodel: parent node not found "); + } } // end of submodel.cxx diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index 95e62f6b7..e6c9f9e1f 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -58,6 +58,7 @@ public: bool first_time; double cd; double weight; + double mass; double contents; bool aero_stabilised; int id; @@ -95,6 +96,7 @@ public: double mass; int id; bool no_roll; + int parent_id; } IC_struct; FGSubmodelMgr(); @@ -144,6 +146,8 @@ private: double _parent_pitch; double _parent_roll; double _parent_speed; + double _parent_ID; + double _x_offset; double _y_offset; double _z_offset; @@ -180,6 +184,7 @@ private: SGPropertyNode_ptr props; SGPropertyNode_ptr _model_added_node; SGPropertyNode_ptr _path_node; + SGPropertyNode_ptr _selected_ac; FGAIManager* ai; @@ -199,6 +204,7 @@ private: void setSubData(int id, string& path, bool serviceable); void valueChanged (SGPropertyNode *); void transform(submodel *); + void setParentNode(int parent_id); bool release(submodel *, double dt); From 118036cdda76ec5557ee4729fb75637a8fc86223 Mon Sep 17 00:00:00 2001 From: Vivian Meazza Date: Mon, 30 Aug 2010 17:15:34 +0100 Subject: [PATCH 3/7] Make the parameter isTanker controllable Signed-off-by: Vivian Meazza --- src/AIModel/AIMultiplayer.cxx | 1012 +++++++++++++++++---------------- 1 file changed, 508 insertions(+), 504 deletions(-) diff --git a/src/AIModel/AIMultiplayer.cxx b/src/AIModel/AIMultiplayer.cxx index d2fb505c0..bd5cc0a90 100644 --- a/src/AIModel/AIMultiplayer.cxx +++ b/src/AIModel/AIMultiplayer.cxx @@ -1,504 +1,508 @@ -// FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft -// -// Based on FGAIAircraft -// Written by David Culp, started October 2003. -// Also by Gregor Richards, started December 2005. -// -// Copyright (C) 2003 David P. Culp - davidculp2@comcast.net -// Copyright (C) 2005 Gregor Richards -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of the -// License, or (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, but -// WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. - -#ifdef HAVE_CONFIG_H -# include -#endif - -#include - -#include "AIMultiplayer.hxx" - -#include - -// #define SG_DEBUG SG_ALERT - -FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) { - no_roll = false; - - mTimeOffsetSet = false; - mAllowExtrapolation = true; - mLagAdjustSystemSpeed = 10; - - aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT); - -} - - -FGAIMultiplayer::~FGAIMultiplayer() { -} - -bool FGAIMultiplayer::init(bool search_in_AI_path) { - props->setStringValue("sim/model/path", model_path.c_str()); - //refuel_node = fgGetNode("systems/refuel/contact", true); - isTanker = false; // do this until this property is - // passed over the net - - string str1 = _getCallsign(); - string str2 = "MOBIL"; - - string::size_type loc1= str1.find( str2, 0 ); - if ( (loc1 != string::npos && str2 != "") ){ - // cout << " string found " << str2 << " in " << str1 << endl; - isTanker = true; - // cout << "isTanker " << isTanker << " " << mCallSign <tie("refuel/contact", SGRawValuePointer(&contact)); - props->setBoolValue("tanker",isTanker); - - props->tie("controls/invisible", - SGRawValuePointer(&invisible)); - -#define AIMPROProp(type, name) \ -SGRawValueMethods(*this, &FGAIMultiplayer::get##name) - -#define AIMPRWProp(type, name) \ -SGRawValueMethods(*this, \ - &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name) - - //props->tie("callsign", AIMPROProp(const char *, CallSign)); - - props->tie("controls/allow-extrapolation", - AIMPRWProp(bool, AllowExtrapolation)); - props->tie("controls/lag-adjust-system-speed", - AIMPRWProp(double, LagAdjustSystemSpeed)); - - -#undef AIMPROProp -#undef AIMPRWProp -} - -void FGAIMultiplayer::unbind() { - FGAIBase::unbind(); - - //props->untie("callsign"); - props->untie("controls/allow-extrapolation"); - props->untie("controls/lag-adjust-system-speed"); - props->untie("controls/invisible"); - props->untie("refuel/contact"); -} - -void FGAIMultiplayer::update(double dt) -{ - using namespace simgear; - - if (dt <= 0) - return; - - FGAIBase::update(dt); - - // Check if we already got data - if (mMotionInfo.empty()) - return; - - // The current simulation time we need to update for, - // note that the simulation time is updated before calling all the - // update methods. Thus it contains the time intervals *end* time - double curtime = globals->get_sim_time_sec(); - - // Get the last available time - MotionInfo::reverse_iterator it = mMotionInfo.rbegin(); - double curentPkgTime = it->second.time; - - // Dynamically optimize the time offset between the feeder and the client - // Well, 'dynamically' means that the dynamic of that update must be very - // slow. You would otherwise notice huge jumps in the multiplayer models. - // The reason is that we want to avoid huge extrapolation times since - // extrapolation is highly error prone. For that we need something - // approaching the average latency of the packets. This first order lag - // component will provide this. We just take the error of the currently - // requested time to the most recent available packet. This is the - // target we want to reach in average. - double lag = it->second.lag; - if (!mTimeOffsetSet) { - mTimeOffsetSet = true; - mTimeOffset = curentPkgTime - curtime - lag; - } else { - double offset = curentPkgTime - curtime - lag; - if ((!mAllowExtrapolation && offset + lag < mTimeOffset) - || (offset - 10 > mTimeOffset)) { - mTimeOffset = offset; - SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to " - "avoid extrapolation: time offset = " << mTimeOffset); - } else { - // the error of the offset, respectively the negative error to avoid - // a minus later ... - double err = offset - mTimeOffset; - // limit errors leading to shorter lag values somehow, that is late - // arriving packets will pessimize the overall lag much more than - // early packets will shorten the overall lag - double sysSpeed; - if (err < 0) { - // Ok, we have some very late packets and nothing newer increase the - // lag by the given speedadjust - sysSpeed = mLagAdjustSystemSpeed*err; - } else { - // We have a too pessimistic display delay shorten that a small bit - sysSpeed = SGMiscd::min(0.1*err*err, 0.5); - } - - // simple euler integration for that first order system including some - // overshooting guard to prevent to aggressive system speeds - // (stiff systems) to explode the systems state - double systemIncrement = dt*sysSpeed; - if (fabs(err) < fabs(systemIncrement)) - systemIncrement = err; - mTimeOffset += systemIncrement; - - SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = " - << mTimeOffset << ", expected longitudinal position error due to " - " current adjustment of the offset: " - << fabs(norm(it->second.linearVel)*systemIncrement)); - } - } - - - // Compute the time in the feeders time scale which fits the current time - // we need to - double tInterp = curtime + mTimeOffset; - - SGVec3d ecPos; - SGQuatf ecOrient; - - if (tInterp <= curentPkgTime) { - // Ok, we need a time prevous to the last available packet, - // that is good ... - - // Find the first packet before the target time - MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp); - if (nextIt == mMotionInfo.begin()) { - SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!"); - // We have no packet before the target time, just use the first one - MotionInfo::iterator firstIt = mMotionInfo.begin(); - ecPos = firstIt->second.position; - ecOrient = firstIt->second.orientation; - speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0; - - std::vector::const_iterator firstPropIt; - std::vector::const_iterator firstPropItEnd; - firstPropIt = firstIt->second.properties.begin(); - firstPropItEnd = firstIt->second.properties.end(); - while (firstPropIt != firstPropItEnd) { - //cout << " Setting property..." << (*firstPropIt)->id; - PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id); - if (pIt != mPropertyMap.end()) - { - //cout << "Found " << pIt->second->getPath() << ":"; - switch ((*firstPropIt)->type) { - case props::INT: - case props::BOOL: - case props::LONG: - pIt->second->setIntValue((*firstPropIt)->int_value); - //cout << "Int: " << (*firstPropIt)->int_value << "\n"; - break; - case props::FLOAT: - case props::DOUBLE: - pIt->second->setFloatValue((*firstPropIt)->float_value); - //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; - break; - case props::STRING: - case props::UNSPECIFIED: - pIt->second->setStringValue((*firstPropIt)->string_value); - //cout << "Str: " << (*firstPropIt)->string_value << "\n"; - break; - default: - // FIXME - currently defaults to float values - pIt->second->setFloatValue((*firstPropIt)->float_value); - //cout << "Unknown: " << (*firstPropIt)->float_value << "\n"; - break; - } - } - else - { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); - } - ++firstPropIt; - } - - } else { - // Ok, we have really found something where our target time is in between - // do interpolation here - MotionInfo::iterator prevIt = nextIt; - --prevIt; - - // Interpolation coefficient is between 0 and 1 - double intervalStart = prevIt->second.time; - double intervalEnd = nextIt->second.time; - double intervalLen = intervalEnd - intervalStart; - double tau = (tInterp - intervalStart)/intervalLen; - - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: [" - << intervalStart << ", " << intervalEnd << "], intervalLen = " - << intervalLen << ", interpolation parameter = " << tau); - - // Here we do just linear interpolation on the position - ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position); - ecOrient = interpolate((float)tau, prevIt->second.orientation, - nextIt->second.orientation); - speed = norm((1-tau)*prevIt->second.linearVel - + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0; - - if (prevIt->second.properties.size() - == nextIt->second.properties.size()) { - std::vector::const_iterator prevPropIt; - std::vector::const_iterator prevPropItEnd; - std::vector::const_iterator nextPropIt; - std::vector::const_iterator nextPropItEnd; - prevPropIt = prevIt->second.properties.begin(); - prevPropItEnd = prevIt->second.properties.end(); - nextPropIt = nextIt->second.properties.begin(); - nextPropItEnd = nextIt->second.properties.end(); - while (prevPropIt != prevPropItEnd) { - PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id); - //cout << " Setting property..." << (*prevPropIt)->id; - - if (pIt != mPropertyMap.end()) - { - //cout << "Found " << pIt->second->getPath() << ":"; - - int ival; - float val; - switch ((*prevPropIt)->type) { - case props::INT: - case props::BOOL: - case props::LONG: - ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) + - tau*((double) (*nextPropIt)->int_value)); - pIt->second->setIntValue(ival); - //cout << "Int: " << ival << "\n"; - break; - case props::FLOAT: - case props::DOUBLE: - val = (1-tau)*(*prevPropIt)->float_value + - tau*(*nextPropIt)->float_value; - //cout << "Flo: " << val << "\n"; - pIt->second->setFloatValue(val); - break; - case props::STRING: - case props::UNSPECIFIED: - //cout << "Str: " << (*nextPropIt)->string_value << "\n"; - pIt->second->setStringValue((*nextPropIt)->string_value); - break; - default: - // FIXME - currently defaults to float values - val = (1-tau)*(*prevPropIt)->float_value + - tau*(*nextPropIt)->float_value; - //cout << "Unk: " << val << "\n"; - pIt->second->setFloatValue(val); - break; - } - } - else - { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); - } - - ++prevPropIt; - ++nextPropIt; - } - } - - // Now throw away too old data - if (prevIt != mMotionInfo.begin()) - { - --prevIt; - - MotionInfo::iterator delIt; - delIt = mMotionInfo.begin(); - - while (delIt != prevIt) - { - std::vector::const_iterator propIt; - std::vector::const_iterator propItEnd; - propIt = delIt->second.properties.begin(); - propItEnd = delIt->second.properties.end(); - - //cout << "Deleting data\n"; - - while (propIt != propItEnd) - { - delete *propIt; - propIt++; - } - - delIt++; - } - - mMotionInfo.erase(mMotionInfo.begin(), prevIt); - } - } - } else { - // Ok, we need to predict the future, so, take the best data we can have - // and do some eom computation to guess that for now. - FGExternalMotionData motionInfo = it->second; - - // The time to predict, limit to 5 seconds - double t = tInterp - motionInfo.time; - t = SGMisc::min(t, 5); - - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: " - "extrapolation time = " << t); - - // Do a few explicit euler steps with the constant acceleration's - // This must be sufficient ... - ecPos = motionInfo.position; - ecOrient = motionInfo.orientation; - SGVec3f linearVel = motionInfo.linearVel; - SGVec3f angularVel = motionInfo.angularVel; - while (0 < t) { - double h = 1e-1; - if (t < h) - h = t; - - SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel)); - ecPos += h*ecVel; - ecOrient += h*ecOrient.derivative(angularVel); - - linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel); - angularVel += h*motionInfo.angularAccel; - - t -= h; - } - - std::vector::const_iterator firstPropIt; - std::vector::const_iterator firstPropItEnd; - speed = norm(linearVel) * SG_METER_TO_NM * 3600.0; - firstPropIt = it->second.properties.begin(); - firstPropItEnd = it->second.properties.end(); - while (firstPropIt != firstPropItEnd) { - PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id); - //cout << " Setting property..." << (*firstPropIt)->id; - - if (pIt != mPropertyMap.end()) - { - switch ((*firstPropIt)->type) { - case props::INT: - case props::BOOL: - case props::LONG: - pIt->second->setIntValue((*firstPropIt)->int_value); - //cout << "Int: " << (*firstPropIt)->int_value << "\n"; - break; - case props::FLOAT: - case props::DOUBLE: - pIt->second->setFloatValue((*firstPropIt)->float_value); - //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; - break; - case props::STRING: - case props::UNSPECIFIED: - pIt->second->setStringValue((*firstPropIt)->string_value); - //cout << "Str: " << (*firstPropIt)->string_value << "\n"; - break; - default: - // FIXME - currently defaults to float values - pIt->second->setFloatValue((*firstPropIt)->float_value); - //cout << "Unk: " << (*firstPropIt)->float_value << "\n"; - break; - } - } - else - { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); - } - - ++firstPropIt; - } - } - - // extract the position - pos = SGGeod::fromCart(ecPos); - altitude_ft = pos.getElevationFt(); - - // The quaternion rotating from the earth centered frame to the - // horizontal local frame - SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(), - (float)pos.getLatitudeRad()); - // The orientation wrt the horizontal local frame - SGQuatf hlOr = conj(qEc2Hl)*ecOrient; - float hDeg, pDeg, rDeg; - hlOr.getEulerDeg(hDeg, pDeg, rDeg); - hdg = hDeg; - roll = rDeg; - pitch = pDeg; - - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: " - << ecPos << ", " << hlOr); - - //###########################// - // do calculations for radar // - //###########################// - double range_ft2 = UpdateRadar(manager); - - //************************************// - // Tanker code // - //************************************// - - - if ( isTanker) { - if ( (range_ft2 < 250.0 * 250.0) && - (y_shift > 0.0) && - (elevation > 0.0) ){ - // refuel_node->setBoolValue(true); - contact = true; - } else { - // refuel_node->setBoolValue(false); - contact = false; - } - } else { - contact = false; - } - - Transform(); -} - -void -FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo, - long stamp) -{ - mLastTimestamp = stamp; - - if (!mMotionInfo.empty()) { - double diff = motionInfo.time - mMotionInfo.rbegin()->first; - - // packet is very old -- MP has probably reset (incl. his timebase) - if (diff < -10.0) - mMotionInfo.clear(); - - // drop packets arriving out of order - else if (diff < 0.0) - return; - } - mMotionInfo[motionInfo.time] = motionInfo; -} - -void -FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val) -{ - SGPropertyNode* pNode = props->getChild(prop.c_str(), true); - pNode->setDoubleValue(val); -} +// FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft +// +// Based on FGAIAircraft +// Written by David Culp, started October 2003. +// Also by Gregor Richards, started December 2005. +// +// Copyright (C) 2003 David P. Culp - davidculp2@comcast.net +// Copyright (C) 2005 Gregor Richards +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include "AIMultiplayer.hxx" + +#include + +// #define SG_DEBUG SG_ALERT + +FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) { + no_roll = false; + + mTimeOffsetSet = false; + mAllowExtrapolation = true; + mLagAdjustSystemSpeed = 10; + + aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT); + +} + + +FGAIMultiplayer::~FGAIMultiplayer() { +} + +bool FGAIMultiplayer::init(bool search_in_AI_path) { + props->setStringValue("sim/model/path", model_path.c_str()); + //refuel_node = fgGetNode("systems/refuel/contact", true); + isTanker = false; // do this until this property is + // passed over the net + + string str1 = _getCallsign(); + string str2 = "MOBIL"; + + string::size_type loc1= str1.find( str2, 0 ); + if ( (loc1 != string::npos && str2 != "") ){ + // cout << " string found " << str2 << " in " << str1 << endl; + isTanker = true; + // cout << "isTanker " << isTanker << " " << mCallSign <tie("refuel/contact", SGRawValuePointer(&contact)); + props->tie("tanker", SGRawValuePointer(&isTanker)); + + props->tie("controls/invisible", + SGRawValuePointer(&invisible)); + +#define AIMPROProp(type, name) \ +SGRawValueMethods(*this, &FGAIMultiplayer::get##name) + +#define AIMPRWProp(type, name) \ +SGRawValueMethods(*this, \ + &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name) + + //props->tie("callsign", AIMPROProp(const char *, CallSign)); + + props->tie("controls/allow-extrapolation", + AIMPRWProp(bool, AllowExtrapolation)); + props->tie("controls/lag-adjust-system-speed", + AIMPRWProp(double, LagAdjustSystemSpeed)); + + +#undef AIMPROProp +#undef AIMPRWProp +} + +void FGAIMultiplayer::unbind() { + FGAIBase::unbind(); + + //props->untie("callsign"); + props->untie("controls/allow-extrapolation"); + props->untie("controls/lag-adjust-system-speed"); + props->untie("controls/invisible"); + props->untie("refuel/contact"); +} + +void FGAIMultiplayer::update(double dt) +{ + using namespace simgear; + + if (dt <= 0) + return; + + FGAIBase::update(dt); + + // Check if we already got data + if (mMotionInfo.empty()) + return; + + // The current simulation time we need to update for, + // note that the simulation time is updated before calling all the + // update methods. Thus it contains the time intervals *end* time + double curtime = globals->get_sim_time_sec(); + + // Get the last available time + MotionInfo::reverse_iterator it = mMotionInfo.rbegin(); + double curentPkgTime = it->second.time; + + // Dynamically optimize the time offset between the feeder and the client + // Well, 'dynamically' means that the dynamic of that update must be very + // slow. You would otherwise notice huge jumps in the multiplayer models. + // The reason is that we want to avoid huge extrapolation times since + // extrapolation is highly error prone. For that we need something + // approaching the average latency of the packets. This first order lag + // component will provide this. We just take the error of the currently + // requested time to the most recent available packet. This is the + // target we want to reach in average. + double lag = it->second.lag; + if (!mTimeOffsetSet) { + mTimeOffsetSet = true; + mTimeOffset = curentPkgTime - curtime - lag; + } else { + double offset = curentPkgTime - curtime - lag; + if ((!mAllowExtrapolation && offset + lag < mTimeOffset) + || (offset - 10 > mTimeOffset)) { + mTimeOffset = offset; + SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to " + "avoid extrapolation: time offset = " << mTimeOffset); + } else { + // the error of the offset, respectively the negative error to avoid + // a minus later ... + double err = offset - mTimeOffset; + // limit errors leading to shorter lag values somehow, that is late + // arriving packets will pessimize the overall lag much more than + // early packets will shorten the overall lag + double sysSpeed; + if (err < 0) { + // Ok, we have some very late packets and nothing newer increase the + // lag by the given speedadjust + sysSpeed = mLagAdjustSystemSpeed*err; + } else { + // We have a too pessimistic display delay shorten that a small bit + sysSpeed = SGMiscd::min(0.1*err*err, 0.5); + } + + // simple euler integration for that first order system including some + // overshooting guard to prevent to aggressive system speeds + // (stiff systems) to explode the systems state + double systemIncrement = dt*sysSpeed; + if (fabs(err) < fabs(systemIncrement)) + systemIncrement = err; + mTimeOffset += systemIncrement; + + SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = " + << mTimeOffset << ", expected longitudinal position error due to " + " current adjustment of the offset: " + << fabs(norm(it->second.linearVel)*systemIncrement)); + } + } + + + // Compute the time in the feeders time scale which fits the current time + // we need to + double tInterp = curtime + mTimeOffset; + + SGVec3d ecPos; + SGQuatf ecOrient; + + if (tInterp <= curentPkgTime) { + // Ok, we need a time prevous to the last available packet, + // that is good ... + + // Find the first packet before the target time + MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp); + if (nextIt == mMotionInfo.begin()) { + SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!"); + // We have no packet before the target time, just use the first one + MotionInfo::iterator firstIt = mMotionInfo.begin(); + ecPos = firstIt->second.position; + ecOrient = firstIt->second.orientation; + speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0; + + std::vector::const_iterator firstPropIt; + std::vector::const_iterator firstPropItEnd; + firstPropIt = firstIt->second.properties.begin(); + firstPropItEnd = firstIt->second.properties.end(); + while (firstPropIt != firstPropItEnd) { + //cout << " Setting property..." << (*firstPropIt)->id; + PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id); + if (pIt != mPropertyMap.end()) + { + //cout << "Found " << pIt->second->getPath() << ":"; + switch ((*firstPropIt)->type) { + case props::INT: + case props::BOOL: + case props::LONG: + pIt->second->setIntValue((*firstPropIt)->int_value); + //cout << "Int: " << (*firstPropIt)->int_value << "\n"; + break; + case props::FLOAT: + case props::DOUBLE: + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; + break; + case props::STRING: + case props::UNSPECIFIED: + pIt->second->setStringValue((*firstPropIt)->string_value); + //cout << "Str: " << (*firstPropIt)->string_value << "\n"; + break; + default: + // FIXME - currently defaults to float values + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Unknown: " << (*firstPropIt)->float_value << "\n"; + break; + } + } + else + { + SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); + } + ++firstPropIt; + } + + } else { + // Ok, we have really found something where our target time is in between + // do interpolation here + MotionInfo::iterator prevIt = nextIt; + --prevIt; + + // Interpolation coefficient is between 0 and 1 + double intervalStart = prevIt->second.time; + double intervalEnd = nextIt->second.time; + double intervalLen = intervalEnd - intervalStart; + double tau = (tInterp - intervalStart)/intervalLen; + + SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: [" + << intervalStart << ", " << intervalEnd << "], intervalLen = " + << intervalLen << ", interpolation parameter = " << tau); + + // Here we do just linear interpolation on the position + ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position); + ecOrient = interpolate((float)tau, prevIt->second.orientation, + nextIt->second.orientation); + speed = norm((1-tau)*prevIt->second.linearVel + + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0; + + if (prevIt->second.properties.size() + == nextIt->second.properties.size()) { + std::vector::const_iterator prevPropIt; + std::vector::const_iterator prevPropItEnd; + std::vector::const_iterator nextPropIt; + std::vector::const_iterator nextPropItEnd; + prevPropIt = prevIt->second.properties.begin(); + prevPropItEnd = prevIt->second.properties.end(); + nextPropIt = nextIt->second.properties.begin(); + nextPropItEnd = nextIt->second.properties.end(); + while (prevPropIt != prevPropItEnd) { + PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id); + //cout << " Setting property..." << (*prevPropIt)->id; + + if (pIt != mPropertyMap.end()) + { + //cout << "Found " << pIt->second->getPath() << ":"; + + int ival; + float val; + switch ((*prevPropIt)->type) { + case props::INT: + case props::BOOL: + case props::LONG: + ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) + + tau*((double) (*nextPropIt)->int_value)); + pIt->second->setIntValue(ival); + //cout << "Int: " << ival << "\n"; + break; + case props::FLOAT: + case props::DOUBLE: + val = (1-tau)*(*prevPropIt)->float_value + + tau*(*nextPropIt)->float_value; + //cout << "Flo: " << val << "\n"; + pIt->second->setFloatValue(val); + break; + case props::STRING: + case props::UNSPECIFIED: + //cout << "Str: " << (*nextPropIt)->string_value << "\n"; + pIt->second->setStringValue((*nextPropIt)->string_value); + break; + default: + // FIXME - currently defaults to float values + val = (1-tau)*(*prevPropIt)->float_value + + tau*(*nextPropIt)->float_value; + //cout << "Unk: " << val << "\n"; + pIt->second->setFloatValue(val); + break; + } + } + else + { + SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); + } + + ++prevPropIt; + ++nextPropIt; + } + } + + // Now throw away too old data + if (prevIt != mMotionInfo.begin()) + { + --prevIt; + + MotionInfo::iterator delIt; + delIt = mMotionInfo.begin(); + + while (delIt != prevIt) + { + std::vector::const_iterator propIt; + std::vector::const_iterator propItEnd; + propIt = delIt->second.properties.begin(); + propItEnd = delIt->second.properties.end(); + + //cout << "Deleting data\n"; + + while (propIt != propItEnd) + { + delete *propIt; + propIt++; + } + + delIt++; + } + + mMotionInfo.erase(mMotionInfo.begin(), prevIt); + } + } + } else { + // Ok, we need to predict the future, so, take the best data we can have + // and do some eom computation to guess that for now. + FGExternalMotionData motionInfo = it->second; + + // The time to predict, limit to 5 seconds + double t = tInterp - motionInfo.time; + t = SGMisc::min(t, 5); + + SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: " + "extrapolation time = " << t); + + // Do a few explicit euler steps with the constant acceleration's + // This must be sufficient ... + ecPos = motionInfo.position; + ecOrient = motionInfo.orientation; + SGVec3f linearVel = motionInfo.linearVel; + SGVec3f angularVel = motionInfo.angularVel; + while (0 < t) { + double h = 1e-1; + if (t < h) + h = t; + + SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel)); + ecPos += h*ecVel; + ecOrient += h*ecOrient.derivative(angularVel); + + linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel); + angularVel += h*motionInfo.angularAccel; + + t -= h; + } + + std::vector::const_iterator firstPropIt; + std::vector::const_iterator firstPropItEnd; + speed = norm(linearVel) * SG_METER_TO_NM * 3600.0; + firstPropIt = it->second.properties.begin(); + firstPropItEnd = it->second.properties.end(); + while (firstPropIt != firstPropItEnd) { + PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id); + //cout << " Setting property..." << (*firstPropIt)->id; + + if (pIt != mPropertyMap.end()) + { + switch ((*firstPropIt)->type) { + case props::INT: + case props::BOOL: + case props::LONG: + pIt->second->setIntValue((*firstPropIt)->int_value); + //cout << "Int: " << (*firstPropIt)->int_value << "\n"; + break; + case props::FLOAT: + case props::DOUBLE: + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; + break; + case props::STRING: + case props::UNSPECIFIED: + pIt->second->setStringValue((*firstPropIt)->string_value); + //cout << "Str: " << (*firstPropIt)->string_value << "\n"; + break; + default: + // FIXME - currently defaults to float values + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Unk: " << (*firstPropIt)->float_value << "\n"; + break; + } + } + else + { + SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); + } + + ++firstPropIt; + } + } + + // extract the position + pos = SGGeod::fromCart(ecPos); + altitude_ft = pos.getElevationFt(); + + // The quaternion rotating from the earth centered frame to the + // horizontal local frame + SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(), + (float)pos.getLatitudeRad()); + // The orientation wrt the horizontal local frame + SGQuatf hlOr = conj(qEc2Hl)*ecOrient; + float hDeg, pDeg, rDeg; + hlOr.getEulerDeg(hDeg, pDeg, rDeg); + hdg = hDeg; + roll = rDeg; + pitch = pDeg; + + SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: " + << ecPos << ", " << hlOr); + + //###########################// + // do calculations for radar // + //###########################// + double range_ft2 = UpdateRadar(manager); + + //************************************// + // Tanker code // + //************************************// + + + if ( isTanker) { + //cout << "IS tanker "; + if ( (range_ft2 < 250.0 * 250.0) && + (y_shift > 0.0) && + (elevation > 0.0) ){ + // refuel_node->setBoolValue(true); + //cout << "in contact" << endl; + contact = true; + } else { + // refuel_node->setBoolValue(false); + //cout << "not in contact" << endl; + contact = false; + } + } else { + //cout << "NOT tanker " << endl; + contact = false; + } + + Transform(); +} + +void +FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo, + long stamp) +{ + mLastTimestamp = stamp; + + if (!mMotionInfo.empty()) { + double diff = motionInfo.time - mMotionInfo.rbegin()->first; + + // packet is very old -- MP has probably reset (incl. his timebase) + if (diff < -10.0) + mMotionInfo.clear(); + + // drop packets arriving out of order + else if (diff < 0.0) + return; + } + mMotionInfo[motionInfo.time] = motionInfo; +} + +void +FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val) +{ + SGPropertyNode* pNode = props->getChild(prop.c_str(), true); + pNode->setDoubleValue(val); +} From 792cb2701f8661748344436d1d8654722fcb05b7 Mon Sep 17 00:00:00 2001 From: Vivian Meazza Date: Mon, 30 Aug 2010 17:16:30 +0100 Subject: [PATCH 4/7] Add tanker property to list of standard properties Signed-off-by: Vivian Meazza --- src/MultiPlayer/multiplaymgr.cxx | 2088 +++++++++++++++--------------- 1 file changed, 1045 insertions(+), 1043 deletions(-) diff --git a/src/MultiPlayer/multiplaymgr.cxx b/src/MultiPlayer/multiplaymgr.cxx index fe1f4c399..8d4783315 100644 --- a/src/MultiPlayer/multiplaymgr.cxx +++ b/src/MultiPlayer/multiplaymgr.cxx @@ -1,1043 +1,1045 @@ -////////////////////////////////////////////////////////////////////// -// -// multiplaymgr.hpp -// -// Written by Duncan McCreanor, started February 2003. -// duncan.mccreanor@airservicesaustralia.com -// -// Copyright (C) 2003 Airservices Australia -// Copyright (C) 2005 Oliver Schroeder -// Copyright (C) 2006 Mathias Froehlich -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of the -// License, or (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, but -// WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -// -// $Id$ -// -////////////////////////////////////////////////////////////////////// - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include -#include -#include -#include // isNaN -#include - -#include -#include -#include -#include - -#include -#include
-#include "multiplaymgr.hxx" -#include "mpmessages.hxx" - -using namespace std; - -#define MAX_PACKET_SIZE 1200 -#define MAX_TEXT_SIZE 128 - -// These constants are provided so that the ident -// command can list file versions -const char sMULTIPLAYMGR_BID[] = "$Id$"; -const char sMULTIPLAYMGR_HID[] = MULTIPLAYTXMGR_HID; - -// A static map of protocol property id values to property paths, -// This should be extendable dynamically for every specific aircraft ... -// For now only that static list -const FGMultiplayMgr::IdPropertyList -FGMultiplayMgr::sIdPropertyList[] = { - {100, "surface-positions/left-aileron-pos-norm", simgear::props::FLOAT}, - {101, "surface-positions/right-aileron-pos-norm", simgear::props::FLOAT}, - {102, "surface-positions/elevator-pos-norm", simgear::props::FLOAT}, - {103, "surface-positions/rudder-pos-norm", simgear::props::FLOAT}, - {104, "surface-positions/flap-pos-norm", simgear::props::FLOAT}, - {105, "surface-positions/speedbrake-pos-norm", simgear::props::FLOAT}, - {106, "gear/tailhook/position-norm", simgear::props::FLOAT}, - {107, "gear/launchbar/position-norm", simgear::props::FLOAT}, - {108, "gear/launchbar/state", simgear::props::STRING}, - {109, "gear/launchbar/holdback-position-norm", simgear::props::FLOAT}, - {110, "canopy/position-norm", simgear::props::FLOAT}, - {111, "surface-positions/wing-pos-norm", simgear::props::FLOAT}, - {112, "surface-positions/wing-fold-pos-norm", simgear::props::FLOAT}, - - {200, "gear/gear[0]/compression-norm", simgear::props::FLOAT}, - {201, "gear/gear[0]/position-norm", simgear::props::FLOAT}, - {210, "gear/gear[1]/compression-norm", simgear::props::FLOAT}, - {211, "gear/gear[1]/position-norm", simgear::props::FLOAT}, - {220, "gear/gear[2]/compression-norm", simgear::props::FLOAT}, - {221, "gear/gear[2]/position-norm", simgear::props::FLOAT}, - {230, "gear/gear[3]/compression-norm", simgear::props::FLOAT}, - {231, "gear/gear[3]/position-norm", simgear::props::FLOAT}, - {240, "gear/gear[4]/compression-norm", simgear::props::FLOAT}, - {241, "gear/gear[4]/position-norm", simgear::props::FLOAT}, - - {300, "engines/engine[0]/n1", simgear::props::FLOAT}, - {301, "engines/engine[0]/n2", simgear::props::FLOAT}, - {302, "engines/engine[0]/rpm", simgear::props::FLOAT}, - {310, "engines/engine[1]/n1", simgear::props::FLOAT}, - {311, "engines/engine[1]/n2", simgear::props::FLOAT}, - {312, "engines/engine[1]/rpm", simgear::props::FLOAT}, - {320, "engines/engine[2]/n1", simgear::props::FLOAT}, - {321, "engines/engine[2]/n2", simgear::props::FLOAT}, - {322, "engines/engine[2]/rpm", simgear::props::FLOAT}, - {330, "engines/engine[3]/n1", simgear::props::FLOAT}, - {331, "engines/engine[3]/n2", simgear::props::FLOAT}, - {332, "engines/engine[3]/rpm", simgear::props::FLOAT}, - {340, "engines/engine[4]/n1", simgear::props::FLOAT}, - {341, "engines/engine[4]/n2", simgear::props::FLOAT}, - {342, "engines/engine[4]/rpm", simgear::props::FLOAT}, - {350, "engines/engine[5]/n1", simgear::props::FLOAT}, - {351, "engines/engine[5]/n2", simgear::props::FLOAT}, - {352, "engines/engine[5]/rpm", simgear::props::FLOAT}, - {360, "engines/engine[6]/n1", simgear::props::FLOAT}, - {361, "engines/engine[6]/n2", simgear::props::FLOAT}, - {362, "engines/engine[6]/rpm", simgear::props::FLOAT}, - {370, "engines/engine[7]/n1", simgear::props::FLOAT}, - {371, "engines/engine[7]/n2", simgear::props::FLOAT}, - {372, "engines/engine[7]/rpm", simgear::props::FLOAT}, - {380, "engines/engine[8]/n1", simgear::props::FLOAT}, - {381, "engines/engine[8]/n2", simgear::props::FLOAT}, - {382, "engines/engine[8]/rpm", simgear::props::FLOAT}, - {390, "engines/engine[9]/n1", simgear::props::FLOAT}, - {391, "engines/engine[9]/n2", simgear::props::FLOAT}, - {392, "engines/engine[9]/rpm", simgear::props::FLOAT}, - - {800, "rotors/main/rpm", simgear::props::FLOAT}, - {801, "rotors/tail/rpm", simgear::props::FLOAT}, - {810, "rotors/main/blade[0]/position-deg", simgear::props::FLOAT}, - {811, "rotors/main/blade[1]/position-deg", simgear::props::FLOAT}, - {812, "rotors/main/blade[2]/position-deg", simgear::props::FLOAT}, - {813, "rotors/main/blade[3]/position-deg", simgear::props::FLOAT}, - {820, "rotors/main/blade[0]/flap-deg", simgear::props::FLOAT}, - {821, "rotors/main/blade[1]/flap-deg", simgear::props::FLOAT}, - {822, "rotors/main/blade[2]/flap-deg", simgear::props::FLOAT}, - {823, "rotors/main/blade[3]/flap-deg", simgear::props::FLOAT}, - {830, "rotors/tail/blade[0]/position-deg", simgear::props::FLOAT}, - {831, "rotors/tail/blade[1]/position-deg", simgear::props::FLOAT}, - - {900, "sim/hitches/aerotow/tow/length", simgear::props::FLOAT}, - {901, "sim/hitches/aerotow/tow/elastic-constant", simgear::props::FLOAT}, - {902, "sim/hitches/aerotow/tow/weight-per-m-kg-m", simgear::props::FLOAT}, - {903, "sim/hitches/aerotow/tow/dist", simgear::props::FLOAT}, - {904, "sim/hitches/aerotow/tow/connected-to-property-node", simgear::props::BOOL}, - {905, "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", simgear::props::STRING}, - {906, "sim/hitches/aerotow/tow/brake-force", simgear::props::FLOAT}, - {907, "sim/hitches/aerotow/tow/end-force-x", simgear::props::FLOAT}, - {908, "sim/hitches/aerotow/tow/end-force-y", simgear::props::FLOAT}, - {909, "sim/hitches/aerotow/tow/end-force-z", simgear::props::FLOAT}, - {930, "sim/hitches/aerotow/is-slave", simgear::props::BOOL}, - {931, "sim/hitches/aerotow/speed-in-tow-direction", simgear::props::FLOAT}, - {932, "sim/hitches/aerotow/open", simgear::props::BOOL}, - {933, "sim/hitches/aerotow/local-pos-x", simgear::props::FLOAT}, - {934, "sim/hitches/aerotow/local-pos-y", simgear::props::FLOAT}, - {935, "sim/hitches/aerotow/local-pos-z", simgear::props::FLOAT}, - - {1001, "controls/flight/slats", simgear::props::FLOAT}, - {1002, "controls/flight/speedbrake", simgear::props::FLOAT}, - {1003, "controls/flight/spoilers", simgear::props::FLOAT}, - {1004, "controls/gear/gear-down", simgear::props::FLOAT}, - {1005, "controls/lighting/nav-lights", simgear::props::FLOAT}, - {1006, "controls/armament/station[0]/jettison-all", simgear::props::BOOL}, - - {1100, "sim/model/variant", simgear::props::INT}, - {1101, "sim/model/livery/file", simgear::props::STRING}, - - {1200, "environment/wildfire/data", simgear::props::STRING}, - - {10001, "sim/multiplay/transmission-freq-hz", simgear::props::STRING}, - {10002, "sim/multiplay/chat", simgear::props::STRING}, - - {10100, "sim/multiplay/generic/string[0]", simgear::props::STRING}, - {10101, "sim/multiplay/generic/string[1]", simgear::props::STRING}, - {10102, "sim/multiplay/generic/string[2]", simgear::props::STRING}, - {10103, "sim/multiplay/generic/string[3]", simgear::props::STRING}, - {10104, "sim/multiplay/generic/string[4]", simgear::props::STRING}, - {10105, "sim/multiplay/generic/string[5]", simgear::props::STRING}, - {10106, "sim/multiplay/generic/string[6]", simgear::props::STRING}, - {10107, "sim/multiplay/generic/string[7]", simgear::props::STRING}, - {10108, "sim/multiplay/generic/string[8]", simgear::props::STRING}, - {10109, "sim/multiplay/generic/string[9]", simgear::props::STRING}, - {10110, "sim/multiplay/generic/string[10]", simgear::props::STRING}, - {10111, "sim/multiplay/generic/string[11]", simgear::props::STRING}, - {10112, "sim/multiplay/generic/string[12]", simgear::props::STRING}, - {10113, "sim/multiplay/generic/string[13]", simgear::props::STRING}, - {10114, "sim/multiplay/generic/string[14]", simgear::props::STRING}, - {10115, "sim/multiplay/generic/string[15]", simgear::props::STRING}, - {10116, "sim/multiplay/generic/string[16]", simgear::props::STRING}, - {10117, "sim/multiplay/generic/string[17]", simgear::props::STRING}, - {10118, "sim/multiplay/generic/string[18]", simgear::props::STRING}, - {10119, "sim/multiplay/generic/string[19]", simgear::props::STRING}, - - {10200, "sim/multiplay/generic/float[0]", simgear::props::FLOAT}, - {10201, "sim/multiplay/generic/float[1]", simgear::props::FLOAT}, - {10202, "sim/multiplay/generic/float[2]", simgear::props::FLOAT}, - {10203, "sim/multiplay/generic/float[3]", simgear::props::FLOAT}, - {10204, "sim/multiplay/generic/float[4]", simgear::props::FLOAT}, - {10205, "sim/multiplay/generic/float[5]", simgear::props::FLOAT}, - {10206, "sim/multiplay/generic/float[6]", simgear::props::FLOAT}, - {10207, "sim/multiplay/generic/float[7]", simgear::props::FLOAT}, - {10208, "sim/multiplay/generic/float[8]", simgear::props::FLOAT}, - {10209, "sim/multiplay/generic/float[9]", simgear::props::FLOAT}, - {10210, "sim/multiplay/generic/float[10]", simgear::props::FLOAT}, - {10211, "sim/multiplay/generic/float[11]", simgear::props::FLOAT}, - {10212, "sim/multiplay/generic/float[12]", simgear::props::FLOAT}, - {10213, "sim/multiplay/generic/float[13]", simgear::props::FLOAT}, - {10214, "sim/multiplay/generic/float[14]", simgear::props::FLOAT}, - {10215, "sim/multiplay/generic/float[15]", simgear::props::FLOAT}, - {10216, "sim/multiplay/generic/float[16]", simgear::props::FLOAT}, - {10217, "sim/multiplay/generic/float[17]", simgear::props::FLOAT}, - {10218, "sim/multiplay/generic/float[18]", simgear::props::FLOAT}, - {10219, "sim/multiplay/generic/float[19]", simgear::props::FLOAT}, - - {10300, "sim/multiplay/generic/int[0]", simgear::props::INT}, - {10301, "sim/multiplay/generic/int[1]", simgear::props::INT}, - {10302, "sim/multiplay/generic/int[2]", simgear::props::INT}, - {10303, "sim/multiplay/generic/int[3]", simgear::props::INT}, - {10304, "sim/multiplay/generic/int[4]", simgear::props::INT}, - {10305, "sim/multiplay/generic/int[5]", simgear::props::INT}, - {10306, "sim/multiplay/generic/int[6]", simgear::props::INT}, - {10307, "sim/multiplay/generic/int[7]", simgear::props::INT}, - {10308, "sim/multiplay/generic/int[8]", simgear::props::INT}, - {10309, "sim/multiplay/generic/int[9]", simgear::props::INT}, - {10310, "sim/multiplay/generic/int[10]", simgear::props::INT}, - {10311, "sim/multiplay/generic/int[11]", simgear::props::INT}, - {10312, "sim/multiplay/generic/int[12]", simgear::props::INT}, - {10313, "sim/multiplay/generic/int[13]", simgear::props::INT}, - {10314, "sim/multiplay/generic/int[14]", simgear::props::INT}, - {10315, "sim/multiplay/generic/int[15]", simgear::props::INT}, - {10316, "sim/multiplay/generic/int[16]", simgear::props::INT}, - {10317, "sim/multiplay/generic/int[17]", simgear::props::INT}, - {10318, "sim/multiplay/generic/int[18]", simgear::props::INT}, - {10319, "sim/multiplay/generic/int[19]", simgear::props::INT} -}; - -const unsigned -FGMultiplayMgr::numProperties = (sizeof(FGMultiplayMgr::sIdPropertyList) - / sizeof(FGMultiplayMgr::sIdPropertyList[0])); - -// Look up a property ID using binary search. -namespace -{ - struct ComparePropertyId - { - bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, - const FGMultiplayMgr::IdPropertyList& rhs) - { - return lhs.id < rhs.id; - } - bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, - unsigned id) - { - return lhs.id < id; - } - bool operator()(unsigned id, - const FGMultiplayMgr::IdPropertyList& rhs) - { - return id < rhs.id; - } - }; - -} -const FGMultiplayMgr::IdPropertyList* FGMultiplayMgr::findProperty(unsigned id) -{ - std::pair result - = std::equal_range(sIdPropertyList, sIdPropertyList + numProperties, id, - ComparePropertyId()); - if (result.first == result.second) { - return 0; - } else { - return result.first; - } -} - -namespace -{ - bool verifyProperties(const xdr_data_t* data, const xdr_data_t* end) - { - using namespace simgear; - const xdr_data_t* xdr = data; - while (xdr < end) { - unsigned id = XDR_decode_uint32(*xdr); - const FGMultiplayMgr::IdPropertyList* plist - = FGMultiplayMgr::findProperty(id); - - if (plist) { - xdr++; - // How we decode the remainder of the property depends on the type - switch (plist->type) { - case props::INT: - case props::BOOL: - case props::LONG: - xdr++; - break; - case props::FLOAT: - case props::DOUBLE: - { - float val = XDR_decode_float(*xdr); - if (osg::isNaN(val)) - return false; - xdr++; - break; - } - case props::STRING: - case props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - // XXX Yes, each byte is padded out to a word! Too late - // to change... - uint32_t length = XDR_decode_uint32(*xdr); - xdr++; - // Old versions truncated the string but left the length - // unadjusted. - if (length > MAX_TEXT_SIZE) - length = MAX_TEXT_SIZE; - xdr += length; - // Now handle the padding - while ((length % 4) != 0) - { - xdr++; - length++; - //cout << "0"; - } - } - break; - default: - // cerr << "Unknown Prop type " << id << " " << type << "\n"; - xdr++; - break; - } - } - else { - // give up; this is a malformed property list. - return false; - } - } - return true; - } -} -////////////////////////////////////////////////////////////////////// -// -// MultiplayMgr constructor -// -////////////////////////////////////////////////////////////////////// -FGMultiplayMgr::FGMultiplayMgr() -{ - mSocket = 0; - mInitialised = false; - mHaveServer = false; -} // FGMultiplayMgr::FGMultiplayMgr() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// MultiplayMgr destructor -// -////////////////////////////////////////////////////////////////////// -FGMultiplayMgr::~FGMultiplayMgr() -{ - Close(); -} // FGMultiplayMgr::~FGMultiplayMgr() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Initialise object -// -////////////////////////////////////////////////////////////////////// -bool -FGMultiplayMgr::init (void) -{ - ////////////////////////////////////////////////// - // Initialise object if not already done - ////////////////////////////////////////////////// - if (mInitialised) { - SG_LOG(SG_NETWORK, SG_WARN, "FGMultiplayMgr::init - already initialised"); - return false; - } - ////////////////////////////////////////////////// - // Set members from property values - ////////////////////////////////////////////////// - short rxPort = fgGetInt("/sim/multiplay/rxport"); - string rxAddress = fgGetString("/sim/multiplay/rxhost"); - short txPort = fgGetInt("/sim/multiplay/txport"); - string txAddress = fgGetString("/sim/multiplay/txhost"); - mCallsign = fgGetString("/sim/multiplay/callsign"); - if (txPort > 0 && !txAddress.empty()) { - mServer.set(txAddress.c_str(), txPort); - if (strncmp (mServer.getHost(), "0.0.0.0", 8) == 0) { - mHaveServer = false; - SG_LOG(SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr - could not resolve '" - << txAddress << "', Multiplayermode disabled"); - } else { - mHaveServer = true; - } - if (rxPort <= 0) - rxPort = txPort; - } - if (rxPort <= 0) { - SG_LOG(SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr - No receiver port, Multiplayermode disabled"); - return (false); - } - if (mCallsign.empty()) - mCallsign = "JohnDoe"; // FIXME: use getpwuid - SG_LOG(SG_NETWORK,SG_INFO,"FGMultiplayMgr::init-txaddress= "<setBlocking(false); - if (mSocket->bind(rxAddress.c_str(), rxPort) != 0) { - perror("bind"); - SG_LOG( SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr::Open - Failed to bind receive socket" ); - return false; - } - mInitialised = true; - return true; -} // FGMultiplayMgr::init() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Closes and deletes the local player object. Closes -// and deletes the tx socket. Resets the object state to unitialised. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::Close (void) -{ - mMultiPlayerMap.clear(); - - if (mSocket) { - mSocket->close(); - delete mSocket; - mSocket = 0; - } - mInitialised = false; -} // FGMultiplayMgr::Close(void) -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Description: Sends the position data for the local position. -// -////////////////////////////////////////////////////////////////////// - -/** - * The buffer that holds a multi-player message, suitably aligned. - */ -union FGMultiplayMgr::MsgBuf -{ - MsgBuf() - { - memset(&Msg, 0, sizeof(Msg)); - } - - T_MsgHdr* msgHdr() - { - return reinterpret_cast(Msg); - } - - const T_MsgHdr* msgHdr() const - { - return reinterpret_cast(Msg); - } - - T_PositionMsg* posMsg() - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr)); - } - - const T_PositionMsg* posMsg() const - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr)); - } - - xdr_data_t* properties() - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr) - + sizeof(T_PositionMsg)); - } - - const xdr_data_t* properties() const - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr) - + sizeof(T_PositionMsg)); - } - /** - * The end of the properties buffer. - */ - xdr_data_t* propsEnd() - { - return reinterpret_cast(Msg + MAX_PACKET_SIZE); - }; - - const xdr_data_t* propsEnd() const - { - return reinterpret_cast(Msg + MAX_PACKET_SIZE); - }; - /** - * The end of properties actually in the buffer. This assumes that - * the message header is valid. - */ - xdr_data_t* propsRecvdEnd() - { - return reinterpret_cast(Msg + msgHdr()->MsgLen); - } - - const xdr_data_t* propsRecvdEnd() const - { - return reinterpret_cast(Msg + msgHdr()->MsgLen); - } - - xdr_data2_t double_val; - char Msg[MAX_PACKET_SIZE]; -}; - -void -FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) -{ - if ((! mInitialised) || (! mHaveServer)) - return; - if (! mHaveServer) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); - return; - } - - MsgBuf msgBuf; - T_PositionMsg* PosMsg = msgBuf.posMsg(); - - strncpy(PosMsg->Model, fgGetString("/sim/model/path"), MAX_MODEL_NAME_LEN); - PosMsg->Model[MAX_MODEL_NAME_LEN - 1] = '\0'; - - PosMsg->time = XDR_encode_double (motionInfo.time); - PosMsg->lag = XDR_encode_double (motionInfo.lag); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->position[i] = XDR_encode_double (motionInfo.position(i)); - SGVec3f angleAxis; - motionInfo.orientation.getAngleAxis(angleAxis); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->orientation[i] = XDR_encode_float (angleAxis(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->linearVel[i] = XDR_encode_float (motionInfo.linearVel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->angularVel[i] = XDR_encode_float (motionInfo.angularVel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->linearAccel[i] = XDR_encode_float (motionInfo.linearAccel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->angularAccel[i] = XDR_encode_float (motionInfo.angularAccel(i)); - - xdr_data_t* ptr = msgBuf.properties(); - std::vector::const_iterator it; - it = motionInfo.properties.begin(); - //cout << "OUTPUT PROPERTIES\n"; - xdr_data_t* msgEnd = msgBuf.propsEnd(); - while (it != motionInfo.properties.end() && ptr + 2 < msgEnd) { - - // First element is the ID. Write it out when we know we have room for - // the whole property. - xdr_data_t id = XDR_encode_uint32((*it)->id); - // The actual data representation depends on the type - switch ((*it)->type) { - case simgear::props::INT: - case simgear::props::BOOL: - case simgear::props::LONG: - *ptr++ = id; - *ptr++ = XDR_encode_uint32((*it)->int_value); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->int_value << "\n"; - break; - case simgear::props::FLOAT: - case simgear::props::DOUBLE: - *ptr++ = id; - *ptr++ = XDR_encode_float((*it)->float_value); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; - break; - case simgear::props::STRING: - case simgear::props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - const char* lcharptr = (*it)->string_value; - - if (lcharptr != 0) - { - // Add the length - ////cout << "String length: " << strlen(lcharptr) << "\n"; - uint32_t len = strlen(lcharptr); - if (len > MAX_TEXT_SIZE) - len = MAX_TEXT_SIZE; - // XXX This should not be using 4 bytes per character! - // If there's not enough room for this property, drop it - // on the floor. - if (ptr + 2 + ((len + 3) & ~3) > msgEnd) - goto escape; - //cout << "String length unint32: " << len << "\n"; - *ptr++ = id; - *ptr++ = XDR_encode_uint32(len); - if (len != 0) - { - // Now the text itself - // XXX This should not be using 4 bytes per character! - int lcount = 0; - while ((*lcharptr != '\0') && (lcount < MAX_TEXT_SIZE)) - { - *ptr++ = XDR_encode_int8(*lcharptr); - lcharptr++; - lcount++; - } - - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " " << len << " " << (*it)->string_value; - - // Now pad if required - while ((lcount % 4) != 0) - { - *ptr++ = XDR_encode_int8(0); - lcount++; - //cout << "0"; - } - - //cout << "\n"; - } - } - else - { - // Nothing to encode - *ptr++ = id; - *ptr++ = XDR_encode_uint32(0); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " 0\n"; - } - } - break; - - default: - //cout << " Unknown Type: " << (*it)->type << "\n"; - *ptr++ = id; - *ptr++ = XDR_encode_float((*it)->float_value);; - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; - break; - } - - ++it; - } -escape: - unsigned msgLen = reinterpret_cast(ptr) - msgBuf.Msg; - FillMsgHdr(msgBuf.msgHdr(), POS_DATA_ID, msgLen); - mSocket->sendto(msgBuf.Msg, msgLen, 0, &mServer); - SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition"); -} // FGMultiplayMgr::SendMyPosition() - -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Name: SendTextMessage -// Description: Sends a message to the player. The message must -// contain a valid and correctly filled out header and optional -// message body. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::SendTextMessage(const string &MsgText) -{ - if (!mInitialised || !mHaveServer) - return; - - T_MsgHdr MsgHdr; - FillMsgHdr(&MsgHdr, CHAT_MSG_ID); - ////////////////////////////////////////////////// - // Divide the text string into blocks that fit - // in the message and send the blocks. - ////////////////////////////////////////////////// - unsigned iNextBlockPosition = 0; - T_ChatMsg ChatMsg; - - char Msg[sizeof(T_MsgHdr) + sizeof(T_ChatMsg)]; - while (iNextBlockPosition < MsgText.length()) { - strncpy (ChatMsg.Text, - MsgText.substr(iNextBlockPosition, MAX_CHAT_MSG_LEN - 1).c_str(), - MAX_CHAT_MSG_LEN); - ChatMsg.Text[MAX_CHAT_MSG_LEN - 1] = '\0'; - memcpy (Msg, &MsgHdr, sizeof(T_MsgHdr)); - memcpy (Msg + sizeof(T_MsgHdr), &ChatMsg, sizeof(T_ChatMsg)); - mSocket->sendto (Msg, sizeof(T_MsgHdr) + sizeof(T_ChatMsg), 0, &mServer); - iNextBlockPosition += MAX_CHAT_MSG_LEN - 1; - - } - - -} // FGMultiplayMgr::SendTextMessage () -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Name: ProcessData -// Description: Processes data waiting at the receive socket. The -// processing ends when there is no more data at the socket. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::Update(void) -{ - if (!mInitialised) - return; - - /// Just for expiry - long stamp = SGTimeStamp::now().getSeconds(); - - ////////////////////////////////////////////////// - // Read the receive socket and process any data - ////////////////////////////////////////////////// - ssize_t bytes; - do { - MsgBuf msgBuf; - ////////////////////////////////////////////////// - // Although the recv call asks for - // MAX_PACKET_SIZE of data, the number of bytes - // returned will only be that of the next - // packet waiting to be processed. - ////////////////////////////////////////////////// - netAddress SenderAddress; - bytes = mSocket->recvfrom(msgBuf.Msg, sizeof(msgBuf.Msg), 0, - &SenderAddress); - ////////////////////////////////////////////////// - // no Data received - ////////////////////////////////////////////////// - if (bytes <= 0) { - if (errno != EAGAIN && errno != 0) // MSVC output "NoError" otherwise - perror("FGMultiplayMgr::MP_ProcessData"); - break; - } - if (bytes <= static_cast(sizeof(T_MsgHdr))) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "received message with insufficient data" ); - break; - } - ////////////////////////////////////////////////// - // Read header - ////////////////////////////////////////////////// - T_MsgHdr* MsgHdr = msgBuf.msgHdr(); - MsgHdr->Magic = XDR_decode_uint32 (MsgHdr->Magic); - MsgHdr->Version = XDR_decode_uint32 (MsgHdr->Version); - MsgHdr->MsgId = XDR_decode_uint32 (MsgHdr->MsgId); - MsgHdr->MsgLen = XDR_decode_uint32 (MsgHdr->MsgLen); - MsgHdr->ReplyPort = XDR_decode_uint32 (MsgHdr->ReplyPort); - MsgHdr->Callsign[MAX_CALLSIGN_LEN -1] = '\0'; - if (MsgHdr->Magic != MSG_MAGIC) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message has invalid magic number!" ); - break; - } - if (MsgHdr->Version != PROTO_VER) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message has invalid protocoll number!" ); - break; - } - if (MsgHdr->MsgLen != bytes) { - SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message from " << MsgHdr->Callsign << " has invalid length!"); - break; - } - ////////////////////////////////////////////////// - // Process messages - ////////////////////////////////////////////////// - switch (MsgHdr->MsgId) { - case CHAT_MSG_ID: - ProcessChatMsg(msgBuf, SenderAddress); - break; - case POS_DATA_ID: - ProcessPosMsg(msgBuf, SenderAddress, stamp); - break; - case UNUSABLE_POS_DATA_ID: - case OLD_OLD_POS_DATA_ID: - case OLD_PROP_MSG_ID: - case OLD_POS_DATA_ID: - break; - default: - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Unknown message Id received: " << MsgHdr->MsgId ); - break; - } - } while (bytes > 0); - - // check for expiry - MultiPlayerMap::iterator it = mMultiPlayerMap.begin(); - while (it != mMultiPlayerMap.end()) { - if (it->second->getLastTimestamp() + 10 < stamp) { - std::string name = it->first; - it->second->setDie(true); - mMultiPlayerMap.erase(it); - it = mMultiPlayerMap.upper_bound(name); - } else - ++it; - } -} // FGMultiplayMgr::ProcessData(void) -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// handle a position message -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, - const netAddress& SenderAddress, long stamp) -{ - const T_MsgHdr* MsgHdr = Msg.msgHdr(); - if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + sizeof(T_PositionMsg)) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Position message received with insufficient data" ); - return; - } - const T_PositionMsg* PosMsg = Msg.posMsg(); - FGExternalMotionData motionInfo; - motionInfo.time = XDR_decode_double(PosMsg->time); - motionInfo.lag = XDR_decode_double(PosMsg->lag); - for (unsigned i = 0; i < 3; ++i) - motionInfo.position(i) = XDR_decode_double(PosMsg->position[i]); - SGVec3f angleAxis; - for (unsigned i = 0; i < 3; ++i) - angleAxis(i) = XDR_decode_float(PosMsg->orientation[i]); - motionInfo.orientation = SGQuatf::fromAngleAxis(angleAxis); - for (unsigned i = 0; i < 3; ++i) - motionInfo.linearVel(i) = XDR_decode_float(PosMsg->linearVel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.angularVel(i) = XDR_decode_float(PosMsg->angularVel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.linearAccel(i) = XDR_decode_float(PosMsg->linearAccel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.angularAccel(i) = XDR_decode_float(PosMsg->angularAccel[i]); - - - //cout << "INPUT MESSAGE\n"; - - // There was a bug in 1.9.0 and before: T_PositionMsg was 196 bytes - // on 32 bit architectures and 200 bytes on 64 bit, and this - // structure is put directly on the wire. By looking at the padding, - // we can sort through the mess, mostly: - // If padding is 0 (which is not a valid property type), then the - // message was produced by a new client or an old 64 bit client that - // happened to have 0 on the stack; - // Else if the property list starting with the padding word is - // well-formed, then the client is probably an old 32 bit client and - // we'll go with that; - // Else it is an old 64-bit client and properties start after the - // padding. - // There is a chance that we could be fooled by garbage in the - // padding looking like a valid property, so verifyProperties() is - // strict about the validity of the property values. - const xdr_data_t* xdr = Msg.properties(); - if (PosMsg->pad != 0) { - if (verifyProperties(&PosMsg->pad, Msg.propsRecvdEnd())) - xdr = &PosMsg->pad; - else if (!verifyProperties(xdr, Msg.propsRecvdEnd())) - goto noprops; - } - while (xdr < Msg.propsRecvdEnd()) { - FGPropertyData* pData = new FGPropertyData; - // simgear::props::Type type = simgear::props::UNSPECIFIED; - - // First element is always the ID - pData->id = XDR_decode_uint32(*xdr); - //cout << pData->id << " "; - xdr++; - - // Check the ID actually exists and get the type - const IdPropertyList* plist = findProperty(pData->id); - - if (plist) - { - pData->type = plist->type; - // How we decode the remainder of the property depends on the type - switch (pData->type) { - case simgear::props::INT: - case simgear::props::BOOL: - case simgear::props::LONG: - pData->int_value = XDR_decode_uint32(*xdr); - xdr++; - //cout << pData->int_value << "\n"; - break; - case simgear::props::FLOAT: - case simgear::props::DOUBLE: - pData->float_value = XDR_decode_float(*xdr); - xdr++; - //cout << pData->float_value << "\n"; - break; - case simgear::props::STRING: - case simgear::props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - uint32_t length = XDR_decode_uint32(*xdr); - xdr++; - //cout << length << " "; - // Old versions truncated the string but left the length unadjusted. - if (length > MAX_TEXT_SIZE) - length = MAX_TEXT_SIZE; - pData->string_value = new char[length + 1]; - //cout << " String: "; - for (unsigned i = 0; i < length; i++) - { - pData->string_value[i] = (char) XDR_decode_int8(*xdr); - xdr++; - //cout << pData->string_value[i]; - } - - pData->string_value[length] = '\0'; - - // Now handle the padding - while ((length % 4) != 0) - { - xdr++; - length++; - //cout << "0"; - } - //cout << "\n"; - } - break; - - default: - pData->float_value = XDR_decode_float(*xdr); - SG_LOG(SG_NETWORK, SG_DEBUG, "Unknown Prop type " << pData->id << " " << pData->type); - xdr++; - break; - } - - motionInfo.properties.push_back(pData); - } - else - { - // We failed to find the property. We'll try the next packet immediately. - SG_LOG(SG_NETWORK, SG_INFO, "FGMultiplayMgr::ProcessPosMsg - " - "message from " << MsgHdr->Callsign << " has unknown property id " - << pData->id); - } - } - noprops: - FGAIMultiplayer* mp = getMultiplayer(MsgHdr->Callsign); - if (!mp) - mp = addMultiplayer(MsgHdr->Callsign, PosMsg->Model); - mp->addMotionInfo(motionInfo, stamp); -} // FGMultiplayMgr::ProcessPosMsg() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// handle a chat message -// FIXME: display chat message withi flightgear -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::ProcessChatMsg(const MsgBuf& Msg, - const netAddress& SenderAddress) -{ - const T_MsgHdr* MsgHdr = Msg.msgHdr(); - if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + 1) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Chat message received with insufficient data" ); - return; - } - - char *chatStr = new char[MsgHdr->MsgLen - sizeof(T_MsgHdr)]; - const T_ChatMsg* ChatMsg - = reinterpret_cast(Msg.Msg + sizeof(T_MsgHdr)); - strncpy(chatStr, ChatMsg->Text, - MsgHdr->MsgLen - sizeof(T_MsgHdr)); - chatStr[MsgHdr->MsgLen - sizeof(T_MsgHdr) - 1] = '\0'; - - SG_LOG (SG_NETWORK, SG_WARN, "Chat [" << MsgHdr->Callsign << "]" - << " " << chatStr); - - delete [] chatStr; -} // FGMultiplayMgr::ProcessChatMsg () -////////////////////////////////////////////////////////////////////// - -void -FGMultiplayMgr::FillMsgHdr(T_MsgHdr *MsgHdr, int MsgId, unsigned _len) -{ - uint32_t len; - switch (MsgId) { - case CHAT_MSG_ID: - len = sizeof(T_MsgHdr) + sizeof(T_ChatMsg); - break; - case POS_DATA_ID: - len = _len; - break; - default: - len = sizeof(T_MsgHdr); - break; - } - MsgHdr->Magic = XDR_encode_uint32(MSG_MAGIC); - MsgHdr->Version = XDR_encode_uint32(PROTO_VER); - MsgHdr->MsgId = XDR_encode_uint32(MsgId); - MsgHdr->MsgLen = XDR_encode_uint32(len); - MsgHdr->ReplyAddress = 0; // Are obsolete, keep them for the server for - MsgHdr->ReplyPort = 0; // now - strncpy(MsgHdr->Callsign, mCallsign.c_str(), MAX_CALLSIGN_LEN); - MsgHdr->Callsign[MAX_CALLSIGN_LEN - 1] = '\0'; -} - -FGAIMultiplayer* -FGMultiplayMgr::addMultiplayer(const std::string& callsign, - const std::string& modelName) -{ - if (0 < mMultiPlayerMap.count(callsign)) - return mMultiPlayerMap[callsign].get(); - - FGAIMultiplayer* mp = new FGAIMultiplayer; - mp->setPath(modelName.c_str()); - mp->setCallSign(callsign); - mMultiPlayerMap[callsign] = mp; - - FGAIManager *aiMgr = (FGAIManager*)globals->get_subsystem("ai_model"); - if (aiMgr) { - aiMgr->attach(mp); - - /// FIXME: that must follow the attach ATM ... - for (unsigned i = 0; i < numProperties; ++i) - mp->addPropertyId(sIdPropertyList[i].id, sIdPropertyList[i].name); - } - - return mp; -} - -FGAIMultiplayer* -FGMultiplayMgr::getMultiplayer(const std::string& callsign) -{ - if (0 < mMultiPlayerMap.count(callsign)) - return mMultiPlayerMap[callsign].get(); - else - return 0; -} +////////////////////////////////////////////////////////////////////// +// +// multiplaymgr.hpp +// +// Written by Duncan McCreanor, started February 2003. +// duncan.mccreanor@airservicesaustralia.com +// +// Copyright (C) 2003 Airservices Australia +// Copyright (C) 2005 Oliver Schroeder +// Copyright (C) 2006 Mathias Froehlich +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ +// +////////////////////////////////////////////////////////////////////// + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include // isNaN +#include + +#include +#include +#include +#include + +#include +#include
+#include "multiplaymgr.hxx" +#include "mpmessages.hxx" + +using namespace std; + +#define MAX_PACKET_SIZE 1200 +#define MAX_TEXT_SIZE 128 + +// These constants are provided so that the ident +// command can list file versions +const char sMULTIPLAYMGR_BID[] = "$Id$"; +const char sMULTIPLAYMGR_HID[] = MULTIPLAYTXMGR_HID; + +// A static map of protocol property id values to property paths, +// This should be extendable dynamically for every specific aircraft ... +// For now only that static list +const FGMultiplayMgr::IdPropertyList +FGMultiplayMgr::sIdPropertyList[] = { + {100, "surface-positions/left-aileron-pos-norm", simgear::props::FLOAT}, + {101, "surface-positions/right-aileron-pos-norm", simgear::props::FLOAT}, + {102, "surface-positions/elevator-pos-norm", simgear::props::FLOAT}, + {103, "surface-positions/rudder-pos-norm", simgear::props::FLOAT}, + {104, "surface-positions/flap-pos-norm", simgear::props::FLOAT}, + {105, "surface-positions/speedbrake-pos-norm", simgear::props::FLOAT}, + {106, "gear/tailhook/position-norm", simgear::props::FLOAT}, + {107, "gear/launchbar/position-norm", simgear::props::FLOAT}, + {108, "gear/launchbar/state", simgear::props::STRING}, + {109, "gear/launchbar/holdback-position-norm", simgear::props::FLOAT}, + {110, "canopy/position-norm", simgear::props::FLOAT}, + {111, "surface-positions/wing-pos-norm", simgear::props::FLOAT}, + {112, "surface-positions/wing-fold-pos-norm", simgear::props::FLOAT}, + + {200, "gear/gear[0]/compression-norm", simgear::props::FLOAT}, + {201, "gear/gear[0]/position-norm", simgear::props::FLOAT}, + {210, "gear/gear[1]/compression-norm", simgear::props::FLOAT}, + {211, "gear/gear[1]/position-norm", simgear::props::FLOAT}, + {220, "gear/gear[2]/compression-norm", simgear::props::FLOAT}, + {221, "gear/gear[2]/position-norm", simgear::props::FLOAT}, + {230, "gear/gear[3]/compression-norm", simgear::props::FLOAT}, + {231, "gear/gear[3]/position-norm", simgear::props::FLOAT}, + {240, "gear/gear[4]/compression-norm", simgear::props::FLOAT}, + {241, "gear/gear[4]/position-norm", simgear::props::FLOAT}, + + {300, "engines/engine[0]/n1", simgear::props::FLOAT}, + {301, "engines/engine[0]/n2", simgear::props::FLOAT}, + {302, "engines/engine[0]/rpm", simgear::props::FLOAT}, + {310, "engines/engine[1]/n1", simgear::props::FLOAT}, + {311, "engines/engine[1]/n2", simgear::props::FLOAT}, + {312, "engines/engine[1]/rpm", simgear::props::FLOAT}, + {320, "engines/engine[2]/n1", simgear::props::FLOAT}, + {321, "engines/engine[2]/n2", simgear::props::FLOAT}, + {322, "engines/engine[2]/rpm", simgear::props::FLOAT}, + {330, "engines/engine[3]/n1", simgear::props::FLOAT}, + {331, "engines/engine[3]/n2", simgear::props::FLOAT}, + {332, "engines/engine[3]/rpm", simgear::props::FLOAT}, + {340, "engines/engine[4]/n1", simgear::props::FLOAT}, + {341, "engines/engine[4]/n2", simgear::props::FLOAT}, + {342, "engines/engine[4]/rpm", simgear::props::FLOAT}, + {350, "engines/engine[5]/n1", simgear::props::FLOAT}, + {351, "engines/engine[5]/n2", simgear::props::FLOAT}, + {352, "engines/engine[5]/rpm", simgear::props::FLOAT}, + {360, "engines/engine[6]/n1", simgear::props::FLOAT}, + {361, "engines/engine[6]/n2", simgear::props::FLOAT}, + {362, "engines/engine[6]/rpm", simgear::props::FLOAT}, + {370, "engines/engine[7]/n1", simgear::props::FLOAT}, + {371, "engines/engine[7]/n2", simgear::props::FLOAT}, + {372, "engines/engine[7]/rpm", simgear::props::FLOAT}, + {380, "engines/engine[8]/n1", simgear::props::FLOAT}, + {381, "engines/engine[8]/n2", simgear::props::FLOAT}, + {382, "engines/engine[8]/rpm", simgear::props::FLOAT}, + {390, "engines/engine[9]/n1", simgear::props::FLOAT}, + {391, "engines/engine[9]/n2", simgear::props::FLOAT}, + {392, "engines/engine[9]/rpm", simgear::props::FLOAT}, + + {800, "rotors/main/rpm", simgear::props::FLOAT}, + {801, "rotors/tail/rpm", simgear::props::FLOAT}, + {810, "rotors/main/blade[0]/position-deg", simgear::props::FLOAT}, + {811, "rotors/main/blade[1]/position-deg", simgear::props::FLOAT}, + {812, "rotors/main/blade[2]/position-deg", simgear::props::FLOAT}, + {813, "rotors/main/blade[3]/position-deg", simgear::props::FLOAT}, + {820, "rotors/main/blade[0]/flap-deg", simgear::props::FLOAT}, + {821, "rotors/main/blade[1]/flap-deg", simgear::props::FLOAT}, + {822, "rotors/main/blade[2]/flap-deg", simgear::props::FLOAT}, + {823, "rotors/main/blade[3]/flap-deg", simgear::props::FLOAT}, + {830, "rotors/tail/blade[0]/position-deg", simgear::props::FLOAT}, + {831, "rotors/tail/blade[1]/position-deg", simgear::props::FLOAT}, + + {900, "sim/hitches/aerotow/tow/length", simgear::props::FLOAT}, + {901, "sim/hitches/aerotow/tow/elastic-constant", simgear::props::FLOAT}, + {902, "sim/hitches/aerotow/tow/weight-per-m-kg-m", simgear::props::FLOAT}, + {903, "sim/hitches/aerotow/tow/dist", simgear::props::FLOAT}, + {904, "sim/hitches/aerotow/tow/connected-to-property-node", simgear::props::BOOL}, + {905, "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", simgear::props::STRING}, + {906, "sim/hitches/aerotow/tow/brake-force", simgear::props::FLOAT}, + {907, "sim/hitches/aerotow/tow/end-force-x", simgear::props::FLOAT}, + {908, "sim/hitches/aerotow/tow/end-force-y", simgear::props::FLOAT}, + {909, "sim/hitches/aerotow/tow/end-force-z", simgear::props::FLOAT}, + {930, "sim/hitches/aerotow/is-slave", simgear::props::BOOL}, + {931, "sim/hitches/aerotow/speed-in-tow-direction", simgear::props::FLOAT}, + {932, "sim/hitches/aerotow/open", simgear::props::BOOL}, + {933, "sim/hitches/aerotow/local-pos-x", simgear::props::FLOAT}, + {934, "sim/hitches/aerotow/local-pos-y", simgear::props::FLOAT}, + {935, "sim/hitches/aerotow/local-pos-z", simgear::props::FLOAT}, + + {1001, "controls/flight/slats", simgear::props::FLOAT}, + {1002, "controls/flight/speedbrake", simgear::props::FLOAT}, + {1003, "controls/flight/spoilers", simgear::props::FLOAT}, + {1004, "controls/gear/gear-down", simgear::props::FLOAT}, + {1005, "controls/lighting/nav-lights", simgear::props::FLOAT}, + {1006, "controls/armament/station[0]/jettison-all", simgear::props::BOOL}, + + {1100, "sim/model/variant", simgear::props::INT}, + {1101, "sim/model/livery/file", simgear::props::STRING}, + + {1200, "environment/wildfire/data", simgear::props::STRING}, + + {1300, "tanker", simgear::props::INT}, + + {10001, "sim/multiplay/transmission-freq-hz", simgear::props::STRING}, + {10002, "sim/multiplay/chat", simgear::props::STRING}, + + {10100, "sim/multiplay/generic/string[0]", simgear::props::STRING}, + {10101, "sim/multiplay/generic/string[1]", simgear::props::STRING}, + {10102, "sim/multiplay/generic/string[2]", simgear::props::STRING}, + {10103, "sim/multiplay/generic/string[3]", simgear::props::STRING}, + {10104, "sim/multiplay/generic/string[4]", simgear::props::STRING}, + {10105, "sim/multiplay/generic/string[5]", simgear::props::STRING}, + {10106, "sim/multiplay/generic/string[6]", simgear::props::STRING}, + {10107, "sim/multiplay/generic/string[7]", simgear::props::STRING}, + {10108, "sim/multiplay/generic/string[8]", simgear::props::STRING}, + {10109, "sim/multiplay/generic/string[9]", simgear::props::STRING}, + {10110, "sim/multiplay/generic/string[10]", simgear::props::STRING}, + {10111, "sim/multiplay/generic/string[11]", simgear::props::STRING}, + {10112, "sim/multiplay/generic/string[12]", simgear::props::STRING}, + {10113, "sim/multiplay/generic/string[13]", simgear::props::STRING}, + {10114, "sim/multiplay/generic/string[14]", simgear::props::STRING}, + {10115, "sim/multiplay/generic/string[15]", simgear::props::STRING}, + {10116, "sim/multiplay/generic/string[16]", simgear::props::STRING}, + {10117, "sim/multiplay/generic/string[17]", simgear::props::STRING}, + {10118, "sim/multiplay/generic/string[18]", simgear::props::STRING}, + {10119, "sim/multiplay/generic/string[19]", simgear::props::STRING}, + + {10200, "sim/multiplay/generic/float[0]", simgear::props::FLOAT}, + {10201, "sim/multiplay/generic/float[1]", simgear::props::FLOAT}, + {10202, "sim/multiplay/generic/float[2]", simgear::props::FLOAT}, + {10203, "sim/multiplay/generic/float[3]", simgear::props::FLOAT}, + {10204, "sim/multiplay/generic/float[4]", simgear::props::FLOAT}, + {10205, "sim/multiplay/generic/float[5]", simgear::props::FLOAT}, + {10206, "sim/multiplay/generic/float[6]", simgear::props::FLOAT}, + {10207, "sim/multiplay/generic/float[7]", simgear::props::FLOAT}, + {10208, "sim/multiplay/generic/float[8]", simgear::props::FLOAT}, + {10209, "sim/multiplay/generic/float[9]", simgear::props::FLOAT}, + {10210, "sim/multiplay/generic/float[10]", simgear::props::FLOAT}, + {10211, "sim/multiplay/generic/float[11]", simgear::props::FLOAT}, + {10212, "sim/multiplay/generic/float[12]", simgear::props::FLOAT}, + {10213, "sim/multiplay/generic/float[13]", simgear::props::FLOAT}, + {10214, "sim/multiplay/generic/float[14]", simgear::props::FLOAT}, + {10215, "sim/multiplay/generic/float[15]", simgear::props::FLOAT}, + {10216, "sim/multiplay/generic/float[16]", simgear::props::FLOAT}, + {10217, "sim/multiplay/generic/float[17]", simgear::props::FLOAT}, + {10218, "sim/multiplay/generic/float[18]", simgear::props::FLOAT}, + {10219, "sim/multiplay/generic/float[19]", simgear::props::FLOAT}, + + {10300, "sim/multiplay/generic/int[0]", simgear::props::INT}, + {10301, "sim/multiplay/generic/int[1]", simgear::props::INT}, + {10302, "sim/multiplay/generic/int[2]", simgear::props::INT}, + {10303, "sim/multiplay/generic/int[3]", simgear::props::INT}, + {10304, "sim/multiplay/generic/int[4]", simgear::props::INT}, + {10305, "sim/multiplay/generic/int[5]", simgear::props::INT}, + {10306, "sim/multiplay/generic/int[6]", simgear::props::INT}, + {10307, "sim/multiplay/generic/int[7]", simgear::props::INT}, + {10308, "sim/multiplay/generic/int[8]", simgear::props::INT}, + {10309, "sim/multiplay/generic/int[9]", simgear::props::INT}, + {10310, "sim/multiplay/generic/int[10]", simgear::props::INT}, + {10311, "sim/multiplay/generic/int[11]", simgear::props::INT}, + {10312, "sim/multiplay/generic/int[12]", simgear::props::INT}, + {10313, "sim/multiplay/generic/int[13]", simgear::props::INT}, + {10314, "sim/multiplay/generic/int[14]", simgear::props::INT}, + {10315, "sim/multiplay/generic/int[15]", simgear::props::INT}, + {10316, "sim/multiplay/generic/int[16]", simgear::props::INT}, + {10317, "sim/multiplay/generic/int[17]", simgear::props::INT}, + {10318, "sim/multiplay/generic/int[18]", simgear::props::INT}, + {10319, "sim/multiplay/generic/int[19]", simgear::props::INT} +}; + +const unsigned +FGMultiplayMgr::numProperties = (sizeof(FGMultiplayMgr::sIdPropertyList) + / sizeof(FGMultiplayMgr::sIdPropertyList[0])); + +// Look up a property ID using binary search. +namespace +{ + struct ComparePropertyId + { + bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, + const FGMultiplayMgr::IdPropertyList& rhs) + { + return lhs.id < rhs.id; + } + bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, + unsigned id) + { + return lhs.id < id; + } + bool operator()(unsigned id, + const FGMultiplayMgr::IdPropertyList& rhs) + { + return id < rhs.id; + } + }; + +} +const FGMultiplayMgr::IdPropertyList* FGMultiplayMgr::findProperty(unsigned id) +{ + std::pair result + = std::equal_range(sIdPropertyList, sIdPropertyList + numProperties, id, + ComparePropertyId()); + if (result.first == result.second) { + return 0; + } else { + return result.first; + } +} + +namespace +{ + bool verifyProperties(const xdr_data_t* data, const xdr_data_t* end) + { + using namespace simgear; + const xdr_data_t* xdr = data; + while (xdr < end) { + unsigned id = XDR_decode_uint32(*xdr); + const FGMultiplayMgr::IdPropertyList* plist + = FGMultiplayMgr::findProperty(id); + + if (plist) { + xdr++; + // How we decode the remainder of the property depends on the type + switch (plist->type) { + case props::INT: + case props::BOOL: + case props::LONG: + xdr++; + break; + case props::FLOAT: + case props::DOUBLE: + { + float val = XDR_decode_float(*xdr); + if (osg::isNaN(val)) + return false; + xdr++; + break; + } + case props::STRING: + case props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + // XXX Yes, each byte is padded out to a word! Too late + // to change... + uint32_t length = XDR_decode_uint32(*xdr); + xdr++; + // Old versions truncated the string but left the length + // unadjusted. + if (length > MAX_TEXT_SIZE) + length = MAX_TEXT_SIZE; + xdr += length; + // Now handle the padding + while ((length % 4) != 0) + { + xdr++; + length++; + //cout << "0"; + } + } + break; + default: + // cerr << "Unknown Prop type " << id << " " << type << "\n"; + xdr++; + break; + } + } + else { + // give up; this is a malformed property list. + return false; + } + } + return true; + } +} +////////////////////////////////////////////////////////////////////// +// +// MultiplayMgr constructor +// +////////////////////////////////////////////////////////////////////// +FGMultiplayMgr::FGMultiplayMgr() +{ + mSocket = 0; + mInitialised = false; + mHaveServer = false; +} // FGMultiplayMgr::FGMultiplayMgr() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// MultiplayMgr destructor +// +////////////////////////////////////////////////////////////////////// +FGMultiplayMgr::~FGMultiplayMgr() +{ + Close(); +} // FGMultiplayMgr::~FGMultiplayMgr() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Initialise object +// +////////////////////////////////////////////////////////////////////// +bool +FGMultiplayMgr::init (void) +{ + ////////////////////////////////////////////////// + // Initialise object if not already done + ////////////////////////////////////////////////// + if (mInitialised) { + SG_LOG(SG_NETWORK, SG_WARN, "FGMultiplayMgr::init - already initialised"); + return false; + } + ////////////////////////////////////////////////// + // Set members from property values + ////////////////////////////////////////////////// + short rxPort = fgGetInt("/sim/multiplay/rxport"); + string rxAddress = fgGetString("/sim/multiplay/rxhost"); + short txPort = fgGetInt("/sim/multiplay/txport"); + string txAddress = fgGetString("/sim/multiplay/txhost"); + mCallsign = fgGetString("/sim/multiplay/callsign"); + if (txPort > 0 && !txAddress.empty()) { + mServer.set(txAddress.c_str(), txPort); + if (strncmp (mServer.getHost(), "0.0.0.0", 8) == 0) { + mHaveServer = false; + SG_LOG(SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr - could not resolve '" + << txAddress << "', Multiplayermode disabled"); + } else { + mHaveServer = true; + } + if (rxPort <= 0) + rxPort = txPort; + } + if (rxPort <= 0) { + SG_LOG(SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr - No receiver port, Multiplayermode disabled"); + return (false); + } + if (mCallsign.empty()) + mCallsign = "JohnDoe"; // FIXME: use getpwuid + SG_LOG(SG_NETWORK,SG_INFO,"FGMultiplayMgr::init-txaddress= "<setBlocking(false); + if (mSocket->bind(rxAddress.c_str(), rxPort) != 0) { + perror("bind"); + SG_LOG( SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr::Open - Failed to bind receive socket" ); + return false; + } + mInitialised = true; + return true; +} // FGMultiplayMgr::init() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Closes and deletes the local player object. Closes +// and deletes the tx socket. Resets the object state to unitialised. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::Close (void) +{ + mMultiPlayerMap.clear(); + + if (mSocket) { + mSocket->close(); + delete mSocket; + mSocket = 0; + } + mInitialised = false; +} // FGMultiplayMgr::Close(void) +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Description: Sends the position data for the local position. +// +////////////////////////////////////////////////////////////////////// + +/** + * The buffer that holds a multi-player message, suitably aligned. + */ +union FGMultiplayMgr::MsgBuf +{ + MsgBuf() + { + memset(&Msg, 0, sizeof(Msg)); + } + + T_MsgHdr* msgHdr() + { + return reinterpret_cast(Msg); + } + + const T_MsgHdr* msgHdr() const + { + return reinterpret_cast(Msg); + } + + T_PositionMsg* posMsg() + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr)); + } + + const T_PositionMsg* posMsg() const + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr)); + } + + xdr_data_t* properties() + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr) + + sizeof(T_PositionMsg)); + } + + const xdr_data_t* properties() const + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr) + + sizeof(T_PositionMsg)); + } + /** + * The end of the properties buffer. + */ + xdr_data_t* propsEnd() + { + return reinterpret_cast(Msg + MAX_PACKET_SIZE); + }; + + const xdr_data_t* propsEnd() const + { + return reinterpret_cast(Msg + MAX_PACKET_SIZE); + }; + /** + * The end of properties actually in the buffer. This assumes that + * the message header is valid. + */ + xdr_data_t* propsRecvdEnd() + { + return reinterpret_cast(Msg + msgHdr()->MsgLen); + } + + const xdr_data_t* propsRecvdEnd() const + { + return reinterpret_cast(Msg + msgHdr()->MsgLen); + } + + xdr_data2_t double_val; + char Msg[MAX_PACKET_SIZE]; +}; + +void +FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) +{ + if ((! mInitialised) || (! mHaveServer)) + return; + if (! mHaveServer) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); + return; + } + + MsgBuf msgBuf; + T_PositionMsg* PosMsg = msgBuf.posMsg(); + + strncpy(PosMsg->Model, fgGetString("/sim/model/path"), MAX_MODEL_NAME_LEN); + PosMsg->Model[MAX_MODEL_NAME_LEN - 1] = '\0'; + + PosMsg->time = XDR_encode_double (motionInfo.time); + PosMsg->lag = XDR_encode_double (motionInfo.lag); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->position[i] = XDR_encode_double (motionInfo.position(i)); + SGVec3f angleAxis; + motionInfo.orientation.getAngleAxis(angleAxis); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->orientation[i] = XDR_encode_float (angleAxis(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->linearVel[i] = XDR_encode_float (motionInfo.linearVel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->angularVel[i] = XDR_encode_float (motionInfo.angularVel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->linearAccel[i] = XDR_encode_float (motionInfo.linearAccel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->angularAccel[i] = XDR_encode_float (motionInfo.angularAccel(i)); + + xdr_data_t* ptr = msgBuf.properties(); + std::vector::const_iterator it; + it = motionInfo.properties.begin(); + //cout << "OUTPUT PROPERTIES\n"; + xdr_data_t* msgEnd = msgBuf.propsEnd(); + while (it != motionInfo.properties.end() && ptr + 2 < msgEnd) { + + // First element is the ID. Write it out when we know we have room for + // the whole property. + xdr_data_t id = XDR_encode_uint32((*it)->id); + // The actual data representation depends on the type + switch ((*it)->type) { + case simgear::props::INT: + case simgear::props::BOOL: + case simgear::props::LONG: + *ptr++ = id; + *ptr++ = XDR_encode_uint32((*it)->int_value); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->int_value << "\n"; + break; + case simgear::props::FLOAT: + case simgear::props::DOUBLE: + *ptr++ = id; + *ptr++ = XDR_encode_float((*it)->float_value); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; + break; + case simgear::props::STRING: + case simgear::props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + const char* lcharptr = (*it)->string_value; + + if (lcharptr != 0) + { + // Add the length + ////cout << "String length: " << strlen(lcharptr) << "\n"; + uint32_t len = strlen(lcharptr); + if (len > MAX_TEXT_SIZE) + len = MAX_TEXT_SIZE; + // XXX This should not be using 4 bytes per character! + // If there's not enough room for this property, drop it + // on the floor. + if (ptr + 2 + ((len + 3) & ~3) > msgEnd) + goto escape; + //cout << "String length unint32: " << len << "\n"; + *ptr++ = id; + *ptr++ = XDR_encode_uint32(len); + if (len != 0) + { + // Now the text itself + // XXX This should not be using 4 bytes per character! + int lcount = 0; + while ((*lcharptr != '\0') && (lcount < MAX_TEXT_SIZE)) + { + *ptr++ = XDR_encode_int8(*lcharptr); + lcharptr++; + lcount++; + } + + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " " << len << " " << (*it)->string_value; + + // Now pad if required + while ((lcount % 4) != 0) + { + *ptr++ = XDR_encode_int8(0); + lcount++; + //cout << "0"; + } + + //cout << "\n"; + } + } + else + { + // Nothing to encode + *ptr++ = id; + *ptr++ = XDR_encode_uint32(0); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " 0\n"; + } + } + break; + + default: + //cout << " Unknown Type: " << (*it)->type << "\n"; + *ptr++ = id; + *ptr++ = XDR_encode_float((*it)->float_value);; + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; + break; + } + + ++it; + } +escape: + unsigned msgLen = reinterpret_cast(ptr) - msgBuf.Msg; + FillMsgHdr(msgBuf.msgHdr(), POS_DATA_ID, msgLen); + mSocket->sendto(msgBuf.Msg, msgLen, 0, &mServer); + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition"); +} // FGMultiplayMgr::SendMyPosition() + +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Name: SendTextMessage +// Description: Sends a message to the player. The message must +// contain a valid and correctly filled out header and optional +// message body. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::SendTextMessage(const string &MsgText) +{ + if (!mInitialised || !mHaveServer) + return; + + T_MsgHdr MsgHdr; + FillMsgHdr(&MsgHdr, CHAT_MSG_ID); + ////////////////////////////////////////////////// + // Divide the text string into blocks that fit + // in the message and send the blocks. + ////////////////////////////////////////////////// + unsigned iNextBlockPosition = 0; + T_ChatMsg ChatMsg; + + char Msg[sizeof(T_MsgHdr) + sizeof(T_ChatMsg)]; + while (iNextBlockPosition < MsgText.length()) { + strncpy (ChatMsg.Text, + MsgText.substr(iNextBlockPosition, MAX_CHAT_MSG_LEN - 1).c_str(), + MAX_CHAT_MSG_LEN); + ChatMsg.Text[MAX_CHAT_MSG_LEN - 1] = '\0'; + memcpy (Msg, &MsgHdr, sizeof(T_MsgHdr)); + memcpy (Msg + sizeof(T_MsgHdr), &ChatMsg, sizeof(T_ChatMsg)); + mSocket->sendto (Msg, sizeof(T_MsgHdr) + sizeof(T_ChatMsg), 0, &mServer); + iNextBlockPosition += MAX_CHAT_MSG_LEN - 1; + + } + + +} // FGMultiplayMgr::SendTextMessage () +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Name: ProcessData +// Description: Processes data waiting at the receive socket. The +// processing ends when there is no more data at the socket. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::Update(void) +{ + if (!mInitialised) + return; + + /// Just for expiry + long stamp = SGTimeStamp::now().getSeconds(); + + ////////////////////////////////////////////////// + // Read the receive socket and process any data + ////////////////////////////////////////////////// + ssize_t bytes; + do { + MsgBuf msgBuf; + ////////////////////////////////////////////////// + // Although the recv call asks for + // MAX_PACKET_SIZE of data, the number of bytes + // returned will only be that of the next + // packet waiting to be processed. + ////////////////////////////////////////////////// + netAddress SenderAddress; + bytes = mSocket->recvfrom(msgBuf.Msg, sizeof(msgBuf.Msg), 0, + &SenderAddress); + ////////////////////////////////////////////////// + // no Data received + ////////////////////////////////////////////////// + if (bytes <= 0) { + if (errno != EAGAIN && errno != 0) // MSVC output "NoError" otherwise + perror("FGMultiplayMgr::MP_ProcessData"); + break; + } + if (bytes <= static_cast(sizeof(T_MsgHdr))) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "received message with insufficient data" ); + break; + } + ////////////////////////////////////////////////// + // Read header + ////////////////////////////////////////////////// + T_MsgHdr* MsgHdr = msgBuf.msgHdr(); + MsgHdr->Magic = XDR_decode_uint32 (MsgHdr->Magic); + MsgHdr->Version = XDR_decode_uint32 (MsgHdr->Version); + MsgHdr->MsgId = XDR_decode_uint32 (MsgHdr->MsgId); + MsgHdr->MsgLen = XDR_decode_uint32 (MsgHdr->MsgLen); + MsgHdr->ReplyPort = XDR_decode_uint32 (MsgHdr->ReplyPort); + MsgHdr->Callsign[MAX_CALLSIGN_LEN -1] = '\0'; + if (MsgHdr->Magic != MSG_MAGIC) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message has invalid magic number!" ); + break; + } + if (MsgHdr->Version != PROTO_VER) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message has invalid protocoll number!" ); + break; + } + if (MsgHdr->MsgLen != bytes) { + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message from " << MsgHdr->Callsign << " has invalid length!"); + break; + } + ////////////////////////////////////////////////// + // Process messages + ////////////////////////////////////////////////// + switch (MsgHdr->MsgId) { + case CHAT_MSG_ID: + ProcessChatMsg(msgBuf, SenderAddress); + break; + case POS_DATA_ID: + ProcessPosMsg(msgBuf, SenderAddress, stamp); + break; + case UNUSABLE_POS_DATA_ID: + case OLD_OLD_POS_DATA_ID: + case OLD_PROP_MSG_ID: + case OLD_POS_DATA_ID: + break; + default: + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Unknown message Id received: " << MsgHdr->MsgId ); + break; + } + } while (bytes > 0); + + // check for expiry + MultiPlayerMap::iterator it = mMultiPlayerMap.begin(); + while (it != mMultiPlayerMap.end()) { + if (it->second->getLastTimestamp() + 10 < stamp) { + std::string name = it->first; + it->second->setDie(true); + mMultiPlayerMap.erase(it); + it = mMultiPlayerMap.upper_bound(name); + } else + ++it; + } +} // FGMultiplayMgr::ProcessData(void) +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// handle a position message +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, + const netAddress& SenderAddress, long stamp) +{ + const T_MsgHdr* MsgHdr = Msg.msgHdr(); + if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + sizeof(T_PositionMsg)) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Position message received with insufficient data" ); + return; + } + const T_PositionMsg* PosMsg = Msg.posMsg(); + FGExternalMotionData motionInfo; + motionInfo.time = XDR_decode_double(PosMsg->time); + motionInfo.lag = XDR_decode_double(PosMsg->lag); + for (unsigned i = 0; i < 3; ++i) + motionInfo.position(i) = XDR_decode_double(PosMsg->position[i]); + SGVec3f angleAxis; + for (unsigned i = 0; i < 3; ++i) + angleAxis(i) = XDR_decode_float(PosMsg->orientation[i]); + motionInfo.orientation = SGQuatf::fromAngleAxis(angleAxis); + for (unsigned i = 0; i < 3; ++i) + motionInfo.linearVel(i) = XDR_decode_float(PosMsg->linearVel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.angularVel(i) = XDR_decode_float(PosMsg->angularVel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.linearAccel(i) = XDR_decode_float(PosMsg->linearAccel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.angularAccel(i) = XDR_decode_float(PosMsg->angularAccel[i]); + + + //cout << "INPUT MESSAGE\n"; + + // There was a bug in 1.9.0 and before: T_PositionMsg was 196 bytes + // on 32 bit architectures and 200 bytes on 64 bit, and this + // structure is put directly on the wire. By looking at the padding, + // we can sort through the mess, mostly: + // If padding is 0 (which is not a valid property type), then the + // message was produced by a new client or an old 64 bit client that + // happened to have 0 on the stack; + // Else if the property list starting with the padding word is + // well-formed, then the client is probably an old 32 bit client and + // we'll go with that; + // Else it is an old 64-bit client and properties start after the + // padding. + // There is a chance that we could be fooled by garbage in the + // padding looking like a valid property, so verifyProperties() is + // strict about the validity of the property values. + const xdr_data_t* xdr = Msg.properties(); + if (PosMsg->pad != 0) { + if (verifyProperties(&PosMsg->pad, Msg.propsRecvdEnd())) + xdr = &PosMsg->pad; + else if (!verifyProperties(xdr, Msg.propsRecvdEnd())) + goto noprops; + } + while (xdr < Msg.propsRecvdEnd()) { + FGPropertyData* pData = new FGPropertyData; + // simgear::props::Type type = simgear::props::UNSPECIFIED; + + // First element is always the ID + pData->id = XDR_decode_uint32(*xdr); + //cout << pData->id << " "; + xdr++; + + // Check the ID actually exists and get the type + const IdPropertyList* plist = findProperty(pData->id); + + if (plist) + { + pData->type = plist->type; + // How we decode the remainder of the property depends on the type + switch (pData->type) { + case simgear::props::INT: + case simgear::props::BOOL: + case simgear::props::LONG: + pData->int_value = XDR_decode_uint32(*xdr); + xdr++; + //cout << pData->int_value << "\n"; + break; + case simgear::props::FLOAT: + case simgear::props::DOUBLE: + pData->float_value = XDR_decode_float(*xdr); + xdr++; + //cout << pData->float_value << "\n"; + break; + case simgear::props::STRING: + case simgear::props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + uint32_t length = XDR_decode_uint32(*xdr); + xdr++; + //cout << length << " "; + // Old versions truncated the string but left the length unadjusted. + if (length > MAX_TEXT_SIZE) + length = MAX_TEXT_SIZE; + pData->string_value = new char[length + 1]; + //cout << " String: "; + for (unsigned i = 0; i < length; i++) + { + pData->string_value[i] = (char) XDR_decode_int8(*xdr); + xdr++; + //cout << pData->string_value[i]; + } + + pData->string_value[length] = '\0'; + + // Now handle the padding + while ((length % 4) != 0) + { + xdr++; + length++; + //cout << "0"; + } + //cout << "\n"; + } + break; + + default: + pData->float_value = XDR_decode_float(*xdr); + SG_LOG(SG_NETWORK, SG_DEBUG, "Unknown Prop type " << pData->id << " " << pData->type); + xdr++; + break; + } + + motionInfo.properties.push_back(pData); + } + else + { + // We failed to find the property. We'll try the next packet immediately. + SG_LOG(SG_NETWORK, SG_INFO, "FGMultiplayMgr::ProcessPosMsg - " + "message from " << MsgHdr->Callsign << " has unknown property id " + << pData->id); + } + } + noprops: + FGAIMultiplayer* mp = getMultiplayer(MsgHdr->Callsign); + if (!mp) + mp = addMultiplayer(MsgHdr->Callsign, PosMsg->Model); + mp->addMotionInfo(motionInfo, stamp); +} // FGMultiplayMgr::ProcessPosMsg() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// handle a chat message +// FIXME: display chat message withi flightgear +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::ProcessChatMsg(const MsgBuf& Msg, + const netAddress& SenderAddress) +{ + const T_MsgHdr* MsgHdr = Msg.msgHdr(); + if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + 1) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Chat message received with insufficient data" ); + return; + } + + char *chatStr = new char[MsgHdr->MsgLen - sizeof(T_MsgHdr)]; + const T_ChatMsg* ChatMsg + = reinterpret_cast(Msg.Msg + sizeof(T_MsgHdr)); + strncpy(chatStr, ChatMsg->Text, + MsgHdr->MsgLen - sizeof(T_MsgHdr)); + chatStr[MsgHdr->MsgLen - sizeof(T_MsgHdr) - 1] = '\0'; + + SG_LOG (SG_NETWORK, SG_WARN, "Chat [" << MsgHdr->Callsign << "]" + << " " << chatStr); + + delete [] chatStr; +} // FGMultiplayMgr::ProcessChatMsg () +////////////////////////////////////////////////////////////////////// + +void +FGMultiplayMgr::FillMsgHdr(T_MsgHdr *MsgHdr, int MsgId, unsigned _len) +{ + uint32_t len; + switch (MsgId) { + case CHAT_MSG_ID: + len = sizeof(T_MsgHdr) + sizeof(T_ChatMsg); + break; + case POS_DATA_ID: + len = _len; + break; + default: + len = sizeof(T_MsgHdr); + break; + } + MsgHdr->Magic = XDR_encode_uint32(MSG_MAGIC); + MsgHdr->Version = XDR_encode_uint32(PROTO_VER); + MsgHdr->MsgId = XDR_encode_uint32(MsgId); + MsgHdr->MsgLen = XDR_encode_uint32(len); + MsgHdr->ReplyAddress = 0; // Are obsolete, keep them for the server for + MsgHdr->ReplyPort = 0; // now + strncpy(MsgHdr->Callsign, mCallsign.c_str(), MAX_CALLSIGN_LEN); + MsgHdr->Callsign[MAX_CALLSIGN_LEN - 1] = '\0'; +} + +FGAIMultiplayer* +FGMultiplayMgr::addMultiplayer(const std::string& callsign, + const std::string& modelName) +{ + if (0 < mMultiPlayerMap.count(callsign)) + return mMultiPlayerMap[callsign].get(); + + FGAIMultiplayer* mp = new FGAIMultiplayer; + mp->setPath(modelName.c_str()); + mp->setCallSign(callsign); + mMultiPlayerMap[callsign] = mp; + + FGAIManager *aiMgr = (FGAIManager*)globals->get_subsystem("ai_model"); + if (aiMgr) { + aiMgr->attach(mp); + + /// FIXME: that must follow the attach ATM ... + for (unsigned i = 0; i < numProperties; ++i) + mp->addPropertyId(sIdPropertyList[i].id, sIdPropertyList[i].name); + } + + return mp; +} + +FGAIMultiplayer* +FGMultiplayMgr::getMultiplayer(const std::string& callsign) +{ + if (0 < mMultiPlayerMap.count(callsign)) + return mMultiPlayerMap[callsign].get(); + else + return 0; +} From b090d3dabce3ad1261fb4b514cb71ed0c8a4d29b Mon Sep 17 00:00:00 2001 From: Durk Talsma Date: Mon, 30 Aug 2010 21:13:16 +0200 Subject: [PATCH 5/7] Enable the collection of AI aircraft position statistics. This will allow for a dramatic speedup of the establishment of AI traffic after program initialization. --- src/Traffic/Schedule.cxx | 25 ++++++++--- src/Traffic/Schedule.hxx | 14 ++++++- src/Traffic/TrafficMgr.cxx | 86 ++++++++++++++++++++++++++++++++++++-- src/Traffic/TrafficMgr.hxx | 16 +++++++ 4 files changed, 130 insertions(+), 11 deletions(-) diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx index 65e8a2403..8b193550b 100644 --- a/src/Traffic/Schedule.cxx +++ b/src/Traffic/Schedule.cxx @@ -115,8 +115,11 @@ FGAISchedule::FGAISchedule(string model, i++) flights.push_back(new FGScheduledFlight((*(*i))));*/ AIManagerRef = 0; - //score = scre; + score = 0; firstRun = true; + runCount = 0; + hits = 0; + initialized = false; } FGAISchedule::FGAISchedule(const FGAISchedule &other) @@ -136,13 +139,17 @@ FGAISchedule::FGAISchedule(const FGAISchedule &other) radius = other.radius; groundOffset = other.groundOffset; flightType = other.flightType; - //score = other.score; + score = other.score; distanceToUser = other.distanceToUser; currentDestination = other.currentDestination; firstRun = other.firstRun; + runCount = other.runCount; + hits = other.hits; + initialized = other.initialized; } + FGAISchedule::~FGAISchedule() { /* for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++) @@ -355,6 +362,14 @@ void FGAISchedule::scheduleFlights() } currentDestination = flight->getArrivalAirport()->getId(); + if (!initialized) { + string departurePort = flight->getDepartureAirport()->getId(); + if (fgGetString("/sim/presets/airport-id") == departurePort) { + hits++; + } + runCount++; + initialized = true; + } time_t arr, dep; dep = flight->getDepartureTime(); @@ -481,12 +496,12 @@ double FGAISchedule::getSpeed() SG_CLAMP_RANGE(speed, 300.0, 500.0); return speed; } -/* + bool compareSchedules(FGAISchedule*a, FGAISchedule*b) { - //return (*a) < (*b); + return (*a) < (*b); } -*/ + // void FGAISchedule::setClosestDistanceToUser() // { diff --git a/src/Traffic/Schedule.hxx b/src/Traffic/Schedule.hxx index 75661bceb..bfda8c0fe 100644 --- a/src/Traffic/Schedule.hxx +++ b/src/Traffic/Schedule.hxx @@ -53,9 +53,12 @@ class FGAISchedule double groundOffset; double distanceToUser; int AIManagerRef; - //int score; + double score; + unsigned int runCount; + unsigned int hits; bool firstRun; double courseToDest; + bool initialized; void scheduleFlights(); @@ -107,10 +110,17 @@ class FGAISchedule const string& getFlightRules () { return (*flights.begin())->getFlightRules (); }; bool getHeavy () { return heavy; }; double getCourse () { return courseToDest; }; + unsigned int getRunCount () { return runCount; }; + unsigned int getHits () { return hits; }; + + void setrunCount(unsigned int count) { runCount = count; }; + void setHits (unsigned int count) { hits = count; }; + void setScore () { score = runCount ? ((double) hits / (double) runCount) : 0; }; + double getScore () { return score; }; FGScheduledFlight*findAvailableFlight (const string ¤tDestination, const string &req); // used to sort in decending order of score: I've probably found a better way to // decending order sorting, but still need to test that. - //bool operator< (const FGAISchedule &other) const { return (score > other.score); }; + bool operator< (const FGAISchedule &other) const { return (score > other.score); }; //void * getAiRef () { return AIManagerRef; }; //FGAISchedule* getAddress () { return this;}; diff --git a/src/Traffic/TrafficMgr.cxx b/src/Traffic/TrafficMgr.cxx index 7c13f0db6..c570c9ebd 100644 --- a/src/Traffic/TrafficMgr.cxx +++ b/src/Traffic/TrafficMgr.cxx @@ -85,10 +85,39 @@ FGTrafficManager::FGTrafficManager() FGTrafficManager:: ~FGTrafficManager() { - for (ScheduleVectorIterator sched = scheduledAircraft.begin(); sched != scheduledAircraft.end(); sched++) - { + // Save the heuristics data + bool saveData = false; + ofstream cachefile; + if (fgGetBool("/sim/traffic-manager/heuristics")) { + SGPath cacheData(fgGetString("/sim/fg-home")); + cacheData.append("ai"); + string airport = fgGetString("/sim/presets/airport-id"); + + if ((airport) != "") { + char buffer[128]; + ::snprintf(buffer, 128, "%c/%c/%c/", + airport[0], airport[1], airport[2]); + cacheData.append(buffer); + if (!cacheData.exists()) { + cacheData.create_dir(0777); + } + cacheData.append(airport + "-cache.txt"); + //cerr << "Saving AI traffic heuristics" << endl; + saveData = true; + cachefile.open(cacheData.str().c_str()); + } + } + for (ScheduleVectorIterator sched = scheduledAircraft.begin(); sched != scheduledAircraft.end(); sched++) { + if (saveData) { + cachefile << (*sched)->getRegistration() << " " + << (*sched)-> getRunCount() << " " + << (*sched)->getHits() << endl; + } delete (*sched); } + if (saveData) { + cachefile.close(); + } scheduledAircraft.clear(); flights.clear(); } @@ -99,9 +128,11 @@ void FGTrafficManager::init() ulDir* d, *d2; ulDirEnt* dent, *dent2; SGPath aircraftDir = globals->get_fg_root(); - SGPath path = aircraftDir; + heuristicsVector heuristics; + HeuristicMap heurMap; + aircraftDir.append("AI/Traffic"); if ((d = ulOpenDir(aircraftDir.c_str())) != NULL) { @@ -130,7 +161,53 @@ void FGTrafficManager::init() } ulCloseDir(d); } - + if (fgGetBool("/sim/traffic-manager/heuristics")) { + //cerr << "Processing Heuristics" << endl; + // Load the heuristics data + SGPath cacheData(fgGetString("/sim/fg-home")); + cacheData.append("ai"); + string airport = fgGetString("/sim/presets/airport-id"); + if ((airport) != "") { + char buffer[128]; + ::snprintf(buffer, 128, "%c/%c/%c/", + airport[0], airport[1], airport[2]); + cacheData.append(buffer); + cacheData.append(airport + "-cache.txt"); + if (cacheData.exists()) { + ifstream data(cacheData.c_str()); + while (1) { + Heuristic *h = new Heuristic; + data >> h->registration >> h->runCount >> h->hits; + if (data.eof()) + break; + heurMap[h->registration] = h; + heuristics.push_back(h); + } + } + } + for (currAircraft = scheduledAircraft.begin(); + currAircraft != scheduledAircraft.end(); + currAircraft++) { + string registration = (*currAircraft)->getRegistration(); + HeuristicMapIterator itr = heurMap.find(registration); + //cerr << "Processing heuristics for" << (*currAircraft)->getRegistration() << endl; + if (itr == heurMap.end()) { + //cerr << "No heuristics found for " << registration << endl; + } else { + (*currAircraft)->setrunCount(itr->second->runCount); + (*currAircraft)->setHits (itr->second->hits); + (*currAircraft)->setScore(); + //cerr <<"Runcount " << itr->second->runCount << ".Hits " << itr->second->hits << endl; + } + } + //cerr << "Done" << endl; + for (heuristicsVectorIterator hvi = heuristics.begin(); + hvi != heuristics.end(); + hvi++) { + delete (*hvi); + } + sort (scheduledAircraft.begin(), scheduledAircraft.end(), compareSchedules); + } currAircraft = scheduledAircraft.begin(); currAircraftClosest = scheduledAircraft.begin(); } @@ -153,6 +230,7 @@ void FGTrafficManager::update(double /*dt*/) { currAircraft = scheduledAircraft.begin(); } + //cerr << "Processing << " << (*currAircraft)->getRegistration() << " with score " << (*currAircraft)->getScore() << endl; if (!((*currAircraft)->update(now, userCart))) { // NOTE: With traffic manager II, this statement below is no longer true diff --git a/src/Traffic/TrafficMgr.hxx b/src/Traffic/TrafficMgr.hxx index c1a4465a8..6c72ce95c 100644 --- a/src/Traffic/TrafficMgr.hxx +++ b/src/Traffic/TrafficMgr.hxx @@ -57,6 +57,22 @@ typedef vector IdList; typedef vector::iterator IdListIterator; +class Heuristic +{ +public: + string registration; + unsigned int runCount; + unsigned int hits; +}; + +typedef vector heuristicsVector; +typedef vector::iterator heuristicsVectorIterator; + +typedef std::map < std::string, Heuristic *> HeuristicMap; +typedef HeuristicMap::iterator HeuristicMapIterator; + + + class FGTrafficManager : public SGSubsystem, public XMLVisitor { From eb45d524a8ea5f612d9734f0415261513985c74f Mon Sep 17 00:00:00 2001 From: Durk Talsma Date: Tue, 31 Aug 2010 13:21:30 +0200 Subject: [PATCH 6/7] Provide an initial heuristic for airports for which we haven't collected any usage data yet. --- src/Traffic/Schedule.cxx | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx index 8b193550b..e86cbcf24 100644 --- a/src/Traffic/Schedule.cxx +++ b/src/Traffic/Schedule.cxx @@ -497,6 +497,19 @@ double FGAISchedule::getSpeed() return speed; } +void FGAISchedule::setScore () +{ + if (runCount) { + score = ((double) hits / (double) runCount); + } else { + if (homePort == fgGetString("/sim/presets/airport-id")) { + score = 0.1; + } else { + score = 0.0; + } + } +} + bool compareSchedules(FGAISchedule*a, FGAISchedule*b) { return (*a) < (*b); From 800352e26f410cbca7c399431da7b90a41b2e416 Mon Sep 17 00:00:00 2001 From: Durk Talsma Date: Tue, 31 Aug 2010 16:57:07 +0200 Subject: [PATCH 7/7] - Whoops, missed committing this tiny but fatal-if-not-included change. --- src/Traffic/Schedule.hxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Traffic/Schedule.hxx b/src/Traffic/Schedule.hxx index bfda8c0fe..cc80e4080 100644 --- a/src/Traffic/Schedule.hxx +++ b/src/Traffic/Schedule.hxx @@ -115,7 +115,7 @@ class FGAISchedule void setrunCount(unsigned int count) { runCount = count; }; void setHits (unsigned int count) { hits = count; }; - void setScore () { score = runCount ? ((double) hits / (double) runCount) : 0; }; + void setScore (); double getScore () { return score; }; FGScheduledFlight*findAvailableFlight (const string ¤tDestination, const string &req); // used to sort in decending order of score: I've probably found a better way to