1 // FGAIEscort - 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.
29 #include <simgear/sg_inlines.h>
30 #include <simgear/math/SGMath.hxx>
31 #include <simgear/math/sg_geodesy.hxx>
34 #include <Main/util.hxx>
35 #include <Main/viewer.hxx>
37 #include <Scenery/scenery.hxx>
38 #include <Scenery/tilemgr.hxx>
40 #include "AIEscort.hxx"
44 FGAIEscort::FGAIEscort() :
64 FGAIEscort::~FGAIEscort() {}
66 void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {
70 FGAIShip::readFromScenario(scFileNode);
72 setName(scFileNode->getStringValue("name", "Escort"));
73 setSMPath(scFileNode->getStringValue("submodel-path", ""));
74 setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));
75 setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));
76 setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));
77 setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));
78 setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));
79 setStnPatrol(scFileNode->getBoolValue("station/patrol", false));
80 setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));
81 setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));
82 setParentName(scFileNode->getStringValue("station/parent", ""));
83 setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));
84 setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));
85 setCallSign(scFileNode->getStringValue("callsign", ""));
92 void FGAIEscort::bind() {
95 props->tie("station/rel-bearing-deg",
96 SGRawValuePointer<double>(&_stn_relbrg));
97 props->tie("station/true-bearing-deg",
98 SGRawValuePointer<double>(&_stn_truebrg));
99 props->tie("station/range-nm",
100 SGRawValuePointer<double>(&_stn_range));
101 props->tie("station/range-limit-nm",
102 SGRawValuePointer<double>(&_stn_limit));
103 props->tie("station/angle-limit-deg",
104 SGRawValuePointer<double>(&_stn_angle_limit));
105 props->tie("station/speed-kts",
106 SGRawValuePointer<double>(&_stn_speed));
107 props->tie("station/height-ft",
108 SGRawValuePointer<double>(&_stn_height));
109 props->tie("controls/update-interval-sec",
110 SGRawValuePointer<double>(&_interval));
111 props->tie("controls/parent-mp-control",
112 SGRawValuePointer<bool>(&_MPControl));
113 props->tie("station/target-range-nm",
114 SGRawValuePointer<double>(&_tgtrange));
115 props->tie("station/target-brg-deg-t",
116 SGRawValuePointer<double>(&_tgtbrg));
117 props->tie("station/patrol",
118 SGRawValuePointer<bool>(&_patrol));
121 void FGAIEscort::unbind() {
124 props->untie("station/rel-bearing-deg");
125 props->untie("station/true-bearing-deg");
126 props->untie("station/range-nm");
127 props->untie("station/range-limit-nm");
128 props->untie("station/angle-limit-deg");
129 props->untie("station/speed-kts");
130 props->untie("station/height-ft");
131 props->untie("controls/update-interval-sec");
135 bool FGAIEscort::init(bool search_in_AI_path) {
136 if (!FGAIShip::init(search_in_AI_path))
142 props->setStringValue("controls/parent-name", _parent.c_str());
144 if (setParentNode()){
147 speed = _parent_speed;
154 void FGAIEscort::update(double dt) {
155 FGAIShip::update(dt);
160 void FGAIEscort::setStnRange(double r) {
164 void FGAIEscort::setStnBrg(double b) {
168 void FGAIEscort::setStnLimit(double l) {
172 void FGAIEscort::setStnAngleLimit(double al) {
173 _stn_angle_limit = al;
176 void FGAIEscort::setStnSpeed(double s) {
180 void FGAIEscort::setStnHtFt(double h) {
184 void FGAIEscort::setStnDegTrue(bool t) {
188 void FGAIEscort::setMaxSpeed(double m) {
192 void FGAIEscort::setUpdateInterval(double i) {
196 void FGAIEscort::setStnPatrol(bool p) {
200 bool FGAIEscort::getGroundElev(SGGeod inpos) {
204 if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
205 _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
208 const vector<string>& names = _material->get_names();
210 _solid = _material->get_solid();
213 props->setStringValue("material/name", names[0].c_str());
215 props->setStringValue("material/name", "");
217 //cout << "material " << names[0].c_str()
218 // << " _elevation_m " << _elevation_m
219 // << " solid " << _solid
220 // << " load " << _load_resistance
221 // << " frictionFactor " << _frictionFactor
233 void FGAIEscort::setParent()
235 double lat = _selected_ac->getDoubleValue("position/latitude-deg");
236 double lon = _selected_ac->getDoubleValue("position/longitude-deg");
237 double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
238 _MPControl = _selected_ac->getBoolValue("controls/mp-control");
240 _selectedpos.setLatitudeDeg(lat);
241 _selectedpos.setLongitudeDeg(lon);
242 _selectedpos.setElevationFt(elevation);
244 _parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");
245 _parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
248 _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);
249 _stn_relbrg = _stn_brg;
250 //cout << _name <<" set rel"<<endl;
252 _stn_truebrg = _stn_brg;
253 _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg);
254 //cout << _name << " set true"<<endl;
259 SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,
262 _tgtpos.setElevationFt(_stn_height);
264 calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
265 _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);
267 _relbrg = calcRelBearingDeg(_tgtbrg, hdg);
271 void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,
272 double &range, double &bearing) const
274 // calculate the bearing and range of the second pos from the first
275 double az2, distance;
276 geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
277 range = distance * SG_METER_TO_NM;
280 double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)
282 double angle = bearing + heading;
283 SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
287 SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {
288 double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
289 double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
290 double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
292 // Transform that one to the horizontal local coordinate system.
293 SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
295 // and postrotate the orientation of the AIModel wrt the horizontal
297 hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
299 // The offset converted to the usual body fixed coordinate system
300 // rotated to the earth fiexed coordinates axis
301 SGVec3d off = hlTrans.backTransform(_off);
303 // Add the position offset of the AIModel to gain the earth centered position
304 SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
306 return cartPos + off;
310 void FGAIEscort::setStationSpeed(){
315 // these are the AI rules for the manoeuvring of escorts
317 if (_MPControl && _tgtrange > 4 * _stn_limit){
318 SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name
319 << " re-aligning to MP pos");
323 }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){
326 if(_tgtrange > 4 * _stn_limit)
327 speed = 4 * -_stn_speed;
331 }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){
334 if(_tgtrange > 4 * _stn_limit)
335 speed = 4 * _stn_speed;
342 angle = 15 * sg_random();
343 speed = 5 * sg_random();
351 double station_speed = _parent_speed + speed;
353 SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);
354 SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);
356 AccelTo(station_speed);
357 TurnTo(_parent_hdg + angle);
358 ClimbTo(_stn_height);
362 void FGAIEscort::RunEscort(double dt){
368 ///////////////////////////////////////////////////////////////////////////
369 // Check execution time (currently once every 0.05 sec or 20 fps)
370 // Add a bit of randomization to prevent the execution of all flight plans
371 // in synchrony, which can add significant periodic framerate flutter.
372 // Randomization removed to get better appearance
373 ///////////////////////////////////////////////////////////////////////////
375 //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
376 if (_dt_count < _next_run)
378 _next_run = _interval /*+ (0.015 * sg_random())*/;