-// 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
+// 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/sg_geodesy.hxx>
+#include <simgear/math/sg_random.h>
+
+#include <math.h>
+#include <Main/util.hxx>
+#include <Viewer/viewer.hxx>
+
+#include <Scenery/scenery.hxx>
+
+#include "AIEscort.hxx"
+
+using std::string;
+
+FGAIEscort::FGAIEscort() :
+FGAIShip(otEscort),
+
+_relbrg (0),
+_parent_speed(0),
+_interval(0),
+_stn_truebrg(0),
+_stn_height(0),
+_stn_speed(0),
+_stn_angle_limit(0),
+_stn_limit(0),
+_max_speed(0),
+_MPControl(false),
+_patrol(false),
+_stn_deg_true(false)
+
+{
+ 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();
+
+ tie("station/rel-bearing-deg",
+ SGRawValuePointer<double>(&_stn_relbrg));
+ tie("station/true-bearing-deg",
+ SGRawValuePointer<double>(&_stn_truebrg));
+ tie("station/range-nm",
+ SGRawValuePointer<double>(&_stn_range));
+ tie("station/range-limit-nm",
+ SGRawValuePointer<double>(&_stn_limit));
+ tie("station/angle-limit-deg",
+ SGRawValuePointer<double>(&_stn_angle_limit));
+ tie("station/speed-kts",
+ SGRawValuePointer<double>(&_stn_speed));
+ tie("station/height-ft",
+ SGRawValuePointer<double>(&_stn_height));
+ tie("controls/update-interval-sec",
+ SGRawValuePointer<double>(&_interval));
+ tie("controls/parent-mp-control",
+ SGRawValuePointer<bool>(&_MPControl));
+ tie("station/target-range-nm",
+ SGRawValuePointer<double>(&_tgtrange));
+ tie("station/target-brg-deg-t",
+ SGRawValuePointer<double>(&_tgtbrg));
+ tie("station/patrol",
+ SGRawValuePointer<bool>(&_patrol));
+}
+
+bool FGAIEscort::init(bool search_in_AI_path) {
+ if (!FGAIShip::init(search_in_AI_path))
+ return false;
+ reinit();
+ return true;
+}
+
+void FGAIEscort::reinit() {
+ invisible = false;
+ no_roll = false;
+
+ props->setStringValue("controls/parent-name", _parent.c_str());
+
+ if (setParentNode()){
+ setParent();
+ pos = _tgtpos;
+ speed = _parent_speed;
+ hdg = _parent_hdg;
+ }
+
+ FGAIShip::reinit();
+}
+
+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;
+}
+
+bool FGAIEscort::getGroundElev(SGGeod inpos) {
+
+ double height_m ;
+
+ const simgear::BVHMaterial* mat = 0;
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &mat, 0)){
+ const SGMaterial* material = dynamic_cast<const SGMaterial*>(mat);
+ _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
+
+ if (material) {
+ const std::vector<std::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()
+{
+ 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);
+
+}
+
+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::calcTrueBearingDeg(double bearing, double heading)
+{
+ double angle = bearing + heading;
+ 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_AI, 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();
+
+ _dt_count = 0;
+
+}
+
+// end AIEscort
projects / flightgear.git / blobdiff