fly/flightgear
1
0
Fork 0
Code Activity

Vivian MEAZZA:

Browse source 
- collision detection for AI objects
- subsubmodels

mf: various modifications; more general cleanup to do
This commit is contained in:
mfranz 2007-06-07 16:30:26 +00:00
parent b693047703
commit 5a73a46cd0
8 changed files with 1146 additions and 559 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

111
src/AIModel/AIBallistic.cxx
View file

@ -26,15 +26,12 @@
#include <simgear/math/point3d.hxx>
#include <simgear/math/sg_random.h>
#include <simgear/scene/material/mat.hxx>
#include <math.h>
#include <vector>
#include <simgear/math/sg_geodesy.hxx>
#include <Scenery/scenery.hxx>
#include "AIBallistic.hxx"
SG_USING_STD(vector);
const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
FGAIBallistic::FGAIBallistic() :
@ -43,11 +40,12 @@ FGAIBallistic::FGAIBallistic() :
_drag_area(0.007),
_life_timer(0.0),
_gravity(32),
// _buoyancy(64),
_buoyancy(0),
_ht_agl_ft(0),
_load_resistance(0),
_solid(false),
_impact_reported(false),
_report_collision(false),
_report_impact(false),
_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
_mat_name("")
{
@ -80,6 +78,9 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
setImpact(scFileNode->getBoolValue("impact", false));
setImpactReportNode(scFileNode->getStringValue("impact-reports"));
setName(scFileNode->getStringValue("name", "Bomb"));
setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
setSMPath(scFileNode->getStringValue("submodel-path", ""));
setSubID(scFileNode->getIntValue("SubID", 0));
}
bool FGAIBallistic::init(bool search_in_AI_path) {
@ -87,6 +88,7 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
props->setStringValue("material/name", _mat_name.c_str());
props->setStringValue("name", _name.c_str());
props->setStringValue("submodels/path", _submodel.c_str());
// start with high value so that animations don't trigger yet
_ht_agl_ft = 10000000;
@ -94,6 +96,7 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
pitch = _elevation;
roll = _rotation;
Transform();
return true;
}
@ -108,6 +111,8 @@ void FGAIBallistic::bind() {
SGRawValuePointer<bool>(&_solid));
props->tie("altitude-agl-ft",
SGRawValuePointer<double>(&_ht_agl_ft));
props->tie("sub-id",
SGRawValuePointer<int>(&_subID));
}
void FGAIBallistic::unbind() {
@ -116,6 +121,7 @@ void FGAIBallistic::unbind() {
props->untie("material/load-resistance");
props->untie("material/solid");
props->untie("altitude-agl-ft");
props->untie("sub-id");
}
void FGAIBallistic::update(double dt) {
@ -184,6 +190,10 @@ void FGAIBallistic::setImpact(bool i) {
_report_impact = i;
}
void FGAIBallistic::setCollision(bool c) {
_report_collision = c;
}
void FGAIBallistic::setImpactReportNode(const string& path) {
if (!path.empty())
_impact_report_node = fgGetNode(path.c_str(), true);
@ -193,9 +203,26 @@ void FGAIBallistic::setName(const string& n) {
_name = n;
}
void FGAIBallistic::setSMPath(const string& s) {
_submodel = s;
}
void FGAIBallistic::setFuseRange(double f) {
_fuse_range = f;
}
void FGAIBallistic::setSubID(int i) {
_subID = i;
//cout << "sub id " << _subID << " name " << _name << endl;
}
void FGAIBallistic::setSubmodel(const string& s) {
_submodel = s;
}
void FGAIBallistic::Run(double dt) {
_life_timer += dt;
// cout << "life timer 1" << _life_timer << dt << endl;
//cout << "life timer" <<_name <<" " << _life_timer << dt << endl;
if (_life_timer > life)
setDie(true);
@ -204,6 +231,7 @@ void FGAIBallistic::Run(double dt) {
double wind_speed_from_north_deg_sec;
double wind_speed_from_east_deg_sec;
double Cdm; // Cd adjusted by Mach Number
double hs;
//randomise Cd by +- 5%
if (_random)
@ -234,8 +262,12 @@ void FGAIBallistic::Run(double dt) {
double speed_fps = speed * SG_KT_TO_FPS;
// calculate vertical and horizontal speed components
vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
double hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
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;
}
// convert horizontal speed (fps) to degrees per second
speed_north_deg_sec = cos(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lat;
@ -274,9 +306,13 @@ void FGAIBallistic::Run(double dt) {
// recalculate total speed
speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
//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
if (altitude_ft < -1000.0)
setDie(true);
@ -311,25 +347,52 @@ void FGAIBallistic::handle_impact() {
_ht_agl_ft = pos.getElevationFt() - elevation_m * SG_METER_TO_FEET;
// report impact by setting property-tied variables
// report impact by setting properties
if (_ht_agl_ft <= 0) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: terrain impact");
report_impact(elevation_m);
_impact_reported = true;
double speed_mps = speed * SG_KT_TO_MPS;
SGPropertyNode *n = props->getNode("impact", true);
n->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
n->setDoubleValue("latitude-deg", pos.getLatitudeDeg());
n->setDoubleValue("elevation-m", elevation_m);
n->setDoubleValue("heading-deg", hdg);
n->setDoubleValue("pitch-deg", pitch);
n->setDoubleValue("roll-deg", roll);
n->setDoubleValue("speed-mps", speed_mps);
n->setDoubleValue("energy-kJ", (_mass * slugs_to_kgs)
* speed_mps * speed_mps / (2 * 1000));
_impact_report_node->setStringValue(props->getPath());
}
}
void FGAIBallistic::handle_collision()
{
const FGAIBase *collision = manager->calcCollision(pos.getElevationFt(),
pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
if (collision) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: HIT!");
report_impact(pos.getElevationM(), collision);
_collision_reported = true;
}
}
void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
{
_impact_lat = pos.getLatitudeDeg();
_impact_lon = pos.getLongitudeDeg();
_impact_elev = elevation;
_impact_speed = speed * SG_KT_TO_MPS;
_impact_hdg = hdg;
_impact_pitch = pitch;
_impact_roll = roll;
SGPropertyNode *n = props->getNode("impact", true);
if (object)
n->setStringValue("type", object->getTypeString());
else
n->setStringValue("type", "terrain");
n->setDoubleValue("longitude-deg", _impact_lon);
n->setDoubleValue("latitude-deg", _impact_lat);
n->setDoubleValue("elevation-m", _impact_elev);
n->setDoubleValue("heading-deg", _impact_hdg);
n->setDoubleValue("pitch-deg", _impact_pitch);
n->setDoubleValue("roll-deg", _impact_roll);
n->setDoubleValue("speed-mps", _impact_speed);
_impact_report_node->setStringValue(props->getPath());
}
// end AIBallistic

32
src/AIModel/AIBallistic.hxx
View file

@ -20,9 +20,16 @@
#ifndef _FG_AIBALLISTIC_HXX
#define _FG_AIBALLISTIC_HXX
#include <math.h>
#include <vector>
#include <simgear/structure/SGSharedPtr.hxx>
#include "AIManager.hxx"
#include "AIBase.hxx"
SG_USING_STD(vector);
SG_USING_STD(list);
class FGAIBallistic : public FGAIBase {
@ -54,8 +61,14 @@ public:
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 setFuseRange(double f);
void setSMPath(const string&);
void setSubID(int i);
void setSubmodel(const string&);
double _getTime() const;
@ -81,17 +94,28 @@ private:
double _ht_agl_ft; // height above ground level
double _load_resistance; // ground load resistanc N/m^2
bool _solid; // if true ground is solid for FDMs
bool _report_impact; // if true an impact point on the terrain is calculated
bool _impact_reported; // if true impact data have been set
SGPropertyNode_ptr _impact_report_node;
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
SGPropertyNode_ptr _impact_report_node; // report node for impact and collision
double _fuse_range;
double _dt_count;
double _next_run;
string _mat_name;
string _name;
string _path;
string _submodel;
void Run(double dt);
void handle_collision();
void handle_impact();
// FGAIBase* _ai;
void report_impact(double elevation, const FGAIBase *target = 0);
};
#endif // _FG_AIBALLISTIC_HXX

482
src/AIModel/AIBase.cxx
View file

@ -24,6 +24,7 @@
# include <config.h>
#endif
#include <simgear/compiler.h>
#include STL_STRING
@ -45,11 +46,9 @@
#include <Scenery/scenery.hxx>
#include <Scripting/NasalSys.hxx>
#include "AIBase.hxx"
#include "AIManager.hxx"
const double FGAIBase::e = 2.71828183;
const double FGAIBase::lbs_to_slugs = 0.031080950172; //conversion factor
@ -59,6 +58,15 @@ FGAIBase::FGAIBase(object_type ot) :
model_removed( fgGetNode("/ai/models/model-removed", true) ),
manager( NULL ),
fp( NULL ),
_impact_lat(0),
_impact_lon(0),
_impact_elev(0),
_impact_hdg(0),
_impact_pitch(0),
_impact_roll(0),
_impact_speed(0),
_refID( _newAIModelID() ),
_otype(ot)
{
@ -67,10 +75,13 @@ FGAIBase::FGAIBase(object_type ot) :
bearing = elevation = range = rdot = 0.0;
x_shift = y_shift = rotation = 0.0;
in_range = false;
invisible = true;
invisible = false;
no_roll = true;
life = 900;
delete_me = false;
_impact_reported = false;
_collision_reported = false;
_subID = 0;
}
FGAIBase::~FGAIBase() {
@ -127,14 +138,14 @@ void FGAIBase::update(double dt) {
void FGAIBase::Transform() {
if (!invisible) {
aip.setPosition(pos);
aip.setPosition(pos);
if (no_roll)
aip.setOrientation(0.0, pitch, hdg);
aip.setOrientation(0.0, pitch, hdg);
else
aip.setOrientation(roll, pitch, hdg);
aip.setOrientation(roll, pitch, hdg);
aip.update();
aip.update();
}
}
@ -143,47 +154,44 @@ bool FGAIBase::init(bool search_in_AI_path) {
if (!model_path.empty()) {
if ( (search_in_AI_path)
&&(model_path.substr(model_path.size() - 4, 4) == ".xml")) {
SGPath ai_path("AI");
ai_path.append(model_path);
try {
model = load3DModel( globals->get_fg_root(), ai_path.str(), props,
if ( search_in_AI_path
&& (model_path.substr(model_path.size() - 4, 4) == ".xml")) {
SGPath ai_path("AI");
ai_path.append(model_path);
try {
model = load3DModel( globals->get_fg_root(), ai_path.str(), props,
globals->get_sim_time_sec() );
} catch (const sg_exception &e) {
model = NULL;
}
} else
model = NULL;
} catch (const sg_exception &e) {
model = NULL;
}
} else
model = NULL;
if (!model) {
try {
model = load3DModel( globals->get_fg_root(), model_path, props,
if (!model.get()) {
try {
model = load3DModel( globals->get_fg_root(), model_path, props,
globals->get_sim_time_sec() );
} catch (const sg_exception &e) {
model = NULL;
}
}
} catch (const sg_exception &e) {
model = NULL;
}
}
}
}
if (model.get()) {
aip.init( model.get() );
aip.setVisible(true);
invisible = false;
globals->get_scenery()->get_scene_graph()->addChild(aip.getSceneGraph());
if (model.get()) {
aip.init( model.get() );
aip.setVisible(true);
invisible = false;
globals->get_scenery()->get_scene_graph()->addChild(aip.getSceneGraph());
fgSetString("/ai/models/model-added", props->getPath());
fgSetString("/ai/models/model-added", props->getPath());
} else {
if (!model_path.empty())
SG_LOG(SG_INPUT, SG_WARN, "AIBase: Could not load model " << model_path);
}
} else if (!model_path.empty()) {
SG_LOG(SG_INPUT, SG_WARN, "AIBase: Could not load model " << model_path);
}
props->setStringValue("submodels/path", _path.c_str());
setDie(false);
return true;
setDie(false);
return true;
}
@ -199,77 +207,73 @@ osg::Node* FGAIBase::load3DModel(const string& fg_root,
}
bool FGAIBase::isa( object_type otype ) {
if ( otype == _otype )
return true;
else
return false;
return otype == _otype;
}
void FGAIBase::bind() {
props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
&FGAIBase::getID));
props->tie("velocities/true-airspeed-kt", SGRawValuePointer<double>(&speed));
props->tie("velocities/vertical-speed-fps",
SGRawValueMethods<FGAIBase,double>(*this,
&FGAIBase::_getVS_fps,
&FGAIBase::_setVS_fps));
props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
&FGAIBase::getID));
props->tie("velocities/true-airspeed-kt", SGRawValuePointer<double>(&speed));
props->tie("velocities/vertical-speed-fps",
SGRawValueMethods<FGAIBase,double>(*this,
&FGAIBase::_getVS_fps,
&FGAIBase::_setVS_fps));
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("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("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("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("orientation/pitch-deg", SGRawValuePointer<double>(&pitch));
props->tie("orientation/roll-deg", SGRawValuePointer<double>(&roll));
props->tie("orientation/true-heading-deg", SGRawValuePointer<double>(&hdg));
props->tie("orientation/pitch-deg", SGRawValuePointer<double>(&pitch));
props->tie("orientation/roll-deg", SGRawValuePointer<double>(&roll));
props->tie("orientation/true-heading-deg", SGRawValuePointer<double>(&hdg));
props->tie("radar/in-range", SGRawValuePointer<bool>(&in_range));
props->tie("radar/bearing-deg", SGRawValuePointer<double>(&bearing));
props->tie("radar/elevation-deg", SGRawValuePointer<double>(&elevation));
props->tie("radar/range-nm", SGRawValuePointer<double>(&range));
props->tie("radar/h-offset", SGRawValuePointer<double>(&horiz_offset));
props->tie("radar/v-offset", SGRawValuePointer<double>(&vert_offset));
props->tie("radar/x-shift", SGRawValuePointer<double>(&x_shift));
props->tie("radar/y-shift", SGRawValuePointer<double>(&y_shift));
props->tie("radar/rotation", SGRawValuePointer<double>(&rotation));
props->tie("radar/ht-diff-ft", SGRawValuePointer<double>(&ht_diff));
props->tie("radar/in-range", SGRawValuePointer<bool>(&in_range));
props->tie("radar/bearing-deg", SGRawValuePointer<double>(&bearing));
props->tie("radar/elevation-deg", SGRawValuePointer<double>(&elevation));
props->tie("radar/range-nm", SGRawValuePointer<double>(&range));
props->tie("radar/h-offset", SGRawValuePointer<double>(&horiz_offset));
props->tie("radar/v-offset", SGRawValuePointer<double>(&vert_offset));
props->tie("radar/x-shift", SGRawValuePointer<double>(&x_shift));
props->tie("radar/y-shift", SGRawValuePointer<double>(&y_shift));
props->tie("radar/rotation", SGRawValuePointer<double>(&rotation));
props->tie("radar/ht-diff-ft", SGRawValuePointer<double>(&ht_diff));
props->tie("subID", SGRawValuePointer<int>(&_subID));
props->tie("controls/lighting/nav-lights",
SGRawValueFunctions<bool>(_isNight));
props->setBoolValue("controls/lighting/beacon", true);
props->setBoolValue("controls/lighting/strobe", true);
props->setBoolValue("controls/glide-path", true);
props->tie("controls/lighting/nav-lights",
SGRawValueFunctions<bool>(_isNight));
props->setBoolValue("controls/lighting/beacon", true);
props->setBoolValue("controls/lighting/strobe", true);
props->setBoolValue("controls/glide-path", true);
props->setStringValue("controls/flight/lateral-mode", "roll");
props->setDoubleValue("controls/flight/target-hdg", hdg);
props->setDoubleValue("controls/flight/target-roll", roll);
props->setStringValue("controls/flight/lateral-mode", "roll");
props->setDoubleValue("controls/flight/target-hdg", hdg);
props->setDoubleValue("controls/flight/target-roll", roll);
props->setStringValue("controls/flight/longitude-mode", "alt");
props->setDoubleValue("controls/flight/target-alt", altitude_ft);
props->setDoubleValue("controls/flight/target-pitch", pitch);
props->setStringValue("controls/flight/longitude-mode", "alt");
props->setDoubleValue("controls/flight/target-alt", altitude_ft);
props->setDoubleValue("controls/flight/target-pitch", pitch);
props->setDoubleValue("controls/flight/target-spd", speed);
@ -306,137 +310,136 @@ void FGAIBase::unbind() {
}
double FGAIBase::UpdateRadar(FGAIManager* manager) {
bool force_on = fgGetBool("/instrumentation/radar/debug-mode", false);
double radar_range_nm = fgGetDouble("/instrumentation/radar/range");
double radar_range_ft2 = radar_range_nm;
radar_range_ft2 *= SG_NM_TO_METER * SG_METER_TO_FEET * 1.1; // + 10%
radar_range_ft2 *= radar_range_ft2;
double radar_range_ft2 = fgGetDouble("/instrumentation/radar/range");
bool force_on = fgGetBool("/instrumentation/radar/debug-mode", false);
radar_range_ft2 *= SG_NM_TO_METER * SG_METER_TO_FEET * 1.1; // + 10%
radar_range_ft2 *= radar_range_ft2;
double user_latitude = manager->get_user_latitude();
double user_longitude = manager->get_user_longitude();
double lat_range = fabs(pos.getLatitudeDeg() - user_latitude) * ft_per_deg_lat;
double lon_range = fabs(pos.getLongitudeDeg() - user_longitude) * ft_per_deg_lon;
double range_ft2 = lat_range*lat_range + lon_range*lon_range;
double user_latitude = manager->get_user_latitude();
double user_longitude = manager->get_user_longitude();
double lat_range = fabs(pos.getLatitudeDeg() - user_latitude) * ft_per_deg_lat;
double lon_range = fabs(pos.getLongitudeDeg() - user_longitude) * ft_per_deg_lon;
double range_ft2 = lat_range*lat_range + lon_range*lon_range;
//
// Test whether the target is within radar range.
//
in_range = (range_ft2 && (range_ft2 <= radar_range_ft2));
//
// Test whether the target is within radar range.
//
in_range = (range_ft2 && (range_ft2 <= radar_range_ft2));
if ( in_range || force_on ) {
props->setBoolValue("radar/in-range", true);
if ( in_range || force_on ) {
props->setBoolValue("radar/in-range", true);
// copy values from the AIManager
double user_altitude = manager->get_user_altitude();
double user_heading = manager->get_user_heading();
double user_pitch = manager->get_user_pitch();
//double user_yaw = manager->get_user_yaw();
//double user_speed = manager->get_user_speed();
// copy values from the AIManager
double user_altitude = manager->get_user_altitude();
double user_heading = manager->get_user_heading();
double user_pitch = manager->get_user_pitch();
//double user_yaw = manager->get_user_yaw();
//double user_speed = manager->get_user_speed();
// calculate range to target in feet and nautical miles
double range_ft = sqrt( range_ft2 );
range = range_ft / 6076.11549;
// calculate range to target in feet and nautical miles
double range_ft = sqrt( range_ft2 );
range = range_ft / 6076.11549;
// calculate bearing to target
if (pos.getLatitudeDeg() >= user_latitude) {
bearing = atan2(lat_range, lon_range) * SG_RADIANS_TO_DEGREES;
if (pos.getLongitudeDeg() >= user_longitude) {
bearing = 90.0 - bearing;
// calculate bearing to target
if (pos.getLatitudeDeg() >= user_latitude) {
bearing = atan2(lat_range, lon_range) * SG_RADIANS_TO_DEGREES;
if (pos.getLongitudeDeg() >= user_longitude) {
bearing = 90.0 - bearing;
} else {
bearing = 270.0 + bearing;
}
} else {
bearing = 270.0 + bearing;
bearing = atan2(lon_range, lat_range) * SG_RADIANS_TO_DEGREES;
if (pos.getLongitudeDeg() >= user_longitude) {
bearing = 180.0 - bearing;
} else {
bearing = 180.0 + bearing;
}
}
} else {
bearing = atan2(lon_range, lat_range) * SG_RADIANS_TO_DEGREES;
if (pos.getLongitudeDeg() >= user_longitude) {
bearing = 180.0 - bearing;
} else {
bearing = 180.0 + bearing;
}
}
// This is an alternate way to compute bearing and distance which
// agrees with the original scheme within about 0.1 degrees.
//
// Point3D start( user_longitude * SGD_DEGREES_TO_RADIANS,
// user_latitude * SGD_DEGREES_TO_RADIANS, 0 );
// Point3D dest( pos.getLongitudeRad(), pos.getLatitudeRad(), 0 );
// double gc_bearing, gc_range;
// calc_gc_course_dist( start, dest, &gc_bearing, &gc_range );
// gc_range *= SG_METER_TO_NM;
// gc_bearing *= SGD_RADIANS_TO_DEGREES;
// printf("orig b = %.3f %.2f gc b= %.3f, %.2f\n",
// bearing, range, gc_bearing, gc_range);
// This is an alternate way to compute bearing and distance which
// agrees with the original scheme within about 0.1 degrees.
//
// Point3D start( user_longitude * SGD_DEGREES_TO_RADIANS,
// user_latitude * SGD_DEGREES_TO_RADIANS, 0 );
// Point3D dest( pos.getLongitudeRad(), pos.getLatitudeRad(), 0 );
// double gc_bearing, gc_range;
// calc_gc_course_dist( start, dest, &gc_bearing, &gc_range );
// gc_range *= SG_METER_TO_NM;
// gc_bearing *= SGD_RADIANS_TO_DEGREES;
// printf("orig b = %.3f %.2f gc b= %.3f, %.2f\n",
// bearing, range, gc_bearing, gc_range);
// calculate look left/right to target, without yaw correction
horiz_offset = bearing - user_heading;
if (horiz_offset > 180.0) horiz_offset -= 360.0;
if (horiz_offset < -180.0) horiz_offset += 360.0;
// calculate look left/right to target, without yaw correction
horiz_offset = bearing - user_heading;
if (horiz_offset > 180.0) horiz_offset -= 360.0;
if (horiz_offset < -180.0) horiz_offset += 360.0;
// calculate elevation to target
elevation = atan2( altitude_ft - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES;
// calculate elevation to target
elevation = atan2( altitude_ft - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES;
// calculate look up/down to target
vert_offset = elevation - user_pitch;
// calculate look up/down to target
vert_offset = elevation - user_pitch;
/* this calculation needs to be fixed, but it isn't important anyway
// calculate range rate
double recip_bearing = bearing + 180.0;
if (recip_bearing > 360.0) recip_bearing -= 360.0;
double my_horiz_offset = recip_bearing - hdg;
if (my_horiz_offset > 180.0) my_horiz_offset -= 360.0;
if (my_horiz_offset < -180.0) my_horiz_offset += 360.0;
rdot = (-user_speed * cos( horiz_offset * SG_DEGREES_TO_RADIANS ))
+(-speed * 1.686 * cos( my_horiz_offset * SG_DEGREES_TO_RADIANS ));
*/
/* this calculation needs to be fixed, but it isn't important anyway
// calculate range rate
double recip_bearing = bearing + 180.0;
if (recip_bearing > 360.0) recip_bearing -= 360.0;
double my_horiz_offset = recip_bearing - hdg;
if (my_horiz_offset > 180.0) my_horiz_offset -= 360.0;
if (my_horiz_offset < -180.0) my_horiz_offset += 360.0;
rdot = (-user_speed * cos( horiz_offset * SG_DEGREES_TO_RADIANS ))
+(-speed * 1.686 * cos( my_horiz_offset * SG_DEGREES_TO_RADIANS ));
*/
// now correct look left/right for yaw
// horiz_offset += user_yaw; // FIXME: WHY WOULD WE WANT TO ADD IN SIDE-SLIP HERE?
// now correct look left/right for yaw
// horiz_offset += user_yaw; // FIXME: WHY WOULD WE WANT TO ADD IN SIDE-SLIP HERE?
// calculate values for radar display
y_shift = (range * cos( horiz_offset * SG_DEGREES_TO_RADIANS)) / radar_range_nm;
x_shift = (range * sin( horiz_offset * SG_DEGREES_TO_RADIANS)) / radar_range_nm;
rotation = hdg - user_heading;
if (rotation < 0.0) rotation += 360.0;
ht_diff = altitude_ft - user_altitude;
// calculate values for radar display
y_shift = range * cos( horiz_offset * SG_DEGREES_TO_RADIANS);
x_shift = range * sin( horiz_offset * SG_DEGREES_TO_RADIANS);
rotation = hdg - user_heading;
if (rotation < 0.0) rotation += 360.0;
ht_diff = altitude_ft - user_altitude;
}
}
return range_ft2;
return range_ft2;
}
/*
* Getters and Setters
*/
SGVec3d FGAIBase::getCartPosAt(const SGVec3d& _off) const {
// Transform that one to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
// The offset converted to the usual body fixed coordinate system
// rotated to the earth fiexed coordinates axis
SGVec3d off = hlTrans.backTransform(_off);
// The offset converted to the usual body fixed coordinate system
// rotated to the earth fiexed coordinates axis
SGVec3d off = hlTrans.backTransform(_off);
// Add the position offset of the AIModel to gain the earth centered position
SGVec3d cartPos = SGVec3d::fromGeod(pos);
// Add the position offset of the AIModel to gain the earth centered position
SGVec3d cartPos = SGVec3d::fromGeod(pos);
return cartPos + off;
return cartPos + off;
}
SGVec3d FGAIBase::getCartPos() const {
// Transform that one to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
// 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);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
SGVec3d cartPos = SGVec3d::fromGeod(pos);
SGVec3d cartPos = SGVec3d::fromGeod(pos);
return cartPos;
return cartPos;
}
double FGAIBase::_getCartPosX() const {
@ -462,14 +465,22 @@ void FGAIBase::_setLatitude ( double latitude ) {
pos.setLatitudeDeg(latitude);
}
void FGAIBase::_setSubID( int s ) {
_subID = s;
}
double FGAIBase::_getLongitude() const {
return pos.getLongitudeDeg();
}
double FGAIBase::_getLatitude () const {
double FGAIBase::_getLatitude() const {
return pos.getLatitudeDeg();
}
double FGAIBase::_getElevationFt () const {
return pos.getElevationFt();
}
double FGAIBase::_getRdot() const {
return rdot;
}
@ -510,10 +521,50 @@ bool FGAIBase::_isNight() {
return (fgGetFloat("/sim/time/sun-angle-rad") > 1.57);
}
bool FGAIBase::_getCollisionData() {
return _collision_reported;
}
bool FGAIBase::_getImpactData() {
return _impact_reported;
}
double FGAIBase::_getImpactLat() const {
return _impact_lat;
}
double FGAIBase::_getImpactLon() const {
return _impact_lon;
}
double FGAIBase::_getImpactElevFt() const {
return _impact_elev * SG_METER_TO_FEET;
}
double FGAIBase::_getImpactPitch() const {
return _impact_pitch;
}
double FGAIBase::_getImpactRoll() const {
return _impact_roll;
}
double FGAIBase::_getImpactHdg() const {
return _impact_hdg;
}
double FGAIBase::_getImpactSpeed() const {
return _impact_speed;
}
int FGAIBase::getID() const {
return _refID;
}
int FGAIBase::_getSubID() const {
return _subID;
}
double FGAIBase::_getSpeed() const {
return speed;
}
@ -534,20 +585,33 @@ const char* FGAIBase::_getPath() {
return _path.c_str();
}
const char* FGAIBase::_getName() {
return _name.c_str();
}
const char* FGAIBase::_getCallsign() {
return _callsign.c_str();
}
const char* FGAIBase::_getSubmodel() {
return _submodel.c_str();
}
void FGAIBase::CalculateMach() {
// Calculate rho at altitude, using standard atmosphere
// For the temperature T and the pressure p,
double altitude = altitude_ft;
if (altitude < 36152) { // curve fits for the troposphere
T = 59 - 0.00356 * altitude;
p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256);
T = 59 - 0.00356 * altitude;
p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256);
} else if ( 36152 < altitude && altitude < 82345 ) { // lower stratosphere
T = -70;
p = 473.1 * pow( e , 1.73 - (0.000048 * altitude) );
T = -70;
p = 473.1 * pow( e , 1.73 - (0.000048 * altitude) );
} else { // upper stratosphere
T = -205.05 + (0.00164 * altitude);
p = 51.97 * pow( ((T + 459.7) / 389.98) , -11.388);
T = -205.05 + (0.00164 * altitude);
p = 51.97 * pow( ((T + 459.7) / 389.98) , -11.388);
}
rho = p / (1718 * (T + 459.7));
@ -563,15 +627,15 @@ void FGAIBase::CalculateMach() {
// calculate Mach number
Mach = speed/a;
// cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach << endl;
// cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach << endl;
}
int FGAIBase::_newAIModelID() {
static int id = 0;
if (!++id)
id++; // id = 0 is not allowed.
if (!++id)
id++; // id = 0 is not allowed.
return id;
return id;
}

52
src/AIModel/AIBase.hxx
View file

@ -33,6 +33,7 @@
#include <Main/fg_props.hxx>
SG_USING_STD(string);
SG_USING_STD(list);
@ -73,8 +74,14 @@ public:
void setZoffset( double z_offset );
void setServiceable ( bool serviceable );
void setDie( bool die );
void setCollisionData( bool i, double lat, double lon, double elev );
void setImpactData( bool d );
void setImpactLat( double lat );
void setImpactLon( double lon );
void setImpactElev( double e );
int getID() const;
int _getSubID() const;
bool getDie();
@ -86,9 +93,14 @@ public:
double _getCartPosZ() const;
string _path;
string _callsign;
string _submodel;
string _name;
protected:
SGPropertyNode_ptr props;
SGPropertyNode_ptr trigger_node;
SGPropertyNode_ptr model_removed; // where to report model removal
FGAIManager* manager;
@ -137,11 +149,23 @@ protected:
bool invisible;
bool no_roll;
bool serviceable;
int _subID;
double life;
FGAIFlightPlan *fp;
bool _impact_reported;
bool _collision_reported;
double _impact_lat;
double _impact_lon;
double _impact_elev;
double _impact_hdg;
double _impact_pitch;
double _impact_roll;
double _impact_speed;
void Transform();
void CalculateMach();
double UpdateRadar(FGAIManager* manager);
@ -149,7 +173,7 @@ protected:
static int _newAIModelID();
private:
const int _refID;
int _refID;
object_type _otype;
public:
@ -163,13 +187,14 @@ public:
void _setAltitude( double _alt );
void _setLongitude( double longitude );
void _setLatitude ( double latitude );
void _setSubID( int s );
double _getVS_fps() const;
double _getAltitude() const;
double _getLongitude() const;
double _getLatitude () const;
double _getLatitude() const;
double _getBearing() const;
double _getElevation() const;
double _getElevationFt() const;
double _getRdot() const;
double _getH_offset() const;
double _getV_offset() const;
@ -182,12 +207,31 @@ public:
double _getHeading() const;
double _get_speed_east_fps() const;
double _get_speed_north_fps() const;
double _get_SubPath() const;
double _getImpactLat() const;
double _getImpactLon() const;
double _getImpactElevFt() const;
double _getImpactHdg() const;
double _getImpactPitch() const;
double _getImpactRoll() const;
double _getImpactSpeed() const;
//unsigned int _getCount() const;
bool _getServiceable() const;
bool _getFirstTime() const;
bool _getImpact();
bool _getImpactData();
bool _getCollisionData();
SGPropertyNode* _getProps() const;
const char* _getPath();
const char* _getCallsign();
const char* _getTriggerNode();
const char* _getName();
const char* _getSubmodel();
// These are used in the Mach number calculations
@ -226,7 +270,6 @@ inline void FGAIBase::setServiceable(bool s) {
serviceable = s;
}
inline void FGAIBase::setSpeed( double speed_KTAS ) {
speed = tgt_speed = speed_KTAS;
}
@ -256,6 +299,7 @@ inline void FGAIBase::setPitch( double newpitch ) {
inline void FGAIBase::setLongitude( double longitude ) {
pos.setLongitudeDeg( longitude );
}
inline void FGAIBase::setLatitude ( double latitude ) {
pos.setLatitudeDeg( latitude );
}

557
src/AIModel/AIManager.cxx
View file

@ -33,302 +33,395 @@
#include "AIStatic.hxx"
#include "AIMultiplayer.hxx"
#include <simgear/math/sg_geodesy.hxx>
FGAIManager::FGAIManager() {
_dt = 0.0;
mNumAiModels = 0;
for (unsigned i = 0; i < FGAIBase::MAX_OBJECTS; ++i)
mNumAiTypeModels[i] = 0;
_dt = 0.0;
mNumAiModels = 0;
for (unsigned i = 0; i < FGAIBase::MAX_OBJECTS; ++i)
mNumAiTypeModels[i] = 0;
}
FGAIManager::~FGAIManager() {
ai_list_iterator ai_list_itr = ai_list.begin();
while(ai_list_itr != ai_list.end()) {
(*ai_list_itr)->unbind();
++ai_list_itr;
}
}
ai_list_iterator ai_list_itr = ai_list.begin();
void FGAIManager::init() {
root = fgGetNode("sim/ai", true);
enabled = root->getNode("enabled", true)->getBoolValue();
if (!enabled)
return;
wind_from_down_node = fgGetNode("/environment/wind-from-down-fps", true);
wind_from_east_node = fgGetNode("/environment/wind-from-east-fps",true);
wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
user_latitude_node = fgGetNode("/position/latitude-deg", true);
user_longitude_node = fgGetNode("/position/longitude-deg", true);
user_altitude_node = fgGetNode("/position/altitude-ft", true);
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_speed_node = fgGetNode("/velocities/uBody-fps", true);
}
void FGAIManager::postinit() {
// postinit, so that it can access the Nasal subsystem
for(int i = 0 ; i < root->nChildren() ; i++) {
SGPropertyNode *aiEntry = root->getChild( i );
if( !strcmp( aiEntry->getName(), "scenario" ) ) {
scenario_filename = aiEntry->getStringValue();
if (!scenario_filename.empty())
processScenario( scenario_filename );
while(ai_list_itr != ai_list.end()) {
(*ai_list_itr)->unbind();
++ai_list_itr;
}
}
}
void
FGAIManager::init() {
root = fgGetNode("sim/ai", true);
void FGAIManager::reinit() {
update(0.0);
ai_list_iterator ai_list_itr = ai_list.begin();
enabled = root->getNode("enabled", true)->getBoolValue();
while(ai_list_itr != ai_list.end()) {
(*ai_list_itr)->reinit();
++ai_list_itr;
}
if (!enabled)
return;
wind_from_down_node = fgGetNode("/environment/wind-from-down-fps", true);
wind_from_east_node = fgGetNode("/environment/wind-from-east-fps",true);
wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
user_latitude_node = fgGetNode("/position/latitude-deg", true);
user_longitude_node = fgGetNode("/position/longitude-deg", true);
user_altitude_node = fgGetNode("/position/altitude-ft", true);
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_speed_node = fgGetNode("/velocities/uBody-fps", true);
}
void FGAIManager::bind() {
root = globals->get_props()->getNode("ai/models", true);
root->tie("count", SGRawValueMethods<FGAIManager, int>(*this,
&FGAIManager::getNumAiObjects));
void
FGAIManager::postinit() {
// postinit, so that it can access the Nasal subsystem
for(int i = 0 ; i < root->nChildren() ; i++) {
SGPropertyNode *aiEntry = root->getChild( i );
if( !strcmp( aiEntry->getName(), "scenario" ) ) {
scenario_filename = aiEntry->getStringValue();
if (!scenario_filename.empty())
processScenario( scenario_filename );
}
}
}
void
FGAIManager::reinit() {
update(0.0);
ai_list_iterator ai_list_itr = ai_list.begin();
void FGAIManager::unbind() {
while(ai_list_itr != ai_list.end()) {
(*ai_list_itr)->reinit();
++ai_list_itr;
}
}
void
FGAIManager::bind() {
root = globals->get_props()->getNode("ai/models", true);
root->tie("count", SGRawValueMethods<FGAIManager, int>(*this,
&FGAIManager::getNumAiObjects));
}
void
FGAIManager::unbind() {
root->untie("count");
}
void
FGAIManager::update(double dt) {
// initialize these for finding nearest thermals
range_nearest = 10000.0;
strength = 0.0;
void FGAIManager::update(double dt) {
// initialize these for finding nearest thermals
range_nearest = 10000.0;
strength = 0.0;
if (!enabled)
return;
if (!enabled)
return;
FGTrafficManager *tmgr = (FGTrafficManager*) globals->get_subsystem("Traffic Manager");
_dt = dt;
FGTrafficManager *tmgr = (FGTrafficManager*) globals->get_subsystem("Traffic Manager");
_dt = dt;
ai_list_iterator ai_list_itr = ai_list.begin();
while(ai_list_itr != ai_list.end()) {
if ((*ai_list_itr)->getDie()) {
tmgr->release((*ai_list_itr)->getID());
--mNumAiModels;
--(mNumAiTypeModels[(*ai_list_itr)->getType()]);
FGAIBase *base = *ai_list_itr;
SGPropertyNode *props = base->_getProps();
ai_list_iterator ai_list_itr = ai_list.begin();
props->setBoolValue("valid", false);
base->unbind();
while(ai_list_itr != ai_list.end()) {
// for backward compatibility reset properties, so that aircraft,
// which don't know the <valid> property, keep working
// TODO: remove after a while
props->setIntValue("id", -1);
props->setBoolValue("radar/in-range", false);
props->setIntValue("refuel/tanker", false);
if ((*ai_list_itr)->getDie()) {
tmgr->release((*ai_list_itr)->getID());
--mNumAiModels;
--(mNumAiTypeModels[(*ai_list_itr)->getType()]);
FGAIBase *base = *ai_list_itr;
SGPropertyNode *props = base->_getProps();
ai_list_itr = ai_list.erase(ai_list_itr);
} else {
fetchUserState();
if ((*ai_list_itr)->isa(FGAIBase::otThermal)) {
FGAIBase *base = *ai_list_itr;
processThermal((FGAIThermal*)base);
} else {
(*ai_list_itr)->update(_dt);
}
++ai_list_itr;
props->setBoolValue("valid", false);
base->unbind();
// for backward compatibility reset properties, so that aircraft,
// which don't know the <valid> property, keep working
// TODO: remove after a while
props->setIntValue("id", -1);
props->setBoolValue("radar/in-range", false);
props->setIntValue("refuel/tanker", false);
ai_list_itr = ai_list.erase(ai_list_itr);
} else {
fetchUserState();
if ((*ai_list_itr)->isa(FGAIBase::otThermal)) {
FGAIBase *base = *ai_list_itr;
processThermal((FGAIThermal*)base);
} else {
(*ai_list_itr)->update(_dt);
}
++ai_list_itr;
}
}
}
wind_from_down_node->setDoubleValue( strength ); // for thermals
wind_from_down_node->setDoubleValue( strength ); // for thermals
}
void
FGAIManager::attach(SGSharedPtr<FGAIBase> model)
{
unsigned idx = mNumAiTypeModels[model->getType()];
const char* typeString = model->getTypeString();
SGPropertyNode* root = globals->get_props()->getNode("ai/models", true);
SGPropertyNode* p;
int i;
for (i=0;i<10000;i++) //find free index in the property tree, if we have
//more than 10000 mp-aircrafts in the property tree we should optimize the mp-server
{
p = root->getNode(typeString, i, false);
if (!p || !p->getBoolValue("valid", false))
break;
if (p->getIntValue("id",-1)==model->getID()) {
p->setStringValue("callsign","***invalid node***"); //debug only, should never set!
//unsigned idx = mNumAiTypeModels[model->getType()];
const char* typeString = model->getTypeString();
SGPropertyNode* root = globals->get_props()->getNode("ai/models", true);
SGPropertyNode* p;
int i;
// find free index in the property tree, if we have
// more than 10000 mp-aircrafts in the property tree we should optimize the mp-server
for (i = 0; i < 10000; i++) {
p = root->getNode(typeString, i, false);
if (!p || !p->getBoolValue("valid", false))
break;
if (p->getIntValue("id",-1)==model->getID()) {
p->setStringValue("callsign","***invalid node***"); //debug only, should never set!
}
}
}
p = root->getNode(typeString, i, true);
model->setManager(this, p);
ai_list.push_back(model);
++mNumAiModels;
++(mNumAiTypeModels[model->getType()]);
model->init(model->getType()==FGAIBase::otAircraft
|| model->getType()==FGAIBase::otMultiplayer
|| model->getType()==FGAIBase::otStatic);
model->bind();
p->setBoolValue("valid", true);
p = root->getNode(typeString, i, true);
model->setManager(this, p);
ai_list.push_back(model);
++mNumAiModels;
++(mNumAiTypeModels[model->getType()]);
model->init(model->getType()==FGAIBase::otAircraft
|| model->getType()==FGAIBase::otMultiplayer
|| model->getType()==FGAIBase::otStatic);
model->bind();
p->setBoolValue("valid", true);
}
void
FGAIManager::destroyObject( int ID ) {
ai_list_iterator ai_list_itr = ai_list.begin();
while(ai_list_itr != ai_list.end()) {
if ((*ai_list_itr)->getID() == ID) {
--mNumAiModels;
--(mNumAiTypeModels[(*ai_list_itr)->getType()]);
(*ai_list_itr)->unbind();
ai_list_itr = ai_list.erase(ai_list_itr);
} else
++ai_list_itr;
}
void FGAIManager::destroyObject( int ID ) {
ai_list_iterator ai_list_itr = ai_list.begin();
while(ai_list_itr != ai_list.end()) {
if ((*ai_list_itr)->getID() == ID) {
--mNumAiModels;
--(mNumAiTypeModels[(*ai_list_itr)->getType()]);
(*ai_list_itr)->unbind();
ai_list_itr = ai_list.erase(ai_list_itr);
} else
++ai_list_itr;
}
}
int
FGAIManager::getNumAiObjects(void) const
{
return mNumAiModels;
return mNumAiModels;
}
void FGAIManager::fetchUserState( void ) {
user_latitude = user_latitude_node->getDoubleValue();
user_longitude = user_longitude_node->getDoubleValue();
user_altitude = user_altitude_node->getDoubleValue();
user_heading = user_heading_node->getDoubleValue();
user_pitch = user_pitch_node->getDoubleValue();
user_yaw = user_yaw_node->getDoubleValue();
user_speed = user_speed_node->getDoubleValue() * 0.592484;
wind_from_east = wind_from_east_node->getDoubleValue();
wind_from_north = wind_from_north_node->getDoubleValue();
void
FGAIManager::fetchUserState( void ) {
user_latitude = user_latitude_node->getDoubleValue();
user_longitude = user_longitude_node->getDoubleValue();
user_altitude = user_altitude_node->getDoubleValue();
user_heading = user_heading_node->getDoubleValue();
user_pitch = user_pitch_node->getDoubleValue();
user_yaw = user_yaw_node->getDoubleValue();
user_speed = user_speed_node->getDoubleValue() * 0.592484;
wind_from_east = wind_from_east_node->getDoubleValue();
wind_from_north = wind_from_north_node->getDoubleValue();
}
// only keep the results from the nearest thermal
void FGAIManager::processThermal( FGAIThermal* thermal ) {
thermal->update(_dt);
if ( thermal->_getRange() < range_nearest ) {
range_nearest = thermal->_getRange();
strength = thermal->getStrength();
}
void
FGAIManager::processThermal( FGAIThermal* thermal ) {
thermal->update(_dt);
if ( thermal->_getRange() < range_nearest ) {
range_nearest = thermal->_getRange();
strength = thermal->getStrength();
}
}
void
FGAIManager::processScenario( const string &filename ) {
void FGAIManager::processScenario( const string &filename ) {
SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename);
SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename);
if (!scenarioTop)
return;
SGPropertyNode* scenarios = scenarioTop->getChild("scenario");
if (!scenarios)
return;
if (!scenarioTop)
return;
for (int i = 0; i < scenarios->nChildren(); i++) {
SGPropertyNode* scEntry = scenarios->getChild(i);
if (strcmp(scEntry->getName(), "entry"))
continue;
std::string type = scEntry->getStringValue("type", "aircraft");
SGPropertyNode* scenarios = scenarioTop->getChild("scenario");
if (type == "aircraft") {
FGAIAircraft* aircraft = new FGAIAircraft;
aircraft->readFromScenario(scEntry);
attach(aircraft);
if (!scenarios)
return;
} else if (type == "ship") {
FGAIShip* ship = new FGAIShip;
ship->readFromScenario(scEntry);
attach(ship);
} else if (type == "carrier") {
FGAICarrier* carrier = new FGAICarrier;
carrier->readFromScenario(scEntry);
attach(carrier);
} else if (type == "thunderstorm") {
FGAIStorm* storm = new FGAIStorm;
storm->readFromScenario(scEntry);
attach(storm);
} else if (type == "thermal") {
FGAIThermal* thermal = new FGAIThermal;
thermal->readFromScenario(scEntry);
attach(thermal);
} else if (type == "ballistic") {
FGAIBallistic* ballistic = new FGAIBallistic;
ballistic->readFromScenario(scEntry);
attach(ballistic);
} else if (type == "static") {
FGAIStatic* aistatic = new FGAIStatic;
aistatic->readFromScenario(scEntry);
attach(aistatic);
for (int i = 0; i < scenarios->nChildren(); i++) {
SGPropertyNode* scEntry = scenarios->getChild(i);
if (strcmp(scEntry->getName(), "entry"))
continue;
std::string type = scEntry->getStringValue("type", "aircraft");
if (type == "aircraft") {
FGAIAircraft* aircraft = new FGAIAircraft;
aircraft->readFromScenario(scEntry);
attach(aircraft);
} else if (type == "ship") {
FGAIShip* ship = new FGAIShip;
ship->readFromScenario(scEntry);
attach(ship);
} else if (type == "carrier") {
FGAICarrier* carrier = new FGAICarrier;
carrier->readFromScenario(scEntry);
attach(carrier);
} else if (type == "thunderstorm") {
FGAIStorm* storm = new FGAIStorm;
storm->readFromScenario(scEntry);
attach(storm);
} else if (type == "thermal") {
FGAIThermal* thermal = new FGAIThermal;
thermal->readFromScenario(scEntry);
attach(thermal);
} else if (type == "ballistic") {
FGAIBallistic* ballistic = new FGAIBallistic;
ballistic->readFromScenario(scEntry);
attach(ballistic);
} else if (type == "static") {
FGAIStatic* aistatic = new FGAIStatic;
aistatic->readFromScenario(scEntry);
attach(aistatic);
}
}
}
}
SGPropertyNode_ptr
FGAIManager::loadScenarioFile(const std::string& filename)
{
SGPath path(globals->get_fg_root());
path.append("AI/" + filename + ".xml");
try {
SGPropertyNode_ptr root = new SGPropertyNode;
readProperties(path.str(), root);
return root;
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_ALERT, "Incorrect path specified for AI "
"scenario: \"" << path.str() << "\"");
return 0;
}
SGPath path(globals->get_fg_root());
path.append("AI/" + filename + ".xml");
try {
SGPropertyNode_ptr root = new SGPropertyNode;
readProperties(path.str(), root);
return root;
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_DEBUG, "Incorrect path specified for AI "
"scenario: \"" << path.str() << "\"");
return 0;
}
}
bool FGAIManager::getStartPosition(const string& id, const string& pid,
SGGeod& geodPos, double& hdng, SGVec3d& uvw)
bool
FGAIManager::getStartPosition(const string& id, const string& pid,
SGGeod& geodPos, double& hdng, SGVec3d& uvw)
{
bool found = false;
SGPropertyNode* root = fgGetNode("sim/ai", true);
if (!root->getNode("enabled", true)->getBoolValue())
return found;
bool found = false;
SGPropertyNode* root = fgGetNode("sim/ai", true);
if (!root->getNode("enabled", true)->getBoolValue())
return found;
for(int i = 0 ; (!found) && i < root->nChildren() ; i++) {
SGPropertyNode *aiEntry = root->getChild( i );
if( !strcmp( aiEntry->getName(), "scenario" ) ) {
string filename = aiEntry->getStringValue();
SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename);
if (scenarioTop) {
SGPropertyNode* scenarios = scenarioTop->getChild("scenario");
if (scenarios) {
for (int i = 0; i < scenarios->nChildren(); i++) {
SGPropertyNode* scEntry = scenarios->getChild(i);
std::string type = scEntry->getStringValue("type");
std::string pnumber = scEntry->getStringValue("pennant-number");
std::string name = scEntry->getStringValue("name");
if (type == "carrier" && (pnumber == id || name == id)) {
SGSharedPtr<FGAICarrier> carrier = new FGAICarrier;
carrier->readFromScenario(scEntry);
if (carrier->getParkPosition(pid, geodPos, hdng, uvw)) {
found = true;
break;
}
for (int i = 0 ; (!found) && i < root->nChildren() ; i++) {
SGPropertyNode *aiEntry = root->getChild( i );
if ( !strcmp( aiEntry->getName(), "scenario" ) ) {
string filename = aiEntry->getStringValue();
SGPropertyNode_ptr scenarioTop = loadScenarioFile(filename);
if (scenarioTop) {
SGPropertyNode* scenarios = scenarioTop->getChild("scenario");
if (scenarios) {
for (int i = 0; i < scenarios->nChildren(); i++) {
SGPropertyNode* scEntry = scenarios->getChild(i);
std::string type = scEntry->getStringValue("type");
std::string pnumber = scEntry->getStringValue("pennant-number");
std::string name = scEntry->getStringValue("name");
if (type == "carrier" && (pnumber == id || name == id)) {
SGSharedPtr<FGAICarrier> carrier = new FGAICarrier;
carrier->readFromScenario(scEntry);
if (carrier->getParkPosition(pid, geodPos, hdng, uvw)) {
found = true;
break;
}
}
}
}
}
}
}
}
}
}
return found;
return found;
}
const FGAIBase *
FGAIManager::calcCollision(double alt, double lat, double lon, double fuse_range)
{
// we specify tgt extent (ft) according to the AIObject type
double tgt_ht[] = {0, 50 ,100, 250, 0, 100, 0, 0, 50, 50};
double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100};
ai_list_iterator ai_list_itr = ai_list.begin();
ai_list_iterator end = ai_list.end();
while (ai_list_itr != end) {
double tgt_alt = (*ai_list_itr)->_getAltitude();
int type = (*ai_list_itr)->getType();
tgt_ht[type] += fuse_range;
if (fabs(tgt_alt - alt) > tgt_ht[type] || type == FGAIBase::otBallistic
|| type == FGAIBase::otStorm || type == FGAIBase::otThermal) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: skipping "
<< fabs(tgt_alt - alt)
<< " "
<< type
);
++ai_list_itr;
continue;
}
double tgt_lat = (*ai_list_itr)->_getLatitude();
double tgt_lon = (*ai_list_itr)->_getLongitude();
int id = (*ai_list_itr)->getID();
double range = calcRange(lat, lon, tgt_lat, tgt_lon);
SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: AI list size "
<< ai_list.size()
<< " type " << type
<< " ID " << id
<< " range " << range
//<< " bearing " << bearing
<< " alt " << tgt_alt
);
tgt_length[type] += fuse_range;
if (range < tgt_length[type]){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: HIT! "
<< " type " << type
<< " ID " << id
<< " range " << range
<< " alt " << tgt_alt
);
return *ai_list_itr;
}
++ai_list_itr;
}
return 0;
}
double
FGAIManager::calcRange(double lat, double lon, double lat2, double lon2) const
{
double course, az2, distance;
//calculate the bearing and range of the second pos from the first
geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance);
distance *= SG_METER_TO_FEET;
return distance;
}
//end AIManager.cxx

14
src/AIModel/AIManager.hxx
View file

@ -52,7 +52,10 @@ public:
ai_list_type ai_list;
inline const list <SGSharedPtr<FGAIBase> >& get_ai_list() const { return ai_list; }
inline const list <SGSharedPtr<FGAIBase> >& get_ai_list() const {
SG_LOG(SG_GENERAL, SG_DEBUG, "AI Manager: AI model return list size " << ai_list.size());
return ai_list;
}
FGAIManager();
~FGAIManager();
@ -63,10 +66,10 @@ public:
void bind();
void unbind();
void update(double dt);
void attach(SGSharedPtr<FGAIBase> model);
void destroyObject( int ID );
const FGAIBase *calcCollision(double alt, double lat, double lon, double fuse_range);
inline double get_user_latitude() const { return user_latitude; }
inline double get_user_longitude() const { return user_longitude; }
@ -93,6 +96,8 @@ private:
int mNumAiTypeModels[FGAIBase::MAX_OBJECTS];
int mNumAiModels;
double calcRange(double lat, double lon, double lat2, double lon2)const;
SGPropertyNode_ptr root;
SGPropertyNode_ptr wind_from_down_node;
SGPropertyNode_ptr user_latitude_node;
@ -102,8 +107,8 @@ private:
SGPropertyNode_ptr user_pitch_node;
SGPropertyNode_ptr user_yaw_node;
SGPropertyNode_ptr user_speed_node;
SGPropertyNode_ptr wind_from_east_node ;
SGPropertyNode_ptr wind_from_north_node ;
SGPropertyNode_ptr wind_from_east_node;
SGPropertyNode_ptr wind_from_north_node;
string scenario_filename;
@ -117,6 +122,7 @@ private:
double wind_from_east;
double wind_from_north;
double _dt;
void fetchUserState( void );
// used by thermals

418
src/AIModel/submodel.cxx
View file

@ -17,6 +17,7 @@
#include <Main/fg_props.hxx>
#include <Main/util.hxx>
#include "AIBase.hxx"
#include "AIManager.hxx"
#include "AIBallistic.hxx"
@ -35,10 +36,12 @@ FGSubmodelMgr::FGSubmodelMgr()
in[3] = out[3] = 1;
string contents_node;
contrail_altitude = 30000;
_count = 0;
}
FGSubmodelMgr::~FGSubmodelMgr()
{}
{
}
void FGSubmodelMgr::init()
{
@ -79,6 +82,10 @@ void FGSubmodelMgr::init()
void FGSubmodelMgr::postinit() {
// postinit, so that the AI list is populated
loadAI();
loadSubmodels();
//TODO reload submodels if an MP ac joins
}
void FGSubmodelMgr::bind()
@ -87,7 +94,6 @@ void FGSubmodelMgr::bind()
void FGSubmodelMgr::unbind()
{
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
(*submodel_iterator)->prop->untie("count");
++submodel_iterator;
@ -97,31 +103,89 @@ void FGSubmodelMgr::unbind()
void FGSubmodelMgr::update(double dt)
{
if (!(_serviceable_node->getBoolValue()))
if (!_serviceable_node->getBoolValue())
return;
int i = -1;
bool in_range = true;
bool trigger = false;
_impact = false;
_hit = false;
sm_list = ai->get_ai_list();
sm_list_iterator sm_list_itr = sm_list.begin();
sm_list_iterator end = sm_list.end();
submodel_iterator = submodels.begin();
while (sm_list_itr != end) {
_impact = (*sm_list_itr)->_getImpactData();
_hit = (*sm_list_itr)->_getCollisionData();
int parent_subID = (*sm_list_itr)->_getSubID();
if ( parent_subID == 0) { // this entry in the list has no associated submodel
++sm_list_itr; // so we can continue
continue;
}
if (_impact || _hit) {
SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit );
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
int child_ID = (*submodel_iterator)->id;
cout << "Impact: parent SubID " << parent_subID << " child_ID " << child_ID << endl;
if ( parent_subID == child_ID ) {
_parent_lat = (*sm_list_itr)->_getImpactLat();
_parent_lon = (*sm_list_itr)->_getImpactLon();
_parent_elev = (*sm_list_itr)->_getImpactElevFt();
_parent_hdg = (*sm_list_itr)->_getImpactHdg();
_parent_pitch = (*sm_list_itr)->_getImpactPitch();
_parent_roll = (*sm_list_itr)->_getImpactRoll();
_parent_speed = (*sm_list_itr)->_getImpactSpeed();
(*submodel_iterator)->first_time = true;
if (release(submodel_iterator, dt)) {
(*sm_list_itr)->setDie(true);
}
}
++submodel_iterator;
}
}
++sm_list_itr;
}
_contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude);
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
while (submodel_iterator != submodels.end()) {
i++;
in_range = true;
if ((*submodel_iterator)->trigger_node != 0) {
trigger = (*submodel_iterator)->trigger_node->getBoolValue();
//cout << (*submodel_iterator)->name << "trigger node found" << trigger << endl;
_trigger_node = (*submodel_iterator)->trigger_node;
trigger = _trigger_node->getBoolValue();
//cout << "trigger node found " << trigger << endl;
} else {
trigger = true;
//cout << (*submodel_iterator)->name << "trigger node not found" << trigger << endl;
//cout << (*submodel_iterator)->name << "trigger node not found " << trigger << endl;
}
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: " << (*submodel_iterator)->id
<< " name " << (*submodel_iterator)->name
<< " in range " << in_range
);
if (trigger) {
int id = (*submodel_iterator)->id;
string name = (*submodel_iterator)->name;
// don't release submodels from AI Objects if they are
// too far away to be seen. id 0 is not an AI model,
// so we can skip the whole process
@ -132,12 +196,13 @@ void FGSubmodelMgr::update(double dt)
if (id == 0) {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: continuing: " << id);
"Submodels: continuing: " << id << " name " << name );
in_range = true;
++sm_list_itr;
continue;
}
int parent_id = (*sm_list_itr)->getID();
int parent_id = (*submodel_iterator)->id;
if (parent_id == id) {
double parent_lat = (*sm_list_itr)->_getLatitude();
@ -145,8 +210,8 @@ void FGSubmodelMgr::update(double dt)
double own_lat = _user_lat_node->getDoubleValue();
double own_lon = _user_lon_node->getDoubleValue();
double range_nm = getRange(parent_lat, parent_lon, own_lat, own_lon);
/* cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl;
cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl;*/
// cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl;
//cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl;
if (range_nm > 15) {
SG_LOG(SG_GENERAL, SG_DEBUG,
@ -159,8 +224,15 @@ void FGSubmodelMgr::update(double dt)
++sm_list_itr;
} // end while
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels end: " << (*submodel_iterator)->id
<< " name " << (*submodel_iterator)->name
<< " count " << (*submodel_iterator)->count
<< " in range " << in_range
);
if ((*submodel_iterator)->count != 0 && in_range)
release((*submodel_iterator), dt);
release((submodel_iterator), dt);
} else
(*submodel_iterator)->first_time = true;
@ -170,28 +242,36 @@ void FGSubmodelMgr::update(double dt)
}
bool FGSubmodelMgr::release(submodel* sm, double dt)
bool FGSubmodelMgr::release(submodel_vector_iterator sm, double dt)
{
//cout << "release id " << (*sm)->id << " name " << (*sm)->name
//<< " first time " << (*sm)->first_time << " repeat " << (*sm)->repeat <<
// endl;
// only run if first time or repeat is set to true
if (!sm->first_time && !sm->repeat)
if (!(*sm)->first_time && !(*sm)->repeat){
//cout<< "not first time " << (*sm)->first_time<< " repeat " << (*sm)->repeat <<endl;
return false;
}
sm->timer += dt;
(*sm)->timer += dt;
if (sm->timer < sm->delay)
if ((*sm)->timer < (*sm)->delay){
//cout << "not yet: timer" << (*sm)->timer << " delay " << (*sm)->delay<< endl;
return false;
}
sm->timer = 0.0;
(*sm)->timer = 0.0;
if (sm->first_time) {
if ((*sm)->first_time) {
dt = 0.0;
sm->first_time = false;
(*sm)->first_time = false;
}
transform(sm); // calculate submodel's initial conditions in world-coordinates
FGAIBallistic* ballist = new FGAIBallistic;
ballist->setPath(sm->model.c_str());
ballist->setPath((*sm)->model.c_str());
ballist->setLatitude(IC.lat);
ballist->setLongitude(IC.lon);
ballist->setAltitude(IC.alt);
@ -202,20 +282,24 @@ bool FGSubmodelMgr::release(submodel* sm, double dt)
ballist->setWind_from_east(IC.wind_from_east);
ballist->setWind_from_north(IC.wind_from_north);
ballist->setMass(IC.mass);
ballist->setDragArea(sm->drag_area);
ballist->setLife(sm->life);
ballist->setBuoyancy(sm->buoyancy);
ballist->setWind(sm->wind);
ballist->setCd(sm->cd);
ballist->setStabilisation(sm->aero_stabilised);
ballist->setNoRoll(sm->no_roll);
ballist->setName(sm->name);
ballist->setImpact(sm->impact);
ballist->setImpactReportNode(sm->impact_reports);
ballist->setDragArea((*sm)->drag_area);
ballist->setLife((*sm)->life);
ballist->setBuoyancy((*sm)->buoyancy);
ballist->setWind((*sm)->wind);
ballist->setCd((*sm)->cd);
ballist->setStabilisation((*sm)->aero_stabilised);
ballist->setNoRoll((*sm)->no_roll);
ballist->setName((*sm)->name);
ballist->setCollision((*sm)->collision);
ballist->setImpact((*sm)->impact);
ballist->setImpactReportNode((*sm)->impact_report);
ballist->setFuseRange((*sm)->fuse_range);
ballist->setSubmodel((*sm)->submodel.c_str());
ballist->setSubID((*sm)->sub_id);
ai->attach(ballist);
if (sm->count > 0)
(sm->count)--;
if ((*sm)->count > 0)
(*sm)->count--;
return true;
}
@ -235,28 +319,58 @@ void FGSubmodelMgr::load()
}
void FGSubmodelMgr::transform(submodel* sm)
void FGSubmodelMgr::transform(submodel_vector_iterator submodel)
{
// get initial conditions
if (sm->contents_node != 0) {
// set initial conditions
if ((*submodel)->contents_node != 0) {
// get the weight of the contents (lbs) and convert to mass (slugs)
sm->contents = sm->contents_node->getDoubleValue();
IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;
(*submodel)->contents = (*submodel)->contents_node->getDoubleValue();
IC.mass = ((*submodel)->weight + (*submodel)->contents) * lbs_to_slugs;
// set contents to 0 in the parent
sm->contents_node->setDoubleValue(0);
(*submodel)->contents_node->setDoubleValue(0);
} else
IC.mass = sm->weight * lbs_to_slugs;
IC.mass = (*submodel)->weight * lbs_to_slugs;
//cout << "mass " << IC.mass << endl;
// cout << "mass " << IC.mass << endl;
if (sm->speed_node != 0)
sm->speed = sm->speed_node->getDoubleValue();
if ((*submodel)->speed_node != 0)
(*submodel)->speed = (*submodel)->speed_node->getDoubleValue();
int ind = sm->id;
int id = (*submodel)->id;
//int sub_id = (*submodel)->sub_id;
string name = (*submodel)->name;
if (ind == 0) {
// set the data for a submodel tied to the main model
//cout << " name " << name << " id " << id << " sub id" << sub_id << endl;
if (_impact || _hit){
// set the data for a submodel tied to a submodel
_count++;
//cout << "Submodels: release sub sub " << _count<< endl;
//cout << " id " << (*submodel)->id
// << " lat " << _parent_lat
// << " lon " << _parent_lon
// << " elev " << _parent_elev
// << " name " << (*submodel)->name
// << endl;
IC.lat = _parent_lat;
IC.lon = _parent_lon;
IC.alt = _parent_elev;
IC.roll = _parent_roll; // rotation about x axis
IC.elevation = _parent_pitch; // rotation about y axis
IC.azimuth = _parent_hdg; // rotation about z axis
IC.speed = _parent_speed;
IC.speed_down_fps = 0;
IC.speed_east_fps = 0;
IC.speed_north_fps = 0;
} else if (id == 0) {
//set the data for a submodel tied to the main model
/*cout << "Submodels: release main sub " << endl;
cout << " name " << (*submodel)->name
<< " id" << (*submodel)->id
<< endl;*/
IC.lat = _user_lat_node->getDoubleValue();
IC.lon = _user_lon_node->getDoubleValue();
IC.alt = _user_alt_node->getDoubleValue();
@ -274,9 +388,9 @@ void FGSubmodelMgr::transform(submodel* sm)
sm_list_iterator end = sm_list.end();
while (sm_list_itr != end) {
int id = (*sm_list_itr)->getID();
int parent_id = (*sm_list_itr)->getID();
if (ind != id) {
if (id != parent_id) {
++sm_list_itr;
continue;
}
@ -303,14 +417,14 @@ void FGSubmodelMgr::transform(submodel* sm)
cout << "speed down " << IC.speed_down_fps << endl ;
cout << "speed east " << IC.speed_east_fps << endl ;
cout << "speed north " << IC.speed_north_fps << endl ;
cout << "parent speed fps in" << IC.speed << "sm speed in " << sm->speed << endl ;*/
cout << "parent speed fps in" << IC.speed << "sm speed in " << (*submodel).speed << endl ;*/
IC.wind_from_east = _user_wind_from_east_node->getDoubleValue();
IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
in[0] = sm->x_offset;
in[1] = sm->y_offset;
in[2] = sm->z_offset;
in[0] = (*submodel)->x_offset;
in[1] = (*submodel)->y_offset;
in[2] = (*submodel)->z_offset;
// pre-process the trig functions
cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
@ -357,43 +471,44 @@ void FGSubmodelMgr::transform(submodel* sm)
// Get submodel initial velocity vector angles in XZ and XY planes.
// This needs to be fixed. This vector should be added to aircraft's vector.
IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
IC.azimuth += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
IC.elevation += ((*submodel)->yaw_offset * sinRx) + ((*submodel)->pitch_offset * cosRx);
IC.azimuth += ((*submodel)->yaw_offset * cosRx) - ((*submodel)->pitch_offset * sinRx);
// calcuate the total speed north
IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
// calculate the total speed north
IC.total_speed_north = (*submodel)->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
// calculate the total speed east
IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
IC.total_speed_east = (*submodel)->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
// calculate the total speed down
IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS)
+ IC.speed_down_fps;
IC.total_speed_down = (*submodel)->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS)
+ IC.speed_down_fps;
// re-calculate speed, elevation and azimuth
IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north
+ IC.total_speed_east * IC.total_speed_east
+ IC.total_speed_down * IC.total_speed_down);
//cout << " speed fps out" << IC.speed << endl ;
IC.azimuth = atan(IC.total_speed_east / IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
// if speeds are low this calculation can become unreliable
if (IC.speed > 1){
IC.azimuth = atan2(IC.total_speed_east , IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
// cout << "azimuth1 " << IC.azimuth<<endl;
// rationalise the output
if (IC.total_speed_north <= 0) {
IC.azimuth = 180 + IC.azimuth;
} else {
if (IC.total_speed_east <= 0)
IC.azimuth = 360 + IC.azimuth;
// rationalise the output
if (IC.azimuth < 0)
IC.azimuth += 360;
else if (IC.azimuth >= 360)
IC.azimuth -= 360;
}
IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
* IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
* SG_RADIANS_TO_DEGREES;
// cout << "azimuth2 " << IC.azimuth<<endl;
IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
* IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
* SG_RADIANS_TO_DEGREES;
}
void FGSubmodelMgr::updatelat(double lat)
@ -435,7 +550,6 @@ void FGSubmodelMgr::loadAI()
double FGSubmodelMgr::getRange(double lat, double lon, double lat2, double lon2) const
{
double course, distance, az2;
//calculate the bearing and range of the second pos from the first
@ -450,7 +564,8 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
SGPath config(globals->get_fg_root());
config.append(path);
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: path " << path);
try {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: Trying to read AI submodels file: " << config.str());
@ -489,10 +604,13 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
sm->weight = entry_node->getDoubleValue("weight", 0.25);
sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
sm->no_roll = entry_node->getBoolValue("no-roll", false);
sm->collision = entry_node->getBoolValue("collision", false);
sm->impact = entry_node->getBoolValue("impact", false);
sm->impact_reports = entry_node->getStringValue("impact-reports");
sm->impact_report = entry_node->getStringValue("impact-reports");
sm->fuse_range = entry_node->getDoubleValue("fuse-range", 0.0);
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false);
sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
sm->submodel = entry_node->getStringValue("submodel-path", "");
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
@ -513,15 +631,22 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
sm->id = id;
sm->first_time = false;
sm->serviceable = serviceable;
sm->sub_id = 0;
sm->prop = fgGetNode("/ai/submodels/submodel", index, true);
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
string submodel = sm->submodel;
sm->prop->setStringValue("submodel", submodel.c_str());
//cout << " set submodel path " << submodel << endl;
if (sm->contents_node != 0)
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
@ -529,8 +654,147 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
submodels.push_back(sm);
}
submodel_iterator = submodels.begin();
}
void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable)
{
SGPropertyNode root;
SGPath config(globals->get_fg_root());
config.append(path);
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: path " << path);
try {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: Trying to read AI submodels file: " << config.str());
readProperties(config.str(), &root);
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: Unable to read AI submodels file: " << config.str());
return;
}
vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
vector<SGPropertyNode_ptr>::iterator it = children.begin();
vector<SGPropertyNode_ptr>::iterator end = children.end();
for (int i = 0; it != end; ++it, i++) {
//cout << "Reading AI submodel " << (*it)->getPath() << endl;
submodel* sm = new submodel;
SGPropertyNode * entry_node = *it;
sm->name = entry_node->getStringValue("name", "none_defined");
sm->model = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
sm->speed = entry_node->getDoubleValue("speed", 2329.4);
sm->repeat = entry_node->getBoolValue("repeat", false);
sm->delay = entry_node->getDoubleValue("delay", 0.25);
sm->count = entry_node->getIntValue("count", 1);
sm->slaved = entry_node->getBoolValue("slaved", false);
sm->x_offset = entry_node->getDoubleValue("x-offset", 0.0);
sm->y_offset = entry_node->getDoubleValue("y-offset", 0.0);
sm->z_offset = entry_node->getDoubleValue("z-offset", 0.0);
sm->yaw_offset = entry_node->getDoubleValue("yaw-offset", 0.0);
sm->pitch_offset = entry_node->getDoubleValue("pitch-offset", 0.0);
sm->drag_area = entry_node->getDoubleValue("eda", 0.034);
sm->life = entry_node->getDoubleValue("life", 900.0);
sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0);
sm->wind = entry_node->getBoolValue("wind", false);
sm->cd = entry_node->getDoubleValue("cd", 0.193);
sm->weight = entry_node->getDoubleValue("weight", 0.25);
sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
sm->no_roll = entry_node->getBoolValue("no-roll", false);
sm->collision = entry_node->getBoolValue("collision", false);
sm->impact = entry_node->getBoolValue("impact", false);
sm->impact_report = entry_node->getStringValue("impact-reports");
sm->fuse_range = entry_node->getDoubleValue("fuse-range", 0.0);
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false);
sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
sm->submodel = entry_node->getStringValue("submodel-path", "");
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
sm->contents = sm->contents_node->getDoubleValue();
const char *trigger_path = entry_node->getStringValue("trigger", 0);
if (trigger_path) {
sm->trigger_node = fgGetNode(trigger_path, true);
sm->trigger_node->setBoolValue(sm->trigger_node->getBoolValue());
} else {
sm->trigger_node = 0;
}
if (sm->speed_node != 0)
sm->speed = sm->speed_node->getDoubleValue();
sm->timer = sm->delay;
sm->id = index;
sm->first_time = false;
sm->serviceable = serviceable;
sm->sub_id = 0;
sm->prop = fgGetNode("/ai/submodels/subsubmodel", index, true);
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
string submodel = sm->submodel;
sm->prop->setStringValue("submodel", submodel.c_str());
// cout << " set submodel path " << submodel<< endl;
if (sm->contents_node != 0)
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
index++;
subsubmodels.push_back(sm);
}
}
void FGSubmodelMgr::loadSubmodels()
{
SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Loading sub submodels");
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
string submodel = (*submodel_iterator)->submodel;
if (!submodel.empty()) {
//int id = (*submodel_iterator)->id;
bool serviceable = true;
SG_LOG(SG_GENERAL, SG_ALERT, "found path sub sub "
<< submodel
<< " index " << index
<< "name " << (*submodel_iterator)->name);
(*submodel_iterator)->sub_id = index;
setSubData(index, submodel, serviceable);
}
++submodel_iterator;
}
subsubmodel_iterator = subsubmodels.begin();
while (subsubmodel_iterator != subsubmodels.end()) {
submodels.push_back(*subsubmodel_iterator);
++subsubmodel_iterator;
}
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
int id = (*submodel_iterator)->id;
SG_LOG(SG_GENERAL, SG_DEBUG,"after pusback "
<< " id " << id
<< " name " << (*submodel_iterator)->name
<< " sub id " << (*submodel_iterator)->sub_id);
++submodel_iterator;
}
}
// end of submodel.cxx

39
src/AIModel/submodel.hxx
View file

@ -16,8 +16,12 @@
#include <AIModel/AIBase.hxx>
#include <vector>
#include <string>
#include <Main/fg_props.hxx>
SG_USING_STD(vector);
SG_USING_STD(string);
SG_USING_STD(list);
class FGAIBase;
@ -59,8 +63,12 @@ public:
int id;
bool no_roll;
bool serviceable;
bool collision;
bool impact;
string impact_reports;
string impact_report;
double fuse_range;
string submodel;
int sub_id;
}
submodel;
@ -84,7 +92,6 @@ public:
double mass;
int id;
bool no_roll;
bool impact;
}
IC_struct;
@ -97,8 +104,6 @@ public:
void bind();
void unbind();
void update(double dt);
bool release(submodel* sm, double dt);
void transform(submodel* sm);
void updatelat(double lat);
private:
@ -107,7 +112,8 @@ private:
typedef submodel_vector_type::const_iterator submodel_vector_iterator;
submodel_vector_type submodels;
submodel_vector_iterator submodel_iterator;
submodel_vector_type subsubmodels;
submodel_vector_iterator submodel_iterator, subsubmodel_iterator;
float trans[3][3];
float in[3];
@ -129,10 +135,21 @@ private:
double x_offset, y_offset, z_offset;
double pitch_offset, yaw_offset;
double _parent_lat;
double _parent_lon;
double _parent_elev;
double _parent_hdg;
double _parent_pitch;
double _parent_roll;
double _parent_speed;
static const double lbs_to_slugs; //conversion factor
double contrail_altitude;
bool _impact;
bool _hit;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _user_lat_node;
SGPropertyNode_ptr _user_lon_node;
@ -150,6 +167,9 @@ private:
SGPropertyNode_ptr _user_speed_north_fps_node;
SGPropertyNode_ptr _contrail_altitude_node;
SGPropertyNode_ptr _contrail_trigger;
SGPropertyNode_ptr _count_node;
SGPropertyNode_ptr _trigger_node;
SGPropertyNode_ptr props;
FGAIManager* ai;
IC_struct IC;
@ -161,11 +181,20 @@ private:
sm_list_type sm_list;
void loadAI();
void loadSubmodels();
void setData(int id, string& path, bool serviceable);
void setSubData(int id, string& path, bool serviceable);
void valueChanged (SGPropertyNode *);
void transform(submodel_vector_iterator);
bool release(submodel_vector_iterator, double dt);
double getRange(double lat, double lon, double lat2, double lon2) const;
int _count;
};
#endif // __SYSTEMS_SUBMODEL_HXX