From: Vivian Meazza Date: Sat, 28 Aug 2010 23:00:09 +0000 (+0100) Subject: Major update of Ballistic objects and submodels. Introduce submodels slaved to AI... X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=dff8099994e9205d0790aaf91e3a6a9795150e74;p=flightgear.git 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 --- 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()); - - if (getHtAGL()){ + 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(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(); + //int subcount = 0; + //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 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; + // ++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);