fly/flightgear
1
0
Fork 0
Code Activity

Wingman code from Vivian Meazza

Browse source
This commit is contained in:
timoore 2008-02-15 11:06:27 +00:00
parent bb90bb4466
commit 79e251383b
10 changed files with 845 additions and 95 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

580
src/AIModel/AIBallistic.cxx
View file

@ -31,11 +31,13 @@
#include "AIBallistic.hxx"
#include <Main/util.hxx>
const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
FGAIBallistic::FGAIBallistic() :
FGAIBase(otBallistic),
FGAIBallistic::FGAIBallistic(object_type ot) :
FGAIBase(ot),
_elevation(0),
_aero_stabilised(false),
_drag_area(0.007),
@ -50,7 +52,14 @@ _gravity(32.1740485564),
_report_impact(false),
_wind(true),
_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
_external_force(false)
_external_force(false),
_slave_to_ac(false),
_slave_load_to_ac(false),
_formate_to_ac(false),
_contents_lb(0),
_mass(0),
_height(0),
_old_height(0)
{
no_roll = false;
@ -77,7 +86,8 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
setWind(scFileNode->getBoolValue("wind", false));
setRoll(scFileNode->getDoubleValue("roll", 0.0));
setCd(scFileNode->getDoubleValue("cd", 0.029));
setMass(scFileNode->getDoubleValue("mass", 0.007));
//setMass(scFileNode->getDoubleValue("mass", 0.007));
setWeight(scFileNode->getDoubleValue("weight", 0.25));
setStabilisation(scFileNode->getBoolValue("aero_stabilized", false));
setNoRoll(scFileNode->getBoolValue("no-roll", false));
setRandom(scFileNode->getBoolValue("random", false));
@ -90,14 +100,29 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
setExternalForce(scFileNode->getBoolValue("external-force", false));
setForcePath(scFileNode->getStringValue("force-path", ""));
setForceStabilisation(scFileNode->getBoolValue("force_stabilized", false));
setXOffset(scFileNode->getDoubleValue("x-offset", 0.0));
setYOffset(scFileNode->getDoubleValue("y-offset", 0.0));
setZOffset(scFileNode->getDoubleValue("z-offset", 0.0));
setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
setLoadOffset(scFileNode->getDoubleValue("load-offset", 0.0));
setSlaved(scFileNode->getBoolValue("slaved", false));
setSlavedLoad(scFileNode->getBoolValue("slaved-load", false));
setContentsNode(scFileNode->getStringValue("contents"));
setRandom(scFileNode->getBoolValue("random", false));
}
bool FGAIBallistic::init(bool search_in_AI_path) {
FGAIBase::init(search_in_AI_path);
_impact_reported = false;
_collision_reported = false;
invisible = false;
_elapsed_time += (sg_random() * 100);
props->setStringValue("material/name", "");
props->setStringValue("name", _name.c_str());
props->setStringValue("submodels/path", _submodel.c_str());
@ -115,31 +140,123 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
void FGAIBallistic::bind() {
// FGAIBase::bind();
props->tie("sim/time/elapsed-sec",
SGRawValueMethods<FGAIBallistic,double>(*this,
&FGAIBallistic::_getTime));
props->tie("mass-slug",
SGRawValueMethods<FGAIBallistic,double>(*this,
&FGAIBallistic::getMass));
props->tie("material/load-resistance",
SGRawValuePointer<double>(&_load_resistance));
props->tie("material/solid",
SGRawValuePointer<bool>(&_solid));
props->tie("altitude-agl-ft",
SGRawValuePointer<double>(&_ht_agl_ft));
props->tie("controls/slave-to-ac",
SGRawValueMethods<FGAIBallistic,bool>
(*this, &FGAIBallistic::getSlaved, &FGAIBallistic::setSlaved));
props->tie("controls/invisible",
SGRawValuePointer<bool>(&invisible));
if(_external_force){
props->tie("controls/force_stabilized",
SGRawValuePointer<bool>(&_force_stabilised));
props->tie("position/global-x",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosX, 0));
props->tie("position/global-y",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosY, 0));
props->tie("position/global-z",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getCartPosZ, 0));
props->tie("velocities/vertical-speed-fps",
SGRawValuePointer<double>(&vs));
props->tie("velocities/true-airspeed-kt",
SGRawValuePointer<double>(&speed));
props->tie("velocities/horizontal-speed-fps",
SGRawValuePointer<double>(&hs));
props->tie("position/altitude-ft",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getAltitude, &FGAIBase::_setAltitude));
props->tie("position/latitude-deg",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLatitude, &FGAIBase::_setLatitude));
props->tie("position/longitude-deg",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getLongitude, &FGAIBase::_setLongitude));
props->tie("orientation/hdg-deg",
SGRawValuePointer<double>(&hdg));
props->tie("orientation/pitch-deg",
SGRawValuePointer<double>(&pitch));
props->tie("orientation/roll-deg",
SGRawValuePointer<double>(&roll));
props->tie("controls/slave-load-to-ac",
SGRawValueMethods<FGAIBallistic,bool>
(*this, &FGAIBallistic::getSlavedLoad, &FGAIBallistic::setSlavedLoad));
props->tie("position/load-offset",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
props->tie("load/distance-to-hitch-ft",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getDistanceLoadToHitch));
props->tie("load/elevation-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getElevLoadToHitch));
props->tie("load/bearing-to-hitch-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getBearingLoadToHitch));
}
}
void FGAIBallistic::unbind() {
// FGAIBase::unbind();
props->untie("sim/time/elapsed-sec");
props->untie("mass-slug");
props->untie("material/load-resistance");
props->untie("material/solid");
props->untie("altitude-agl-ft");
props->untie("controls/slave-to-ac");
props->untie("controls/invisible");
if(_external_force){
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");
}
}
void FGAIBallistic::update(double dt) {
FGAIBase::update(dt);
_setUserPos();
if (_slave_to_ac){
slaveToAC(dt);
Transform();
setHitchVelocity(dt);
} else if (_formate_to_ac){
formateToAC(dt);
Transform();
setHitchVelocity(dt);
} else if (!invisible){
Run(dt);
Transform();
}
}
void FGAIBallistic::setAzimuth(double az) {
hdg = _azimuth = az;
}
@ -149,7 +266,7 @@ void FGAIBallistic::setElevation(double el) {
}
void FGAIBallistic::setRoll(double rl) {
_rotation = rl;
roll = _rotation = rl;
}
void FGAIBallistic::setStabilisation(bool val) {
@ -196,6 +313,9 @@ void FGAIBallistic::setMass(double m) {
_mass = m;
}
void FGAIBallistic::setWeight(double w) {
_weight_lb = w;
}
void FGAIBallistic::setRandom(bool r) {
_random = r;
}
@ -230,27 +350,67 @@ void FGAIBallistic::setFuseRange(double f) {
_fuse_range = f;
}
void FGAIBallistic::setXOffset(double x) {
_x_offset = x;
}
void FGAIBallistic::setYOffset(double y) {
_y_offset = y;
}
void FGAIBallistic::setZOffset(double z) {
_z_offset = z;
}
void FGAIBallistic::setSubID(int i) {
_subID = i;
//cout << "sub id " << _subID << " name " << _name << endl;
}
void FGAIBallistic::setSubmodel(const string& s) {
_submodel = s;
}
void FGAIBallistic::setGroundOffset(double g) {
_ground_offset = g;
}
void FGAIBallistic::setLoadOffset(double l) {
_load_offset = l;
}
double FGAIBallistic::getLoadOffset() const {
return _load_offset;
}
void FGAIBallistic::setSlaved(bool s) {
_slave_to_ac = s;
}
void FGAIBallistic::setFormate(bool f) {
_formate_to_ac = f;
}
void FGAIBallistic::setContentsNode(const string& path) {
if (!path.empty()) {
_contents_node = fgGetNode(path.c_str(), true);
}
}
bool FGAIBallistic::getSlaved() const {
return _slave_to_ac;
}
double FGAIBallistic::getMass() const {
return _mass;
}
double FGAIBallistic::getContents() {
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);
}
void FGAIBallistic::setSlavedLoad(bool l) {
_slave_load_to_ac = l;
}
bool FGAIBallistic::getSlavedLoad() const {
return _slave_load_to_ac;
}
void FGAIBallistic::setForcePath(const string& p) {
_force_path = p;
if (!_force_path.empty()) {
@ -298,30 +458,85 @@ double FGAIBallistic::getRecip(double az){
}
}
void FGAIBallistic::setPitch(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::setHdg(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::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){
double recip = getRecip(hdg);
double c = dt / (coeff + dt);
//we need to ensure that we turn the short way to the new hdg
if (_azimuth < recip && _azimuth < hdg && hdg > 180) {
hdg = ((_azimuth + 360) * c) + (hdg * (1 - c));
} else if (_azimuth > recip && _azimuth > hdg && hdg <= 180){
hdg = ((_azimuth - 360) * c) + (hdg * (1 - c));
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));
} else {
hdg = (_azimuth * c) + (hdg * (1 - c));
hdg = (az * c) + (hdg * (1 - c));
}
}
double FGAIBallistic::getTgtXOffset() const {
return _tgt_x_offset;
}
double FGAIBallistic::getTgtYOffset() const {
return _tgt_y_offset;
}
double FGAIBallistic::getTgtZOffset() const {
return _tgt_z_offset;
}
void FGAIBallistic::setTgtXOffset(double x){
_tgt_x_offset = x;
}
void FGAIBallistic::setTgtYOffset(double y){
_tgt_y_offset = y;
}
void FGAIBallistic::setTgtZOffset(double z){
_tgt_z_offset = z;
}
void FGAIBallistic::slaveToAC(double dt){
setHitchPos();
pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
pos.setElevationFt(hitchpos.getElevationFt());
setHeading(manager->get_user_heading());
setPitch(manager->get_user_pitch() + _pitch_offset);
setBank(manager->get_user_roll() + _roll_offset);
setSpeed(manager->get_user_speed());
//update the mass (slugs)
_mass = (_weight_lb + getContents()) / slugs_to_lbs;
/*cout <<"_mass "<<_mass <<" " << getContents()
<<" " << getContents() / slugs_to_lbs << endl;*/
}
void FGAIBallistic::Run(double dt) {
_life_timer += dt;
// if life = -1 the object does not die
if (_life_timer > life && life != -1)
setDie(true);
//set the contents in the appropriate tank or other property in the parent to zero
setContents(0);
//randomise Cd by +- 5%
if (_random)
_Cd = _Cd * 0.95 + (0.05 * sg_random());
@ -337,7 +552,7 @@ void FGAIBallistic::Run(double dt) {
else
Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd;
//cout << " Mach , " << Mach << " , Cdm , " << Cdm << " ballistic speed kts //"<< speed << endl;
//cout << "Mach " << Mach << " Cdm " << Cdm << "// ballistic speed kts "<< speed << endl;
// drag = Cd * 0.5 * rho * speed * speed * drag_area;
// rho is adjusted for altitude in void FGAIBase::update,
@ -351,7 +566,7 @@ void FGAIBallistic::Run(double dt) {
speed = 0.0;
double speed_fps = speed * SG_KT_TO_FPS;
double hs;
//double hs;
// calculate vertical and horizontal speed components
if (speed == 0.0) {
@ -412,10 +627,15 @@ void FGAIBallistic::Run(double dt) {
if (getHtAGL()){
double deadzone = 0.1;
if ( _ht_agl_ft <= (0 + _z_offset + deadzone) && _solid){
if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
normal_force_lbs = (_mass * slugs_to_lbs) - v_force_lbs;
pos.setElevationFt((_elevation_m * SG_METER_TO_FEET) + _z_offset);
vs = 0;
if ( normal_force_lbs < 0 )
normal_force_lbs = 0;
pos.setElevationFt(0 + _ground_offset);
if (vs < 0)
vs = -vs * 0.5;
// calculate friction
// we assume a static Coefficient of Friction (mu) of 0.62 (wood on concrete)
@ -423,11 +643,12 @@ void FGAIBallistic::Run(double dt) {
static_friction_force_lbs = mu * normal_force_lbs * _frictionFactor;
//adjust horizontal force
if (h_force_lbs <= static_friction_force_lbs && hs <= 0.1)
//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;
else
dynamic_friction_force_lbs = (static_friction_force_lbs * 0.75);
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
@ -452,8 +673,8 @@ void FGAIBallistic::Run(double dt) {
force_speed_north_fps = cos(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
force_speed_east_fps = sin(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
double friction_force_speed_north_fps = cos(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
double friction_force_speed_east_fps = sin(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
friction_force_speed_north_fps = cos(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
friction_force_speed_east_fps = sin(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
// convert horizontal speed (fps) to degrees per second
force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat;
@ -467,14 +688,6 @@ void FGAIBallistic::Run(double dt) {
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;
// set new position
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 );
pos.setLongitudeDeg( pos.getLongitudeDeg()
+ (speed_east_deg_sec - wind_speed_from_east_deg_sec
+ force_speed_east_deg_sec + friction_force_speed_east_deg_sec) * dt );
//recombine the horizontal velocity components
hs = sqrt(((speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps)
* (speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps))
@ -490,10 +703,32 @@ void FGAIBallistic::Run(double dt) {
if (vs <= 0.00001 && vs >= -0.00001)
vs = 0;
// adjust altitude (feet) and set new elevation
altitude_ft = pos.getElevationFt();
altitude_ft += vs * dt;
pos.setElevationFt(altitude_ft);
// set new position
if(_slave_load_to_ac) {
setHitchPos();
pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
pos.setElevationFt(hitchpos.getElevationFt());
if (getHtAGL()){
double deadzone = 0.1;
if (_ht_agl_ft <= (0 + _ground_offset + deadzone) && _solid){
pos.setElevationFt(0 + _ground_offset);
} else {
pos.setElevationFt(hitchpos.getElevationFt() + _load_offset);
}
}
} 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 );
pos.setLongitudeDeg( pos.getLongitudeDeg()
+ (speed_east_deg_sec - wind_speed_from_east_deg_sec
+ force_speed_east_deg_sec + friction_force_speed_east_deg_sec) * dt );
pos.setElevationFt(pos.getElevationFt() + vs * dt);
}
// recalculate total speed
if ( vs == 0 && hs == 0)
@ -503,8 +738,9 @@ void FGAIBallistic::Run(double dt) {
// recalculate elevation and azimuth (velocity vectors)
_elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
_azimuth = atan2((speed_east_fps + force_speed_east_fps),
(speed_north_fps + force_speed_north_fps)) * SG_RADIANS_TO_DEGREES;
_azimuth = atan2((speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps),
(speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps))
* SG_RADIANS_TO_DEGREES;
// rationalise azimuth
if (_azimuth < 0)
@ -514,21 +750,23 @@ void FGAIBallistic::Run(double dt) {
const double coeff = 0.9;
// we assume a symetrical MI about the pitch and yaw axis
setPitch(_elevation, dt, coeff);
setHdg(dt, coeff);
setPch(_elevation, dt, coeff);
setHdg(_azimuth, dt, coeff);
} else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
const double coeff = 0.9;
double ratio = h_force_lbs/(_mass * slugs_to_lbs);
if (ratio > 1) ratio = 1;
if (ratio < -1) ratio = -1;
double force_pitch = acos(ratio) * SG_RADIANS_TO_DEGREES;
if (force_pitch <= force_elevation_deg)
force_pitch = force_elevation_deg;
// we assume a symetrical MI about the pitch and yaw axis
setPitch(force_pitch,dt, coeff);
setHdg(dt, coeff);
setPch(force_pitch,dt, coeff);
setHdg(_azimuth, dt, coeff);
}
//do impacts and collisions
@ -539,7 +777,7 @@ void FGAIBallistic::Run(double dt) {
handle_collision();
// set destruction flag if altitude less than sea level -1000
if (altitude_ft < -1000.0)
if (altitude_ft < -1000.0 && life != -1)
setDie(true);
} // end Run
@ -559,8 +797,9 @@ void FGAIBallistic::handle_impact() {
report_impact(_elevation_m);
_impact_reported = true;
// kill the AIObject if there is no subsubmodel
if (_subID == 0)
if (life == -1){
invisible = true;
} else if (_subID == 0) // kill the AIObject if there is no subsubmodel
setDie(true);
}
}
@ -604,5 +843,222 @@ void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
_impact_report_node->setStringValue(props->getPath());
}
// end AIBallistic
SGVec3d FGAIBallistic::getCartUserPos() const {
SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
return cartUserPos;
}
SGVec3d FGAIBallistic::getCartHitchPos() const{
// convert geodetic positions to geocentered
SGVec3d cartuserPos = getCartUserPos();
SGVec3d cartPos = getCartPos();
// Transform to the right coordinate frame, configuration is done in
// the x-forward, y-right, z-up coordinates (feet), computation
// in the simulation usual body x-forward, y-right, z-down coordinates
// (meters) )
SGVec3d _off(_x_offset * SG_FEET_TO_METER,
_y_offset * SG_FEET_TO_METER,
-_z_offset * SG_FEET_TO_METER);
// Transform the user position to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
// and postrotate the orientation of the user model wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(
manager->get_user_heading(),
manager->get_user_pitch(),
manager->get_user_roll());
// The offset converted to the usual body fixed coordinate system
// rotated to the earth-fixed coordinates axis
SGVec3d off = hlTrans.backTransform(_off);
// Add the position offset of the user model to get the geocentered position
SGVec3d offsetPos = cartuserPos + off;
return offsetPos;
}
void FGAIBallistic::setHitchPos(){
// convert the hitch geocentered position to geodetic
SGVec3d carthitchPos = getCartHitchPos();
SGGeodesy::SGCartToGeod(carthitchPos, hitchpos);
}
double FGAIBallistic::getDistanceLoadToHitch() const {
//calculate the distance load to hitch
SGVec3d carthitchPos = getCartHitchPos();
SGVec3d cartPos = getCartPos();
SGVec3d diff = carthitchPos - cartPos;
double distance = norm(diff);
return distance * SG_METER_TO_FEET;
}
void FGAIBallistic::setHitchVelocity(double dt) {
//calculate the distance from the previous hitch position
SGVec3d carthitchPos = getCartHitchPos();
SGVec3d diff = carthitchPos - _oldcarthitchPos;
double distance = norm(diff);
//calculate speed knots
speed = (distance/dt) * SG_MPS_TO_KT;
//now calulate the angle between the old and current hitch positions (degrees)
double angle = 0;
double daltM = hitchpos.getElevationM() - oldhitchpos.getElevationM();
if (fabs(distance) < SGLimits<float>::min()) {
angle = 0;
} else {
double sAngle = daltM/distance;
sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
angle = SGMiscd::rad2deg(asin(sAngle));
}
_elevation = angle;
//calculate the bearing of the new hitch position from the old
double az1, az2, dist;
geo_inverse_wgs_84(oldhitchpos, hitchpos, &az1, &az2, &dist);
_azimuth = az1;
// and finally store the new values
_oldcarthitchPos = carthitchPos;
oldhitchpos = hitchpos;
}
double FGAIBallistic::getElevLoadToHitch() const {
// now the angle, positive angles are upwards
double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER;
double angle = 0;
double daltM = hitchpos.getElevationM() - pos.getElevationM();
if (fabs(distance) < SGLimits<float>::min()) {
angle = 0;
} else {
double sAngle = daltM/distance;
sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
angle = SGMiscd::rad2deg(asin(sAngle));
}
return angle;
}
double FGAIBallistic::getBearingLoadToHitch() const {
//calculate the bearing and range of the second pos from the first
double az1, az2, distance;
geo_inverse_wgs_84(pos, hitchpos, &az1, &az2, &distance);
return az1;
}
double FGAIBallistic::getRelBrgHitchToUser() const {
//calculate the relative bearing
double az1, az2, distance;
geo_inverse_wgs_84(hitchpos, userpos, &az1, &az2, &distance);
double rel_brg = az1 - hdg;
if (rel_brg > 180)
rel_brg -= 360;
return rel_brg;
}
double FGAIBallistic::getElevHitchToUser() const {
//calculate the distance from the user position
SGVec3d carthitchPos = getCartHitchPos();
SGVec3d cartuserPos = getCartUserPos();
SGVec3d diff = cartuserPos - carthitchPos;
double distance = norm(diff);
double angle = 0;
double daltM = userpos.getElevationM() - hitchpos.getElevationM();
// now the angle, positive angles are upwards
if (fabs(distance) < SGLimits<float>::min()) {
angle = 0;
} 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){
double c = dt / (coeff + dt);
_x_offset = (_tgt_x_offset * c) + (_x_offset * (1 - c));
_y_offset = (_tgt_y_offset * c) + (_y_offset * (1 - c));
_z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
}
void FGAIBallistic::formateToAC(double dt){
setTgtOffsets(dt, 25);
setHitchPos();
setHitchVelocity(dt);
// 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(hitchpos.getLatitudeDeg());
pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
if (getHtAGL()){
if(_ht_agl_ft <= 10) {
_height = userpos.getElevationFt();
} else if (_ht_agl_ft > 10 && _ht_agl_ft <= 150 ) {
setHt(userpos.getElevationFt(), dt, 1.0);
} else if (_ht_agl_ft > 150 && _ht_agl_ft <= 250) {
setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.75);
} else
setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.5);
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);
}
setSpeed(speed);
}
// end AIBallistic

99
src/AIModel/AIBallistic.hxx
View file

@ -3,6 +3,8 @@
// Written by David Culp, started November 2003.
// - davidculp2@comcast.net
//
// With major additions by Vivian Meazza, Feb 2008
//
// 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
@ -36,7 +38,7 @@ class FGAIBallistic : public FGAIBase {
public:
FGAIBallistic();
FGAIBallistic(object_type ot = otBallistic);
~FGAIBallistic();
void readFromScenario(SGPropertyNode* scFileNode);
@ -47,6 +49,10 @@ public:
void update(double dt);
FGAIBallistic *ballistic;
void Run(double dt);
void setAzimuth( double az );
void setElevation( double el );
void setRoll( double rl );
@ -59,25 +65,44 @@ public:
void setWind( bool val );
void setCd( double c );
void setMass( double m );
void setWeight( double w );
void setNoRoll( bool nr );
void setRandom( bool r );
void setName(const string&);
void setCollision(bool c);
void setImpact(bool i);
void setImpactReportNode(const string&);
void setContentsNode(const string&);
void setFuseRange(double f);
void setSMPath(const string&);
void setSubID(int i);
void setSubmodel(const string&);
void setExternalForce( bool f );
void setForcePath(const string&);
double _getTime() const;
bool getHtAGL();
void setForceStabilisation( bool val );
void setXOffset(double x);
void setYOffset(double y);
void setZOffset(double z);
void setGroundOffset(double g);
void setLoadOffset(double l);
void setSlaved(bool s);
void setSlavedLoad(bool s);
void setHitchPos();
void setPch (double e, double dt, double c);
void setHdg (double az, double dt, double c);
void setBnk(double r, double dt, double c);
void setHt(double h, double dt, double c);
void setHitchVelocity(double dt);
void setFormate(bool f);
double _getTime() const;
double getRelBrgHitchToUser() const;
double getElevHitchToUser() const;
double getLoadOffset() const;
double getContents();
SGVec3d getCartHitchPos() const;
bool getHtAGL();
bool getSlaved() const;
bool getSlavedLoad() const;
virtual const char* getTypeString(void) const { return "ballistic"; }
static const double slugs_to_kgs; //conversion factor
@ -87,40 +112,64 @@ public:
SGPropertyNode_ptr _force_azimuth_node;
SGPropertyNode_ptr _force_elevation_node;
private:
SGGeod hitchpos;
double _height;
double _ht_agl_ft; // height above ground level
double _azimuth; // degrees true
double _elevation; // degrees
double _rotation; // degrees
bool _formate_to_ac;
void setTgtXOffset(double x);
void setTgtYOffset(double y);
void setTgtZOffset(double z);
void setTgtOffsets(double dt, double c);
double getTgtXOffset() const;
double getTgtYOffset() const;
double getTgtZOffset() const;
double _tgt_x_offset;
double _tgt_y_offset;
double _tgt_z_offset;
double _elapsed_time;
private:
virtual void reinit() { init(); }
bool _aero_stabilised; // if true, object will align with trajectory
double _drag_area; // equivalent drag area in ft2
double _life_timer; // seconds
double _gravity; // fps2
double _buoyancy; // fps2
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
bool _random; // modifier for Cd
double _ht_agl_ft; // height above ground level
double _load_resistance; // ground load resistanc N/m^2
double _frictionFactor; // dimensionless modifier for Coefficient of Friction
bool _solid; // if true ground is solid for FDMs
double _elevation_m; // ground elevation in meters
bool _force_stabilised;// if true, object will align to external force
double _x_offset;
double _y_offset;
double _z_offset;
bool _slave_to_ac; // if true, object will be slaved to the parent ac pos and orientation
bool _slave_load_to_ac;// if true, object will be slaved to the parent ac pos
double _contents_lb; // contents of the object
double _weight_lb; // weight of the object (no contents if appropriate) (lbs)
bool _report_collision; // if true a collision point with AI Objects is calculated
bool _report_impact; // if true an impact point on the terrain is calculated
bool _external_force; // if true then apply external force
SGPropertyNode_ptr _impact_report_node; // report node for impact and collision
SGPropertyNode_ptr _contents_node; // report node for impact and collision
double _fuse_range;
double _distance;
double _dt_count;
double _next_run;
@ -131,14 +180,30 @@ private:
const SGMaterial* _material;
void Run(double dt);
void handle_collision();
void handle_impact();
void setPitch (double e, double dt, double c);
void setHdg (double dt, double c);
void report_impact(double elevation, const FGAIBase *target = 0);
void slaveToAC(double dt);
void setContents(double c);
void formateToAC(double dt);
SGVec3d getCartUserPos() 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 _force;
double _old_height;
SGVec3d _oldcarthitchPos;
SGGeod oldhitchpos;
};

31
src/AIModel/AIBase.cxx
View file

@ -138,6 +138,7 @@ void FGAIBase::update(double dt) {
void FGAIBase::Transform() {
if (!invisible) {
aip.setVisible(true);
aip.setPosition(pos);
if (no_roll)
@ -146,6 +147,9 @@ void FGAIBase::Transform() {
aip.setOrientation(roll, pitch, hdg);
aip.update();
} else {
aip.setVisible(false);
aip.update();
}
}
@ -435,15 +439,7 @@ SGVec3d FGAIBase::getCartPosAt(const SGVec3d& _off) const {
}
SGVec3d FGAIBase::getCartPos() const {
// Transform that one to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
SGVec3d cartPos = SGVec3d::fromGeod(pos);
return cartPos;
}
@ -470,6 +466,12 @@ void FGAIBase::_setLatitude ( double latitude ) {
pos.setLatitudeDeg(latitude);
}
void FGAIBase::_setUserPos(){
userpos.setLatitudeDeg(manager->get_user_latitude());
userpos.setLongitudeDeg(manager->get_user_longitude());
userpos.setElevationM(manager->get_user_altitude() * SG_FEET_TO_METER);
}
void FGAIBase::_setSubID( int s ) {
_subID = s;
}
@ -586,6 +588,18 @@ double FGAIBase::_getHeading() const {
return hdg;
}
double FGAIBase::_getXOffset() const {
return _x_offset;
}
double FGAIBase::_getYOffset() const {
return _y_offset;
}
double FGAIBase::_getZOffset() const {
return _z_offset;
}
const char* FGAIBase::_getPath() const {
return model_path.c_str();
}
@ -606,7 +620,6 @@ const char* FGAIBase::_getSubmodel() const {
return _submodel.c_str();
}
void FGAIBase::CalculateMach() {
// Calculate rho at altitude, using standard atmosphere
// For the temperature T and the pressure p,

44
src/AIModel/AIBase.hxx
View file

@ -44,7 +44,8 @@ class FGAIBase : public SGReferenced {
public:
enum object_type { otNull = 0, otAircraft, otShip, otCarrier, otBallistic,
otRocket, otStorm, otThermal, otStatic, otMultiplayer,
otRocket, otStorm, otThermal, otStatic, otWingman,
otMultiplayer,
MAX_OBJECTS }; // Needs to be last!!!
FGAIBase(object_type ot);
@ -73,6 +74,9 @@ public:
void setXoffset( double x_offset );
void setYoffset( double y_offset );
void setZoffset( double z_offset );
void setPitchoffset( double x_offset );
void setRolloffset( double y_offset );
void setYawoffset( double z_offset );
void setServiceable ( bool serviceable );
void setDie( bool die );
void setCollisionData( bool i, double lat, double lon, double elev );
@ -93,11 +97,22 @@ public:
double _getCartPosY() const;
double _getCartPosZ() const;
double _x_offset;
double _y_offset;
double _z_offset;
double _pitch_offset;
double _roll_offset;
double _yaw_offset;
string _path;
string _callsign;
string _submodel;
string _name;
SGGeod userpos;
protected:
SGPropertyNode_ptr props;
@ -189,6 +204,7 @@ public:
void _setLongitude( double longitude );
void _setLatitude ( double latitude );
void _setSubID( int s );
void _setUserPos();
double _getVS_fps() const;
double _getAltitude() const;
@ -215,7 +231,9 @@ public:
double _getImpactPitch() const;
double _getImpactRoll() const;
double _getImpactSpeed() const;
double _getXOffset() const;
double _getYOffset() const;
double _getZOffset() const;
//unsigned int _getCount() const;
bool _getServiceable() const;
@ -310,7 +328,29 @@ inline void FGAIBase::setLatitude ( double latitude ) {
inline void FGAIBase::setCallSign(const string& s) {
_callsign = s;
}
inline void FGAIBase::setXoffset(double x) {
_x_offset = x;
}
inline void FGAIBase::setYoffset(double y) {
_y_offset = y;
}
inline void FGAIBase::setZoffset(double z) {
_z_offset = z;
}
inline void FGAIBase::setPitchoffset(double p) {
_pitch_offset = p;
}
inline void FGAIBase::setRolloffset(double r) {
_roll_offset = r;
}
inline void FGAIBase::setYawoffset(double y) {
_yaw_offset = y;
}
inline void FGAIBase::setDie( bool die ) { delete_me = die; }

11
src/AIModel/AIManager.cxx
View file

@ -33,6 +33,7 @@
#include "AIStatic.hxx"
#include "AIMultiplayer.hxx"
#include "AITanker.hxx"
#include "AIWingman.hxx"
#include <simgear/math/sg_geodesy.hxx>
@ -73,6 +74,7 @@ FGAIManager::init() {
user_heading_node = fgGetNode("/orientation/heading-deg", true);
user_pitch_node = fgGetNode("/orientation/pitch-deg", true);
user_yaw_node = fgGetNode("/orientation/side-slip-deg", true);
user_roll_node = fgGetNode("/orientation/roll-deg", true);
user_speed_node = fgGetNode("/velocities/uBody-fps", true);
}
@ -155,8 +157,6 @@ FGAIManager::update(double dt) {
props->setIntValue("id", -1);
props->setBoolValue("radar/in-range", false);
props->setIntValue("refuel/tanker", false);
props->setStringValue("sim/multiplay/chat", "");
props->setStringValue("sim/multiplay/transmission-freq-hz", "");
ai_list_itr = ai_list.erase(ai_list_itr);
} else {
@ -240,6 +240,7 @@ FGAIManager::fetchUserState( void ) {
user_pitch = user_pitch_node->getDoubleValue();
user_yaw = user_yaw_node->getDoubleValue();
user_speed = user_speed_node->getDoubleValue() * 0.592484;
user_roll = user_roll_node->getDoubleValue();
wind_from_east = wind_from_east_node->getDoubleValue();
wind_from_north = wind_from_north_node->getDoubleValue();
}
@ -280,6 +281,12 @@ FGAIManager::processScenario( const string &filename ) {
FGAITanker* tanker = new FGAITanker;
tanker->readFromScenario(scEntry);
attach(tanker);
} else if (type == "wingman") {
FGAIWingman* wingman = new FGAIWingman;
wingman->readFromScenario(scEntry);
attach(wingman);
} else if (type == "aircraft") {
FGAIAircraft* aircraft = new FGAIAircraft;
aircraft->readFromScenario(scEntry);

3
src/AIModel/AIManager.hxx
View file

@ -80,6 +80,7 @@ public:
inline double get_user_speed() const {return user_speed; }
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; }
int getNumAiObjects(void) const;
@ -106,6 +107,7 @@ private:
SGPropertyNode_ptr user_heading_node;
SGPropertyNode_ptr user_pitch_node;
SGPropertyNode_ptr user_yaw_node;
SGPropertyNode_ptr user_roll_node;
SGPropertyNode_ptr user_speed_node;
SGPropertyNode_ptr wind_from_east_node;
SGPropertyNode_ptr wind_from_north_node;
@ -116,6 +118,7 @@ private:
double user_heading;
double user_pitch;
double user_yaw;
double user_roll;
double user_speed;
double wind_from_east;
double wind_from_north;

118
src/AIModel/AIWingman.cxx Normal file
View file

@ -0,0 +1,118 @@
// FGAIWingman - FGAIBllistic-derived class creates an AI Wingman
//
// Written by Vivian Meazza, started February 2008.
// - 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 "AIWingman.hxx"
FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman)
{
invisible = false;
_formate_to_ac = true;
}
FGAIWingman::~FGAIWingman() {}
void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) {
if (!scFileNode)
return;
FGAIBase::readFromScenario(scFileNode);
setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
setElevation(scFileNode->getDoubleValue("elevation", 0.0));
setLife(scFileNode->getDoubleValue("life", -1));
setNoRoll(scFileNode->getBoolValue("no-roll", false));
setName(scFileNode->getStringValue("name", "Wingman"));
setSMPath(scFileNode->getStringValue("submodel-path", ""));
setSubID(scFileNode->getIntValue("SubID", 0));
setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
setFormate(scFileNode->getBoolValue("formate", true));
}
void FGAIWingman::bind() {
FGAIBallistic::bind();
props->tie("load/rel-brg-to-user-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getRelBrgHitchToUser));
props->tie("load/elev-to-user-deg",
SGRawValueMethods<FGAIBallistic,double>
(*this, &FGAIBallistic::getElevHitchToUser));
props->tie("velocities/vertical-speed-fps",
SGRawValuePointer<double>(&vs));
props->tie("position/x-offset",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
props->tie("position/y-offset",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
props->tie("position/z-offset",
SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
props->tie("position/tgt-x-offset",
SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset));
props->tie("position/tgt-y-offset",
SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset));
props->tie("position/tgt-z-offset",
SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset));
}
void FGAIWingman::unbind() {
FGAIBallistic::unbind();
props->untie("load/rel-brg-to-user-deg");
props->untie("load/elev-to-user-deg");
props->untie("velocities/vertical-speed-fps");
props->untie("position/x-offset");
props->untie("position/y-offset");
props->untie("position/z-offset");
props->untie("position/tgt-x-offset");
props->untie("position/tgt-y-offset");
props->untie("position/tgt-z-offset");
}
bool FGAIWingman::init(bool search_in_AI_path) {
if (!FGAIBallistic::init(search_in_AI_path))
return false;
invisible = false;
_tgt_x_offset = _x_offset;
_tgt_y_offset = _y_offset;
_tgt_z_offset = _z_offset;
hdg = _azimuth;
pitch = _elevation;
roll = _rotation;
_ht_agl_ft = 1e10;
return true;
}
void FGAIWingman::update(double dt) {
FGAIBallistic::update(dt);
}
// end AIWingman

47
src/AIModel/AIWingman.hxx Normal file
View file

@ -0,0 +1,47 @@
// FGAIWingman - FGAIBllistic-derived class creates an AI Wingman
//
// Written by Vivian Meazza, started February 2008.
// - 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_AIWINGMAN_HXX
#define _FG_AIWINGMAN_HXX
#include "AIBallistic.hxx"
#include "AIManager.hxx"
#include "AIBase.hxx"
class FGAIWingman : public FGAIBallistic {
public:
FGAIWingman();
virtual ~FGAIWingman();
virtual void readFromScenario(SGPropertyNode* scFileNode);
virtual void bind();
virtual void unbind();
virtual const char* getTypeString(void) const { return "wingman"; }
bool init(bool search_in_AI_path=false);
private:
virtual void reinit() { init(); }
virtual void update (double dt);
};
#endif // FG_AIWINGMAN_HXX

1
src/AIModel/Makefile.am
View file

@ -16,6 +16,7 @@ libAIModel_a_SOURCES = submodel.cxx submodel.hxx \
AICarrier.hxx AICarrier.cxx \
AIStatic.hxx AIStatic.cxx \
AITanker.cxx AITanker.hxx \
AIWingman.cxx AIWingman.hxx\
performancedata.cxx performancedata.hxx \
performancedb.cxx performancedb.hxx

6
src/AIModel/submodel.hxx
View file

@ -117,9 +117,9 @@ private:
float in[3];
float out[3];
double Rx, Ry, Rz;
double Sx, Sy, Sz;
double Tx, Ty, Tz;
//double Rx, Ry, Rz;
//double Sx, Sy, Sz;
//double Tx, Ty, Tz;
float cosRx, sinRx;
float cosRy, sinRy;