From d096455b7fa93f3731016426ccfcfc695d79df58 Mon Sep 17 00:00:00 2001 From: Tim Moore Date: Sun, 1 Nov 2009 00:15:14 +0100 Subject: [PATCH] Add support for high speed trains. Change update rate to main frame rate, and move framerate-heavy functions to a 20Hz update cycle. Jitter is much reduced, at a small cost to framerate Signed-off-by: v meazza Conflicts: src/AIModel/AIGroundVehicle.cxx --- src/AIModel/AIGroundVehicle.cxx | 89 +++++++++++++++++++++------------ src/AIModel/AIGroundVehicle.hxx | 5 +- src/AIModel/AIShip.cxx | 77 +++++++++++++++++++--------- src/AIModel/AIShip.hxx | 13 +++-- 4 files changed, 121 insertions(+), 63 deletions(-) diff --git a/src/AIModel/AIGroundVehicle.cxx b/src/AIModel/AIGroundVehicle.cxx index fce0e8c82..f7a6a1d9f 100644 --- a/src/AIModel/AIGroundVehicle.cxx +++ b/src/AIModel/AIGroundVehicle.cxx @@ -45,7 +45,9 @@ _dt_count(0), _next_run(0), _parent_x_offset(0), _parent_y_offset(0), -_parent("") +_parent_z_offset(0), +_parent(""), +_break_count(0) { invisible = false; @@ -64,16 +66,18 @@ void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) { setSMPath(scFileNode->getStringValue("submodel-path", "")); 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 +103,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 +125,17 @@ void FGAIGroundVehicle::unbind() { FGAIShip::unbind(); props->untie("controls/constants/elevation-coeff"); + props->untie("controls/constants/pitch-coeff"); props->untie("position/ht-AGL-ft"); - props->untie("controls/constants/pitch-coeff"); 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"); @@ -145,9 +155,9 @@ bool FGAIGroundVehicle::init(bool search_in_AI_path) { 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 +180,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; } @@ -201,9 +215,12 @@ void FGAIGroundVehicle::setParentName(const string& 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); + 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::getGroundElev(SGGeod inpos) { @@ -242,10 +259,10 @@ bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) { 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); @@ -261,9 +278,9 @@ bool FGAIGroundVehicle::getPitch() { double elev_rear = 0; 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; @@ -297,26 +314,26 @@ bool FGAIGroundVehicle::getPitch() { 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; - + 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; @@ -367,13 +384,17 @@ void FGAIGroundVehicle::setParent() { * 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); _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"); SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0); @@ -534,7 +555,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 +566,6 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ if(_parent == ""){ AccelTo(prev->speed); - FGAIShip::update(_dt_count); _dt_count = 0; return; } @@ -554,12 +574,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 +608,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 +634,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){ } else setSpeed(_parent_speed); - FGAIShip::update(_dt_count); +// FGAIShip::update(_dt_count); _dt_count = 0; } diff --git a/src/AIModel/AIGroundVehicle.hxx b/src/AIModel/AIGroundVehicle.hxx index 9be902c8a..f1f4085a9 100644 --- a/src/AIModel/AIGroundVehicle.hxx +++ b/src/AIModel/AIGroundVehicle.hxx @@ -53,6 +53,7 @@ private: void setContactX2offset(double x2); void setXOffset(double x); void setYOffset(double y); + void setZOffset(double z); void setPitchCoeff(double pc); void setElevCoeff(double ec); @@ -91,8 +92,8 @@ private: double _x_offset, _y_offset; double _range_ft; double _relbrg; - double _parent_speed, _parent_x_offset, _parent_y_offset; - double _dt_count, _next_run; + double _parent_speed, _parent_x_offset, _parent_y_offset, _parent_z_offset; + double _dt_count, _next_run, _break_count; const SGMaterial* _material; const SGPropertyNode *_selected_ac; diff --git a/src/AIModel/AIShip.cxx b/src/AIModel/AIShip.cxx index 76b470d6d..8fbb4beb5 100644 --- a/src/AIModel/AIShip.cxx +++ b/src/AIModel/AIShip.cxx @@ -68,7 +68,8 @@ _old_range(0), _range_rate(0), _roll_constant(0.001), _hdg_constant(0.01), -_roll_factor(-0.0083335) +_roll_factor(-0.0083335), +_restart(false) { invisible = false; @@ -89,6 +90,7 @@ void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) { setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000)); std::string flightplan = scFileNode->getStringValue("flightplan"); setRepeat(scFileNode->getBoolValue("repeat", false)); + setRestart(scFileNode->getBoolValue("restart", false)); setStartTime(scFileNode->getStringValue("time", "")); setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5)); setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15)); @@ -196,6 +198,8 @@ void FGAIShip::bind() { SGRawValuePointer(&_proportion)); props->tie("controls/fixed-turn-radius-ft", SGRawValuePointer(&_fixed_turn_radius)); + props->tie("controls/restart", + SGRawValuePointer(&_restart)); } void FGAIShip::unbind() { @@ -231,6 +235,7 @@ void FGAIShip::unbind() { props->untie("controls/constants/lead-angle/proportion"); props->untie("controls/fixed-turn-radius-ft"); props->untie("controls/constants/speed"); + props->untie("controls/restart"); } void FGAIShip::update(double dt) { @@ -332,17 +337,26 @@ void FGAIShip::Run(double dt) { //we assume that at slow speed ships will manoeuvre using engines/bow thruster - if (fabs(speed)<=5) - _sp_turn_radius_ft = _fixed_turn_radius; - else { - // adjust turn radius for speed. The equation is very approximate. - // we need to allow for negative speeds - if (type == "ship") - _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft; - else - _sp_turn_radius_ft = turn_radius_ft; + if(type == "ship" || type == "carrier"){ + + if (fabs(speed)<=5) + _sp_turn_radius_ft = _fixed_turn_radius; + else { + // adjust turn radius for speed. The equation is very approximate. + // we need to allow for negative speeds + _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft; + } + + } else { + + if (fabs(speed) <= 40) + _sp_turn_radius_ft = _fixed_turn_radius; + else { + // adjust turn radius for speed. + _sp_turn_radius_ft = turn_radius_ft; + } + } - } if (_rudder <= -0.25 || _rudder >= 0.25) { // adjust turn radius for _rudder angle. The equation is even more approximate. @@ -404,7 +418,7 @@ void FGAIShip::Run(double dt) { } // set the _rudder limit by speed - if (type == "ship"){ + if (type == "ship" || type == "carrier"){ if (speed <= 40) rudder_limit = (-0.825 * speed) + 35; @@ -456,13 +470,6 @@ void FGAIShip::ClimbTo(double altitude) { } void FGAIShip::TurnTo(double heading) { - //double relbrg_corr = _relbrg; - - //if ( relbrg_corr > 5) - // relbrg_corr = 5; - //else if( relbrg_corr < -5) - // relbrg_corr = -5; - tgt_heading = heading - _lead_angle + _tow_angle; SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); _hdg_lock = true; @@ -511,6 +518,10 @@ void FGAIShip::setRepeat(bool r) { _repeat = r; } +void FGAIShip::setRestart(bool r) { + _restart = r; +} + void FGAIShip::setMissed(bool m) { _missed = m; props->setBoolValue("waypoint/missed", _missed); @@ -548,6 +559,15 @@ void FGAIShip::setFixedTurnRadius(double ftr) { _fixed_turn_radius = ftr; } +void FGAIShip::setInitialTunnel(bool t) { + _initial_tunnel = t; + setTunnel(_initial_tunnel); +} + +void FGAIShip::setTunnel(bool t) { + _tunnel = t; +} + void FGAIShip::setWPNames() { if (prev != 0) @@ -614,7 +634,7 @@ void FGAIShip::ProcessFlightPlan(double dt) { if (_dt_count < _next_run && _start_sec < time_sec) return; - _next_run = 1.0 + (0.5 * sg_random()); + _next_run = 0.05 + (0.025 * sg_random()); double until_time_sec = 0; _missed = false; @@ -657,7 +677,7 @@ void FGAIShip::ProcessFlightPlan(double dt) { if (_next_name == "TUNNEL"){ _tunnel = !_tunnel; - //SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " TUNNEL "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " " << sp_turn_radius_nm ); fp->IncrementWaypoint(false); next = fp->getNextWaypoint(); @@ -674,7 +694,7 @@ void FGAIShip::ProcessFlightPlan(double dt) { }else if(_next_name == "END" || fp->getNextWaypoint() == 0) { if (_repeat) { - SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << " Flightplan repeating "); fp->restart(); prev = curr; curr = fp->getCurrentWaypoint(); @@ -687,6 +707,10 @@ void FGAIShip::ProcessFlightPlan(double dt) { _missed_count = 0; _lead_angle = 0; AccelTo(prev->speed); + } else if (_restart){ + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan restarting "); + _missed_count = 0; + initFlightPlan(); } else { SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan dying "); setDie(true); @@ -802,6 +826,7 @@ bool FGAIShip::initFlightPlan() { bool init = false; _start_sec = 0; + _tunnel = _initial_tunnel; fp->restart(); fp->IncrementWaypoint(false); @@ -859,7 +884,7 @@ bool FGAIShip::initFlightPlan() { _missed_count = 0; _new_waypoint = true; - SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " done initialising waypoints "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " done initialising waypoints " << _tunnel); if (prev) init = true; @@ -1068,15 +1093,17 @@ void FGAIShip::setXTrackError() { double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude); double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range; + double factor = -0.0045 * speed + 1; + double limit = _lead_angle_limit * factor; if (_wp_range > 0){ _lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES; } else _lead_angle = 0; - _lead_angle *= _lead_angle_gain; + _lead_angle *= _lead_angle_gain * factor; _xtrack_error = xtrack_error_nm * 6076.1155; - SG_CLAMP_RANGE(_lead_angle, -_lead_angle_limit, _lead_angle_limit); + SG_CLAMP_RANGE(_lead_angle, -limit, limit); } diff --git a/src/AIModel/AIShip.hxx b/src/AIModel/AIShip.hxx index 0526df32a..37689856a 100644 --- a/src/AIModel/AIShip.hxx +++ b/src/AIModel/AIShip.hxx @@ -61,6 +61,9 @@ public: void setRudderConstant(double rc); void setSpeedConstant(double sc); void setFixedTurnRadius(double ft); + void setTunnel(bool t); + void setInitialTunnel(bool t); + void setWPNames(); void setWPPos(); double sign(double x); @@ -69,13 +72,13 @@ public: bool _serviceable; bool _waiting; bool _new_waypoint; - bool _tunnel; - + bool _tunnel, _initial_tunnel; + bool _restart; virtual const char* getTypeString(void) const { return "ship"; } double _rudder_constant, _speed_constant, _hdg_constant, _limit ; double _elevation_m, _elevation_ft; - double _missed_range, _tow_angle, _wait_count, _wp_range; + double _missed_range, _tow_angle, _wait_count, _missed_count,_wp_range; FGAIFlightPlan::waypoint* prev; // the one behind you FGAIFlightPlan::waypoint* curr; // the one ahead @@ -92,6 +95,7 @@ private: virtual void reinit() { init(); } void setRepeat(bool r); + void setRestart(bool r); void setMissed(bool m); void setServiceable(bool s); @@ -119,7 +123,7 @@ private: double _roll_constant, _roll_factor; double _sp_turn_radius_ft, _rd_turn_radius_ft, _fixed_turn_radius; double _old_range, _range_rate; - double _dt_count, _missed_count; + double _dt_count; double _next_run; double _missed_time_sec; double _start_sec; @@ -137,6 +141,7 @@ private: bool _repeat; bool _fp_init; bool _missed; + };