X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIGroundVehicle.cxx;h=4708bd30f693c9110f817981796635b017649fbc;hb=6b1391e404b11e0d9a5bfcd11c1307e2e9531616;hp=e65ea5b7ee7d6eba8326b165c7407211598080c7;hpb=8ca420ec53ae9a4d2ca731c3a0db5fb68c2961e4;p=flightgear.git diff --git a/src/AIModel/AIGroundVehicle.cxx b/src/AIModel/AIGroundVehicle.cxx index e65ea5b7e..4708bd30f 100644 --- a/src/AIModel/AIGroundVehicle.cxx +++ b/src/AIModel/AIGroundVehicle.cxx @@ -26,7 +26,6 @@ #include
#include -#include #include #include "AIGroundVehicle.hxx" @@ -37,20 +36,19 @@ FGAIShip(otGroundVehicle), _pitch(0), _pitch_deg(0), _speed_kt(0), -_selected_ac(0), _range_ft(0), _relbrg (0), _parent_speed(0), -_dt_count(0), -_next_run(0), _parent_x_offset(0), _parent_y_offset(0), _parent_z_offset(0), -_parent(""), +_dt_count(0), +_next_run(0), _break_count(0) { invisible = false; + _parent = ""; } FGAIGroundVehicle::~FGAIGroundVehicle() {} @@ -61,9 +59,9 @@ void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) { FGAIShip::readFromScenario(scFileNode); - setNoRoll(scFileNode->getBoolValue("no-roll", true)); setName(scFileNode->getStringValue("name", "groundvehicle")); - setSMPath(scFileNode->getStringValue("submodel-path", "")); + setParentName(scFileNode->getStringValue("parent", "")); + setNoRoll(scFileNode->getBoolValue("no-roll", true)); setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0)); setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0)); setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 35.0)); @@ -74,7 +72,6 @@ void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) { setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0)); setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1)); setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25)); - setParentName(scFileNode->getStringValue("parent", "")); setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 1.0)); setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0)); setInitialTunnel(scFileNode->getBoolValue("tunnel", false)); @@ -125,7 +122,7 @@ void FGAIGroundVehicle::unbind() { FGAIShip::unbind(); props->untie("controls/constants/elevation-coeff"); - props->untie("controls/constants/pitch-coeff"); + props->untie("controls/constants/pitch-coeff"); props->untie("position/ht-AGL-ft"); props->untie("hitch/rel-bearing-deg"); props->untie("hitch/tow-angle-deg"); @@ -150,11 +147,26 @@ bool FGAIGroundVehicle::init(bool search_in_AI_path) { _limit = 200; no_roll = true; + props->setStringValue("controls/parent-name", _parent.c_str()); + + if (setParentNode()){ + _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft"); + _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft"); + _parent_z_offset = _selected_ac->getDoubleValue("hitch/z-offset-ft"); + _hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft") + * SG_FEET_TO_METER; + _hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft") + * SG_FEET_TO_METER; + _hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft") + * SG_FEET_TO_METER; + setParent(); + } + return true; } void FGAIGroundVehicle::update(double dt) { - // SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name ); + // SG_LOG(SG_AI, SG_ALERT, "updating GroundVehicle: " << _name ); FGAIShip::update(dt); RunGroundVehicle(dt); @@ -210,10 +222,6 @@ void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){ _pitch_deg = (p * c) + (_pitch_deg * (1 - c)); } -void FGAIGroundVehicle::setParentName(const string& p) { - _parent = p; -} - void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){ ta *= _tow_angle_gain; double factor = -0.0045 * speed + 1; @@ -223,39 +231,6 @@ void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){ SG_CLAMP_RANGE(_tow_angle, -limit, limit); } -bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) { - - double height_m ; - - if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){ - _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET; - - if (_material) { - const vector& names = _material->get_names(); - - _solid = _material->get_solid(); - - if (!names.empty()) - props->setStringValue("material/name", names[0].c_str()); - else - props->setStringValue("material/name", ""); - - //cout << "material " << names[0].c_str() - // << " _elevation_m " << _elevation_m - // << " solid " << _solid - // << " load " << _load_resistance - // << " frictionFactor " << _frictionFactor - // << endl; - - } - - return true; - } else { - return false; - } - -} - bool FGAIGroundVehicle::getPitch() { if (!_tunnel){ @@ -276,7 +251,7 @@ bool FGAIGroundVehicle::getPitch() { double rear_elev_m = 0; double elev_front = 0; double elev_rear = 0; - double max_alt = 10000; + //double max_alt = 10000; if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000), elev_front, &_material, 0)){ @@ -305,7 +280,7 @@ bool FGAIGroundVehicle::getPitch() { } else { - if (prev->altitude == 0 || curr->altitude == 0) return false; + if (prev->getAltitude() == 0 || curr->getAltitude() == 0) return false; static double distance; static double d_alt; @@ -314,20 +289,20 @@ bool FGAIGroundVehicle::getPitch() { if (_new_waypoint){ //cout << "new waypoint, calculating pitch " << endl; - curr_alt = curr->altitude; - prev_alt = prev->altitude; + curr_alt = curr->getAltitude(); + prev_alt = prev->getAltitude(); //cout << "prev_alt" <altitude; - distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude), - SGGeod::fromDeg(curr->longitude, curr->latitude)); + distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->getLongitude(), prev->getLatitude()), + SGGeod::fromDeg(curr->getLongitude(), curr->getLatitude())); _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES; //cout << "new waypoint, calculating pitch " << _pitch << // " " << _pitch_offset << " " << _elevation <longitude, curr->latitude)); + SGGeod::fromDeg(curr->getLongitude(), curr->getLatitude())); /*cout << "tunnel " << _tunnel << " distance prev & curr " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET @@ -342,81 +317,35 @@ bool FGAIGroundVehicle::getPitch() { } - getGroundElev(pos); - return true; } -void FGAIGroundVehicle::setParent() { - - 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"); - - if (!model->nChildren()){ - continue; - } - if (name == _parent) { - _selected_ac = model; // save selected model for last iteration - break; - } - - } - if (!model) - continue; +void FGAIGroundVehicle::setParent(){ - }// end for loop + double lat = _selected_ac->getDoubleValue("position/latitude-deg"); + double lon = _selected_ac->getDoubleValue("position/longitude-deg"); + double elevation = _selected_ac->getDoubleValue("position/altitude-ft"); - if (_selected_ac != 0){ - const string name = _selected_ac->getStringValue("name"); - double lat = _selected_ac->getDoubleValue("position/latitude-deg"); - double lon = _selected_ac->getDoubleValue("position/longitude-deg"); - double elevation = _selected_ac->getDoubleValue("position/altitude-ft"); - double hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft") - * SG_FEET_TO_METER; - double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft") - * SG_FEET_TO_METER; - double hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft") - * SG_FEET_TO_METER; - - _selectedpos.setLatitudeDeg(lat); - _selectedpos.setLongitudeDeg(lon); - _selectedpos.setElevationFt(elevation); + _selectedpos.setLatitudeDeg(lat); + _selectedpos.setLongitudeDeg(lon); + _selectedpos.setElevationFt(elevation); - _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft"); - _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft"); - _parent_z_offset = _selected_ac->getDoubleValue("hitch/z-offset-ft"); + _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt"); - _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt"); + SGVec3d rear_hitch(-_hitch_x_offset_m, _hitch_y_offset_m, 0); + SGVec3d RearHitch = getCartHitchPosAt(rear_hitch); - SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0); - SGVec3d RearHitch = getCartHitchPosAt(rear_hitch); + SGGeod rearpos = SGGeod::fromCart(RearHitch); - SGGeod rearpos = SGGeod::fromCart(RearHitch); + double user_lat = rearpos.getLatitudeDeg(); + double user_lon = rearpos.getLongitudeDeg(); - double user_lat = rearpos.getLatitudeDeg(); - double user_lon = rearpos.getLongitudeDeg(); - - double range, bearing; - - calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), - user_lat, user_lon, range, bearing); - _range_ft = range * 6076.11549; - _relbrg = calcRelBearingDeg(bearing, hdg); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name - << " parent not found: dying "); - setDie(true); - } + double range, bearing; + calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), + user_lat, user_lon, range, bearing); + _range_ft = range * 6076.11549; + _relbrg = calcRelBearingDeg(bearing, hdg); } void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2, @@ -428,12 +357,6 @@ void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, do range = distance * SG_METER_TO_NM; } -double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading) -{ - double angle = bearing - heading; - SG_NORMALIZE_RANGE(angle, -180.0, 180.0); - return angle; -} SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const { double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg"); @@ -462,13 +385,13 @@ void FGAIGroundVehicle::AdvanceFP(){ double count = 0; string parent_next_name =_selected_ac->getStringValue("waypoint/name-next"); - while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->getName() != "END" && count < 5){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name <<" advancing waypoint to: " << parent_next_name); - if (fp->getNextWaypoint()->name == parent_next_name){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name - << " not setting waypoint already at: " << fp->getNextWaypoint()->name); + if (fp->getNextWaypoint()->getName() == parent_next_name){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " not setting waypoint already at: " << fp->getNextWaypoint()->getName()); return; } @@ -477,9 +400,9 @@ void FGAIGroundVehicle::AdvanceFP(){ curr = fp->getCurrentWaypoint(); next = fp->getNextWaypoint(); - if (fp->getNextWaypoint()->name == parent_next_name){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name - << " waypoint set to: " << fp->getNextWaypoint()->name); + if (fp->getNextWaypoint()->getName() == parent_next_name){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " waypoint set to: " << fp->getNextWaypoint()->getName()); return; } @@ -487,15 +410,15 @@ void FGAIGroundVehicle::AdvanceFP(){ }// end while loop - while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END" + while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->getName() != "END" && count > -10){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name << " retreating waypoint to: " << parent_next_name - << " at: " << fp->getNextWaypoint()->name); + << " at: " << fp->getNextWaypoint()->getName()); - if (fp->getNextWaypoint()->name == parent_next_name){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name - << " not setting waypoint already at:" << fp->getNextWaypoint()->name ); + if (fp->getNextWaypoint()->getName() == parent_next_name){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " not setting waypoint already at:" << fp->getNextWaypoint()->getName() ); return; } @@ -504,9 +427,9 @@ void FGAIGroundVehicle::AdvanceFP(){ curr = fp->getCurrentWaypoint(); next = fp->getNextWaypoint(); - if (fp->getNextWaypoint()->name == parent_next_name){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name - << " waypoint set to: " << fp->getNextWaypoint()->name); + if (fp->getNextWaypoint()->getName() == parent_next_name){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " waypoint set to: " << fp->getNextWaypoint()->getName()); return; } @@ -517,7 +440,7 @@ void FGAIGroundVehicle::AdvanceFP(){ void FGAIGroundVehicle::setTowSpeed(){ - double diff = _range_ft - _x_offset; + //double diff = _range_ft - _x_offset; double x = 0; if (_range_ft > _x_offset * 3) x = 50; @@ -565,7 +488,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ } if(_parent == ""){ - AccelTo(prev->speed); + AccelTo(prev->getSpeed()); _dt_count = 0; return; } @@ -574,10 +497,10 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ string parent_next_name = _selected_ac->getStringValue("waypoint/name-next"); bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting"); - bool parent_restart = _selected_ac->getBoolValue("controls/restart"); + //bool parent_restart = _selected_ac->getBoolValue("controls/restart"); - if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + if (parent_next_name == "END" && fp->getNextWaypoint()->getName() != "END" ){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name << " setting END: getting new waypoints "); AdvanceFP(); setWPNames(); @@ -585,21 +508,21 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ if(_restart) _missed_count = 200; /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/ } else if (parent_waiting && !_waiting){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name << " setting WAIT/WAITUNTIL: getting new waypoints "); AdvanceFP(); setWPNames(); _waiting = true; - } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){ - SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->getName() == "WAIT"){ + SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name << " wait done: getting new waypoints "); _waiting = false; _wait_count = 0; fp->IncrementWaypoint(false); next = fp->getNextWaypoint(); - if (next->name == "WAITUNTIL" || next->name == "WAIT" - || next->name == "END"){ + if (next->getName() == "WAITUNTIL" || next->getName() == "WAIT" + || next->getName() == "END"){ } else { prev = curr; fp->IncrementWaypoint(false); @@ -610,7 +533,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ setWPNames(); } else if (_range_ft > (_x_offset +_parent_x_offset)* 4 ){ - SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name + SG_LOG(SG_AI, SG_ALERT, "AIGroundVeh1cle: " << _name << " rescue: reforming train " << _range_ft );