-// FGAIEscort - FGAIShip-derived class creates an AI Ground Vehicle
-// by adding a ground following utility
-//
-// Written by Vivian Meazza, started August 2009.
-// - vivian.meazza at lineone.net
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of the
-// License, or (at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful, but
-// WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
-
-#ifdef HAVE_CONFIG_H
-# include <config.h>
-#endif
-
-#include <algorithm>
-#include <string>
-#include <vector>
-
-#include <simgear/sg_inlines.h>
-#include <simgear/math/SGMath.hxx>
-#include <simgear/math/sg_geodesy.hxx>
-
-#include <math.h>
-#include <Main/util.hxx>
-#include <Main/viewer.hxx>
-
-#include <Scenery/scenery.hxx>
-#include <Scenery/tilemgr.hxx>
-
-#include "AIEscort.hxx"
-
-using std::string;
-
-FGAIEscort::FGAIEscort() :
-FGAIShip(otEscort),
-
-_selected_ac(0),
-_relbrg (0),
-_stn_truebrg(0),
-_parent_speed(0),
-_stn_limit(0),
-_stn_angle_limit(0),
-_stn_speed(0),
-_stn_height(0),
-_max_speed(0),
-_interval(0),
-_MPControl(false),
-_patrol(false),
-_stn_deg_true(false),
-_parent("")
-
-{
- invisible = false;
-}
-
-FGAIEscort::~FGAIEscort() {}
-
-void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {
- if (!scFileNode)
- return;
-
- FGAIShip::readFromScenario(scFileNode);
-
- setName(scFileNode->getStringValue("name", "Escort"));
- setSMPath(scFileNode->getStringValue("submodel-path", ""));
- setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));
- setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));
- setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));
- setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));
- setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));
- setStnPatrol(scFileNode->getBoolValue("station/patrol", false));
- setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));
- setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));
- setParentName(scFileNode->getStringValue("station/parent", ""));
- setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));
- setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));
- setCallSign(scFileNode->getStringValue("callsign", ""));
-
- if(_patrol)
- sg_srandom_time();
-
-}
-
-void FGAIEscort::bind() {
- FGAIShip::bind();
-
- props->tie("station/rel-bearing-deg",
- SGRawValuePointer<double>(&_stn_relbrg));
- props->tie("station/true-bearing-deg",
- SGRawValuePointer<double>(&_stn_truebrg));
- props->tie("station/range-nm",
- SGRawValuePointer<double>(&_stn_range));
- props->tie("station/range-limit-nm",
- SGRawValuePointer<double>(&_stn_limit));
- props->tie("station/angle-limit-deg",
- SGRawValuePointer<double>(&_stn_angle_limit));
- props->tie("station/speed-kts",
- SGRawValuePointer<double>(&_stn_speed));
- props->tie("station/height-ft",
- SGRawValuePointer<double>(&_stn_height));
- props->tie("controls/update-interval-sec",
- SGRawValuePointer<double>(&_interval));
- props->tie("controls/parent-mp-control",
- SGRawValuePointer<bool>(&_MPControl));
- props->tie("station/target-range-nm",
- SGRawValuePointer<double>(&_tgtrange));
- props->tie("station/target-brg-deg-t",
- SGRawValuePointer<double>(&_tgtbrg));
- props->tie("station/patrol",
- SGRawValuePointer<bool>(&_patrol));
-}
-
-void FGAIEscort::unbind() {
- FGAIShip::unbind();
-
- props->untie("station/rel-bearing-deg");
- props->untie("station/true-bearing-deg");
- props->untie("station/range-nm");
- props->untie("station/range-limit-nm");
- props->untie("station/angle-limit-deg");
- props->untie("station/speed-kts");
- props->untie("station/height-ft");
- props->untie("controls/update-interval-sec");
-
-}
-
-bool FGAIEscort::init(bool search_in_AI_path) {
- if (!FGAIShip::init(search_in_AI_path))
- return false;
-
- invisible = false;
- no_roll = false;
-
- props->setStringValue("controls/parent-name", _parent.c_str());
- setParent();
- pos = _tgtpos;
- speed = _parent_speed;
- hdg = _parent_hdg;
-
- return true;
-}
-
-void FGAIEscort::update(double dt) {
- FGAIShip::update(dt);
-
- RunEscort(dt);
-}
-
-void FGAIEscort::setStnRange(double r) {
- _stn_range = r;
-}
-
-void FGAIEscort::setStnBrg(double b) {
- _stn_brg = b;
-}
-
-void FGAIEscort::setStnLimit(double l) {
- _stn_limit = l;
-}
-
-void FGAIEscort::setStnAngleLimit(double al) {
- _stn_angle_limit = al;
-}
-
-void FGAIEscort::setStnSpeed(double s) {
- _stn_speed = s;
-}
-
-void FGAIEscort::setStnHtFt(double h) {
- _stn_height = h;
-}
-
-void FGAIEscort::setStnDegTrue(bool t) {
- _stn_deg_true = t;
-}
-
-void FGAIEscort::setMaxSpeed(double m) {
- _max_speed = m;
-}
-
-void FGAIEscort::setUpdateInterval(double i) {
- _interval = i;
-}
-
-void FGAIEscort::setStnPatrol(bool p) {
- _patrol = p;
-}
-
-void FGAIEscort::setParentName(const string& p) {
- _parent = p;
-}
-
-bool FGAIEscort::getGroundElev(SGGeod inpos) {
-
- double height_m ;
-
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
- _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
-
- if (_material) {
- const vector<string>& names = _material->get_names();
-
- _solid = _material->get_solid();
-
- if (!names.empty())
- props->setStringValue("material/name", names[0].c_str());
- else
- props->setStringValue("material/name", "");
-
- //cout << "material " << names[0].c_str()
- // << " _elevation_m " << _elevation_m
- // << " solid " << _solid
- // << " load " << _load_resistance
- // << " frictionFactor " << _frictionFactor
- // << endl;
-
- }
-
- return true;
- } else {
- return false;
- }
-
-}
-
-void FGAIEscort::setParent() {
-
- const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);
-
- for (int i = ai->nChildren() - 1; i >= -1; i--) {
- SGPropertyNode_ptr model;
-
- if (i < 0) { // last iteration: selected model
- model = _selected_ac;
- } else {
- model = ai->getChild(i);
- string path = ai->getPath();
- const string name = model->getStringValue("name");
-
- if (!model->nChildren()){
- continue;
- }
- if (name == _parent) {
- _selected_ac = model; // save selected model for last iteration
- break;
- }
-
- }
- if (!model)
- continue;
-
- }// end for loop
-
- if (_selected_ac != 0){
- const string name = _selected_ac->getStringValue("name");
- double lat = _selected_ac->getDoubleValue("position/latitude-deg");
- double lon = _selected_ac->getDoubleValue("position/longitude-deg");
- double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
- _MPControl = _selected_ac->getBoolValue("controls/mp-control");
-
- _selectedpos.setLatitudeDeg(lat);
- _selectedpos.setLongitudeDeg(lon);
- _selectedpos.setElevationFt(elevation);
-
- _parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");
- _parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
-
- if(!_stn_deg_true){
- _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);
- _stn_relbrg = _stn_brg;
- //cout << _name <<" set rel"<<endl;
- } else {
- _stn_truebrg = _stn_brg;
- _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg);
- //cout << _name << " set true"<<endl;
- }
-
- double course2;
-
- SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,
- _tgtpos, course2);
-
- _tgtpos.setElevationFt(_stn_height);
-
- calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
- _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);
-
- _relbrg = calcRelBearingDeg(_tgtbrg, hdg);
-
- } else {
- SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name
- << " parent not found: dying ");
- setDie(true);
- }
-
-}
-
-void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,
- double &range, double &bearing) const
-{
- // calculate the bearing and range of the second pos from the first
- double az2, distance;
- geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
- range = distance * SG_METER_TO_NM;
-}
-
-double FGAIEscort::calcRelBearingDeg(double bearing, double heading)
-{
- double angle = bearing - heading;
- SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
- return angle;
-}
-
-double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)
-{
- double angle = bearing + heading;
- SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
- return angle;
-}
-
-double FGAIEscort::calcRecipBearingDeg(double bearing)
-{
- double angle = bearing - 180;
- SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
- return angle;
-}
-
-SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {
- double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
- double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
- double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
-
- // Transform that one to the horizontal local coordinate system.
- SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
-
- // and postrotate the orientation of the AIModel wrt the horizontal
- // local frame
- hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
-
- // The offset converted to the usual body fixed coordinate system
- // rotated to the earth fiexed coordinates axis
- SGVec3d off = hlTrans.backTransform(_off);
-
- // Add the position offset of the AIModel to gain the earth centered position
- SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
-
- return cartPos + off;
-}
-
-
-void FGAIEscort::setStationSpeed(){
-
- double speed = 0;
- double angle = 0;
-
- // these are the AI rules for the manoeuvring of escorts
-
- if (_MPControl && _tgtrange > 4 * _stn_limit){
- SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name
- << " re-aligning to MP pos");
- pos = _tgtpos;
- speed = 0;
- angle = 0;
- }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){
- angle =_relbrg;
-
- if(_tgtrange > 4 * _stn_limit)
- speed = 4 * -_stn_speed;
- else
- speed = -_stn_speed;
-
- }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){
- angle = _relbrg;
-
- if(_tgtrange > 4 * _stn_limit)
- speed = 4 * _stn_speed;
- else
- speed = _stn_speed;
-
- } else {
-
- if(_patrol){
- angle = 15 * sg_random();
- speed = 5 * sg_random();
- } else {
- angle = 0;
- speed = 0;
- }
-
- }
-
- double station_speed = _parent_speed + speed;
-
- SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);
- SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);
-
- AccelTo(station_speed);
- TurnTo(_parent_hdg + angle);
- ClimbTo(_stn_height);
-
-}
-
-void FGAIEscort::RunEscort(double dt){
-
- _dt_count += dt;
-
-
-
- ///////////////////////////////////////////////////////////////////////////
- // Check execution time (currently once every 0.05 sec or 20 fps)
- // Add a bit of randomization to prevent the execution of all flight plans
- // in synchrony, which can add significant periodic framerate flutter.
- // Randomization removed to get better appearance
- ///////////////////////////////////////////////////////////////////////////
-
- //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
- if (_dt_count < _next_run)
- return;
- _next_run = _interval /*+ (0.015 * sg_random())*/;
-
- if(_parent == ""){
- return;
- }
-
- setParent();
- setStationSpeed();
- //getGroundElev(pos);
-
- _dt_count = 0;
-
-}
-
-// end AIGroundvehicle
+// FGAIEscort - FGAIShip-derived class creates an AI Ground Vehicle\r
+// by adding a ground following utility\r
+//\r
+// Written by Vivian Meazza, started August 2009.\r
+// - vivian.meazza at lineone.net\r
+//\r
+// This program is free software; you can redistribute it and/or\r
+// modify it under the terms of the GNU General Public License as\r
+// published by the Free Software Foundation; either version 2 of the\r
+// License, or (at your option) any later version.\r
+//\r
+// This program is distributed in the hope that it will be useful, but\r
+// WITHOUT ANY WARRANTY; without even the implied warranty of\r
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r
+// General Public License for more details.\r
+//\r
+// You should have received a copy of the GNU General Public License\r
+// along with this program; if not, write to the Free Software\r
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\r
+\r
+#ifdef HAVE_CONFIG_H\r
+# include <config.h>\r
+#endif\r
+\r
+#include <algorithm>\r
+#include <string>\r
+#include <vector>\r
+\r
+#include <simgear/sg_inlines.h>\r
+#include <simgear/math/SGMath.hxx>\r
+#include <simgear/math/sg_geodesy.hxx>\r
+\r
+#include <math.h>\r
+#include <Main/util.hxx>\r
+#include <Main/viewer.hxx>\r
+\r
+#include <Scenery/scenery.hxx>\r
+#include <Scenery/tilemgr.hxx>\r
+\r
+#include "AIEscort.hxx"\r
+\r
+using std::string;\r
+\r
+FGAIEscort::FGAIEscort() :\r
+FGAIShip(otEscort),\r
+\r
+_selected_ac(0),\r
+_relbrg (0),\r
+_stn_truebrg(0),\r
+_parent_speed(0),\r
+_stn_limit(0),\r
+_stn_angle_limit(0),\r
+_stn_speed(0),\r
+_stn_height(0),\r
+_max_speed(0),\r
+_interval(0),\r
+_MPControl(false),\r
+_patrol(false),\r
+_stn_deg_true(false),\r
+_parent("")\r
+\r
+{\r
+ invisible = false;\r
+}\r
+\r
+FGAIEscort::~FGAIEscort() {}\r
+\r
+void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) {\r
+ if (!scFileNode)\r
+ return;\r
+\r
+ FGAIShip::readFromScenario(scFileNode);\r
+\r
+ setName(scFileNode->getStringValue("name", "Escort"));\r
+ setSMPath(scFileNode->getStringValue("submodel-path", ""));\r
+ setStnRange(scFileNode->getDoubleValue("station/range-nm", 1));\r
+ setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0));\r
+ setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2));\r
+ setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0));\r
+ setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5));\r
+ setStnPatrol(scFileNode->getBoolValue("station/patrol", false));\r
+ setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0));\r
+ setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false));\r
+ setParentName(scFileNode->getStringValue("station/parent", ""));\r
+ setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0));\r
+ setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0));\r
+ setCallSign(scFileNode->getStringValue("callsign", ""));\r
+\r
+ if(_patrol)\r
+ sg_srandom_time();\r
+\r
+}\r
+\r
+void FGAIEscort::bind() {\r
+ FGAIShip::bind();\r
+\r
+ props->tie("station/rel-bearing-deg",\r
+ SGRawValuePointer<double>(&_stn_relbrg));\r
+ props->tie("station/true-bearing-deg",\r
+ SGRawValuePointer<double>(&_stn_truebrg));\r
+ props->tie("station/range-nm",\r
+ SGRawValuePointer<double>(&_stn_range));\r
+ props->tie("station/range-limit-nm",\r
+ SGRawValuePointer<double>(&_stn_limit));\r
+ props->tie("station/angle-limit-deg",\r
+ SGRawValuePointer<double>(&_stn_angle_limit));\r
+ props->tie("station/speed-kts",\r
+ SGRawValuePointer<double>(&_stn_speed));\r
+ props->tie("station/height-ft",\r
+ SGRawValuePointer<double>(&_stn_height));\r
+ props->tie("controls/update-interval-sec",\r
+ SGRawValuePointer<double>(&_interval));\r
+ props->tie("controls/parent-mp-control",\r
+ SGRawValuePointer<bool>(&_MPControl));\r
+ props->tie("station/target-range-nm",\r
+ SGRawValuePointer<double>(&_tgtrange));\r
+ props->tie("station/target-brg-deg-t",\r
+ SGRawValuePointer<double>(&_tgtbrg));\r
+ props->tie("station/patrol",\r
+ SGRawValuePointer<bool>(&_patrol));\r
+}\r
+\r
+void FGAIEscort::unbind() {\r
+ FGAIShip::unbind();\r
+\r
+ props->untie("station/rel-bearing-deg");\r
+ props->untie("station/true-bearing-deg");\r
+ props->untie("station/range-nm");\r
+ props->untie("station/range-limit-nm");\r
+ props->untie("station/angle-limit-deg");\r
+ props->untie("station/speed-kts");\r
+ props->untie("station/height-ft");\r
+ props->untie("controls/update-interval-sec");\r
+\r
+}\r
+\r
+bool FGAIEscort::init(bool search_in_AI_path) {\r
+ if (!FGAIShip::init(search_in_AI_path))\r
+ return false;\r
+\r
+ invisible = false;\r
+ no_roll = false;\r
+\r
+ props->setStringValue("controls/parent-name", _parent.c_str());\r
+ setParentNode();\r
+ pos = _tgtpos;\r
+ speed = _parent_speed;\r
+ hdg = _parent_hdg;\r
+\r
+ return true;\r
+}\r
+\r
+void FGAIEscort::update(double dt) {\r
+ FGAIShip::update(dt);\r
+\r
+ RunEscort(dt);\r
+}\r
+\r
+void FGAIEscort::setStnRange(double r) {\r
+ _stn_range = r;\r
+}\r
+\r
+void FGAIEscort::setStnBrg(double b) {\r
+ _stn_brg = b;\r
+}\r
+\r
+void FGAIEscort::setStnLimit(double l) {\r
+ _stn_limit = l;\r
+}\r
+\r
+void FGAIEscort::setStnAngleLimit(double al) {\r
+ _stn_angle_limit = al;\r
+}\r
+\r
+void FGAIEscort::setStnSpeed(double s) {\r
+ _stn_speed = s;\r
+}\r
+\r
+void FGAIEscort::setStnHtFt(double h) {\r
+ _stn_height = h;\r
+}\r
+\r
+void FGAIEscort::setStnDegTrue(bool t) {\r
+ _stn_deg_true = t;\r
+}\r
+\r
+void FGAIEscort::setMaxSpeed(double m) {\r
+ _max_speed = m;\r
+}\r
+\r
+void FGAIEscort::setUpdateInterval(double i) {\r
+ _interval = i;\r
+}\r
+\r
+void FGAIEscort::setStnPatrol(bool p) {\r
+ _patrol = p;\r
+}\r
+\r
+void FGAIEscort::setParentName(const string& p) {\r
+ _parent = p;\r
+}\r
+\r
+bool FGAIEscort::getGroundElev(SGGeod inpos) {\r
+\r
+ double height_m ;\r
+\r
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){\r
+ _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;\r
+\r
+ if (_material) {\r
+ const vector<string>& names = _material->get_names();\r
+\r
+ _solid = _material->get_solid();\r
+\r
+ if (!names.empty())\r
+ props->setStringValue("material/name", names[0].c_str());\r
+ else\r
+ props->setStringValue("material/name", "");\r
+\r
+ //cout << "material " << names[0].c_str()\r
+ // << " _elevation_m " << _elevation_m\r
+ // << " solid " << _solid\r
+ // << " load " << _load_resistance\r
+ // << " frictionFactor " << _frictionFactor\r
+ // << endl;\r
+\r
+ }\r
+\r
+ return true;\r
+ } else {\r
+ return false;\r
+ }\r
+\r
+}\r
+\r
+void FGAIEscort::setParentNode() {\r
+\r
+ const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);\r
+\r
+ for (int i = ai->nChildren() - 1; i >= -1; i--) {\r
+ SGPropertyNode_ptr model;\r
+\r
+ if (i < 0) { // last iteration: selected model\r
+ model = _selected_ac;\r
+ } else {\r
+ model = ai->getChild(i);\r
+ string path = ai->getPath();\r
+ const string name = model->getStringValue("name");\r
+\r
+ if (!model->nChildren()){\r
+ continue;\r
+ }\r
+ if (name == _parent) {\r
+ _selected_ac = model; // save selected model for last iteration\r
+ break;\r
+ }\r
+\r
+ }\r
+ if (!model)\r
+ continue;\r
+\r
+ }// end for loop\r
+\r
+ if (_selected_ac != 0){\r
+ const string name = _selected_ac->getStringValue("name");\r
+ setParent();\r
+\r
+ //double lat = _selected_ac->getDoubleValue("position/latitude-deg");\r
+ //double lon = _selected_ac->getDoubleValue("position/longitude-deg");\r
+ //double elevation = _selected_ac->getDoubleValue("position/altitude-ft");\r
+ //_MPControl = _selected_ac->getBoolValue("controls/mp-control");\r
+\r
+ //_selectedpos.setLatitudeDeg(lat);\r
+ //_selectedpos.setLongitudeDeg(lon);\r
+ //_selectedpos.setElevationFt(elevation);\r
+\r
+ //_parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");\r
+ //_parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
+\r
+ //if(!_stn_deg_true){\r
+ // _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);\r
+ // _stn_relbrg = _stn_brg;\r
+ // //cout << _name <<" set rel"<<endl;\r
+ //} else {\r
+ // _stn_truebrg = _stn_brg;\r
+ // _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg); \r
+ // //cout << _name << " set true"<<endl;\r
+ //}\r
+\r
+ //double course2;\r
+\r
+ //SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,\r
+ // _tgtpos, course2);\r
+\r
+ //_tgtpos.setElevationFt(_stn_height);\r
+\r
+ //calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),\r
+ // _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);\r
+\r
+ //_relbrg = calcRelBearingDeg(_tgtbrg, hdg);\r
+\r
+ } else {\r
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name\r
+ << " parent not found: dying ");\r
+ setDie(true);\r
+ }\r
+\r
+}\r
+\r
+void FGAIEscort::setParent()\r
+{\r
+ double lat = _selected_ac->getDoubleValue("position/latitude-deg");\r
+ double lon = _selected_ac->getDoubleValue("position/longitude-deg");\r
+ double elevation = _selected_ac->getDoubleValue("position/altitude-ft");\r
+ _MPControl = _selected_ac->getBoolValue("controls/mp-control");\r
+\r
+ _selectedpos.setLatitudeDeg(lat);\r
+ _selectedpos.setLongitudeDeg(lon);\r
+ _selectedpos.setElevationFt(elevation);\r
+\r
+ _parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts");\r
+ _parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
+\r
+ if(!_stn_deg_true){\r
+ _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg);\r
+ _stn_relbrg = _stn_brg;\r
+ //cout << _name <<" set rel"<<endl;\r
+ } else {\r
+ _stn_truebrg = _stn_brg;\r
+ _stn_relbrg = calcRelBearingDeg(_stn_brg, _parent_hdg); \r
+ //cout << _name << " set true"<<endl;\r
+ }\r
+\r
+ double course2;\r
+\r
+ SGGeodesy::direct( _selectedpos, _stn_truebrg, _stn_range * SG_NM_TO_METER,\r
+ _tgtpos, course2);\r
+\r
+ _tgtpos.setElevationFt(_stn_height);\r
+\r
+ calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),\r
+ _tgtpos.getLatitudeDeg(), _tgtpos.getLongitudeDeg(), _tgtrange, _tgtbrg);\r
+\r
+ _relbrg = calcRelBearingDeg(_tgtbrg, hdg);\r
+\r
+}\r
+\r
+void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2,\r
+ double &range, double &bearing) const\r
+{\r
+ // calculate the bearing and range of the second pos from the first\r
+ double az2, distance;\r
+ geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);\r
+ range = distance * SG_METER_TO_NM;\r
+}\r
+\r
+double FGAIEscort::calcRelBearingDeg(double bearing, double heading)\r
+{\r
+ double angle = bearing - heading;\r
+ SG_NORMALIZE_RANGE(angle, -180.0, 180.0);\r
+ return angle;\r
+}\r
+\r
+double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)\r
+{\r
+ double angle = bearing + heading;\r
+ SG_NORMALIZE_RANGE(angle, 0.0, 360.0);\r
+ return angle;\r
+}\r
+\r
+double FGAIEscort::calcRecipBearingDeg(double bearing)\r
+{\r
+ double angle = bearing - 180;\r
+ SG_NORMALIZE_RANGE(angle, 0.0, 360.0);\r
+ return angle;\r
+}\r
+\r
+SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {\r
+ double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");\r
+ double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");\r
+ double roll = _selected_ac->getDoubleValue("orientation/roll-deg");\r
+\r
+ // Transform that one to the horizontal local coordinate system.\r
+ SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);\r
+\r
+ // and postrotate the orientation of the AIModel wrt the horizontal\r
+ // local frame\r
+ hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);\r
+\r
+ // The offset converted to the usual body fixed coordinate system\r
+ // rotated to the earth fiexed coordinates axis\r
+ SGVec3d off = hlTrans.backTransform(_off);\r
+\r
+ // Add the position offset of the AIModel to gain the earth centered position\r
+ SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);\r
+\r
+ return cartPos + off;\r
+}\r
+\r
+\r
+void FGAIEscort::setStationSpeed(){\r
+\r
+ double speed = 0;\r
+ double angle = 0;\r
+\r
+ // these are the AI rules for the manoeuvring of escorts\r
+\r
+ if (_MPControl && _tgtrange > 4 * _stn_limit){\r
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name\r
+ << " re-aligning to MP pos");\r
+ pos = _tgtpos;\r
+ speed = 0;\r
+ angle = 0;\r
+ }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){\r
+ angle =_relbrg;\r
+\r
+ if(_tgtrange > 4 * _stn_limit)\r
+ speed = 4 * -_stn_speed;\r
+ else\r
+ speed = -_stn_speed;\r
+\r
+ }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){\r
+ angle = _relbrg;\r
+\r
+ if(_tgtrange > 4 * _stn_limit)\r
+ speed = 4 * _stn_speed;\r
+ else\r
+ speed = _stn_speed;\r
+\r
+ } else {\r
+\r
+ if(_patrol){\r
+ angle = 15 * sg_random();\r
+ speed = 5 * sg_random();\r
+ } else {\r
+ angle = 0;\r
+ speed = 0;\r
+ }\r
+\r
+ }\r
+\r
+ double station_speed = _parent_speed + speed;\r
+\r
+ SG_CLAMP_RANGE(station_speed, 5.0, _max_speed);\r
+ SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit);\r
+\r
+ AccelTo(station_speed);\r
+ TurnTo(_parent_hdg + angle);\r
+ ClimbTo(_stn_height);\r
+\r
+}\r
+\r
+void FGAIEscort::RunEscort(double dt){\r
+\r
+ _dt_count += dt;\r
+\r
+\r
+\r
+ ///////////////////////////////////////////////////////////////////////////\r
+ // Check execution time (currently once every 0.05 sec or 20 fps)\r
+ // Add a bit of randomization to prevent the execution of all flight plans\r
+ // in synchrony, which can add significant periodic framerate flutter.\r
+ // Randomization removed to get better appearance\r
+ ///////////////////////////////////////////////////////////////////////////\r
+\r
+ //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;\r
+ if (_dt_count < _next_run)\r
+ return;\r
+ _next_run = _interval /*+ (0.015 * sg_random())*/;\r
+\r
+ if(_parent == ""){\r
+ return;\r
+ }\r
+\r
+ setParent();\r
+ setStationSpeed();\r
+ //getGroundElev(pos);\r
+\r
+ _dt_count = 0;\r
+\r
+}\r
+\r
+// end AIGroundvehicle\r