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 <Scenery/tilemgr.hxx>
30 #include <Airports/dynamics.hxx>
32 #include "AIGroundVehicle.hxx"
34 FGAIGroundVehicle::FGAIGroundVehicle() :
35 FGAIShip(otGroundVehicle),
54 FGAIGroundVehicle::~FGAIGroundVehicle() {}
56 void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
60 FGAIShip::readFromScenario(scFileNode);
62 setNoRoll(scFileNode->getBoolValue("no-roll", true));
63 setName(scFileNode->getStringValue("name", "groundvehicle"));
64 setSMPath(scFileNode->getStringValue("submodel-path", ""));
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", 38.55));
68 setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
69 setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
70 setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
71 setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
72 setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
73 setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
74 setParentName(scFileNode->getStringValue("parent", ""));
75 setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0));
76 setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
77 //we may need these later for towed vehicles
78 // setSubID(scFileNode->getIntValue("SubID", 0));
79 // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
80 // setFormate(scFileNode->getBoolValue("formate", true));
83 void FGAIGroundVehicle::bind() {
86 props->tie("controls/constants/elevation-coeff",
87 SGRawValuePointer<double>(&_elevation_coeff));
88 props->tie("controls/constants/pitch-coeff",
89 SGRawValuePointer<double>(&_pitch_coeff));
90 props->tie("position/ht-AGL-ft",
91 SGRawValuePointer<double>(&_ht_agl_ft));
92 props->tie("hitch/rel-bearing-deg",
93 SGRawValuePointer<double>(&_relbrg));
94 props->tie("hitch/tow-angle-deg",
95 SGRawValuePointer<double>(&_tow_angle));
96 props->tie("hitch/range-ft",
97 SGRawValuePointer<double>(&_range_ft));
98 props->tie("hitch/x-offset-ft",
99 SGRawValuePointer<double>(&_x_offset));
100 props->tie("hitch/y-offset-ft",
101 SGRawValuePointer<double>(&_y_offset));
102 props->tie("hitch/parent-x-offset-ft",
103 SGRawValuePointer<double>(&_parent_x_offset));
104 props->tie("hitch/parent-y-offset-ft",
105 SGRawValuePointer<double>(&_parent_y_offset));
106 props->tie("controls/constants/tow-angle/gain",
107 SGRawValuePointer<double>(&_tow_angle_gain));
108 props->tie("controls/constants/tow-angle/limit-deg",
109 SGRawValuePointer<double>(&_tow_angle_limit));
110 props->tie("controls/contact-x1-offset-ft",
111 SGRawValuePointer<double>(&_contact_x1_offset));
112 props->tie("controls/contact-x2-offset-ft",
113 SGRawValuePointer<double>(&_contact_x2_offset));
116 void FGAIGroundVehicle::unbind() {
119 props->untie("controls/constants/elevation-coeff");
120 props->untie("position/ht-AGL-ft");
121 props->untie("controls/constants/pitch-coeff");
122 props->untie("hitch/rel-bearing-deg");
123 props->untie("hitch/tow-angle-deg");
124 props->untie("hitch/range-ft");
125 props->untie("hitch/x-offset-ft");
126 props->untie("hitch/y-offset-ft");
127 props->untie("hitch/parent-x-offset-ft");
128 props->untie("hitch/parent-y-offset-ft");
129 props->untie("controls/constants/tow-angle/gain");
130 props->untie("controls/constants/tow-angle/limit-deg");
131 props->untie("controls/contact-x1-offset-ft");
132 props->untie("controls/contact-x2-offset-ft");
135 bool FGAIGroundVehicle::init(bool search_in_AI_path) {
136 if (!FGAIShip::init(search_in_AI_path))
146 void FGAIGroundVehicle::update(double dt) {
147 // SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
149 RunGroundVehicle(dt);
150 // FGAIShip::update(dt);
153 void FGAIGroundVehicle::setNoRoll(bool nr) {
157 void FGAIGroundVehicle::setContactX1offset(double x1) {
158 _contact_x1_offset = x1;
161 void FGAIGroundVehicle::setContactX2offset(double x2) {
162 _contact_x2_offset = x2;
165 void FGAIGroundVehicle::setXOffset(double x) {
169 void FGAIGroundVehicle::setYOffset(double y) {
173 void FGAIGroundVehicle::setPitchCoeff(double pc) {
177 void FGAIGroundVehicle::setElevCoeff(double ec) {
178 _elevation_coeff = ec;
181 void FGAIGroundVehicle::setTowAngleGain(double g) {
185 void FGAIGroundVehicle::setTowAngleLimit(double l) {
186 _tow_angle_limit = l;
189 void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){
190 double c = dt / (coeff + dt);
191 _elevation_ft = (h * c) + (_elevation_ft * (1 - c));
194 void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){
195 double c = dt / (coeff + dt);
196 _pitch_deg = (p * c) + (_pitch_deg * (1 - c));
199 void FGAIGroundVehicle::setParentName(const string& p) {
203 void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
204 //_tow_angle = ta * _tow_angle_gain;
205 _tow_angle = pow(ta,2) * sign(ta);
206 SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
209 bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
213 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
214 _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
217 const vector<string>& names = _material->get_names();
219 _solid = _material->get_solid();
222 props->setStringValue("material/name", names[0].c_str());
224 props->setStringValue("material/name", "");
226 //cout << "material " << names[0].c_str()
227 // << " _elevation_m " << _elevation_m
228 // << " solid " << _solid
229 // << " load " << _load_resistance
230 // << " frictionFactor " << _frictionFactor
242 bool FGAIGroundVehicle::getPitch() {
246 double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
247 double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
248 double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
250 SGVec3d front(-contact_offset_x1_m, 0, 0);
251 SGVec3d rear(-contact_offset_x2_m, 0, 0);
252 SGVec3d Front = getCartPosAt(front);
253 SGVec3d Rear = getCartPosAt(rear);
255 SGGeod geodFront = SGGeod::fromCart(Front);
256 SGGeod geodRear = SGGeod::fromCart(Rear);
258 double front_elev_m = 0;
259 double rear_elev_m = 0;
260 double elev_front = 0;
261 double elev_rear = 0;
262 double max_alt = 10000;
264 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000), elev_front,
266 front_elev_m = elev_front;
270 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 3000),
271 elev_rear, &_material, 0)){
272 rear_elev_m = elev_rear;
277 double diff = front_elev_m - rear_elev_m;
278 _pitch = atan2 (diff,
279 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
280 _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
282 double diff = rear_elev_m - front_elev_m;
283 _pitch = atan2 (diff,
284 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
285 _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
291 if (prev->altitude == 0 || curr->altitude == 0) return false;
293 static double distance;
295 static double curr_alt;
296 static double prev_alt;
299 cout << "new waypoint, calculating pitch " << endl;
300 curr_alt = curr->altitude * SG_METER_TO_FEET;
301 prev_alt = prev->altitude * SG_METER_TO_FEET;
302 d_alt = curr_alt - prev_alt;
304 distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude),
305 SGGeod::fromDeg(curr->longitude, curr->latitude));
307 _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
308 // cout << "new waypoint, calculating pitch " << _pitch << endl;
312 double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
313 SGGeod::fromDeg(curr->longitude, curr->latitude));
315 //cout << "tunnel " << _tunnel
316 // << " distance curr & prev " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
317 // << " distance to go " << distance_to_go * SG_METER_TO_FEET
318 // << " d_alt ft " << d_alt
321 if (distance_to_go > distance)
322 _elevation = prev_alt;
324 _elevation = curr_alt - (tan(_pitch * SG_DEGREES_TO_RADIANS) * distance_to_go * SG_METER_TO_FEET);
333 void FGAIGroundVehicle::setParent() {
335 const SGPropertyNode *ai = fgGetNode("/ai/models", true);
337 for (int i = ai->nChildren() - 1; i >= -1; i--) {
338 const SGPropertyNode *model;
340 if (i < 0) { // last iteration: selected model
341 model = _selected_ac;
343 model = ai->getChild(i);
344 string path = ai->getPath();
345 const string name = model->getStringValue("name");
347 if (!model->nChildren()){
350 if (name == _parent) {
351 _selected_ac = model; // save selected model for last iteration
361 if (_selected_ac != 0){
362 const string name = _selected_ac->getStringValue("name");
363 double lat = _selected_ac->getDoubleValue("position/latitude-deg");
364 double lon = _selected_ac->getDoubleValue("position/longitude-deg");
365 double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
366 double hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft")
368 double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
371 _selectedpos.setLatitudeDeg(lat);
372 _selectedpos.setLongitudeDeg(lon);
373 _selectedpos.setElevationFt(elevation);
375 _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
376 _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
377 _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
379 SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0);
380 SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
382 SGGeod rearpos = SGGeod::fromCart(RearHitch);
384 double user_lat = rearpos.getLatitudeDeg();
385 double user_lon = rearpos.getLongitudeDeg();
387 double range, bearing;
389 calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
390 user_lat, user_lon, range, bearing);
391 _range_ft = range * 6076.11549;
392 _relbrg = calcRelBearingDeg(bearing, hdg);
394 SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
395 << " parent not found: dying ");
401 void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2,
402 double &range, double &bearing) const
404 // calculate the bearing and range of the second pos from the first
405 double az2, distance;
406 geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
407 range = distance * SG_METER_TO_NM;
410 double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
412 double angle = bearing - heading;
413 SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
417 SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const {
418 double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
419 double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
420 double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
422 // Transform that one to the horizontal local coordinate system.
423 SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
425 // and postrotate the orientation of the AIModel wrt the horizontal
427 hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
429 // The offset converted to the usual body fixed coordinate system
430 // rotated to the earth fiexed coordinates axis
431 SGVec3d off = hlTrans.backTransform(_off);
433 // Add the position offset of the AIModel to gain the earth centered position
434 SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
436 return cartPos + off;
439 void FGAIGroundVehicle::AdvanceFP(){
442 string parent_next_name =_selected_ac->getStringValue("waypoint/name-next");
444 while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){
445 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
446 <<" advancing waypoint to: " << parent_next_name);
448 if (fp->getNextWaypoint()->name == parent_next_name){
449 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
450 << " not setting waypoint already at: " << fp->getNextWaypoint()->name);
455 fp->IncrementWaypoint(false);
456 curr = fp->getCurrentWaypoint();
457 next = fp->getNextWaypoint();
459 if (fp->getNextWaypoint()->name == parent_next_name){
460 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
461 << " waypoint set to: " << fp->getNextWaypoint()->name);
469 while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END"
471 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
472 << " retreating waypoint to: " << parent_next_name
473 << " at: " << fp->getNextWaypoint()->name);
475 if (fp->getNextWaypoint()->name == parent_next_name){
476 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
477 << " not setting waypoint already at:" << fp->getNextWaypoint()->name );
482 fp->DecrementWaypoint(false);
483 curr = fp->getCurrentWaypoint();
484 next = fp->getNextWaypoint();
486 if (fp->getNextWaypoint()->name == parent_next_name){
487 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
488 << " waypoint set to: " << fp->getNextWaypoint()->name);
497 void FGAIGroundVehicle::setTowSpeed(){
499 double diff = _range_ft - _x_offset;
502 if (_range_ft > _x_offset * 3) x = 50;
504 if (_relbrg < -90 || _relbrg > 90){
505 setSpeed(_parent_speed - 5 - x);
506 //cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
507 }else if (_range_ft > _x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
508 setSpeed(_parent_speed + 1 + x);
509 //cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
510 // << diff << " range " << _range_ft << endl;
511 } else if (_range_ft < _x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
512 setSpeed(_parent_speed - 1 - x);
513 //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
514 // << diff << " " << _range_ft << endl;
516 setSpeed(_parent_speed);
517 //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl;
522 void FGAIGroundVehicle::RunGroundVehicle(double dt){
526 ///////////////////////////////////////////////////////////////////////////
527 // Check execution time (currently once every 0.05 sec or 20 fps)
528 // Add a bit of randomization to prevent the execution of all flight plans
529 // in synchrony, which can add significant periodic framerate flutter.
530 // Randomization removed to get better appearance
531 ///////////////////////////////////////////////////////////////////////////
533 //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
534 if (_dt_count < _next_run)
537 _next_run = 0.055 /*+ (0.015 * sg_random())*/;
540 setElevation(_elevation, _dt_count, _elevation_coeff);
541 ClimbTo(_elevation_ft);
542 setPitch(_pitch, _dt_count, _pitch_coeff);
547 AccelTo(prev->speed);
548 FGAIShip::update(_dt_count);
555 string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
556 bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
558 if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
559 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
560 << " setting END: getting new waypoints ");
563 /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
564 } else if (parent_waiting && !_waiting){
565 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
566 << " setting WAIT/WAITUNTIL: getting new waypoints ");
570 } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
571 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
572 << " wait done: getting new waypoints ");
575 fp->IncrementWaypoint(false);
576 next = fp->getNextWaypoint();
578 if (next->name == "WAITUNTIL" || next->name == "WAIT"
579 || next->name == "END"){
582 fp->IncrementWaypoint(false);
583 curr = fp->getCurrentWaypoint();
584 next = fp->getNextWaypoint();
588 } else if (_range_ft > _parent_x_offset * 4){
589 SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
590 << " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
592 setTowAngle(0, dt, 1);
593 setSpeed(_parent_speed * 2);
595 } else if (_parent_speed > 1){
598 setTowAngle(_relbrg, dt, 1);
600 } else if (_parent_speed < -1){
605 setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
607 setTowAngle(-(180 - _relbrg), dt, 1);
610 setSpeed(_parent_speed);
612 FGAIShip::update(_dt_count);
617 // end AIGroundvehicle