_azimuth(0.0),
_elevation(0.0),
_rotation(0.0),
-_formate_to_ac(false),
_aero_stabilised(false),
_drag_area(0.007),
_life_timer(0.0),
_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
_old_height(0),
_elapsed_time(0),
+_speed(),
hs(0)
{
(*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
props->tie("load/distance-to-hitch-ft",
SGRawValueMethods<FGAIBallistic,double>
- (*this, &FGAIBallistic::getDistanceLoadToHitch));
+ (*this, &FGAIBallistic::getDistanceToHitch));
props->tie("load/elevation-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
- (*this, &FGAIBallistic::getElevLoadToHitch));
+ (*this, &FGAIBallistic::getElevToHitch));
props->tie("load/bearing-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
- (*this, &FGAIBallistic::getBearingLoadToHitch));
+ (*this, &FGAIBallistic::getBearingToHitch));
props->tie("material/load-resistance",
SGRawValuePointer<double>(&_load_resistance));
}
FGAIBase::update(dt);
_setUserPos();
- if (_formate_to_ac){
- formateToAC(dt);
- Transform();
- } else if (_slave_to_ac){
+ if (_slave_to_ac){
slaveToAC(dt);
Transform();
} else if (!invisible){
_slave_to_ac = s;
}
-void FGAIBallistic::setFormate(bool f) {
- _formate_to_ac = f;
-}
-
void FGAIBallistic::setContentsPath(const string& path) {
_contents_path = path;
void FGAIBallistic::setParentPos() {
if (_pnode != 0) {
+ //cout << "set parent pos" << endl;
+
double lat = _p_lat_node->getDoubleValue();
double lon = _p_lon_node->getDoubleValue();
double alt = _p_alt_node->getDoubleValue();
return _slave_to_ac;
}
-bool FGAIBallistic::getFormate() const {
- return _formate_to_ac;
-}
-
double FGAIBallistic::getMass() const {
return _mass;
}
roll = (r * c) + (roll * (1 - c));
}
+void FGAIBallistic::setSpd(double s, double dt, double coeff){
+ double c = dt / (coeff + dt);
+ _speed = (s * c) + (_speed * (1 - c));
+}
+
void FGAIBallistic::setHt(double h, double dt, double coeff){
double c = dt / (coeff + dt);
_height = (h * c) + (_height * (1 - c));
}
-void FGAIBallistic::setHdg(double az, double dt, double coeff){
+int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff){
double recip = getRecip(hdg);
double c = dt / (coeff + dt);
//we need to ensure that we turn the short way to the new hdg
- if (az < recip && az < hdg && hdg > 180) {
- hdg = ((az + 360) * c) + (hdg * (1 - c));
- } else if (az > recip && az > hdg && hdg <= 180){
- hdg = ((az - 360) * c) + (hdg * (1 - c));
+ if (tgt_hdg < recip && tgt_hdg < hdg && hdg > 180) {
+ hdg = ((tgt_hdg + 360) * c) + (hdg * (1 - c));
+// cout << "case 1: right turn" << endl;
+ } else if (tgt_hdg > recip && tgt_hdg > hdg && hdg <= 180){
+ hdg = ((tgt_hdg - 360) * c) + (hdg * (1 - c));
+// cout << "case 2: left turn" << endl;
} else {
- hdg = (az * c) + (hdg * (1 - c));
+ hdg = (tgt_hdg * c) + (hdg * (1 - c));
+// cout << "case 4: left turn" << endl;
}
+ return -1;
}
double FGAIBallistic::getTgtXOffset() const {
}
-double FGAIBallistic::getDistanceLoadToHitch() const {
+double FGAIBallistic::getDistanceToHitch() const {
//calculate the distance load to hitch
SGVec3d carthitchPos = getCartHitchPos();
SGVec3d cartPos = getCartPos();
return distance * SG_METER_TO_FEET;
}
-
-double FGAIBallistic::getElevLoadToHitch() const {
+double FGAIBallistic::getElevToHitch() const {
// now the angle, positive angles are upwards
- double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER;
+ double distance = getDistanceToHitch() * SG_FEET_TO_METER;
double angle = 0;
double daltM = _offsetpos.getElevationM() - pos.getElevationM();
return angle;
}
-double FGAIBallistic::getBearingLoadToHitch() const {
+double FGAIBallistic::getBearingToHitch() const {
//calculate the bearing and range of the second pos from the first
- double az1, az2, distance;
+ double distance = getDistanceToHitch() * SG_FEET_TO_METER;
+ double az1, az2;
geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance);
double rel_brg = az1 - hdg;
- if (rel_brg > 180)
- rel_brg -= 360;
+ SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0);
return rel_brg;
}
_z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
}
-void FGAIBallistic::formateToAC(double dt){
-
- double hdg, pch, rll, agl, ht = 0;
-
- setTgtOffsets(dt, 25);
-
- if (_pnode != 0) {
- setParentPos();
- hdg = _p_hdg_node->getDoubleValue();
- pch = _p_pch_node->getDoubleValue();
- rll = _p_rll_node->getDoubleValue();
- agl = _p_agl_node->getDoubleValue();
- ht = _p_alt_node->getDoubleValue();
- setOffsetPos(_parentpos, hdg, pch, rll);
- setSpeed(_p_spd_node->getDoubleValue());
- }else {
- hdg = manager->get_user_heading();
- pch = manager->get_user_pitch();
- rll = manager->get_user_roll();
- agl = manager->get_user_agl();
- ht = manager->get_user_altitude();
- setOffsetPos(userpos, hdg, pch, rll);
- setSpeed(manager->get_user_speed());
- }
-
- // elapsed time has a random initialisation so that each
- // wingman moves differently
- _elapsed_time += dt;
- // we derive a sine based factor to give us smoothly
- // varying error between -1 and 1
- double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
- double r_angle = 5 * factor;
- double p_angle = 2.5 * factor;
- double h_angle = 5 * factor;
- double h_feet = 3 * factor;
-
- pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
- pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
-
- if(agl <= 10) {
- _height = ht;
- //cout << "ht case1" << endl;
- } else if (agl > 10 && agl <= 150 ) {
- setHt(ht, dt, 1.0);
- //cout << "ht case2" << endl;
- } else if (agl > 150 && agl <= 250) {
- setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
- //cout << "ht case3" << endl;
- } else{
- setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
- //cout << "ht case4" << endl;
- }
-
- pos.setElevationFt(_height);
-
- // these calculations are unreliable at slow speeds
- if(speed >= 10) {
- setHdg(_azimuth + h_angle, dt, 0.9);
- setPch(_elevation + p_angle + _pitch_offset, dt, 0.9);
-
- if (roll <= 115 && roll >= -115)
- setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5);
- else
- roll = manager->get_user_roll() + r_angle + _roll_offset;
-
- } else {
- setHdg(manager->get_user_heading(), dt, 0.9);
- setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9);
- setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9);
- }
-
- setOffsetVelocity(dt, pos);
-}
void FGAIBallistic::calcVSHS(){
// calculate vertical and horizontal speed components
double speed_fps = speed * SG_KT_TO_FPS;
-_z_offset * SG_FEET_TO_METER);
// Transform the user position to the horizontal local coordinate system.
- SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
+ SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
// and postrotate the orientation of the user model wrt the horizontal
// local frame
void setSlaved(bool s);
void setSlavedLoad(bool s);
void setPch (double e, double dt, double c);
- void setHdg (double az, double dt, double c);
+ int setHdg (double az, double dt, double c);
void setBnk(double r, double dt, double c);
void setHt(double h, double dt, double c);
- void setFormate(bool f);
+ void setSpd(double s, double dt, double c);
void setParentNodes(const SGPropertyNode_ptr);
void setParentPos();
+ void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
+ void setOffsetVelocity(double dt, SGGeod pos);
+
double _getTime() const;
double getRelBrgHitchToUser() const;
double getElevHitchToUser() const;
double getLoadOffset() const;
double getContents();
+ double getDistanceToHitch() const;
+ double getElevToHitch() const;
+ double getBearingToHitch() const;
SGVec3d getCartHitchPos() const;
bool getHtAGL(double start);
bool getSlaved() const;
- bool getFormate() const;
+// bool getFormate() const;
bool getSlavedLoad() const;
virtual const char* getTypeString(void) const { return "ballistic"; }
SGPropertyNode_ptr _force_azimuth_node;
SGPropertyNode_ptr _force_elevation_node;
+ SGPropertyNode_ptr _pnode; // node for parent model
+ SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
+ SGPropertyNode_ptr _p_lat_node;
+ SGPropertyNode_ptr _p_lon_node;
+ SGPropertyNode_ptr _p_alt_node;
+ SGPropertyNode_ptr _p_agl_node;
+ SGPropertyNode_ptr _p_ori_node;
+ SGPropertyNode_ptr _p_pch_node;
+ SGPropertyNode_ptr _p_rll_node;
+ SGPropertyNode_ptr _p_hdg_node;
+ SGPropertyNode_ptr _p_vel_node;
+ SGPropertyNode_ptr _p_spd_node;
+
double _height;
+ double _speed;
double _ht_agl_ft; // height above ground level
double _azimuth; // degrees true
double _elevation; // degrees
double _rotation; // degrees
double _speed_north_fps;
double _speed_east_fps;
+ double _wind_from_east; // fps
+ double _wind_from_north; // fps
+
+ double hs;
- bool _formate_to_ac;
void setTgtXOffset(double x);
void setTgtYOffset(double y);
double _tgt_z_offset;
double _elapsed_time;
+ SGGeod _parentpos;
+ SGGeod _oldpos;
+ SGGeod _offsetpos;
+ SGGeod _oldoffsetpos;
+
private:
virtual void reinit() { init(); }
double _life_timer; // seconds
double _gravity; // fps^2
double _buoyancy; // fps^2
- double _wind_from_east; // fps
- double _wind_from_north; // fps
bool _wind; // if true, local wind will be applied to object
double _Cd; // drag coefficient
double _mass; // slugs
SGPropertyNode_ptr _impact_report_node; // report node for impact and collision
SGPropertyNode_ptr _contents_node; // node for droptank etc. contents
- SGPropertyNode_ptr _pnode; // node for parent model
- SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
- SGPropertyNode_ptr _p_lat_node;
- SGPropertyNode_ptr _p_lon_node;
- SGPropertyNode_ptr _p_alt_node;
- SGPropertyNode_ptr _p_agl_node;
- SGPropertyNode_ptr _p_ori_node;
- SGPropertyNode_ptr _p_pch_node;
- SGPropertyNode_ptr _p_rll_node;
- SGPropertyNode_ptr _p_hdg_node;
- SGPropertyNode_ptr _p_vel_node;
- SGPropertyNode_ptr _p_spd_node;
+ //SGPropertyNode_ptr _pnode; // node for parent model
+ //SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
+ //SGPropertyNode_ptr _p_lat_node;
+ //SGPropertyNode_ptr _p_lon_node;
+ //SGPropertyNode_ptr _p_alt_node;
+ //SGPropertyNode_ptr _p_agl_node;
+ //SGPropertyNode_ptr _p_ori_node;
+ //SGPropertyNode_ptr _p_pch_node;
+ //SGPropertyNode_ptr _p_rll_node;
+ //SGPropertyNode_ptr _p_hdg_node;
+ //SGPropertyNode_ptr _p_vel_node;
+ //SGPropertyNode_ptr _p_spd_node;
double _fuse_range;
double _distance;
void report_impact(double elevation, const FGAIBase *target = 0);
void slaveToAC(double dt);
void setContents(double c);
- void formateToAC(double dt);
void calcVSHS();
void calcNE();
- void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
- void setOffsetVelocity(double dt, SGGeod pos);
+ //void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
+ //void setOffsetVelocity(double dt, SGGeod pos);
SGVec3d getCartUserPos() const;
SGVec3d getCartOffsetPos(SGGeod pos, double heading, double pitch, double roll) const;
- double getDistanceLoadToHitch() const;
- double getElevLoadToHitch() const;
- double getBearingLoadToHitch() const;
+ //double getDistanceLoadToHitch() const;
+ //double getElevLoadToHitch() const;
+ //double getBearingLoadToHitch() const;
double getRecip(double az);
double getMass() const;
- double hs;
double _ground_offset;
double _load_offset;
double _old_height;
SGVec3d _oldcartoffsetPos;
SGVec3d _oldcartPos;
- SGGeod _parentpos;
- SGGeod _oldpos;
- SGGeod _offsetpos;
- SGGeod _oldoffsetpos;
+ //SGGeod _parentpos;
+ //SGGeod _oldpos;
+ //SGGeod _offsetpos;
+ //SGGeod _oldoffsetpos;
};
_impact_pitch(0),
_impact_roll(0),
_impact_speed(0),
+ _max_speed(300),
_refID( _newAIModelID() ),
_otype(ot),
#include <simgear/misc/sg_path.hxx>
#include <simgear/structure/SGSharedPtr.hxx>
#include <simgear/structure/SGReferenced.hxx>
+#include <simgear/sg_inlines.h>
+
+#include <simgear/math/sg_geodesy.hxx>
+
#include <Main/fg_props.hxx>
void setImpactElev( double e );
void setParentName(const string& p);
void setName(const string& n);
+ void setMaxSpeed(double kts);
+
+ 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);
+
bool setParentNode();
int getID() const;
double _roll_offset;
double _yaw_offset;
+ double _max_speed;
+
string _path;
string _callsign;
string _submodel;
inline FGAIBase::object_type FGAIBase::getType() { return _otype; }
+inline void FGAIBase::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;
+}
+
+inline double FGAIBase::calcRelBearingDeg(double bearing, double heading){
+ double angle = bearing - heading;
+ SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
+ return angle;
+}
+
+inline double FGAIBase::calcTrueBearingDeg(double bearing, double heading){
+ double angle = bearing + heading;
+ SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
+ return angle;
+}
+
+inline double FGAIBase::calcRecipBearingDeg(double bearing){
+ double angle = bearing - 180;
+ SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
+ return angle;
+}
+
+inline void FGAIBase::setMaxSpeed(double m) {
+ _max_speed = m;
+}
+
#endif // _FG_AIBASE_HXX
}
-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");
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);
+// 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;
void
FGAIManager::fetchUserState( void ) {
+
user_latitude = user_latitude_node->getDoubleValue();
user_longitude = user_longitude_node->getDoubleValue();
user_altitude = user_altitude_node->getDoubleValue();
inline double get_wind_from_east() const {return wind_from_east; }
inline double get_wind_from_north() const {return wind_from_north; }
inline double get_user_roll() const { return user_roll; }
- inline double get_user_agl() const { return user_agl; }
+ inline double get_user_agl() const { return user_altitude_agl; }
int getNumAiObjects(void) const;
void processScenario( const string &filename );
- static SGPropertyNode_ptr loadScenarioFile(const std::string& filename);
+ static SGPropertyNode_ptr loadScenarioFile(const std::string& filename);
- static bool getStartPosition(const string& id, const string& pid,
- SGGeod& geodPos, double& hdng, SGVec3d& uvw);
+ static bool getStartPosition(const string& id, const string& pid,
+ SGGeod& geodPos, double& hdng, SGVec3d& uvw);
private:
double user_yaw;
double user_roll;
double user_speed;
- double user_agl;
double wind_from_east;
double wind_from_north;
double _dt;
# include <config.h>
#endif
+#include <simgear/sg_inlines.h>
+#include <simgear/math/SGMath.hxx>
+
+
#include "AIWingman.hxx"
-FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman)
+FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman),
+_formate_to_ac(true),
+_break_angle(-90),
+_break(false),
+_join(false),
+_coeff_hdg(5.0),
+_coeff_pch(5.0)
+
{
invisible = false;
- _formate_to_ac = true;
+ _parent="";
+ tgt_heading = 250;
}
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
setFormate(scFileNode->getBoolValue("formate", true));
+ setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 300.0));
+ setCoeffHdg(scFileNode->getDoubleValue("coefficients/heading", 5.0));
+ setCoeffPch(scFileNode->getDoubleValue("coefficients/pitch", 5.0));
+ setCoeffBnk(scFileNode->getDoubleValue("coefficients/bank", 4.0));
+ setCoeffSpd(scFileNode->getDoubleValue("coefficients/speed", 2.0));
+
+
}
void FGAIWingman::bind() {
FGAIBallistic::bind();
+ props->untie("controls/slave-to-ac");
+
props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
&FGAIBase::getID));
props->tie("subID", SGRawValueMethods<FGAIBase,int>(*this,
&FGAIBase::_getLongitude,
&FGAIBase::_setLongitude));
+ props->tie("controls/break", SGRawValuePointer<bool>(&_break));
+ props->tie("controls/join", SGRawValuePointer<bool>(&_join));
+
props->tie("controls/formate-to-ac",
- SGRawValueMethods<FGAIBallistic,bool>
- (*this, &FGAIBallistic::getFormate, &FGAIBallistic::setFormate));
+ SGRawValueMethods<FGAIWingman,bool>
+ (*this, &FGAIWingman::getFormate, &FGAIWingman::setFormate));
+ props->tie("controls/tgt-heading-deg",
+ SGRawValueMethods<FGAIWingman,double>
+ (*this, &FGAIWingman::getTgtHdg, &FGAIWingman::setTgtHdg));
+ props->tie("controls/tgt-speed-kt",
+ SGRawValueMethods<FGAIWingman,double>
+ (*this, &FGAIWingman::getTgtSpd, &FGAIWingman::setTgtSpd));
+ props->tie("controls/break-deg-rel",
+ SGRawValueMethods<FGAIWingman,double>
+ (*this, &FGAIWingman::getBrkAng, &FGAIWingman::setBrkAng));
+ props->tie("controls/coefficients/heading",
+ SGRawValuePointer<double>(&_coeff_hdg));
+ props->tie("controls/coefficients/pitch",
+ SGRawValuePointer<double>(&_coeff_pch));
+ props->tie("controls/coefficients/bank",
+ SGRawValuePointer<double>(&_coeff_bnk));
+ props->tie("controls/coefficients/speed",
+ SGRawValuePointer<double>(&_coeff_spd));
+
props->tie("orientation/pitch-deg", SGRawValuePointer<double>(&pitch));
props->untie("orientation/true-heading-deg");
props->untie("controls/formate-to-ac");
+ props->untie("controls/tgt-heading-deg");
+ props->untie("controls/tgt-speed-kt");
+ props->untie("controls/break-deg-rel");
+ props->untie("controls/break");
+ props->untie("controls/join");
+ props->untie("controls/coefficients/heading");
+ props->untie("controls/coefficients/pitch");
+ props->untie("controls/coefficients/bank");
+ props->untie("controls/coefficients/speed");
props->untie("submodels/serviceable");
_ht_agl_ft = 1e10;
props->setStringValue("submodels/path", _path.c_str());
+
+ if(_parent != ""){
+ setParentNode();
+ }
+
+ setParentNodes(_selected_ac);
return true;
}
void FGAIWingman::update(double dt) {
- FGAIBallistic::update(dt);
+// FGAIBallistic::update(dt);
+
+ if (_formate_to_ac){
+ formateToAC(dt);
+ Transform();
+ setBrkHdg(_break_angle);
+ }else if (_break) {
+ FGAIBase::update(dt);
+ tgt_altitude_ft = altitude_ft;
+ tgt_speed = speed;
+ tgt_roll = roll;
+ tgt_pitch = pitch;
+ Break(dt);
+ Transform();
+ } else {
+ Join(dt);
+ Transform();
+ }
+
+}
+
+double FGAIWingman::calcDistanceM(SGGeod pos1, SGGeod pos2) const {
+ //calculate the distance load to hitch
+ SGVec3d cartPos1 = SGVec3d::fromGeod(pos1);
+ SGVec3d cartPos2 = SGVec3d::fromGeod(pos2);
+
+ SGVec3d diff = cartPos1 - cartPos2;
+ double distance = norm(diff);
+ return distance;
+}
+
+double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
+
+ double angle = 0;
+ double distance = calcDistanceM(pos1, pos2);
+ double daltM = pos1.getElevationM() - pos2.getElevationM();
+
+ if (fabs(distance) < SGLimits<float>::min()) {
+ angle = 0;
+ } else {
+ double sAngle = daltM/range;
+ sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
+ angle = SGMiscd::rad2deg(asin(sAngle));
+ }
+
+ return angle;
}
+void FGAIWingman::formateToAC(double dt){
+
+ double p_hdg, p_pch, p_rll, p_agl, p_ht = 0;
+
+ setTgtOffsets(dt, 25);
+
+ if (_pnode != 0) {
+ setParentPos();
+ p_hdg = _p_hdg_node->getDoubleValue();
+ p_pch = _p_pch_node->getDoubleValue();
+ p_rll = _p_rll_node->getDoubleValue();
+ //agl = _p_agl_node->getDoubleValue();
+ p_ht = _p_alt_node->getDoubleValue();
+ setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
+ setSpeed(_p_spd_node->getDoubleValue());
+ }else {
+ _setUserPos();
+ p_hdg = manager->get_user_heading();
+ p_pch = manager->get_user_pitch();
+ p_rll = manager->get_user_roll();
+ //agl = manager->get_user_agl();
+ p_ht = manager->get_user_altitude();
+ setOffsetPos(userpos, p_hdg,p_pch, p_rll);
+ setSpeed(manager->get_user_speed());
+ }
+
+ // elapsed time has a random initialisation so that each
+ // wingman moves differently
+ _elapsed_time += dt;
+
+ // we derive a sine based factor to give us smoothly
+ // varying error between -1 and 1
+ double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
+ double r_angle = 5 * factor;
+ double p_angle = 2.5 * factor;
+ double h_angle = 5 * factor;
+ double h_feet = 3 * factor;
+
+ p_agl = manager->get_user_agl();
+
+ if(p_agl <= 10) {
+ _height = p_ht;
+ //cout << "ht case1 " ;
+ } else if (p_agl > 10 && p_agl <= 150 ) {
+ setHt(p_ht, dt, 1.0);
+ //cout << "ht case2 " ;
+ } else if (p_agl > 150 && p_agl <= 250) {
+ setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
+ //cout << "ht case3 " ;
+ } else{
+ setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
+ //cout << "ht case4 " ;
+ }
+
+ pos.setElevationFt(_height);
+ pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
+ pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
+
+ // these calculations are unreliable at slow speeds
+ if(speed >= 10) {
+ setHdg(p_hdg + h_angle, dt, 0.9);
+ setPch(p_pch + p_angle + _pitch_offset, dt, 0.9);
+
+ if (roll <= 115 && roll >= -115)
+ setBnk(p_rll + r_angle + _roll_offset, dt, 0.5);
+ else
+ roll = p_rll + r_angle + _roll_offset;
+
+ } else {
+ setHdg(p_hdg, dt, 0.9);
+ setPch(p_pch + _pitch_offset, dt, 0.9);
+ setBnk(p_rll + _roll_offset, dt, 0.9);
+ }
+
+ setOffsetVelocity(dt, pos);
+}// end formateToAC
+
+void FGAIWingman::Break(double dt) {
+
+ Run(dt);
+
+ //calculate the turn direction: 1 = right, -1 = left
+ double rel_brg = calcRelBearingDeg(tgt_heading, hdg);
+ int turn = SGMiscd::sign(rel_brg);
+
+ // set heading and pitch
+ setHdg(tgt_heading, dt, _coeff_hdg);
+ setPch(0, dt, _coeff_pch);
+
+ if (fabs(tgt_heading - hdg) >= 10)
+ setBnk(45 * turn , dt, _coeff_bnk);
+ else
+ setBnk(0, dt, _coeff_bnk);
+
+} // end Break
+
+void FGAIWingman::Join(double dt) {
+
+ double range, bearing, az2;
+ double parent_hdg, parent_spd, parent_ht= 0;
+ double p_hdg, p_pch, p_rll = 0;
+
+ setTgtOffsets(dt, 25);
+
+ if (_pnode != 0) {
+ setParentPos();
+ p_hdg = _p_hdg_node->getDoubleValue();
+ p_pch = _p_pch_node->getDoubleValue();
+ p_rll = _p_rll_node->getDoubleValue();
+ setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
+ parent_hdg = _p_hdg_node->getDoubleValue();
+ parent_spd = _p_spd_node->getDoubleValue();
+ }else {
+ _setUserPos();
+ p_hdg = manager->get_user_heading();
+ p_pch = manager->get_user_pitch();
+ p_rll = manager->get_user_roll();
+ setOffsetPos(userpos, p_hdg, p_pch, p_rll);
+ parent_hdg = manager->get_user_heading();
+ parent_spd = manager->get_user_speed();
+ }
+
+ setSpeed(parent_spd);
+
+ double distance = calcDistanceM(pos, _offsetpos);
+ double daltM = _offsetpos.getElevationM() - pos.getElevationM();
+ double limit = 10;
+ double hdg_l_lim = parent_hdg - limit;
+ SG_NORMALIZE_RANGE(hdg_l_lim, 0.0, 360.0);
+ double hdg_r_lim = parent_hdg + limit;
+ SG_NORMALIZE_RANGE(hdg_r_lim, 0.0, 360.0);
+
+ if (distance <= 2 && fabs(daltM) <= 2 &&
+ (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
+ _height = _offsetpos.getElevationFt();
+ _formate_to_ac = true;
+ _join = false;
+
+ SG_LOG(SG_GENERAL, SG_ALERT, _name << " joined " << " RANGE " << distance
+ << " SPEED " << speed );
+
+ return;
+ }
+
+ geo_inverse_wgs_84(pos, _offsetpos, &bearing, &az2, &range);
+
+ double rel_brg = calcRelBearingDeg(bearing, hdg);
+ double recip_brg = calcRecipBearingDeg(bearing);
+ double angle = calcAngle(distance,_offsetpos, pos);
+ double approx_angle = atan2(daltM, range);
+ double frm_spd = 50; // formation speed
+ double join_rnge = 1000.0;
+ double recip_parent_hdg = calcRecipBearingDeg(parent_hdg);
+ int turn = SGMiscd::sign(rel_brg);// turn direction: 1 = right, -1 = left
+
+ if (range <= join_rnge && (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
+
+ //these are the rules governing joining
+
+ if ((rel_brg <= -175 || rel_brg >= 175) && range <=10 ){
+ // station is behind us - back up a bit
+ setSpeed(parent_spd - ((frm_spd/join_rnge) * range));
+ setHdg(recip_brg, dt, _coeff_hdg);
+ setPch(angle, dt, _coeff_pch);
+ //cout << _name << " backing up HEADING " << hdg
+ // << " RANGE " << range;
+ } else if (rel_brg >= -5 || rel_brg <= 5) {
+ // station is in front of us - slow down
+ setSpeed(parent_spd + ((frm_spd/100) * range));
+ //SGMiscd::clip
+ setHdg(bearing, dt, 1.5);
+ setPch(angle, dt, _coeff_pch);
+ //cout << _name << " slowing HEADING " << hdg
+ // << " RANGE " << range <<endl;
+ } else if ( range <=10 ){
+ // station is to one side - equal speed and turn towards
+ setSpd(parent_spd , dt, 2.0);
+ setSpeed(_speed);
+ setHdg(parent_hdg + (5 * turn), dt, _coeff_hdg);
+ //cout << _name << " equal speed HEADING " << hdg
+ // << " RANGE " << range<< endl;
+ } else {
+ // we missed it - equal speed and turn to recip
+ setSpd(parent_spd , dt, 2.0);
+ setSpeed(_speed);
+ setHdg(recip_brg, dt, _coeff_hdg);
+ //cout << _name << " WHOOPS!! missed join HEADING " << hdg
+ // << " RANGE " << range<< endl;
+ }
+
+ } else if (range <= join_rnge) {
+ // we missed it - equal speed and turn to recip
+ setSpd(parent_spd , dt, 2.0);
+ setSpeed(_speed);
+ setHdg(recip_brg , dt, _coeff_hdg);
+ //cout << _name << " WHOOPS!! missed approach HEADING " << hdg
+ // << " " << recip_brg
+ // /*<< " " << recip_parent_hdg*/
+ // << " RANGE " << range<< endl;
+ } else if (range > join_rnge && range <= 2000 ){
+ //approach phase
+ //cout << _name << " approach HEADING " << hdg
+ // << " RANGE " << range<< endl;
+ setSpd(parent_spd + frm_spd, dt, 2.0);
+ setSpeed(_speed);
+ setHdg(bearing, dt, _coeff_hdg);
+ setPch(angle, dt, _coeff_pch);
+ } else {
+ //hurry up
+ //cout << _name << " hurry up HEADING " << hdg
+ // << " RANGE " << range<< endl;
+ setSpd(_max_speed -10, dt, 2.0);
+ setSpeed(_speed);
+ setHdg(bearing, dt, _coeff_hdg);
+ setPch(angle, dt, _coeff_pch);
+ }
+
+ Run(dt);
+
+ // set roll
+
+ if (fabs(bearing - hdg) >= 10)
+ setBnk(45 * turn , dt, _coeff_bnk);
+ else
+ setBnk(0, dt, _coeff_bnk);
+
+} // end Join
+
+void FGAIWingman::Run(double dt) {
+
+ // don't let speed become negative
+ SG_CLAMP_RANGE(speed, 100.0, _max_speed);
+
+ double speed_fps = speed * SG_KT_TO_FPS;
+
+ // calculate vertical and horizontal speed components
+ if (speed == 0.0) {
+ hs = vs = 0.0;
+ } else {
+ vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
+ hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
+ }
+
+ //cout << "vs hs " << vs << " " << hs << endl;
+
+ //resolve horizontal speed into north and east components:
+ double speed_north_fps = cos(hdg / SG_RADIANS_TO_DEGREES) * hs;
+ double speed_east_fps = sin(hdg / SG_RADIANS_TO_DEGREES) * hs;
+
+ // convert horizontal speed (fps) to degrees per second
+ double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
+ double speed_east_deg_sec = speed_east_fps / ft_per_deg_lon;
+
+ //get wind components
+ _wind_from_north = manager->get_wind_from_north();
+ _wind_from_east = manager->get_wind_from_east();
+
+ // convert wind speed (fps) to degrees lat/lon per second
+ double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
+ double wind_speed_from_east_deg_sec = _wind_from_east / ft_per_deg_lon;
+
+ //recombine the horizontal velocity components
+ hs = sqrt(((speed_north_fps) * (speed_north_fps))
+ + ((speed_east_fps)* (speed_east_fps )));
+
+ if (hs <= 0.00001)
+ hs = 0;
+
+ if (vs <= 0.00001 && vs >= -0.00001)
+ vs = 0;
+
+ //cout << "lat " << pos.getLatitudeDeg()<< endl;
+ // set new position
+ pos.setLatitudeDeg( pos.getLatitudeDeg()
+ + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt );
+ pos.setLongitudeDeg( pos.getLongitudeDeg()
+ + (speed_east_deg_sec - wind_speed_from_east_deg_sec ) * dt );
+ pos.setElevationFt(pos.getElevationFt() + vs * dt);
+
+ //cout << _name << " run hs " << hs << " vs " << vs << endl;
+
+ // recalculate total speed
+ if ( vs == 0 && hs == 0)
+ speed = 0;
+ else
+ speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
+
+ // recalculate elevation and azimuth (velocity vectors)
+ pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
+ hdg = atan2((speed_east_fps),(speed_north_fps))* SG_RADIANS_TO_DEGREES;
+
+ // rationalise heading
+ SG_NORMALIZE_RANGE(hdg, 0.0, 360.0);
+
+}// end Run
+
// end AIWingman
#include "AIManager.hxx"
#include "AIBase.hxx"
+#include <simgear/sg_inlines.h>
+#include <simgear/math/SGMath.hxx>
+
+
class FGAIWingman : public FGAIBallistic {
public:
FGAIWingman();
virtual void reinit() { init(); }
virtual void update (double dt);
+ void formateToAC(double dt);
+ void Break(double dt);
+ void Join(double dt);
+ void Run(double dt);
+
+ double getDistanceToOffset() const;
+ double getElevToOffset() const;
+
+ double calcAngle(double rangeM, SGGeod pos1, SGGeod pos2);
+ double calcDistanceM(SGGeod pos1, SGGeod pos2) const;
+
+ bool _formate_to_ac;
+ bool _break;
+ bool _join;
+
+ double _break_angle; //degrees relative
+ double _coeff_hdg; //dimensionless coefficient
+ double _coeff_pch; //dimensionless coefficient
+ double _coeff_bnk; //dimensionless coefficient
+ double _coeff_spd; //dimensionless coefficient
+
+
+
+ inline void setFormate(bool f);
+ inline void setTgtHdg(double hdg);
+ inline void setTgtSpd(double spd);
+ inline void setBrkHdg(double angle);
+ inline void setBrkAng(double angle);
+ inline void setCoeffHdg(double h);
+ inline void setCoeffPch(double p);
+ inline void setCoeffBnk(double r);
+ inline void setCoeffSpd(double s);
+
+ inline bool getFormate() const { return _formate_to_ac;}
+
+ inline double getTgtHdg() const { return tgt_heading;}
+ inline double getTgtSpd() const { return tgt_speed;}
+ inline double getBrkAng() const { return _break_angle;}
+
+ inline SGVec3d getCartInPos(SGGeod in_pos) const;
+
};
+void FGAIWingman::setFormate(bool f) {
+ _formate_to_ac = f;
+}
+
+void FGAIWingman::setTgtHdg(double h) {
+ tgt_heading = h;
+}
+
+void FGAIWingman::setTgtSpd(double s) {
+ tgt_speed = s;
+}
+
+void FGAIWingman::setBrkHdg(double a){
+ tgt_heading = hdg + a ;
+ SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0);
+}
+
+void FGAIWingman::setBrkAng(double a){
+ _break_angle = a ;
+ SG_NORMALIZE_RANGE(_break_angle, -180.0, 180.0);
+}
+
+void FGAIWingman::setCoeffHdg(double h){
+ _coeff_hdg = h;
+}
+
+void FGAIWingman::setCoeffPch(double p){
+ _coeff_pch = p;
+}
+
+void FGAIWingman::setCoeffBnk(double b){
+ _coeff_bnk = b;
+}
+
+void FGAIWingman::setCoeffSpd(double s){
+ _coeff_spd = s;
+}
+
+//bool FGAIWingman::getFormate() const {
+// return _formate_to_ac;
+//}
+
+//double FGAIWingman::getTgtHdg() const{
+// return tgt_heading;
+//}
+
+//double FGAIWingman::getTgtSpd() const{
+// return tgt_speed;
+//}
+
+//double FGAIWingman::getBrkAng() const{
+// return _break_angle;
+//}
+
+SGVec3d FGAIWingman::getCartInPos(SGGeod in_pos) const {
+ SGVec3d cartPos = SGVec3d::fromGeod(in_pos);
+ return cartPos;
+}
+
#endif // FG_AIWINGMAN_HXX
projects / flightgear.git / commitdiff