1 // FGAIEscort - FGAIShip-derived class creates an AI Ground Vehicle
\r
2 // by adding a ground following utility
\r
4 // Written by Vivian Meazza, started August 2009.
\r
5 // - vivian.meazza at lineone.net
\r
7 // This program is free software; you can redistribute it and/or
\r
8 // modify it under the terms of the GNU General Public License as
\r
9 // published by the Free Software Foundation; either version 2 of the
\r
10 // License, or (at your option) any later version.
\r
12 // This program is distributed in the hope that it will be useful, but
\r
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
\r
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
\r
15 // General Public License for more details.
\r
17 // You should have received a copy of the GNU General Public License
\r
18 // along with this program; if not, write to the Free Software
\r
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
\r
21 #ifdef HAVE_CONFIG_H
\r
22 # include <config.h>
\r
25 #include <algorithm>
\r
29 #include <simgear/sg_inlines.h>
\r
30 #include <simgear/math/SGMath.hxx>
\r
31 #include <simgear/math/sg_geodesy.hxx>
\r
34 #include <Main/util.hxx>
\r
35 #include <Main/viewer.hxx>
\r
37 #include <Scenery/scenery.hxx>
\r
38 #include <Scenery/tilemgr.hxx>
\r
40 #include "AIEscort.hxx"
\r
44 FGAIEscort::FGAIEscort() :
\r
52 _stn_angle_limit(0),
\r
59 _stn_deg_true(false),
\r
66 FGAIEscort::~FGAIEscort() {}
\r
68 void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {
\r
72 FGAIShip::readFromScenario(scFileNode);
\r
74 setName(scFileNode->getStringValue("name", "Escort"));
\r
75 setSMPath(scFileNode->getStringValue("submodel-path", ""));
\r
76 setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));
\r
77 setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));
\r
78 setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));
\r
79 setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));
\r
80 setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));
\r
81 setStnPatrol(scFileNode->getBoolValue("station/patrol", false));
\r
82 setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));
\r
83 setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));
\r
84 setParentName(scFileNode->getStringValue("station/parent", ""));
\r
85 setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));
\r
86 setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));
\r
87 setCallSign(scFileNode->getStringValue("callsign", ""));
\r
94 void FGAIEscort::bind() {
\r
97 props->tie("station/rel-bearing-deg",
\r
98 SGRawValuePointer<double>(&_stn_relbrg));
\r
99 props->tie("station/true-bearing-deg",
\r
100 SGRawValuePointer<double>(&_stn_truebrg));
\r
101 props->tie("station/range-nm",
\r
102 SGRawValuePointer<double>(&_stn_range));
\r
103 props->tie("station/range-limit-nm",
\r
104 SGRawValuePointer<double>(&_stn_limit));
\r
105 props->tie("station/angle-limit-deg",
\r
106 SGRawValuePointer<double>(&_stn_angle_limit));
\r
107 props->tie("station/speed-kts",
\r
108 SGRawValuePointer<double>(&_stn_speed));
\r
109 props->tie("station/height-ft",
\r
110 SGRawValuePointer<double>(&_stn_height));
\r
111 props->tie("controls/update-interval-sec",
\r
112 SGRawValuePointer<double>(&_interval));
\r
113 props->tie("controls/parent-mp-control",
\r
114 SGRawValuePointer<bool>(&_MPControl));
\r
115 props->tie("station/target-range-nm",
\r
116 SGRawValuePointer<double>(&_tgtrange));
\r
117 props->tie("station/target-brg-deg-t",
\r
118 SGRawValuePointer<double>(&_tgtbrg));
\r
119 props->tie("station/patrol",
\r
120 SGRawValuePointer<bool>(&_patrol));
\r
123 void FGAIEscort::unbind() {
\r
124 FGAIShip::unbind();
\r
126 props->untie("station/rel-bearing-deg");
\r
127 props->untie("station/true-bearing-deg");
\r
128 props->untie("station/range-nm");
\r
129 props->untie("station/range-limit-nm");
\r
130 props->untie("station/angle-limit-deg");
\r
131 props->untie("station/speed-kts");
\r
132 props->untie("station/height-ft");
\r
133 props->untie("controls/update-interval-sec");
\r
137 bool FGAIEscort::init(bool search_in_AI_path) {
\r
138 if (!FGAIShip::init(search_in_AI_path))
\r
144 props->setStringValue("controls/parent-name", _parent.c_str());
\r
147 speed = _parent_speed;
\r
153 void FGAIEscort::update(double dt) {
\r
154 FGAIShip::update(dt);
\r
159 void FGAIEscort::setStnRange(double r) {
\r
163 void FGAIEscort::setStnBrg(double b) {
\r
167 void FGAIEscort::setStnLimit(double l) {
\r
171 void FGAIEscort::setStnAngleLimit(double al) {
\r
172 _stn_angle_limit = al;
\r
175 void FGAIEscort::setStnSpeed(double s) {
\r
179 void FGAIEscort::setStnHtFt(double h) {
\r
183 void FGAIEscort::setStnDegTrue(bool t) {
\r
187 void FGAIEscort::setMaxSpeed(double m) {
\r
191 void FGAIEscort::setUpdateInterval(double i) {
\r
195 void FGAIEscort::setStnPatrol(bool p) {
\r
199 void FGAIEscort::setParentName(const string& p) {
\r
203 bool FGAIEscort::getGroundElev(SGGeod inpos) {
\r
207 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
\r
208 _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
\r
211 const vector<string>& names = _material->get_names();
\r
213 _solid = _material->get_solid();
\r
215 if (!names.empty())
\r
216 props->setStringValue("material/name", names[0].c_str());
\r
218 props->setStringValue("material/name", "");
\r
220 //cout << "material " << names[0].c_str()
\r
221 // << " _elevation_m " << _elevation_m
\r
222 // << " solid " << _solid
\r
223 // << " load " << _load_resistance
\r
224 // << " frictionFactor " << _frictionFactor
\r
236 void FGAIEscort::setParentNode() {
\r
238 const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);
\r
240 for (int i = ai->nChildren() - 1; i >= -1; i--) {
\r
241 SGPropertyNode_ptr model;
\r
243 if (i < 0) { // last iteration: selected model
\r
244 model = _selected_ac;
\r
246 model = ai->getChild(i);
\r
247 string path = ai->getPath();
\r
248 const string name = model->getStringValue("name");
\r
250 if (!model->nChildren()){
\r
253 if (name == _parent) {
\r
254 _selected_ac = model; // save selected model for last iteration
\r
264 if (_selected_ac != 0){
\r
265 const string name = _selected_ac->getStringValue("name");
\r
268 //double lat = _selected_ac->getDoubleValue("position/latitude-deg");
\r
269 //double lon = _selected_ac->getDoubleValue("position/longitude-deg");
\r
270 //double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
\r
271 //_MPControl = _selected_ac->getBoolValue("controls/mp-control");
\r
273 //_selectedpos.setLatitudeDeg(lat);
\r
274 //_selectedpos.setLongitudeDeg(lon);
\r
275 //_selectedpos.setElevationFt(elevation);
\r
277 //_parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");
\r
278 //_parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
\r
280 //if(!_stn_deg_true){
\r
281 // _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);
\r
282 // _stn_relbrg = _stn_brg;
\r
283 // //cout << _name <<" set rel"<<endl;
\r
285 // _stn_truebrg = _stn_brg;
\r
286 // _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg);
\r
287 // //cout << _name << " set true"<<endl;
\r
292 //SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,
\r
293 // _tgtpos, course2);
\r
295 //_tgtpos.setElevationFt(_stn_height);
\r
297 //calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
\r
298 // _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);
\r
300 //_relbrg = calcRelBearingDeg(_tgtbrg, hdg);
\r
303 SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name
\r
304 << " parent not found: dying ");
\r
310 void FGAIEscort::setParent()
\r
312 double lat = _selected_ac->getDoubleValue("position/latitude-deg");
\r
313 double lon = _selected_ac->getDoubleValue("position/longitude-deg");
\r
314 double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
\r
315 _MPControl = _selected_ac->getBoolValue("controls/mp-control");
\r
317 _selectedpos.setLatitudeDeg(lat);
\r
318 _selectedpos.setLongitudeDeg(lon);
\r
319 _selectedpos.setElevationFt(elevation);
\r
321 _parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");
\r
322 _parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
\r
324 if(!_stn_deg_true){
\r
325 _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);
\r
326 _stn_relbrg = _stn_brg;
\r
327 //cout << _name <<" set rel"<<endl;
\r
329 _stn_truebrg = _stn_brg;
\r
330 _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg);
\r
331 //cout << _name << " set true"<<endl;
\r
336 SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,
\r
339 _tgtpos.setElevationFt(_stn_height);
\r
341 calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
\r
342 _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);
\r
344 _relbrg = calcRelBearingDeg(_tgtbrg, hdg);
\r
348 void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,
\r
349 double &range, double &bearing) const
\r
351 // calculate the bearing and range of the second pos from the first
\r
352 double az2, distance;
\r
353 geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
\r
354 range = distance * SG_METER_TO_NM;
\r
357 double FGAIEscort::calcRelBearingDeg(double bearing, double heading)
\r
359 double angle = bearing - heading;
\r
360 SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
\r
364 double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)
\r
366 double angle = bearing + heading;
\r
367 SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
\r
371 double FGAIEscort::calcRecipBearingDeg(double bearing)
\r
373 double angle = bearing - 180;
\r
374 SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
\r
378 SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {
\r
379 double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
\r
380 double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
\r
381 double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
\r
383 // Transform that one to the horizontal local coordinate system.
\r
384 SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
\r
386 // and postrotate the orientation of the AIModel wrt the horizontal
\r
388 hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
\r
390 // The offset converted to the usual body fixed coordinate system
\r
391 // rotated to the earth fiexed coordinates axis
\r
392 SGVec3d off = hlTrans.backTransform(_off);
\r
394 // Add the position offset of the AIModel to gain the earth centered position
\r
395 SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
\r
397 return cartPos + off;
\r
401 void FGAIEscort::setStationSpeed(){
\r
406 // these are the AI rules for the manoeuvring of escorts
\r
408 if (_MPControl && _tgtrange > 4 * _stn_limit){
\r
409 SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name
\r
410 << " re-aligning to MP pos");
\r
414 }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){
\r
417 if(_tgtrange > 4 * _stn_limit)
\r
418 speed = 4 * -_stn_speed;
\r
420 speed = -_stn_speed;
\r
422 }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){
\r
425 if(_tgtrange > 4 * _stn_limit)
\r
426 speed = 4 * _stn_speed;
\r
428 speed = _stn_speed;
\r
433 angle = 15 * sg_random();
\r
434 speed = 5 * sg_random();
\r
442 double station_speed = _parent_speed + speed;
\r
444 SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);
\r
445 SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);
\r
447 AccelTo(station_speed);
\r
448 TurnTo(_parent_hdg + angle);
\r
449 ClimbTo(_stn_height);
\r
453 void FGAIEscort::RunEscort(double dt){
\r
459 ///////////////////////////////////////////////////////////////////////////
\r
460 // Check execution time (currently once every 0.05 sec or 20 fps)
\r
461 // Add a bit of randomization to prevent the execution of all flight plans
\r
462 // in synchrony, which can add significant periodic framerate flutter.
\r
463 // Randomization removed to get better appearance
\r
464 ///////////////////////////////////////////////////////////////////////////
\r
466 //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
\r
467 if (_dt_count < _next_run)
\r
469 _next_run = _interval /*+ (0.015 * sg_random())*/;
\r
477 //getGroundElev(pos);
\r
483 // end AIGroundvehicle
\r