-// 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
-// FGAIGroundVehicle - 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.
-
-#ifndef _FG_AIESCORT_HXX
-#define _FG_AIESCORT_HXX
-
-#include <string>
-#include <list>
-
-#include <simgear/compiler.h>
-
-#include "AIBase.hxx"
-
-#include "AIShip.hxx"
-
-#include "AIManager.hxx"
-#include "AIBase.hxx"
-
-class FGAIEscort : public FGAIShip {
-public:
- FGAIEscort();
- virtual ~FGAIEscort();
-
- virtual void readFromScenario(SGPropertyNode* scFileNode);
- virtual void bind();
- virtual void unbind();
- virtual const char* getTypeString(void) const { return "escort"; }
-
- bool init(bool search_in_AI_path=false);
-
-private:
-
- virtual void reinit() { init(); }
- virtual void update (double dt);
-
- void setParentName(const std::string& p);
- void setParent();
- void setStnRange(double r);
- void setStnBrg(double y);
- void setStationSpeed();
- void setStnLimit(double l);
- void setStnAngleLimit(double l);
- void setStnSpeed(double s);
- void setStnHtFt(double h);
- void setStnPatrol(bool p);
- void setStnDegTrue(bool t);
-
- void setMaxSpeed(double m);
- void setUpdateInterval(double i);
-
- void RunEscort(double dt);
-
- bool getGroundElev(SGGeod inpos);
-
- SGVec3d getCartHitchPosAt(const SGVec3d& off) const;
-
- void calcRangeBearing(double lat, double lon, double lat2, double lon2,
- double &range, double &bearing) const;
- double calcRelBearingDeg(double bearing, double heading);
- double calcTrueBearingDeg(double bearing, double heading);
- double calcRecipBearingDeg(double bearing);
-
- SGGeod _selectedpos;
- SGGeod _tgtpos;
-
- bool _solid; // if true ground is solid for FDMs
- double _load_resistance; // ground load resistanc N/m^2
- double _frictionFactor; // dimensionless modifier for Coefficient of Friction
- double _tgtrange, _tgtbrg;
- double _ht_agl_ft;
- double _relbrg, _truebrg;
- double _parent_speed, _parent_hdg;
- double _interval;
-
- double _stn_relbrg, _stn_truebrg, _stn_brg, _stn_range, _stn_height;
- double _stn_speed, _stn_angle_limit, _stn_limit;
-
- double _max_speed;
-
- const SGMaterial* _material;
- SGPropertyNode_ptr _selected_ac;
-
- bool _MPControl, _patrol, _stn_deg_true;
-
- std::string _parent;
-
-};
-
-#endif // FG_AIGROUNDVEHICLE_HXX
+// FGAIGroundVehicle - 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
+#ifndef _FG_AIESCORT_HXX\r
+#define _FG_AIESCORT_HXX\r
+\r
+#include <string>\r
+#include <list>\r
+\r
+#include <simgear/compiler.h>\r
+\r
+#include "AIBase.hxx"\r
+\r
+#include "AIShip.hxx"\r
+\r
+#include "AIManager.hxx"\r
+#include "AIBase.hxx"\r
+\r
+class FGAIEscort : public FGAIShip {\r
+public:\r
+ FGAIEscort();\r
+ virtual ~FGAIEscort();\r
+\r
+ virtual void readFromScenario(SGPropertyNode* scFileNode);\r
+ virtual void bind();\r
+ virtual void unbind();\r
+ virtual const char* getTypeString(void) const { return "escort"; }\r
+\r
+ bool init(bool search_in_AI_path=false);\r
+\r
+private:\r
+\r
+ virtual void reinit() { init(); }\r
+ virtual void update (double dt);\r
+\r
+ void setParentName(const std::string& p);\r
+ void setParentNode();\r
+ void setStnRange(double r);\r
+ void setStnBrg(double y);\r
+ void setStationSpeed();\r
+ void setStnLimit(double l);\r
+ void setStnAngleLimit(double l);\r
+ void setStnSpeed(double s);\r
+ void setStnHtFt(double h);\r
+ void setStnPatrol(bool p);\r
+ void setStnDegTrue(bool t);\r
+ void setParent();\r
+\r
+ void setMaxSpeed(double m);\r
+ void setUpdateInterval(double i);\r
+\r
+ void RunEscort(double dt);\r
+\r
+ bool getGroundElev(SGGeod inpos);\r
+\r
+ SGVec3d getCartHitchPosAt(const SGVec3d& off) const;\r
+\r
+ void calcRangeBearing(double lat, double lon, double lat2, double lon2,\r
+ double &range, double &bearing) const;\r
+ double calcRelBearingDeg(double bearing, double heading);\r
+ double calcTrueBearingDeg(double bearing, double heading);\r
+ double calcRecipBearingDeg(double bearing);\r
+\r
+ SGGeod _selectedpos;\r
+ SGGeod _tgtpos;\r
+\r
+ bool _solid; // if true ground is solid for FDMs\r
+ double _load_resistance; // ground load resistanc N/m^2\r
+ double _frictionFactor; // dimensionless modifier for Coefficient of Friction\r
+ double _tgtrange, _tgtbrg;\r
+ double _ht_agl_ft;\r
+ double _relbrg, _truebrg;\r
+ double _parent_speed, _parent_hdg;\r
+ double _interval;\r
+\r
+ double _stn_relbrg, _stn_truebrg, _stn_brg, _stn_range, _stn_height;\r
+ double _stn_speed, _stn_angle_limit, _stn_limit;\r
+\r
+ double _max_speed;\r
+\r
+ const SGMaterial* _material;\r
+ SGPropertyNode_ptr _selected_ac;\r
+\r
+ bool _MPControl, _patrol, _stn_deg_true;\r
+\r
+ std::string _parent;\r
+\r
+};\r
+\r
+#endif // FG_AIGROUNDVEHICLE_HXX\r
-// FGAIGroundVehicle - 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.
-
-#ifndef _FG_AIGROUNDVEHICLE_HXX
-#define _FG_AIGROUNDVEHICLE_HXX
-
-#include <math.h>
-#include <vector>
-#include <simgear/structure/SGSharedPtr.hxx>
-#include <simgear/scene/material/mat.hxx>
-
-#include "AIShip.hxx"
-
-#include "AIManager.hxx"
-#include "AIBase.hxx"
-
-class FGAIGroundVehicle : public FGAIShip {
-public:
- FGAIGroundVehicle();
- virtual ~FGAIGroundVehicle();
-
- virtual void readFromScenario(SGPropertyNode* scFileNode);
- virtual void bind();
- virtual void unbind();
- virtual const char* getTypeString(void) const { return "groundvehicle"; }
-
- bool init(bool search_in_AI_path=false);
-
-private:
-
- virtual void reinit() { init(); }
- virtual void update (double dt);
-
- void setNoRoll(bool nr);
- void setContactX1offset(double x1);
- void setContactX2offset(double x2);
- void setXOffset(double x);
- void setYOffset(double y);
- void setZOffset(double z);
-
- void setPitchCoeff(double pc);
- void setElevCoeff(double ec);
- void setTowAngleGain(double g);
- void setTowAngleLimit(double l);
- void setElevation(double _elevation, double dt, double _elevation_coeff);
- void setPitch(double _pitch, double dt, double _pitch_coeff);
- void setTowAngle(double _relbrg, double dt, double _towangle_coeff);
- void setParentName(const string& p);
- void setTrainSpeed(double s, double dt, double coeff);
- void setParent();
- void AdvanceFP();
- void setTowSpeed();
- void RunGroundVehicle(double dt);
-
- bool getGroundElev(SGGeod inpos);
- bool getPitch();
-
- SGVec3d getCartHitchPosAt(const SGVec3d& off) const;
-
- void calcRangeBearing(double lat, double lon, double lat2, double lon2,
- double &range, double &bearing) const;
- double calcRelBearingDeg(double bearing, double heading);
-
- SGGeod _selectedpos;
-
- bool _solid; // if true ground is solid for FDMs
- double _load_resistance; // ground load resistanc N/m^2
- double _frictionFactor; // dimensionless modifier for Coefficient of Friction
- double _elevation, _elevation_coeff;
- double _tow_angle_gain, _tow_angle_limit;
- double _ht_agl_ft;
- double _contact_x1_offset, _contact_x2_offset, _contact_z_offset;
- double _pitch, _pitch_coeff, _pitch_deg;
- double _speed_coeff, _speed_kt;
- double _x_offset, _y_offset;
- double _range_ft;
- double _relbrg;
- double _parent_speed, _parent_x_offset, _parent_y_offset, _parent_z_offset;
- double _dt_count, _next_run, _break_count;
-
- const SGMaterial* _material;
- SGPropertyNode_ptr _selected_ac;
-
- string _parent;
-
-};
-
-#endif // FG_AIGROUNDVEHICLE_HXX
+// FGAIGroundVehicle - 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
+#ifndef _FG_AIGROUNDVEHICLE_HXX\r
+#define _FG_AIGROUNDVEHICLE_HXX\r
+\r
+#include <math.h>\r
+#include <vector>\r
+#include <simgear/structure/SGSharedPtr.hxx>\r
+#include <simgear/scene/material/mat.hxx>\r
+\r
+#include "AIShip.hxx"\r
+\r
+#include "AIManager.hxx"\r
+#include "AIBase.hxx"\r
+\r
+class FGAIGroundVehicle : public FGAIShip {\r
+public:\r
+ FGAIGroundVehicle();\r
+ virtual ~FGAIGroundVehicle();\r
+\r
+ virtual void readFromScenario(SGPropertyNode* scFileNode);\r
+ virtual void bind();\r
+ virtual void unbind();\r
+ virtual const char* getTypeString(void) const { return "groundvehicle"; }\r
+\r
+ bool init(bool search_in_AI_path=false);\r
+\r
+private:\r
+\r
+ virtual void reinit() { init(); }\r
+ virtual void update (double dt);\r
+\r
+ void setNoRoll(bool nr);\r
+ void setContactX1offset(double x1);\r
+ void setContactX2offset(double x2);\r
+ void setXOffset(double x);\r
+ void setYOffset(double y);\r
+ void setZOffset(double z);\r
+\r
+ void setPitchCoeff(double pc);\r
+ void setElevCoeff(double ec);\r
+ void setTowAngleGain(double g);\r
+ void setTowAngleLimit(double l);\r
+ void setElevation(double _elevation, double dt, double _elevation_coeff);\r
+ void setPitch(double _pitch, double dt, double _pitch_coeff);\r
+ void setTowAngle(double _relbrg, double dt, double _towangle_coeff);\r
+ void setParentName(const string& p);\r
+ void setTrainSpeed(double s, double dt, double coeff);\r
+ void setParent();\r
+ void setParentNode();\r
+ void AdvanceFP();\r
+ void setTowSpeed();\r
+ void RunGroundVehicle(double dt);\r
+\r
+ bool getGroundElev(SGGeod inpos);\r
+ bool getPitch();\r
+\r
+ SGVec3d getCartHitchPosAt(const SGVec3d& off) const;\r
+\r
+ void calcRangeBearing(double lat, double lon, double lat2, double lon2,\r
+ double &range, double &bearing) const;\r
+ double calcRelBearingDeg(double bearing, double heading);\r
+\r
+ SGGeod _selectedpos;\r
+\r
+ bool _solid; // if true ground is solid for FDMs\r
+ double _load_resistance; // ground load resistanc N/m^2\r
+ double _frictionFactor; // dimensionless modifier for Coefficient of Friction\r
+ double _elevation, _elevation_coeff;\r
+ double _tow_angle_gain, _tow_angle_limit;\r
+ double _ht_agl_ft;\r
+ double _contact_x1_offset, _contact_x2_offset, _contact_z_offset;\r
+ double _pitch, _pitch_coeff, _pitch_deg;\r
+ double _speed_coeff, _speed_kt;\r
+ double _x_offset, _y_offset;\r
+ double _range_ft;\r
+ double _relbrg;\r
+ double _parent_speed, _parent_x_offset, _parent_y_offset, _parent_z_offset;\r
+ double _hitch_x_offset_m, _hitch_y_offset_m, _hitch_z_offset_m;\r
+ double _dt_count, _next_run, _break_count;\r
+\r
+ const SGMaterial* _material;\r
+ SGPropertyNode_ptr _selected_ac;\r
+\r
+ string _parent;\r
+\r
+};\r
+\r
+#endif // FG_AIGROUNDVEHICLE_HXX\r
projects / flightgear.git / commitdiff