#include "AIBallistic.hxx"
#include <Main/util.hxx>
+#include <Environment/gravity.hxx>
using namespace simgear;
+using std::string;
const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
FGAIBallistic::FGAIBallistic(object_type ot) :
-FGAIBase(ot),
+FGAIBase(ot, false),
_height(0.0),
_speed(0),
_ht_agl_ft(0.0),
_rotation(0.0),
hs(0),
_elapsed_time(0),
+_az_random_error(0.0),
+_el_random_error(0.0),
_aero_stabilised(false),
_drag_area(0.007),
-_life_timer(0.0),
-_gravity(32.1740485564),
+_cd(0.029),
+_init_cd(0.029),
+_cd_randomness(0.0),
_buoyancy(0),
+_life_timer(0.0),
_wind(true),
_mass(0),
_random(false),
+_life_randomness(0.0),
_load_resistance(0),
_solid(false),
_force_stabilised(false),
_report_impact(false),
_external_force(false),
_report_expiry(false),
-_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
-_old_height(0)
+_impact_report_node(fgGetNode("/ai/models/model-impact", true))
{
no_roll = false;
bool FGAIBallistic::init(bool search_in_AI_path) {
FGAIBase::init(search_in_AI_path);
+ reinit();
+ return true;
+}
+void FGAIBallistic::reinit() {
_impact_reported = false;
_collision_reported = false;
_expiry_reported = false;
_elapsed_time += (sg_random() * 100);
+ _life_timer = 0;
+
props->setStringValue("material/name", "");
props->setStringValue("name", _name.c_str());
props->setStringValue("submodels/path", _path.c_str());
- if (_slave_to_ac){
+ if (_slave_to_ac) {
props->setStringValue("force/path", _force_path.c_str());
props->setStringValue("contents/path", _contents_path.c_str());
}
+ //cout << "init: name " << _name.c_str() << " _life_timer " << _life_timer
+ // << endl;
+
//if(_parent != ""){
// setParentNode();
//}
Transform();
- if(_parent != ""){
+ if (_parent != "") {
setParentNode();
}
setParentNodes(_selected_ac);
- return true;
+ FGAIBase::reinit();
}
void FGAIBallistic::bind() {
// FGAIBase::bind();
- props->tie("sim/time/elapsed-sec",
+ _tiedProperties.setRoot(props);
+ tie("sim/time/elapsed-sec",
SGRawValueMethods<FGAIBallistic,double>(*this,
&FGAIBallistic::_getTime, &FGAIBallistic::setTime));
- //props->tie("mass-slug",
+ //tie("mass-slug",
// SGRawValueMethods<FGAIBallistic,double>(*this,
// &FGAIBallistic::getMass));
- props->tie("material/solid",
+ tie("material/solid",
SGRawValuePointer<bool>(&_solid));
- props->tie("altitude-agl-ft",
+ tie("altitude-agl-ft",
SGRawValuePointer<double>(&_ht_agl_ft));
- props->tie("controls/slave-to-ac",
+ tie("controls/slave-to-ac",
SGRawValueMethods<FGAIBallistic,bool>
(*this, &FGAIBallistic::getSlaved, &FGAIBallistic::setSlaved));
- props->tie("controls/invisible",
+ tie("controls/invisible",
SGRawValuePointer<bool>(&invisible));
- if(_external_force || _slave_to_ac){
- props->tie("controls/force_stabilized",
+ if (_external_force || _slave_to_ac) {
+ tie("controls/force_stabilized",
SGRawValuePointer<bool>(&_force_stabilised));
- props->tie("position/global-x",
+ tie("position/global-x",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosX, 0));
- props->tie("position/global-y",
+ tie("position/global-y",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosY, 0));
- props->tie("position/global-z",
+ tie("position/global-z",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosZ, 0));
- props->tie("velocities/vertical-speed-fps",
+ tie("velocities/vertical-speed-fps",
SGRawValuePointer<double>(&vs));
- props->tie("velocities/true-airspeed-kt",
+ tie("velocities/true-airspeed-kt",
SGRawValuePointer<double>(&speed));
- props->tie("velocities/horizontal-speed-fps",
+ tie("velocities/horizontal-speed-fps",
SGRawValuePointer<double>(&hs));
- props->tie("position/altitude-ft",
+ tie("position/altitude-ft",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getElevationFt, &FGAIBase::_setAltitude));
- props->tie("position/latitude-deg",
+ tie("position/latitude-deg",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLatitude, &FGAIBase::_setLatitude));
- props->tie("position/longitude-deg",
+ tie("position/longitude-deg",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLongitude, &FGAIBase::_setLongitude));
- props->tie("orientation/hdg-deg",
+ tie("orientation/hdg-deg",
SGRawValuePointer<double>(&hdg));
- props->tie("orientation/pitch-deg",
+ tie("orientation/pitch-deg",
SGRawValuePointer<double>(&pitch));
- props->tie("orientation/roll-deg",
+ tie("orientation/roll-deg",
SGRawValuePointer<double>(&roll));
- props->tie("controls/slave-load-to-ac",
+ tie("controls/slave-load-to-ac",
SGRawValueMethods<FGAIBallistic,bool>
(*this, &FGAIBallistic::getSlavedLoad, &FGAIBallistic::setSlavedLoad));
- props->tie("position/load-offset",
+ tie("position/load-offset",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
- props->tie("load/distance-to-hitch-ft",
+ tie("load/distance-to-hitch-ft",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getDistanceToHitch));
- props->tie("load/elevation-to-hitch-deg",
+ tie("load/elevation-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getElevToHitch));
- props->tie("load/bearing-to-hitch-deg",
+ tie("load/bearing-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getBearingToHitch));
- props->tie("material/load-resistance",
+ tie("material/load-resistance",
SGRawValuePointer<double>(&_load_resistance));
}
-
}
-void FGAIBallistic::unbind() {
-// FGAIBase::unbind();
-
- props->untie("sim/time/elapsed-sec");
- props->untie("mass-slug");
- props->untie("material/solid");
- props->untie("altitude-agl-ft");
- props->untie("controls/slave-to-ac");
- props->untie("controls/invisible");
-
- if(_external_force || _slave_to_ac){
- props->untie("position/global-y");
- props->untie("position/global-x");
- props->untie("position/global-z");
- props->untie("velocities/vertical-speed-fps");
- props->untie("velocities/true-airspeed-kt");
- props->untie("velocities/horizontal-speed-fps");
- props->untie("position/altitude-ft");
- props->untie("position/latitude-deg");
- props->untie("position/longitude-deg");
- props->untie("position/ht-agl-ft");
- props->untie("orientation/hdg-deg");
- props->untie("orientation/pitch-deg");
- props->untie("orientation/roll-deg");
- props->untie("controls/force_stabilized");
- props->untie("position/load-offset");
- props->untie("load/distance-to-hitch-ft");
- props->untie("load/elevation-to-hitch-deg");
- props->untie("load/bearing-to-hitch-deg");
- props->untie("material/load-resistance");
- }
-}
-
-void FGAIBallistic::update(double dt) {
+void FGAIBallistic::update(double dt)
+{
FGAIBase::update(dt);
- _setUserPos();
- if (_slave_to_ac){
+ if (_slave_to_ac) {
slaveToAC(dt);
Transform();
- } else if (!invisible){
+ }
+ else if (!invisible) {
Run(dt);
Transform();
}
}
void FGAIBallistic::setAzimuth(double az) {
-
if (_random)
- hdg = _azimuth = (az - 5 ) + (10 * sg_random());
+ hdg = _azimuth = az - _az_random_error + 2 * _az_random_error * sg_random();
else
hdg = _azimuth = az;
+}
- //cout << _name << " init hdg " << hdg << " random " << _random << endl;
+void FGAIBallistic::setAzimuthRandomError(double error) {
+ _az_random_error = error;
+}
+
+void FGAIBallistic::setElevationRandomError(double error) {
+ _el_random_error = error;
}
void FGAIBallistic::setElevation(double el) {
- pitch = _elevation = el;
+ if (_random)
+ pitch = _elevation = el - _el_random_error + 2 * _el_random_error * sg_random();
+ else
+ pitch = _elevation = el;
}
void FGAIBallistic::setRoll(double rl) {
}
void FGAIBallistic::setLife(double seconds) {
-
- if (_random){
- life = seconds * _randomness + (seconds * (1 -_randomness) * sg_random());
- //cout << "life " << life << endl;
- } else
+ if (_random)
+ life = seconds * _life_randomness + (seconds * (1 -_life_randomness) * sg_random());
+ else
life = seconds;
}
_wind = val;
}
-void FGAIBallistic::setCd(double c) {
- _Cd = c;
+void FGAIBallistic::setCd(double cd) {
+ _cd = cd;
+ _init_cd = cd;
+}
+
+void FGAIBallistic::setCdRandomness(double randomness) {
+ _cd_randomness = randomness;
}
void FGAIBallistic::setMass(double m) {
_weight_lb = w;
}
-void FGAIBallistic::setRandomness(double r) {
- _randomness = r;
+void FGAIBallistic::setLifeRandomness(double randomness) {
+ _life_randomness = randomness;
}
void FGAIBallistic::setRandom(bool r) {
}
void FGAIBallistic::setImpactReportNode(const string& path) {
-
if (!path.empty())
_impact_report_node = fgGetNode(path.c_str(), true);
}
}
void FGAIBallistic::setContentsPath(const string& path) {
-
_contents_path = path;
if (!path.empty()) {
}
void FGAIBallistic::setContentsNode(SGPropertyNode_ptr node) {
-
if (node != 0) {
_contents_node = node;
_contents_path = _contents_node->getDisplayName();
}
void FGAIBallistic::setParentNodes(SGPropertyNode_ptr node) {
-
if (node != 0) {
_pnode = node;
_p_pos_node = _pnode->getChild("position", 0, true);
_p_vel_node = _pnode->getChild("velocities", 0, true);
_p_spd_node = _p_vel_node->getChild("true-airspeed-kt", 0, true);
}
-
}
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();
_parentpos.setLongitudeDeg(lon);
_parentpos.setLatitudeDeg(lat);
_parentpos.setElevationFt(alt);
-
}
-
}
bool FGAIBallistic::getSlaved() const {
}
double FGAIBallistic::getContents() {
- if(_contents_node){
- _contents_lb = _contents_node->getChild("level-lbs",0,1)->getDoubleValue();
+ if (_contents_node) {
+ _contents_lb = _contents_node->getChild("level-lbs", 0, 1)->getDoubleValue();
}
return _contents_lb;
}
void FGAIBallistic::setContents(double c) {
- if(_contents_node)
- _contents_lb = _contents_node->getChild("level-gal_us",0,1)->setDoubleValue(c);
+ if (_contents_node)
+ _contents_lb = _contents_node->getChild("level-gal_us", 0, 1)->setDoubleValue(c);
}
void FGAIBallistic::setSlavedLoad(bool l) {
}
}
-bool FGAIBallistic::getHtAGL(double start){
-
+bool FGAIBallistic::getHtAGL(double start) {
+ const simgear::BVHMaterial* mat = 0;
if (getGroundElevationM(SGGeod::fromGeodM(pos, start),
- _elevation_m, &_material)) {
+ _elevation_m, &mat)) {
+ const SGMaterial* material = dynamic_cast<const SGMaterial*>(mat);
_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
- if (_material) {
- const vector<string>& names = _material->get_names();
- _solid = _material->get_solid();
- _load_resistance = _material->get_load_resistance();
- _frictionFactor =_material->get_friction_factor();
+ if (material) {
+ const std::vector<string>& names = material->get_names();
+ _solid = material->get_solid();
+ _load_resistance = material->get_load_resistance();
+ _frictionFactor = material->get_friction_factor();
if (!names.empty())
props->setStringValue("material/name", names[0].c_str());
//<< " load " << _load_resistance
//<< " frictionFactor " << _frictionFactor
//<< endl;
-
}
return true;
- } else {
+ }
+ else {
return false;
}
-
}
-double FGAIBallistic::getRecip(double az){
+double FGAIBallistic::getRecip(double az) {
// calculate the reciprocal of the input azimuth
- if(az - 180 < 0){
+ if (az - 180 < 0) {
return az + 180;
- } else {
+ }
+ else {
return az - 180;
}
}
-void FGAIBallistic::setPch(double e, double dt, double coeff){
+void FGAIBallistic::setPch(double e, double dt, double coeff) {
double c = dt / (coeff + dt);
pitch = (e * c) + (pitch * (1 - c));
}
-void FGAIBallistic::setBnk(double r, double dt, double coeff){
+void FGAIBallistic::setBnk(double r, double dt, double coeff) {
double c = dt / (coeff + dt);
roll = (r * c) + (roll * (1 - c));
}
-void FGAIBallistic::setSpd(double s, double dt, double coeff){
+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){
+void FGAIBallistic::setHt(double h, double dt, double coeff) {
double c = dt / (coeff + dt);
_height = (h * c) + (_height * (1 - c));
}
-int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff){
+int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff) {
double recip = getRecip(hdg);
double c = dt / (coeff + dt);
//cout << "set heading " << tgt_hdg << endl;
return _tgt_z_offset;
}
-void FGAIBallistic::setTgtXOffset(double x){
+void FGAIBallistic::setTgtXOffset(double x) {
_tgt_x_offset = x;
}
-void FGAIBallistic::setTgtYOffset(double y){
+void FGAIBallistic::setTgtYOffset(double y) {
_tgt_y_offset = y;
}
-void FGAIBallistic::setTgtZOffset(double z){
+void FGAIBallistic::setTgtZOffset(double z) {
_tgt_z_offset = z;
}
-void FGAIBallistic::slaveToAC(double dt){
-
+void FGAIBallistic::slaveToAC(double dt) {
if (invisible)
return;
- double hdg, pch, rll, agl = 0;
+ double hdg, pch, rll;//, agl = 0;
if (_pnode != 0) {
setParentPos();
hdg = _p_hdg_node->getDoubleValue();
pch = _p_pch_node->getDoubleValue();
rll = _p_rll_node->getDoubleValue();
- agl = _p_agl_node->getDoubleValue();
+// agl = _p_agl_node->getDoubleValue();
setOffsetPos(_parentpos, hdg, pch, rll);
setSpeed(_p_spd_node->getDoubleValue());
- }else {
+ }
+ else {
hdg = manager->get_user_heading();
pch = manager->get_user_pitch();
rll = manager->get_user_roll();
- agl = manager->get_user_agl();
- setOffsetPos(userpos, hdg, pch, rll);
+// agl = manager->get_user_agl();
+ setOffsetPos(globals->get_aircraft_position(), hdg, pch, rll);
setSpeed(manager->get_user_speed());
}
void FGAIBallistic::Run(double dt) {
_life_timer += dt;
+
+ //_pass += 1;
+ //cout<<"AIBallistic run: name " << _name.c_str()
+ // << " dt " << dt << " _life_timer " << _life_timer << " pass " << _pass << endl;
// if life = -1 the object does not die
- if (_life_timer > life && life != -1){
-
- if (_report_expiry && !_expiry_reported && !_impact_reported && !_collision_reported){
- //cout<<"AIBallistic: expiry"<< endl;
+ if (_life_timer > life && life != -1) {
+ if (_report_expiry && !_expiry_reported && !_impact_reported && !_collision_reported) {
+ //cout<<"AIBallistic run: name " << _name.c_str() << " expiry "
+ //<< " _life_timer " << _life_timer<< endl;
handle_expiry();
- } else
+ }
+ else {
+ //cout<<"AIBallistic run: name " << _name.c_str()
+ // << " die " << " _life_timer " << _life_timer << endl;
setDie(true);
+ }
+ setTime(0);
}
- //set the contents in the appropriate tank or other property in the parent to zero
+ // Set the contents in the appropriate tank or other property in the parent to zero
setContents(0);
- //randomise Cd by +- 10%
- if (_random)
- _Cd = _Cd * 0.90 + (0.10 * sg_random());
+ if (_random) {
+ // Keep the new Cd within +- 10% of the current Cd to avoid a fluctuating value
+ double cd_min = _cd * 0.9;
+ double cd_max = _cd * 1.1;
+
+ // Randomize Cd by +- a certain percentage of the initial Cd
+ _cd = _init_cd * (1 - _cd_randomness + 2 * _cd_randomness * sg_random());
+
+ if (_cd < cd_min) _cd = cd_min;
+ if (_cd > cd_max) _cd = cd_max;
+ }
// Adjust Cd by Mach number. The equations are based on curves
// for a conventional shell/bullet (no boat-tail).
double Cdm;
if (Mach < 0.7)
- Cdm = 0.0125 * Mach + _Cd;
- else if (Mach < 1.2 )
- Cdm = 0.3742 * pow(Mach, 2) - 0.252 * Mach + 0.0021 + _Cd;
+ Cdm = 0.0125 * Mach + _cd;
+ else if (Mach < 1.2)
+ Cdm = 0.3742 * pow(Mach, 2) - 0.252 * Mach + 0.0021 + _cd;
else
- Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd;
+ Cdm = 0.2965 * pow(Mach, -1.1506) + _cd;
//cout <<_name << " Mach " << Mach << " Cdm " << Cdm
// << " ballistic speed kts "<< speed << endl;
speed -= (Cdm * 0.5 * rho * speed * speed * _drag_area/_mass) * dt;
// don't let speed become negative
- if ( speed < 0.0 )
+ if (speed < 0.0)
speed = 0.0;
- double speed_fps = speed * SG_KT_TO_FPS;
+// double speed_fps = speed * SG_KT_TO_FPS;
// calculate vertical and horizontal speed components
calcVSHS();
//and convert horizontal speed (fps) to degrees per second
calcNE();
- // if wind not required, set to zero
+ // If wind not required, set to zero
if (!_wind) {
_wind_from_north = 0;
_wind_from_east = 0;
- } else {
+ }
+ else {
_wind_from_north = manager->get_wind_from_north();
_wind_from_east = manager->get_wind_from_east();
}
- //calculate velocity due to external force
+ // Calculate velocity due to external force
double force_speed_north_deg_sec = 0;
double force_speed_east_deg_sec = 0;
-// double vs_force_fps = 0;
double hs_force_fps = 0;
double v_force_acc_fpss = 0;
double force_speed_north_fps = 0;
force_elevation_deg = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
force_azimuth_deg = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
- //resolve force into vertical and horizontal components:
+ // Resolve force into vertical and horizontal components:
double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
- //ground interaction
- //we don't do this if impacts are calculated
- if(!_report_impact){
-
- if (getHtAGL(10000)){
- double deadzone = 0.1;
-
- if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
- normal_force_lbs = (_mass * slugs_to_lbs) - v_force_lbs;
-
- if ( normal_force_lbs < 0 )
- normal_force_lbs = 0;
+ // Perform ground interaction if impacts are not calculated
+ if (!_report_impact && getHtAGL(10000)) {
+ double deadzone = 0.1;
- pos.setElevationFt(0 + _ground_offset);
- if (vs < 0)
- vs = -vs * 0.5;
+ if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid) {
+ normal_force_lbs = (_mass * slugs_to_lbs) - v_force_lbs;
- // calculate friction
- // we assume a static Coefficient of Friction (mu) of 0.62 (wood on concrete)
- double mu = 0.62;
+ if (normal_force_lbs < 0)
+ normal_force_lbs = 0;
- static_friction_force_lbs = mu * normal_force_lbs * _frictionFactor;
+ pos.setElevationFt(0 + _ground_offset);
+ if (vs < 0)
+ vs = -vs * 0.5;
- //adjust horizontal force. We assume that a speed of <= 5 fps is static
- if (h_force_lbs <= static_friction_force_lbs && hs <= 5){
- h_force_lbs = hs = 0;
- _speed_north_fps = _speed_east_fps = 0;
- } else
- dynamic_friction_force_lbs = (static_friction_force_lbs * 0.95);
+ // Calculate friction. We assume a static coefficient of
+ // friction (mu) of 0.62 (wood on concrete)
+ double mu = 0.62;
- //ignore wind when on the ground for now
- //TODO fix this
- _wind_from_north = 0;
- _wind_from_east = 0;
+ static_friction_force_lbs = mu * normal_force_lbs * _frictionFactor;
+ // Adjust horizontal force. We assume that a speed of <= 5 fps is static
+ if (h_force_lbs <= static_friction_force_lbs && hs <= 5) {
+ h_force_lbs = hs = 0;
+ _speed_north_fps = _speed_east_fps = 0;
}
+ else
+ dynamic_friction_force_lbs = (static_friction_force_lbs * 0.95);
+ // Ignore wind when on the ground for now
+ //TODO fix this
+ _wind_from_north = 0;
+ _wind_from_east = 0;
}
-
- } //endif
+ }
//acceleration = (force(lbsf)/mass(slugs))
- v_force_acc_fpss = v_force_lbs/_mass;
- normal_force_fpss = normal_force_lbs/_mass;
- double h_force_acc_fpss = h_force_lbs/_mass;
- double dynamic_friction_acc_fpss = dynamic_friction_force_lbs/_mass;
+ v_force_acc_fpss = v_force_lbs / _mass;
+ normal_force_fpss = normal_force_lbs / _mass;
+ double h_force_acc_fpss = h_force_lbs / _mass;
+ double dynamic_friction_acc_fpss = dynamic_friction_force_lbs / _mass;
// velocity = acceleration * dt
hs_force_fps = h_force_acc_fpss * dt;
hs = 0;
// adjust vertical speed for acceleration of gravity, buoyancy, and vertical force
- vs -= (_gravity - _buoyancy - v_force_acc_fpss - normal_force_fpss) * dt;
+ double gravity = SG_METER_TO_FEET * (Environment::Gravity::instance()->getGravity(pos));
+ vs -= (gravity - _buoyancy - v_force_acc_fpss - normal_force_fpss) * dt;
if (vs <= 0.00001 && vs >= -0.00001)
vs = 0;
// set new position
- if(_slave_load_to_ac) {
+ if (_slave_load_to_ac) {
setOffsetPos(pos,
manager->get_user_heading(),
manager->get_user_pitch(),
pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
pos.setElevationFt(_offsetpos.getElevationFt());
- if (getHtAGL(10000)){
+ if (getHtAGL(10000)) {
double deadzone = 0.1;
- if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
+ if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid) {
pos.setElevationFt(0 + _ground_offset);
- } else {
+ }
+ else {
pos.setElevationFt(_offsetpos.getElevationFt() + _load_offset);
}
-
}
- } else {
+ }
+ else {
pos.setLatitudeDeg( pos.getLatitudeDeg()
+ (speed_north_deg_sec - wind_speed_from_north_deg_sec
+ force_speed_north_deg_sec + friction_force_speed_north_deg_sec) * dt );
// we assume a symetrical MI about the pitch and yaw axis
setPch(_elevation, dt, coeff);
setHdg(_azimuth, dt, coeff);
- } else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
+ }
+ else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
//cout<< "_force_stabilised "<< endl;
const double coeff = 0.9;
setHdg(_azimuth, dt, coeff);
}
- //do impacts and collisions
+ // Do impacts and collisions
if (_report_impact && !_impact_reported)
handle_impact();
if (_report_collision && !_collision_reported)
handle_collision();
- // set destruction flag if altitude less than sea level -1000
+ // Set destruction flag if altitude less than sea level -1000
if (altitude_ft < -1000.0 && life != -1)
setDie(true);
-
-} // end Run
+}
double FGAIBallistic::_getTime() const {
return _life_timer;
}
-void FGAIBallistic::setTime(double s){
+void FGAIBallistic::setTime(double s) {
_life_timer = s;
}
-void FGAIBallistic::handle_impact() {
+void FGAIBallistic::handleEndOfLife(double elevation) {
+ report_impact(elevation);
+
+ // Make the submodel invisible if the submodel is immortal, otherwise kill it if it has no subsubmodels
+ if (life == -1) {
+ invisible = true;
+ }
+ else if (_subID == 0) {
+ // Kill the AIObject if there is no subsubmodel
+ setDie(true);
+ }
+}
- // try terrain intersection
+void FGAIBallistic::handle_impact() {
+ // Try terrain intersection
double start = pos.getElevationM() + 100;
- if(!getHtAGL(start))
+ if (!getHtAGL(start))
return;
if (_ht_agl_ft <= 0) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: terrain impact material" << _mat_name);
- report_impact(_elevation_m);
+ SG_LOG(SG_AI, SG_DEBUG, "AIBallistic: terrain impact material" << _mat_name);
_impact_reported = true;
-
- if (life == -1){
- invisible = true;
- } else if (_subID == 0) // kill the AIObject if there is no subsubmodel
- setDie(true);
+ handleEndOfLife(_elevation_m);
}
}
void FGAIBallistic::handle_expiry() {
-
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: handle_expiry " << pos.getElevationM());
-
- report_impact(pos.getElevationM());
_expiry_reported = true;
-
- if (life == -1){
- invisible = true;
- } else if (_subID == 0){ // kill the AIObject if there is no subsubmodel
- setDie(true);
- }
-
+ handleEndOfLife(pos.getElevationM());
}
void FGAIBallistic::handle_collision()
else
n->setStringValue("type", "terrain");
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object impact " << _name
- << " lon " <<_impact_lon << " lat " <<_impact_lat);
+ SG_LOG(SG_AI, SG_DEBUG, "AIBallistic: object impact " << _name
+ << " lon " <<_impact_lon << " lat " <<_impact_lat << " sec " << _life_timer);
n->setDoubleValue("longitude-deg", _impact_lon);
n->setDoubleValue("latitude-deg", _impact_lat);
_impact_report_node->setStringValue(props->getPath());
}
-SGVec3d FGAIBallistic::getCartUserPos() const {
- SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
- return cartUserPos;
-}
-
-SGVec3d FGAIBallistic::getCartHitchPos() const{
-
+SGVec3d FGAIBallistic::getCartHitchPos() const {
// convert geodetic positions to geocentered
- SGVec3d cartuserPos = SGVec3d::fromGeod(userpos);
+ SGVec3d cartuserPos = globals->get_aircraft_position_cart();
+
//SGVec3d cartPos = getCartPos();
// Transform to the right coordinate frame, configuration is done in
-_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(globals->get_aircraft_position());
// and postrotate the orientation of the user model wrt the horizontal
// local frame
// Add the position offset of the user model to get the geocentered position
SGVec3d offsetPos = cartuserPos + off;
-
return offsetPos;
}
-void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, double roll){
- // convert the hitch geocentered position to geodetic
-
+void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, double roll) {
+ // Convert the hitch geocentered position to geodetic
SGVec3d cartoffsetPos = getCartOffsetPos(inpos, heading, pitch, roll);
-
- //SGVec3d cartoffsetPos = getCartHitchPos();
-
- //SGGeodesy::SGCartToGeod(cartoffsetPos, hitchpos);
SGGeodesy::SGCartToGeod(cartoffsetPos, _offsetpos);
-
}
double FGAIBallistic::getDistanceToHitch() const {
//calculate the relative bearing
double az1, az2, distance;
- geo_inverse_wgs_84(_offsetpos, userpos, &az1, &az2, &distance);
+ geo_inverse_wgs_84(_offsetpos, globals->get_aircraft_position(), &az1, &az2, &distance);
double rel_brg = az1 - hdg;
}
double FGAIBallistic::getElevHitchToUser() const {
-
- //calculate the distance from the user position
+ // Calculate the distance from the user position
SGVec3d carthitchPos = getCartHitchPos();
- SGVec3d cartuserPos = getCartUserPos();
+ SGVec3d cartuserPos = globals->get_aircraft_position_cart();
SGVec3d diff = cartuserPos - carthitchPos;
double distance = norm(diff);
double angle = 0;
- double daltM = userpos.getElevationM() - _offsetpos.getElevationM();
+ double daltM = globals->get_aircraft_position().getElevationM() - _offsetpos.getElevationM();
- // now the angle, positive angles are upwards
+ // Now the angle, positive angles are upwards
if (fabs(distance) < SGLimits<float>::min()) {
angle = 0;
- } else {
+ }
+ else {
double sAngle = daltM/distance;
sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
angle = SGMiscd::rad2deg(asin(sAngle));
return angle;
}
-void FGAIBallistic::setTgtOffsets(double dt, double coeff){
+void FGAIBallistic::setTgtOffsets(double dt, double coeff) {
double c = dt / (coeff + dt);
_x_offset = (_tgt_x_offset * c) + (_x_offset * (1 - c));
_z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
}
-
-void FGAIBallistic::calcVSHS(){
- // calculate vertical and horizontal speed components
+void FGAIBallistic::calcVSHS() {
+ // Calculate vertical and horizontal speed components
double speed_fps = speed * SG_KT_TO_FPS;
if (speed == 0.0) {
hs = vs = 0.0;
- } else {
+ }
+ else {
vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
}
}
-void FGAIBallistic::calcNE(){
- //resolve horizontal speed into north and east components:
+void FGAIBallistic::calcNE() {
+ // Resolve horizontal speed into north and east components:
_speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
_speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
- // convert horizontal speed (fps) to degrees per second
+ // Convert horizontal speed (fps) to degrees per second
speed_north_deg_sec = _speed_north_fps / ft_per_deg_lat;
speed_east_deg_sec = _speed_east_fps / ft_per_deg_lon;
-
}
SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading,
double user_pitch, double user_roll
- ) const{
-
- // convert geodetic positions to geocentered
- SGVec3d cartuserPos = SGVec3d::fromGeod(inpos);
- //SGVec3d cartuserPos = getCartUserPos();
- //SGVec3d cartPos = getCartPos();
+ ) const {
+ // Convert geodetic positions to geocentered
+ SGVec3d cartuserPos = SGVec3d::fromGeod(inpos);
// Transform to the right coordinate frame, configuration is done in
// the x-forward, y-right, z-up coordinates (feet), computation
// Transform the user position to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
- // and postrotate the orientation of the user model wrt the horizontal
+ // And postrotate the orientation of the user model wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(
user_heading,
}
void FGAIBallistic::setOffsetVelocity(double dt, SGGeod offsetpos) {
- //calculate the distance from the previous offset position
+ // Calculate the distance from the previous offset position
SGVec3d cartoffsetPos = SGVec3d::fromGeod(offsetpos);
SGVec3d diff = cartoffsetPos - _oldcartoffsetPos;
double distance = norm(diff);
- //calculate speed knots
- speed = (distance/dt) * SG_MPS_TO_KT;
+ // Calculate speed knots
+ speed = (distance / dt) * SG_MPS_TO_KT;
- //now calulate the angle between the old and current postion positions (degrees)
+ // Now calulate the angle between the old and current postion positions (degrees)
double angle = 0;
double daltM = offsetpos.getElevationM() - _oldoffsetpos.getElevationM();
if (fabs(distance) < SGLimits<float>::min()) {
angle = 0;
- } else {
- double sAngle = daltM/distance;
+ }
+ else {
+ double sAngle = daltM / distance;
sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
angle = SGMiscd::rad2deg(asin(sAngle));
}
_elevation = angle;
- //calculate vertical and horizontal speed components
+ // Calculate vertical and horizontal speed components
calcVSHS();
- //calculate the bearing of the new offset position from the old
- //don't do this if speed is low
+ // Calculate the bearing of the new offset position from the old
+ // Don't do this if speed is low
//cout << "speed " << speed << endl;
- if (speed > 0.1){
+ if (speed > 0.1) {
double az1, az2, dist;
geo_inverse_wgs_84(_oldoffsetpos, offsetpos, &az1, &az2, &dist);
_azimuth = az1;
//cout << "offset az " << _azimuth << endl;
- } else {
+ }
+ else {
_azimuth = hdg;
//cout << " slow offset az " << _azimuth << endl;
}
- //resolve horizontal speed into north and east components:
+ // Resolve horizontal speed into north and east components
calcNE();
- // and finally store the new values
+ // And finally store the new values
_oldcartoffsetPos = cartoffsetPos;
_oldoffsetpos = offsetpos;
}
-
-// end AIBallistic
projects / flightgear.git / blobdiff