X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIGroundVehicle.cxx;h=257a73517c3397e78eeb3d18043b3c3d881ff8c7;hb=ff985e43df33be1698b994f6e0ee1f6e573ba390;hp=21bc80d5ed70a5f46ae069c026ce35aca6f69667;hpb=21122fa03ebfb676eacf9c705fb4b2a31416eb78;p=flightgear.git diff --git a/src/AIModel/AIGroundVehicle.cxx b/src/AIModel/AIGroundVehicle.cxx index 21bc80d5e..257a73517 100644 --- a/src/AIModel/AIGroundVehicle.cxx +++ b/src/AIModel/AIGroundVehicle.cxx @@ -37,18 +37,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("") +_parent_z_offset(0), +_dt_count(0), +_next_run(0), +_break_count(0) { invisible = false; + _parent = ""; } FGAIGroundVehicle::~FGAIGroundVehicle() {} @@ -59,21 +60,22 @@ 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", 38.55)); + setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 35.0)); setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0)); + setZOffset(scFileNode->getDoubleValue("hitch-z-offset", 0.0)); setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0)); setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0)); 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", 2.0)); + setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 1.0)); setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0)); + setInitialTunnel(scFileNode->getBoolValue("tunnel", false)); //we may need these later for towed vehicles // setSubID(scFileNode->getIntValue("SubID", 0)); // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0)); @@ -99,10 +101,14 @@ void FGAIGroundVehicle::bind() { SGRawValuePointer(&_x_offset)); props->tie("hitch/y-offset-ft", SGRawValuePointer(&_y_offset)); + props->tie("hitch/z-offset-ft", + SGRawValuePointer(&_z_offset)); props->tie("hitch/parent-x-offset-ft", SGRawValuePointer(&_parent_x_offset)); props->tie("hitch/parent-y-offset-ft", SGRawValuePointer(&_parent_y_offset)); + props->tie("hitch/parent-z-offset-ft", + SGRawValuePointer(&_parent_z_offset)); props->tie("controls/constants/tow-angle/gain", SGRawValuePointer(&_tow_angle_gain)); props->tie("controls/constants/tow-angle/limit-deg", @@ -117,15 +123,17 @@ void FGAIGroundVehicle::unbind() { FGAIShip::unbind(); props->untie("controls/constants/elevation-coeff"); - props->untie("position/ht-AGL-ft"); props->untie("controls/constants/pitch-coeff"); + props->untie("position/ht-AGL-ft"); props->untie("hitch/rel-bearing-deg"); props->untie("hitch/tow-angle-deg"); props->untie("hitch/range-ft"); props->untie("hitch/x-offset-ft"); props->untie("hitch/y-offset-ft"); + props->untie("hitch/z-offset-ft"); props->untie("hitch/parent-x-offset-ft"); props->untie("hitch/parent-y-offset-ft"); + props->untie("hitch/parent-y-offset-ft"); props->untie("controls/constants/tow-angle/gain"); props->untie("controls/constants/tow-angle/limit-deg"); props->untie("controls/contact-x1-offset-ft"); @@ -140,14 +148,29 @@ 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 ); + FGAIShip::update(dt); RunGroundVehicle(dt); -// FGAIShip::update(dt); } void FGAIGroundVehicle::setNoRoll(bool nr) { @@ -170,6 +193,10 @@ void FGAIGroundVehicle::setYOffset(double y) { _y_offset = y; } +void FGAIGroundVehicle::setZOffset(double z) { + _z_offset = z; +} + void FGAIGroundVehicle::setPitchCoeff(double pc) { _pitch_coeff = pc; } @@ -196,56 +223,22 @@ 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){ - //_tow_angle = ta * _tow_angle_gain; - _tow_angle = pow(ta,2) * sign(ta); - SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_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; - } - + ta *= _tow_angle_gain; + double factor = -0.0045 * speed + 1; + double limit = _tow_angle_limit * factor; +// cout << "speed "<< speed << " _factor " << _factor<<" " <<_tow_angle_limit<< endl; + _tow_angle = pow(ta,2) * sign(ta) * factor; + SG_CLAMP_RANGE(_tow_angle, -limit, limit); } bool FGAIGroundVehicle::getPitch() { if (!_tunnel){ - double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0); double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER; double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER; + double _z_offset_m = _parent_z_offset * SG_FEET_TO_METER; SGVec3d front(-contact_offset_x1_m, 0, 0); SGVec3d rear(-contact_offset_x2_m, 0, 0); @@ -259,11 +252,11 @@ 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)){ - front_elev_m = elev_front; + if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000), + elev_front, &_material, 0)){ + front_elev_m = elev_front + _z_offset_m; } else return false; @@ -296,27 +289,27 @@ bool FGAIGroundVehicle::getPitch() { static double prev_alt; if (_new_waypoint){ - cout << "new waypoint, calculating pitch " << endl; - curr_alt = curr->altitude * SG_METER_TO_FEET; - prev_alt = prev->altitude * SG_METER_TO_FEET; - d_alt = curr_alt - prev_alt; - + //cout << "new waypoint, calculating pitch " << endl; + curr_alt = curr->altitude; + prev_alt = prev->altitude; + //cout << "prev_alt" <altitude; distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude), SGGeod::fromDeg(curr->longitude, curr->latitude)); - _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES; -// cout << "new waypoint, calculating pitch " << _pitch << endl; + //cout << "new waypoint, calculating pitch " << _pitch << + // " " << _pitch_offset << " " << _elevation <longitude, curr->latitude)); - //cout << "tunnel " << _tunnel - // << " distance curr & prev " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET - // << " distance to go " << distance_to_go * SG_METER_TO_FEET - // << " d_alt ft " << d_alt - // << endl; + /*cout << "tunnel " << _tunnel + << " distance prev & curr " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET + << " distance to go " << distance_to_go * SG_METER_TO_FEET + << " d_alt ft " << d_alt + << endl;*/ if (distance_to_go > distance) _elevation = prev_alt; @@ -325,77 +318,35 @@ bool FGAIGroundVehicle::getPitch() { } - getGroundElev(pos); - return true; } -void FGAIGroundVehicle::setParent() { - - const SGPropertyNode *ai = fgGetNode("/ai/models", true); - - for (int i = ai->nChildren() - 1; i >= -1; i--) { - const SGPropertyNode *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; - - }// end for loop +void FGAIGroundVehicle::setParent(){ - 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 lat = _selected_ac->getDoubleValue("position/latitude-deg"); + double lon = _selected_ac->getDoubleValue("position/longitude-deg"); + double elevation = _selected_ac->getDoubleValue("position/altitude-ft"); - _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_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, @@ -407,12 +358,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"); @@ -496,7 +441,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; @@ -534,7 +479,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ if (_dt_count < _next_run) return; - _next_run = 0.055 /*+ (0.015 * sg_random())*/; + _next_run = 0.05 /*+ (0.015 * sg_random())*/; if (getPitch()){ setElevation(_elevation, _dt_count, _elevation_coeff); @@ -545,7 +490,6 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ if(_parent == ""){ AccelTo(prev->speed); - FGAIShip::update(_dt_count); _dt_count = 0; return; } @@ -554,12 +498,15 @@ 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"); if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){ SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name << " setting END: getting new waypoints "); AdvanceFP(); setWPNames(); + setTunnel(_initial_tunnel); + 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 @@ -585,12 +532,14 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ } setWPNames(); - } else if (_range_ft > _parent_x_offset * 4){ - SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name - << " rescue: reforming train " << _range_ft << " " << _x_offset * 15); + } else if (_range_ft > (_x_offset +_parent_x_offset)* 4 + ){ + SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name + << " rescue: reforming train " << _range_ft + ); setTowAngle(0, dt, 1); - setSpeed(_parent_speed * 2); + setSpeed(_parent_speed + (10 * sign(_parent_speed))); } else if (_parent_speed > 1){ @@ -609,7 +558,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ } else setSpeed(_parent_speed); - FGAIShip::update(_dt_count); +// FGAIShip::update(_dt_count); _dt_count = 0; }