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),
49 FGAIGroundVehicle::~FGAIGroundVehicle() {}
51 void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
55 FGAIShip::readFromScenario(scFileNode);
57 setNoRoll(scFileNode->getBoolValue("no-roll", true));
58 setName(scFileNode->getStringValue("name", "groundvehicle"));
59 setSMPath(scFileNode->getStringValue("submodel-path", ""));
60 setContactX1offset(scFileNode->getDoubleValue("contact_x1_offset", 0.0));
61 setContactX2offset(scFileNode->getDoubleValue("contact_x2_offset", 0.0));
62 setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55));
63 setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
64 setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
65 setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
66 setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.5));
67 setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.5));
68 setParentName(scFileNode->getStringValue("parent", ""));
69 setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0));
70 setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
71 //we may need these later for towed vehicles
72 // setSubID(scFileNode->getIntValue("SubID", 0));
73 // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
74 // setFormate(scFileNode->getBoolValue("formate", true));
77 void FGAIGroundVehicle::bind() {
80 props->tie("controls/constants/elevation-coeff",
81 SGRawValuePointer<double>(&_elevation_coeff));
82 props->tie("controls/constants/pitch-coeff",
83 SGRawValuePointer<double>(&_pitch_coeff));
84 props->tie("position/ht-AGL-ft",
85 SGRawValuePointer<double>(&_ht_agl_ft));
86 props->tie("hitch/rel-bearing-deg",
87 SGRawValuePointer<double>(&_relbrg));
88 props->tie("hitch/tow-angle-deg",
89 SGRawValuePointer<double>(&_tow_angle));
90 props->tie("hitch/range-ft",
91 SGRawValuePointer<double>(&_range_ft));
92 props->tie("hitch/x-offset-ft",
93 SGRawValuePointer<double>(&_x_offset));
94 props->tie("hitch/parent-x-offset-ft",
95 SGRawValuePointer<double>(&_parent_x_offset));
96 props->tie("controls/constants/tow-angle/gain",
97 SGRawValuePointer<double>(&_tow_angle_gain));
98 props->tie("controls/constants/tow-angle/limit-deg",
99 SGRawValuePointer<double>(&_tow_angle_limit));
102 //we may need these later for towed vehicles
104 // (*this, &FGAIBallistic::getElevHitchToUser));
105 //props->tie("position/x-offset",
106 // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
107 //props->tie("position/y-offset",
108 // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
109 //props->tie("position/z-offset",
110 // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
111 //props->tie("position/tgt-x-offset",
112 // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset));
113 //props->tie("position/tgt-y-offset",
114 // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset));
115 //props->tie("position/tgt-z-offset",
116 // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset));
119 void FGAIGroundVehicle::unbind() {
122 props->untie("controls/constants/elevation-coeff");
123 props->untie("position/ht-AGL-ft");
124 props->untie("controls/constants/pitch-coeff");
125 props->untie("hitch/rel-bearing-deg");
126 props->untie("hitch/tow-angle-deg");
127 props->untie("hitch/range-ft");
128 props->untie("hitch/x-offset-ft");
129 props->untie("hitch/parent-x-offset-ft");
130 props->untie("controls/constants/tow-angle/gain");
131 props->untie("controls/constants/tow-angle/limit-deg");
133 //we may need these later for towed vehicles
134 //props->untie("load/rel-brg-to-user-deg");
135 //props->untie("load/elev-to-user-deg");
136 //props->untie("velocities/vertical-speed-fps");
137 //props->untie("position/x-offset");
138 //props->untie("position/y-offset");
139 //props->untie("position/z-offset");
140 //props->untie("position/tgt-x-offset");
141 //props->untie("position/tgt-y-offset");
142 //props->untie("position/tgt-z-offset");
145 bool FGAIGroundVehicle::init(bool search_in_AI_path) {
146 if (!FGAIShip::init(search_in_AI_path))
157 void FGAIGroundVehicle::update(double dt) {
158 // SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
161 setElevation(_elevation, dt, _elevation_coeff);
162 ClimbTo(_elevation_ft);
163 setPitch(_pitch, dt, _pitch_coeff);
170 string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
171 bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
172 _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
174 if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
175 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
176 << " setting END: getting new waypoints ");
179 /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
180 } else if (parent_waiting && !_waiting){
181 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
182 << " setting WAIT/WAITUNTIL: getting new waypoints ");
186 } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
187 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
188 << " wait done: getting new waypoints ");
191 fp->IncrementWaypoint(false);
192 next = fp->getNextWaypoint();
194 if (next->name == "WAITUNTIL" || next->name == "WAIT"
195 || next->name == "END"){
198 fp->IncrementWaypoint(false);
199 curr = fp->getCurrentWaypoint();
200 next = fp->getNextWaypoint();
204 } else if (_range_ft > 1000){
206 SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
207 << " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
209 setTowAngle(0, dt, 1);
210 setSpeed(_parent_speed * 2);
212 } else if (_parent_speed > 1){
215 setTowAngle(_relbrg, dt, 1);
217 } else if (_parent_speed < -1){
222 setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
224 setTowAngle(-(180 - _relbrg), dt, 1);
227 setSpeed(_parent_speed);
230 FGAIShip::update(dt);
233 void FGAIGroundVehicle::setNoRoll(bool nr) {
237 void FGAIGroundVehicle::setContactX1offset(double x1) {
238 _contact_x1_offset = x1;
241 void FGAIGroundVehicle::setContactX2offset(double x2) {
242 _contact_x2_offset = x2;
245 void FGAIGroundVehicle::setXOffset(double x) {
249 void FGAIGroundVehicle::setPitchCoeff(double pc) {
253 void FGAIGroundVehicle::setElevCoeff(double ec) {
254 _elevation_coeff = ec;
257 void FGAIGroundVehicle::setTowAngleGain(double g) {
261 void FGAIGroundVehicle::setTowAngleLimit(double l) {
262 _tow_angle_limit = l;
265 void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){
266 double c = dt / (coeff + dt);
267 _elevation_ft = (h * c) + (_elevation_ft * (1 - c));
270 void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){
271 double c = dt / (coeff + dt);
272 _pitch_deg = (p * c) + (_pitch_deg * (1 - c));
275 void FGAIGroundVehicle::setParentName(const string& p) {
279 void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
280 ta *= _tow_angle_gain;
281 //_tow_angle = pow(ta,2) * sign(ta);
282 //if (_tow_angle > _tow_angle_limit) _tow_angle = _tow_angle_limit;
283 //if (_tow_angle < -_tow_angle_limit) _tow_angle = -_tow_angle_limit;
284 SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
287 bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
289 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000),
290 _elevation_m, &_material)){
291 _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
294 const vector<string>& names = _material->get_names();
296 _solid = _material->get_solid();
297 _load_resistance = _material->get_load_resistance();
298 _frictionFactor =_material->get_friction_factor();
299 _elevation = _elevation_m * SG_METER_TO_FEET;
302 props->setStringValue("material/name", names[0].c_str());
304 props->setStringValue("material/name", "");
306 //cout << "material " << names[0].c_str()
307 // << " _elevation_m " << _elevation_m
308 // << " solid " << _solid
309 // << " load " << _load_resistance
310 // << " frictionFactor " << _frictionFactor
322 bool FGAIGroundVehicle::getPitch() {
324 double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
325 double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
326 double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
328 SGVec3d front(-contact_offset_x1_m, 0, 0);
329 SGVec3d rear(-contact_offset_x2_m, 0, 0);
330 SGVec3d Front = getCartPosAt(front);
331 SGVec3d Rear = getCartPosAt(rear);
333 SGGeod geodFront = SGGeod::fromCart(Front);
334 SGGeod geodRear = SGGeod::fromCart(Rear);
336 double front_elev_m = 0;
337 double rear_elev_m = 0;
338 double elev_front = 0;
339 double elev_rear = 0;
341 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000),
342 elev_front, &_material)){
344 front_elev_m = elev_front;
353 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
354 elev_rear, &_material)){
355 rear_elev_m = elev_rear;
357 // rear_elev_m = elev_rear;
364 double diff = front_elev_m - rear_elev_m;
365 _pitch = atan2 (diff,
366 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
367 _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
369 double diff = rear_elev_m - front_elev_m;
370 _pitch = atan2 (diff,
371 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
372 _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
379 void FGAIGroundVehicle::setParent() {
381 const SGPropertyNode *ai = fgGetNode("/ai/models", true);
383 for (int i = ai->nChildren() - 1; i >= -1; i--) {
384 const SGPropertyNode *model;
386 if (i < 0) { // last iteration: selected model
387 model = _selected_ac;
389 model = ai->getChild(i);
390 const string name = model->getStringValue("name");
392 if (!model->nChildren()){
395 if (name == _parent) {
396 _selected_ac = model; // save selected model for last iteration
406 if (_selected_ac != 0){
407 const string name = _selected_ac->getStringValue("name");
408 double lat = _selected_ac->getDoubleValue("position/latitude-deg");
409 double lon = _selected_ac->getDoubleValue("position/longitude-deg");
410 double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
411 double hitch_offset_m = _x_offset * SG_FEET_TO_METER;
412 _selectedpos.setLatitudeDeg(lat);
413 _selectedpos.setLongitudeDeg(lon);
414 _selectedpos.setElevationFt(elevation);
416 SGVec3d rear_hitch(-hitch_offset_m, 0, 0);
417 SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
419 SGGeod rearpos = SGGeod::fromCart(RearHitch);
421 double user_lat = rearpos.getLatitudeDeg();
422 double user_lon = rearpos.getLongitudeDeg();
424 double range, bearing;
426 calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
427 user_lat, user_lon, range, bearing);
428 _range_ft = range * 6076.11549;
429 _relbrg = calcRelBearingDeg(bearing, hdg);
431 SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
432 << " parent not found: dying ");
438 void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2,
439 double &range, double &bearing) const
441 // calculate the bearing and range of the second pos from the first
442 double az2, distance;
443 geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
444 range = distance *= SG_METER_TO_NM;
447 double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
449 double angle = bearing - heading;
451 SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
456 SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const {
457 double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
458 double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
459 double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
461 // Transform that one to the horizontal local coordinate system.
462 SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
464 // and postrotate the orientation of the AIModel wrt the horizontal
466 hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
468 // The offset converted to the usual body fixed coordinate system
469 // rotated to the earth fiexed coordinates axis
470 SGVec3d off = hlTrans.backTransform(_off);
472 // Add the position offset of the AIModel to gain the earth centered position
473 SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
475 return cartPos + off;
478 void FGAIGroundVehicle::AdvanceFP(){
481 string parent_next_name =_selected_ac->getStringValue("waypoint/name-next");
483 while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){
484 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
485 <<" advancing waypoint to: " << parent_next_name);
487 if (fp->getNextWaypoint()->name == parent_next_name){
488 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
489 << " not setting waypoint already at: " << fp->getNextWaypoint()->name);
494 fp->IncrementWaypoint(false);
495 curr = fp->getCurrentWaypoint();
496 next = fp->getNextWaypoint();
498 if (fp->getNextWaypoint()->name == parent_next_name){
499 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
500 << " waypoint set to: " << fp->getNextWaypoint()->name);
508 while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END"
510 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
511 << " retreating waypoint to: " << parent_next_name
512 << " at: " << fp->getNextWaypoint()->name);
514 if (fp->getNextWaypoint()->name == parent_next_name){
515 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
516 << " not setting waypoint already at:" << fp->getNextWaypoint()->name );
521 fp->DecrementWaypoint(false);
522 curr = fp->getCurrentWaypoint();
523 next = fp->getNextWaypoint();
525 if (fp->getNextWaypoint()->name == parent_next_name){
526 SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
527 << " waypoint set to: " << fp->getNextWaypoint()->name);
536 void FGAIGroundVehicle::setTowSpeed(){
538 _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
540 // double diff = _range_ft - _parent_x_offset;
543 if (_range_ft > _x_offset * 3) x = 50;
545 if (_relbrg < -90 || _relbrg > 90){
546 setSpeed(_parent_speed - 5 - x);
547 //cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
548 }else if (_range_ft > _parent_x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
549 setSpeed(_parent_speed + 1 + x);
550 //cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
551 // << diff << " range " << _range_ft << endl;
552 } else if (_range_ft < _parent_x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
553 setSpeed(_parent_speed - 1 - x);
554 //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
555 // << diff << " " << _range_ft << endl;
557 setSpeed(_parent_speed);
558 //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl;
562 // end AIGroundvehicle