1 // FGAIGroundVehicle - FGAIShip-derived class creates an AI Ground Vehicle
2 // by adding a ground following utility
4 // Written by Vivian Meazza, started August 2009.
5 // - vivian.meazza at lineone.net
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
25 #include <simgear/sg_inlines.h>
27 #include <Main/viewer.hxx>
28 #include <Scenery/scenery.hxx>
29 #include <Airports/dynamics.hxx>
31 #include "AIGroundVehicle.hxx"
33 FGAIGroundVehicle::FGAIGroundVehicle() :
34 FGAIShip(otGroundVehicle),
54 FGAIGroundVehicle::~FGAIGroundVehicle() {}
56 void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
60 FGAIShip::readFromScenario(scFileNode);
62 setName(scFileNode->getStringValue("name", "groundvehicle"));
63 setParentName(scFileNode->getStringValue("parent", ""));
64 setNoRoll(scFileNode->getBoolValue("no-roll", true));
65 setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0));
66 setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0));
67 setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 35.0));
68 setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
69 setZOffset(scFileNode->getDoubleValue("hitch-z-offset", 0.0));
70 setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
71 setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
72 setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
73 setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
74 setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
75 setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 1.0));
76 setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
77 setInitialTunnel(scFileNode->getBoolValue("tunnel", false));
78 //we may need these later for towed vehicles
79 // setSubID(scFileNode->getIntValue("SubID", 0));
80 // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
81 // setFormate(scFileNode->getBoolValue("formate", true));
84 void FGAIGroundVehicle::bind() {
87 props->tie("controls/constants/elevation-coeff",
88 SGRawValuePointer<double>(&_elevation_coeff));
89 props->tie("controls/constants/pitch-coeff",
90 SGRawValuePointer<double>(&_pitch_coeff));
91 props->tie("position/ht-AGL-ft",
92 SGRawValuePointer<double>(&_ht_agl_ft));
93 props->tie("hitch/rel-bearing-deg",
94 SGRawValuePointer<double>(&_relbrg));
95 props->tie("hitch/tow-angle-deg",
96 SGRawValuePointer<double>(&_tow_angle));
97 props->tie("hitch/range-ft",
98 SGRawValuePointer<double>(&_range_ft));
99 props->tie("hitch/x-offset-ft",
100 SGRawValuePointer<double>(&_x_offset));
101 props->tie("hitch/y-offset-ft",
102 SGRawValuePointer<double>(&_y_offset));
103 props->tie("hitch/z-offset-ft",
104 SGRawValuePointer<double>(&_z_offset));
105 props->tie("hitch/parent-x-offset-ft",
106 SGRawValuePointer<double>(&_parent_x_offset));
107 props->tie("hitch/parent-y-offset-ft",
108 SGRawValuePointer<double>(&_parent_y_offset));
109 props->tie("hitch/parent-z-offset-ft",
110 SGRawValuePointer<double>(&_parent_z_offset));
111 props->tie("controls/constants/tow-angle/gain",
112 SGRawValuePointer<double>(&_tow_angle_gain));
113 props->tie("controls/constants/tow-angle/limit-deg",
114 SGRawValuePointer<double>(&_tow_angle_limit));
115 props->tie("controls/contact-x1-offset-ft",
116 SGRawValuePointer<double>(&_contact_x1_offset));
117 props->tie("controls/contact-x2-offset-ft",
118 SGRawValuePointer<double>(&_contact_x2_offset));
121 void FGAIGroundVehicle::unbind() {
124 props->untie("controls/constants/elevation-coeff");
125 props->untie("controls/constants/pitch-coeff");
126 props->untie("position/ht-AGL-ft");
127 props->untie("hitch/rel-bearing-deg");
128 props->untie("hitch/tow-angle-deg");
129 props->untie("hitch/range-ft");
130 props->untie("hitch/x-offset-ft");
131 props->untie("hitch/y-offset-ft");
132 props->untie("hitch/z-offset-ft");
133 props->untie("hitch/parent-x-offset-ft");
134 props->untie("hitch/parent-y-offset-ft");
135 props->untie("hitch/parent-y-offset-ft");
136 props->untie("controls/constants/tow-angle/gain");
137 props->untie("controls/constants/tow-angle/limit-deg");
138 props->untie("controls/contact-x1-offset-ft");
139 props->untie("controls/contact-x2-offset-ft");
142 bool FGAIGroundVehicle::init(bool search_in_AI_path) {
143 if (!FGAIShip::init(search_in_AI_path))
149 void FGAIGroundVehicle::reinit() {
154 props->setStringValue("controls/parent-name", _parent.c_str());
156 if (setParentNode()){
157 _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
158 _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
159 _parent_z_offset = _selected_ac->getDoubleValue("hitch/z-offset-ft");
160 _hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft")
162 _hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
164 _hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft")
172 void FGAIGroundVehicle::update(double dt) {
173 // SG_LOG(SG_AI, SG_ALERT, "updating GroundVehicle: " << _name );
174 FGAIShip::update(dt);
176 RunGroundVehicle(dt);
179 void FGAIGroundVehicle::setNoRoll(bool nr) {
183 void FGAIGroundVehicle::setContactX1offset(double x1) {
184 _contact_x1_offset = x1;
187 void FGAIGroundVehicle::setContactX2offset(double x2) {
188 _contact_x2_offset = x2;
191 void FGAIGroundVehicle::setXOffset(double x) {
195 void FGAIGroundVehicle::setYOffset(double y) {
199 void FGAIGroundVehicle::setZOffset(double z) {
203 void FGAIGroundVehicle::setPitchCoeff(double pc) {
207 void FGAIGroundVehicle::setElevCoeff(double ec) {
208 _elevation_coeff = ec;
211 void FGAIGroundVehicle::setTowAngleGain(double g) {
215 void FGAIGroundVehicle::setTowAngleLimit(double l) {
216 _tow_angle_limit = l;
219 void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){
220 double c = dt / (coeff + dt);
221 _elevation_ft = (h * c) + (_elevation_ft * (1 - c));
224 void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){
225 double c = dt / (coeff + dt);
226 _pitch_deg = (p * c) + (_pitch_deg * (1 - c));
229 void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
230 ta *= _tow_angle_gain;
231 double factor = -0.0045 * speed + 1;
232 double limit = _tow_angle_limit * factor;
233 // cout << "speed "<< speed << " _factor " << _factor<<" " <<_tow_angle_limit<< endl;
234 _tow_angle = pow(ta,2) * sign(ta) * factor;
235 SG_CLAMP_RANGE(_tow_angle, -limit, limit);
238 bool FGAIGroundVehicle::getPitch() {
241 double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
242 double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
243 double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
244 double _z_offset_m = _parent_z_offset * SG_FEET_TO_METER;
246 SGVec3d front(-contact_offset_x1_m, 0, 0);
247 SGVec3d rear(-contact_offset_x2_m, 0, 0);
248 SGVec3d Front = getCartPosAt(front);
249 SGVec3d Rear = getCartPosAt(rear);
251 SGGeod geodFront = SGGeod::fromCart(Front);
252 SGGeod geodRear = SGGeod::fromCart(Rear);
254 double front_elev_m = 0;
255 double rear_elev_m = 0;
256 double elev_front = 0;
257 double elev_rear = 0;
258 //double max_alt = 10000;
260 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000),
261 elev_front, &_material, 0)){
262 front_elev_m = elev_front + _z_offset_m;
266 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 3000),
267 elev_rear, &_material, 0)){
268 rear_elev_m = elev_rear;
273 double diff = front_elev_m - rear_elev_m;
274 _pitch = atan2 (diff,
275 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
276 _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
278 double diff = rear_elev_m - front_elev_m;
279 _pitch = atan2 (diff,
280 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
281 _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
287 if (prev->getAltitude() == 0 || curr->getAltitude() == 0) return false;
289 static double distance;
291 static double curr_alt;
292 static double prev_alt;
295 //cout << "new waypoint, calculating pitch " << endl;
296 curr_alt = curr->getAltitude();
297 prev_alt = prev->getAltitude();
298 //cout << "prev_alt" <<prev_alt << endl;
299 d_alt = (curr_alt - prev_alt) * SG_METER_TO_FEET;
300 //_elevation = prev->altitude;
301 distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->getLongitude(), prev->getLatitude()),
302 SGGeod::fromDeg(curr->getLongitude(), curr->getLatitude()));
303 _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
304 //cout << "new waypoint, calculating pitch " << _pitch <<
305 // " " << _pitch_offset << " " << _elevation <<endl;
308 double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
309 SGGeod::fromDeg(curr->getLongitude(), curr->getLatitude()));
311 /*cout << "tunnel " << _tunnel
312 << " distance prev & curr " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
313 << " distance to go " << distance_to_go * SG_METER_TO_FEET
314 << " d_alt ft " << d_alt
317 if (distance_to_go > distance)
318 _elevation = prev_alt;
320 _elevation = curr_alt - (tan(_pitch * SG_DEGREES_TO_RADIANS) * distance_to_go * SG_METER_TO_FEET);
327 void FGAIGroundVehicle::setParent(){
329 double lat = _selected_ac->getDoubleValue("position/latitude-deg");
330 double lon = _selected_ac->getDoubleValue("position/longitude-deg");
331 double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
333 _selectedpos.setLatitudeDeg(lat);
334 _selectedpos.setLongitudeDeg(lon);
335 _selectedpos.setElevationFt(elevation);
337 _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
339 SGVec3d rear_hitch(-_hitch_x_offset_m, _hitch_y_offset_m, 0);
340 SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
342 SGGeod rearpos = SGGeod::fromCart(RearHitch);
344 double user_lat = rearpos.getLatitudeDeg();
345 double user_lon = rearpos.getLongitudeDeg();
347 double range, bearing;
349 calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
350 user_lat, user_lon, range, bearing);
351 _range_ft = range * 6076.11549;
352 _relbrg = calcRelBearingDeg(bearing, hdg);
355 void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2,
356 double &range, double &bearing) const
358 // calculate the bearing and range of the second pos from the first
359 double az2, distance;
360 geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
361 range = distance * SG_METER_TO_NM;
365 SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const {
366 double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
367 double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
368 double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
370 // Transform that one to the horizontal local coordinate system.
371 SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
373 // and postrotate the orientation of the AIModel wrt the horizontal
375 hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
377 // The offset converted to the usual body fixed coordinate system
378 // rotated to the earth fiexed coordinates axis
379 SGVec3d off = hlTrans.backTransform(_off);
381 // Add the position offset of the AIModel to gain the earth centered position
382 SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
384 return cartPos + off;
387 void FGAIGroundVehicle::AdvanceFP(){
390 string parent_next_name =_selected_ac->getStringValue("waypoint/name-next");
392 while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->getName() != "END" && count < 5){
393 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
394 <<" advancing waypoint to: " << parent_next_name);
396 if (fp->getNextWaypoint()->getName() == parent_next_name){
397 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
398 << " not setting waypoint already at: " << fp->getNextWaypoint()->getName());
403 fp->IncrementWaypoint(false);
404 curr = fp->getCurrentWaypoint();
405 next = fp->getNextWaypoint();
407 if (fp->getNextWaypoint()->getName() == parent_next_name){
408 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
409 << " waypoint set to: " << fp->getNextWaypoint()->getName());
417 while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->getName() != "END"
419 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
420 << " retreating waypoint to: " << parent_next_name
421 << " at: " << fp->getNextWaypoint()->getName());
423 if (fp->getNextWaypoint()->getName() == parent_next_name){
424 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
425 << " not setting waypoint already at:" << fp->getNextWaypoint()->getName() );
430 fp->DecrementWaypoint(false);
431 curr = fp->getCurrentWaypoint();
432 next = fp->getNextWaypoint();
434 if (fp->getNextWaypoint()->getName() == parent_next_name){
435 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
436 << " waypoint set to: " << fp->getNextWaypoint()->getName());
445 void FGAIGroundVehicle::setTowSpeed(){
447 //double diff = _range_ft - _x_offset;
450 if (_range_ft > _x_offset * 3) x = 50;
452 if (_relbrg < -90 || _relbrg > 90){
453 setSpeed(_parent_speed - 5 - x);
454 //cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
455 }else if (_range_ft > _x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
456 setSpeed(_parent_speed + 1 + x);
457 //cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
458 // << diff << " range " << _range_ft << endl;
459 } else if (_range_ft < _x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
460 setSpeed(_parent_speed - 1 - x);
461 //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
462 // << diff << " " << _range_ft << endl;
464 setSpeed(_parent_speed);
465 //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl;
470 void FGAIGroundVehicle::RunGroundVehicle(double dt){
474 ///////////////////////////////////////////////////////////////////////////
475 // Check execution time (currently once every 0.05 sec or 20 fps)
476 // Add a bit of randomization to prevent the execution of all flight plans
477 // in synchrony, which can add significant periodic framerate flutter.
478 // Randomization removed to get better appearance
479 ///////////////////////////////////////////////////////////////////////////
481 //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
482 if (_dt_count < _next_run)
485 _next_run = 0.05 /*+ (0.015 * sg_random())*/;
488 setElevation(_elevation, _dt_count, _elevation_coeff);
489 ClimbTo(_elevation_ft);
490 setPitch(_pitch, _dt_count, _pitch_coeff);
495 AccelTo(prev->getSpeed());
502 string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
503 bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
504 //bool parent_restart = _selected_ac->getBoolValue("controls/restart");
506 if (parent_next_name == "END" && fp->getNextWaypoint()->getName() != "END" ){
507 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
508 << " setting END: getting new waypoints ");
511 setTunnel(_initial_tunnel);
512 if(_restart) _missed_count = 200;
513 /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
514 } else if (parent_waiting && !_waiting){
515 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
516 << " setting WAIT/WAITUNTIL: getting new waypoints ");
520 } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->getName() == "WAIT"){
521 SG_LOG(SG_AI, SG_DEBUG, "AIGroundVeh1cle: " << _name
522 << " wait done: getting new waypoints ");
525 fp->IncrementWaypoint(false);
526 next = fp->getNextWaypoint();
528 if (next->getName() == "WAITUNTIL" || next->getName() == "WAIT"
529 || next->getName() == "END"){
532 fp->IncrementWaypoint(false);
533 curr = fp->getCurrentWaypoint();
534 next = fp->getNextWaypoint();
538 } else if (_range_ft > (_x_offset +_parent_x_offset)* 4
540 SG_LOG(SG_AI, SG_ALERT, "AIGroundVeh1cle: " << _name
541 << " rescue: reforming train " << _range_ft
544 setTowAngle(0, dt, 1);
545 setSpeed(_parent_speed + (10 * sign(_parent_speed)));
547 } else if (_parent_speed > 1){
550 setTowAngle(_relbrg, dt, 1);
552 } else if (_parent_speed < -1){
557 setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
559 setTowAngle(-(180 - _relbrg), dt, 1);
562 setSpeed(_parent_speed);
564 // FGAIShip::update(_dt_count);
569 // end AIGroundvehicle