diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index c42d0d775..556f20fc6 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -48,7 +48,9 @@ FGAIBallistic::FGAIBallistic() : _solid(false), _report_collision(false), _report_impact(false), - _impact_report_node(fgGetNode("/ai/models/model-impact", true)) + _impact_report_node(fgGetNode("/ai/models/model-impact", true)), + _external_force(false) + { no_roll = false; } @@ -192,7 +194,12 @@ void FGAIBallistic::setCollision(bool c) { _report_collision = c; } +void FGAIBallistic::setExternalForce(bool f) { + _external_force = f; +} + void FGAIBallistic::setImpactReportNode(const string& path) { + if (!path.empty()) _impact_report_node = fgGetNode(path.c_str(), true); } @@ -218,6 +225,16 @@ void FGAIBallistic::setSubmodel(const string& s) { _submodel = s; } +void FGAIBallistic::setForcePath(const string& p) { + _force_path = p; + if (!_force_path.empty()) { + SGPropertyNode *fnode = fgGetNode(_force_path.c_str(), 0, true ); + _force_node = fnode->getChild("force-lb", 0, true); + _force_azimuth_node = fnode->getChild("force-azimuth-deg", 0, true); + _force_elevation_node = fnode->getChild("force-elevation-deg", 0, true); + } +} + void FGAIBallistic::Run(double dt) { _life_timer += dt; //cout << "life timer" <<_name <<" " << _life_timer << dt << endl; @@ -263,9 +280,13 @@ void FGAIBallistic::Run(double dt) { hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps; } + //resolve horizontal speed into north and east components: + double speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs; + double speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs; + // convert horizontal speed (fps) to degrees per second - double speed_north_deg_sec = cos(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lat; - double speed_east_deg_sec = sin(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lon; + double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat; + double speed_east_deg_sec = speed_east_fps / ft_per_deg_lon; // if wind not required, set to zero if (!_wind) { @@ -273,38 +294,108 @@ void FGAIBallistic::Run(double dt) { _wind_from_east = 0; } - // convert wind speed (fps) to degrees per second + // convert wind speed (fps) to degrees lat/lon per second double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat; double wind_speed_from_east_deg_sec = _wind_from_east / ft_per_deg_lon; + //calculate velocity due to external force + double force_speed_north_deg_sec = 0; + double force_speed_east_deg_sec = 0; + double vs_force_fps = 0; + double hs_force_fps = 0; + double v_force_acc_fpss = 0; + double force_speed_north_fps = 0; + double force_speed_east_fps = 0; + + if (_external_force) { + SGPropertyNode *n = fgGetNode(_force_path.c_str(), true); + double force_lbs = n->getChild("force-lb", 0, true)->getDoubleValue(); + double force_elevation_deg = n->getChild("force-elevation-deg", 0, true)->getDoubleValue(); + double force_azimuth_deg = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue(); + + //resolve force into vertical and horizontal components: + double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS ); + double h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS ); + + //acceleration = (force(lbsf)/mass(slugs)) + v_force_acc_fpss = (v_force_lbs/_mass); + double h_force_acc_fpss = (h_force_lbs/_mass); + + // velocity = acceleration * dt + hs_force_fps = h_force_acc_fpss * dt; + + //resolve horizontal speed into north and east components: + 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; + + // convert horizontal speed (fps) to degrees per second + double force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat; + double force_speed_east_deg_sec = force_speed_east_fps / ft_per_deg_lon; + + //recombine the horizontal velocity components + hs = sqrt(((speed_north_fps + force_speed_north_fps) * (speed_north_fps + force_speed_north_fps)) + + ((speed_east_fps + force_speed_east_fps) * (speed_east_fps + force_speed_east_fps))); + + /*cout << "mass " << _mass + << " force " << force_lbs + << " elevation " << force_elevation_deg + << " azimuth " << force_azimuth_deg + << endl; */ + + //cout << " _hs_force_fps " << hs_force_fps + //<< " force_speed_north_fps " << force_speed_north_fps + //<< " force_speed_east_fps " << force_speed_east_fps + //<< " speed_north_fps " << speed_north_fps + //<< " speed_east_fps " << speed_east_fps + //<< endl; + } + // set new position pos.setLatitudeDeg( pos.getLatitudeDeg() - + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt ); + + (speed_north_deg_sec - wind_speed_from_north_deg_sec + force_speed_north_deg_sec) * dt ); pos.setLongitudeDeg( pos.getLongitudeDeg() - + (speed_east_deg_sec - wind_speed_from_east_deg_sec) * dt ); + + (speed_east_deg_sec - wind_speed_from_east_deg_sec + force_speed_east_deg_sec) * dt ); - // adjust vertical speed for acceleration of gravity and buoyancy - vs -= (_gravity - _buoyancy) * dt; + // adjust vertical speed for acceleration of gravity, buoyancy, and vertical force + //v_force_acc_fpss = 0; + vs -= (_gravity - _buoyancy - v_force_acc_fpss) * dt; - // adjust altitude (feet) + // adjust altitude (feet) and set new elevation altitude_ft += vs * dt; pos.setElevationFt(altitude_ft); - // recalculate elevation (velocity vector) if aerostabilized - /*cout << _name << ": " << "aero_stabilised " << _aero_stabilised - << " pitch " << pitch <<" vs " << vs <<endl ;*/ + // 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; + + // rationalise azimuth + if (_azimuth < 0) _azimuth += 360; if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter const double coeff = 0.9; double c = dt / (coeff + dt); - //cout << "c " << c << endl; - _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES; + pitch = (_elevation * c) + (pitch * (1 - c)); + + if (_azimuth - hdg > 180) { + hdg = (_azimuth * c) - (hdg * (1 - c)); + } else { + hdg = (_azimuth * c) + (hdg * (1 - c)); + } + } // recalculate total speed speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS; + /*cout << "_elevation " << _elevation + << " pitch " << pitch + <<" _yaw " << _yaw + << " hdg " << hdg + << " speed " << speed + << endl;*/ + //do impacts and collisions if (_report_impact && !_impact_reported) handle_impact(); diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx index ca92b8b89..abfa68582 100644 --- a/src/AIModel/AIBallistic.hxx +++ b/src/AIModel/AIBallistic.hxx @@ -68,13 +68,17 @@ public: void setSMPath(const string&); void setSubID(int i); void setSubmodel(const string&); - - + void setExternalForce( bool f ); + void setForcePath(const string&); double _getTime() const; virtual const char* getTypeString(void) const { return "ballistic"; } static const double slugs_to_kgs; //conversion factor + SGPropertyNode_ptr _force_node; + SGPropertyNode_ptr _force_azimuth_node; + SGPropertyNode_ptr _force_elevation_node; + private: double _azimuth; // degrees true @@ -94,9 +98,11 @@ 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 + //double _hs_force_fps; // horizontal speed due to external force 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 @@ -108,6 +114,7 @@ private: string _name; string _path; string _submodel; + string _force_path; void Run(double dt); void handle_collision(); diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index 2c3947c14..774820dc8 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -184,11 +184,11 @@ void FGSubmodelMgr::update(double dt) sm_list_iterator end = sm_list.end(); while (sm_list_itr != end) { + in_range = true; if (id == 0) { SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: continuing: " << id << " name " << name ); - in_range = true; ++sm_list_itr; continue; } @@ -198,15 +198,16 @@ void FGSubmodelMgr::update(double dt) if (parent_id == id) { double parent_lat = (*sm_list_itr)->_getLatitude(); double parent_lon = (*sm_list_itr)->_getLongitude(); + string parent_name = (*sm_list_itr)->_getName(); 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 name " << parent_name << ", "<< parent_id << ", "<< parent_lat << ", " << parent_lon << endl; + //cout << "own name " << own_lat << ", " << own_lon << " range " << range_nm << endl; if (range_nm > 15) { SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels: skipping release: " << id); + "Submodels: skipping release, out of range: " << id); in_range = false; } } @@ -284,6 +285,8 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) ballist->setFuseRange(sm->fuse_range); ballist->setSubmodel(sm->submodel.c_str()); ballist->setSubID(sm->sub_id); + ballist->setExternalForce(sm->ext_force); + ballist->setForcePath(sm->force_path.c_str()); ai->attach(ballist); if (sm->count > 0) @@ -309,14 +312,18 @@ void FGSubmodelMgr::transform(submodel *sm) // set initial conditions if (sm->contents_node != 0) { // get the weight of the contents (lbs) and convert to mass (slugs) - sm->contents = sm->contents_node->getDoubleValue(); + sm->contents = sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue(); + //cout << "transform: contents " << sm->contents << endl; IC.mass = (sm->weight + sm->contents) * lbs_to_slugs; + //cout << "mass inc contents" << IC.mass << endl; // set contents to 0 in the parent - sm->contents_node->setDoubleValue(0); - } else { + sm->contents_node->getChild("level-gal_us",0,1)->setDoubleValue(0); + /*cout << "contents " << sm->contents_node->getChild("level-gal_us")->getDoubleValue() + << " " << sm->contents_node->getChild("level-lbs",0,1)->getDoubleValue() + << endl;*/ + } else IC.mass = sm->weight * lbs_to_slugs; - } // cout << "mass " << IC.mass << endl; @@ -592,7 +599,8 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) 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", ""); - + sm->ext_force = entry_node->getBoolValue("external-force", false); + sm->force_path = entry_node->getStringValue("force-path", ""); //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) sm->contents = sm->contents_node->getDoubleValue(); @@ -606,6 +614,7 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) } SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: trigger " << sm->trigger_node->getBoolValue() ); + if (sm->speed_node != 0) sm->speed = sm->speed_node->getDoubleValue(); @@ -629,6 +638,10 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->prop->setStringValue("submodel", submodel.c_str()); //cout << " set submodel path " << submodel << endl; + string force_path = sm->force_path; + sm->prop->setStringValue("force_path", force_path.c_str()); + //cout << "set force_path " << force_path << endl; + if (sm->contents_node != 0) sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents))); @@ -691,6 +704,8 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) 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", ""); + sm->ext_force = entry_node->getBoolValue("external-force", false); + sm->force_path = entry_node->getStringValue("force-path", ""); //cout << "sm->contents_node " << sm->contents_node << endl; if (sm->contents_node != 0) @@ -726,6 +741,10 @@ void FGSubmodelMgr::setSubData(int id, string& path, bool serviceable) sm->prop->setStringValue("submodel", submodel.c_str()); // cout << " set submodel path " << submodel<< endl; + string force_path = sm->force_path; + sm->prop->setStringValue("force_path", force_path.c_str()); + //cout << "set force_path " << force_path << endl; + if (sm->contents_node != 0) sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents))); @@ -768,7 +787,7 @@ void FGSubmodelMgr::loadSubmodels() while (submodel_iterator != submodels.end()) { int id = (*submodel_iterator)->id; - SG_LOG(SG_GENERAL, SG_DEBUG,"after pusback " + SG_LOG(SG_GENERAL, SG_DEBUG,"after pushback " << " id " << id << " name " << (*submodel_iterator)->name << " sub id " << (*submodel_iterator)->sub_id); diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index 87beb5945..177741e4b 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -68,6 +68,8 @@ public: double fuse_range; string submodel; int sub_id; + bool ext_force; + string force_path; } submodel; typedef struct {