From 4df7a3e9f8c1e8b450a3674ec4aab497fda0a514 Mon Sep 17 00:00:00 2001 From: ehofman Date: Sun, 3 Jul 2005 09:39:14 +0000 Subject: [PATCH] =?utf8?q?Mathias=20Fr=F6hlich:?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit I have introduced the posibility to start directly on the carrier. With that patch you will have a --carrrier=id argument where id can either be the pennant number configured in the nimitz scenario or the carriers name also configured in the carriers scenario. Additionaly you can use --parkpos=id to select different positions on the carrier. They are also configured in the scenario file. That includes the switch of the whole FGInterface class to make use of the groundcache. That means that an aircraft no longer uses the current elevation value from the scenery class. It rather has its own local cache of the aircrafts environment which is setup in the common_init method of FGInterface and updated either manually by calling FGInterface::get_groundlevel_m(lat, lon, alt_m); or implicitly by calling the above method in the FGInterface::_updateGeo*Position(lat, lon, alt); methods. A call get_groundlevel_m rebuilds the groundcache if the request is outside the range of the cache. Note that for the real usage of the groundcache including the correct information about the movement of objects and the velocity information, you still need to set up the groundcache in the usual way like YASim and JSBSim currently does. If you use the native interface, you will get only static objects correctly. But for FDM's only using one single ground level for a whole step this is IMO sufficient. The AIManager gets a way to return the location of a object which is placed wrt an AI Object. At the moment it only honours AICarriers for that. That method is a static one, which loads the scenario file for that reason and throws it away afterwards. This looked like the aprioriate way, because the AIManager is initialized much later in flightgears bootstrap, and I did not find an easy way to reorder that for my needs. Since this additional load is very small and does only happen if such a relative location is required, I think that this is ok. Note that moving on the carrier will only work correctly for JSBSim and YASim, but you should now be able to start and move on every not itself moving object with any FDM. --- src/AIModel/AIBase.cxx | 38 ++++++- src/AIModel/AIBase.hxx | 16 ++- src/AIModel/AICarrier.cxx | 27 ++++- src/AIModel/AICarrier.hxx | 7 +- src/AIModel/AIManager.cxx | 39 ++++++- src/AIModel/AIManager.hxx | 5 +- src/AIModel/AIScenario.cxx | 17 +-- src/AIModel/AIScenario.hxx | 2 +- src/FDM/ExternalPipe/ExternalPipe.cxx | 2 +- src/FDM/LaRCsim/LaRCsim.cxx | 6 +- src/FDM/flight.cxx | 144 ++++++++++++++++---------- src/FDM/flight.hxx | 10 ++ src/FDM/groundcache.cxx | 54 +++++----- src/FDM/groundcache.hxx | 5 +- src/Main/fg_init.cxx | 54 +++++++++- src/Main/options.cxx | 19 ++++ 16 files changed, 343 insertions(+), 102 deletions(-) diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index e71b2346f..d9feab65d 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -70,9 +71,10 @@ FGAIBase::FGAIBase() FGAIBase::~FGAIBase() { // Unregister that one at the scenery manager - globals->get_scenery()->unregister_placement_transform(aip.getTransform()); - - globals->get_scenery()->get_scene_graph()->removeKid(aip.getSceneGraph()); + if (globals->get_scenery()) { + globals->get_scenery()->unregister_placement_transform(aip.getTransform()); + globals->get_scenery()->get_scene_graph()->removeKid(aip.getSceneGraph()); + } // unbind(); SGPropertyNode *root = globals->get_props()->getNode("ai/models", true); root->removeChild(_type_str.c_str(), index); @@ -322,6 +324,36 @@ double FGAIBase::UpdateRadar(FGAIManager* manager) return range_ft2; } +Point3D +FGAIBase::getCartPosAt(const Point3D& off) const +{ + // The offset converted to the usual body fixed coordinate system. + sgdVec3 sgdOff; + sgdSetVec3(sgdOff, -off.x(), off.z(), -off.y()); + + // Transform that one to the horizontal local coordinate system. + sgdMat4 hlTrans; + sgdMakeRotMat4(hlTrans, hdg, pitch, roll); + sgdXformPnt3(sgdOff, hlTrans); + + // Now transform to the wgs84 earth centeres system. + Point3D pos2(pos.lon()* SGD_DEGREES_TO_RADIANS, + pos.lat() * SGD_DEGREES_TO_RADIANS, + pos.elev() * SG_FEET_TO_METER); + Point3D cartPos3D = sgGeodToCart(pos2); + sgdMat4 ecTrans; + sgdMakeCoordMat4(ecTrans, cartPos3D.x(), cartPos3D.y(), cartPos3D.z(), + pos.lon(), 0, - 90 - pos.lat()); + sgdXformPnt3(sgdOff, ecTrans); + + return Point3D(sgdOff[0], sgdOff[1], sgdOff[2]); +} + +Point3D +FGAIBase::getGeocPosAt(const Point3D& off) const +{ + return sgCartToGeod(getCartPosAt(off)); +} /* * getters and Setters diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index f24ebd86a..ac859e1ff 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -36,6 +36,17 @@ SG_USING_STD(list); class FGAIManager; class FGAIFlightPlan; +struct ParkPosition { + ParkPosition(const ParkPosition& pp) + : name(pp.name), offset(pp.offset), heading_deg(pp.heading_deg) + {} + ParkPosition(const string& n, const Point3D& off = Point3D(), double heading = 0) + : name(n), offset(off), heading_deg(heading) + {} + string name; + Point3D offset; + double heading_deg; +}; typedef struct { string callsign; @@ -73,7 +84,7 @@ typedef struct { list solid_objects; // List of solid object names list wire_objects; // List of wire object names list catapult_objects; // List of catapult object names - list ppositions; // List of positions on a carrier where an aircraft can start. + list ppositions; // List of positions on a carrier where an aircraft can start. Point3D flols_offset; // used by carrier objects, in meters double radius; // used by ship objects, in feet string name; // used by carrier objects @@ -117,6 +128,9 @@ public: void setDie( bool die ); bool getDie(); + Point3D getCartPosAt(const Point3D& off) const; + Point3D getGeocPosAt(const Point3D& off) const; + protected: SGPropertyNode_ptr props; diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index 45757dd9a..161dbe536 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -33,6 +33,9 @@ #include "AICarrier.hxx" +#include "AIScenario.hxx" + + FGAICarrier::FGAICarrier(FGAIManager* mgr) : FGAIShip(mgr) { _type_str = "carrier"; _otype = otCarrier; @@ -53,7 +56,7 @@ void FGAICarrier::setCatapultObjects(const list& co) { catapult_objects = co; } -void FGAICarrier::setParkingPositions(const list& p) { +void FGAICarrier::setParkingPositions(const list& p) { ppositions = p; } @@ -135,6 +138,28 @@ void FGAICarrier::unbind() { props->untie("controls/flols/angle-degs"); } +bool FGAICarrier::getParkPosition(const string& id, Point3D& geodPos, + double& hdng, sgdVec3 uvw) +{ + list::iterator it = ppositions.begin(); + while (it != ppositions.end()) { + // Take either the specified one or the first one ... + if ((*it).name == id || id.empty()) { + ParkPosition ppos = *it; + geodPos = getGeocPosAt(ppos.offset); + hdng = hdg + ppos.heading_deg; + double shdng = sin(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); + double chdng = cos(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); + double speed_fps = speed*1.6878099; + sgdSetVec3(uvw, chdng*speed_fps, shdng*speed_fps, 0); + return true; + } + ++it; + } + + return false; +} + void FGAICarrier::mark_nohot(ssgEntity* e) { if (e->isAKindOf(ssgTypeBranch())) { ssgBranch* br = (ssgBranch*)e; diff --git a/src/AIModel/AICarrier.hxx b/src/AIModel/AICarrier.hxx index 2844e7b4b..fd03644a3 100644 --- a/src/AIModel/AICarrier.hxx +++ b/src/AIModel/AICarrier.hxx @@ -78,7 +78,7 @@ public: void setSolidObjects(const list& solid_objects); void setWireObjects(const list& wire_objects); void setCatapultObjects(const list& catapult_objects); - void setParkingPositions(const list& p); + void setParkingPositions(const list& p); void setSign(const string& ); void setFlolsOffset(const Point3D& off); @@ -89,7 +89,8 @@ public: bool init(); - static SGPropertyNode* getStartPosition(const string& id); + bool getParkPosition(const string& id, Point3D& geodPos, + double& hdng, sgdVec3 uvw); private: @@ -102,7 +103,7 @@ private: list solid_objects; // List of solid object names list wire_objects; // List of wire object names list catapult_objects; // List of catapult object names - list ppositions; // List of positions where an aircraft can start. + list ppositions; // List of positions where an aircraft can start. string sign; // The sign of this carrier. // Velocity wrt earth. diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index fd121ed28..3fe490b86 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -370,7 +370,7 @@ void FGAIManager::processThermal( FGAIThermal* thermal ) { } -void FGAIManager::processScenario( string &filename ) { +void FGAIManager::processScenario( const string &filename ) { FGAIScenario* s = new FGAIScenario( filename ); for (int i=0;inEntries();i++) { FGAIModelEntity* en = s->getNextEntry(); @@ -423,5 +423,42 @@ void FGAIManager::setModel(const string& path, ssgBranch *model) loadedModels.push_back(FGModelID(path,model)); } +bool FGAIManager::getStartPosition(const string& id, const string& pid, + Point3D& geodPos, double& heading, + sgdVec3 uvw) +{ + SGPropertyNode* root = fgGetNode("sim/ai", true); + if (!root->getNode("enabled", true)->getBoolValue()) + return 0; + + bool found = false; + string filename = root->getNode("scenario", true)->getStringValue(); + FGAIScenario* s = new FGAIScenario( filename ); + for (int i=0; inEntries(); i++) { + FGAIModelEntity* en = s->getNextEntry(); + if (en && en->m_type == "carrier" && + (en->pennant_number == id || en->name == id)) { + FGAICarrier* ai_carrier = new FGAICarrier(0); + ai_carrier->setHeading(en->heading); + ai_carrier->setSpeed(en->speed); + ai_carrier->setAltitude(en->altitude); + ai_carrier->setLongitude(en->longitude); + ai_carrier->setLatitude(en->latitude); + ai_carrier->setBank(en->rudder); + ai_carrier->setParkingPositions(en->ppositions); + + if (ai_carrier->getParkPosition(pid, geodPos, heading, uvw)) { + delete ai_carrier; + found = true; + break; + } + + delete ai_carrier; + } + } + delete s; + + return found; +} //end AIManager.cxx diff --git a/src/AIModel/AIManager.hxx b/src/AIModel/AIManager.hxx index 25c3b3d9b..1eb1bf179 100644 --- a/src/AIModel/AIManager.hxx +++ b/src/AIModel/AIManager.hxx @@ -103,11 +103,14 @@ public: return (0 < ot && ot < FGAIBase::MAX_OBJECTS) ? numObjects[ot] : numObjects[0]; } - void processScenario( string &filename ); + void processScenario( const string &filename ); ssgBranch * getModel(const string& path); void setModel(const string& path, ssgBranch *model); + static bool getStartPosition(const string& id, const string& pid, + Point3D& geodPos, double& hdng, sgdVec3 uvw); + private: bool initDone; diff --git a/src/AIModel/AIScenario.cxx b/src/AIModel/AIScenario.cxx index 385491f08..5f88bca45 100644 --- a/src/AIModel/AIScenario.cxx +++ b/src/AIModel/AIScenario.cxx @@ -35,10 +35,10 @@ static list getAllStringNodeVals(const char* name, SGPropertyNode * entry_node); -static list +static list getAllOffsetNodeVals(const char* name, SGPropertyNode * entry_node); -FGAIScenario::FGAIScenario(string &filename) +FGAIScenario::FGAIScenario(const string &filename) { int i; SGPath path( globals->get_fg_root() ); @@ -105,8 +105,8 @@ FGAIScenario::FGAIScenario(string &filename) en->catapult_objects = getAllStringNodeVals("catapult", entry_node); en->solid_objects = getAllStringNodeVals("solid", entry_node); en->ppositions = getAllOffsetNodeVals("parking-pos", entry_node); - list flolspos = getAllOffsetNodeVals("flols-pos", entry_node); - en->flols_offset = flolspos.front(); + list flolspos = getAllOffsetNodeVals("flols-pos", entry_node); + en->flols_offset = flolspos.front().offset; en->fp = NULL; if (en->flightplan != ""){ @@ -161,18 +161,21 @@ getAllStringNodeVals(const char* name, SGPropertyNode * entry_node) return retval; } -static list +static list getAllOffsetNodeVals(const char* name, SGPropertyNode * entry_node) { - list retval; + list retval; vector::iterator it; vector children = entry_node->getChildren(name); for (it = children.begin(); it != children.end(); ++it) { + string name = (*it)->getStringValue("name", "unnamed"); double offset_x = (*it)->getDoubleValue("x-offset-m", 0); double offset_y = (*it)->getDoubleValue("y-offset-m", 0); double offset_z = (*it)->getDoubleValue("z-offset-m", 0); - retval.push_back(Point3D(offset_x, offset_y, offset_z)); + double hd = (*it)->getDoubleValue("heading-offset-deg", 0); + ParkPosition pp(name, Point3D(offset_x, offset_y, offset_z), hd); + retval.push_back(pp); } return retval; diff --git a/src/AIModel/AIScenario.hxx b/src/AIModel/AIScenario.hxx index f7b8f47cd..a6b46ccc2 100644 --- a/src/AIModel/AIScenario.hxx +++ b/src/AIModel/AIScenario.hxx @@ -34,7 +34,7 @@ class FGAIScenario { public: - FGAIScenario(string &filename); + FGAIScenario(const string &filename); ~FGAIScenario(); FGAIModelEntity* getNextEntry( void ); diff --git a/src/FDM/ExternalPipe/ExternalPipe.cxx b/src/FDM/ExternalPipe/ExternalPipe.cxx index 422023ed1..4cd13bc81 100644 --- a/src/FDM/ExternalPipe/ExternalPipe.cxx +++ b/src/FDM/ExternalPipe/ExternalPipe.cxx @@ -431,7 +431,7 @@ static void process_set_command( const string_list &tokens ) { cur_fdm_state->_updateGeodeticPosition( lat_rad, lon_rad, alt_m * SG_METER_TO_FEET ); - double agl_m = alt_m - globals->get_scenery()->get_cur_elev(); + double agl_m = alt_m - cur_fdm_state->get_Runway_altitude_m(); cur_fdm_state->_set_Altitude_AGL( agl_m * SG_METER_TO_FEET ); } else if ( tokens[1] == "euler_angles" ) { double phi_rad = atof( tokens[2].c_str() ); diff --git a/src/FDM/LaRCsim/LaRCsim.cxx b/src/FDM/LaRCsim/LaRCsim.cxx index 5d0564f06..df59b52d2 100644 --- a/src/FDM/LaRCsim/LaRCsim.cxx +++ b/src/FDM/LaRCsim/LaRCsim.cxx @@ -285,8 +285,10 @@ void FGLaRCsim::update( double dt ) { Brake_pct[1] = globals->get_controls()->get_brake_left(); // Inform LaRCsim of the local terrain altitude - // Runway_altitude = get_Runway_altitude(); - Runway_altitude = fgGetDouble("/position/ground-elev-m") * SG_METER_TO_FEET; + Runway_altitude + = get_groundlevel_m(Latitude, Longitude, Altitude * SG_FEET_TO_METER) + * SG_METER_TO_FEET; + // Weather /* V_north_airmass = get_V_north_airmass(); V_east_airmass = get_V_east_airmass(); diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 86324a831..cedeb29a7 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -188,20 +188,23 @@ FGInterface::common_init () // Set initial position SG_LOG( SG_FLIGHT, SG_INFO, "...initializing position..." ); - set_Longitude( fgGetDouble("/sim/presets/longitude-deg") - * SGD_DEGREES_TO_RADIANS ); - set_Latitude( fgGetDouble("/sim/presets/latitude-deg") - * SGD_DEGREES_TO_RADIANS ); - double ground_elev_m = globals->get_scenery()->get_cur_elev(); + double lon = fgGetDouble("/sim/presets/longitude-deg") + * SGD_DEGREES_TO_RADIANS; + double lat = fgGetDouble("/sim/presets/latitude-deg") + * SGD_DEGREES_TO_RADIANS; + double alt_ft = fgGetDouble("/sim/presets/altitude-ft"); + double alt_m = alt_ft * SG_FEET_TO_METER; + set_Longitude( lon ); + set_Latitude( lat ); + + double ground_elev_m = get_groundlevel_m(lat, lon, alt_m); double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET; - fgSetDouble("/position/ground-elev-m", ground_elev_m); _set_Runway_altitude ( ground_elev_ft ); - if ( fgGetBool("/sim/presets/onground") - || fgGetDouble("/sim/presets/altitude-ft") < ground_elev_ft ) { - fgSetDouble("/position/altitude-ft", ground_elev_ft); - set_Altitude( ground_elev_ft ); + if ( fgGetBool("/sim/presets/onground") || alt_ft < ground_elev_ft ) { + fgSetDouble("/position/altitude-ft", ground_elev_ft + 0.1); + set_Altitude( ground_elev_ft + 0.1); } else { - set_Altitude( fgGetDouble("/sim/presets/altitude-ft") ); + set_Altitude( alt_ft ); } // Set ground elevation @@ -305,6 +308,15 @@ FGInterface::bind () fgSetArchivable("/position/altitude-ft"); fgTie("/position/altitude-agl-ft", this, &FGInterface::get_Altitude_AGL); // read-only + fgSetArchivable("/position/ground-elev-ft"); + fgTie("/position/ground-elev-ft", this, + &FGInterface::get_Runway_altitude); // read-only + fgSetArchivable("/position/ground-elev-m"); + fgTie("/position/ground-elev-m", this, + &FGInterface::get_Runway_altitude_m); // read-only + fgSetArchivable("/position/sea-level-radius-ft"); + fgTie("/position/sea-level-radius-ft", this, + &FGInterface::get_Sea_level_radius); // read-only // Orientation fgTie("/orientation/roll-deg", this, @@ -438,6 +450,9 @@ FGInterface::unbind () fgUntie("/position/longitude-deg"); fgUntie("/position/altitude-ft"); fgUntie("/position/altitude-agl-ft"); + fgUntie("/position/ground-elev-ft"); + fgUntie("/position/ground-elev-m"); + fgUntie("/position/sea-level-radius-ft"); fgUntie("/orientation/roll-deg"); fgUntie("/orientation/pitch-deg"); fgUntie("/orientation/heading-deg"); @@ -497,7 +512,9 @@ void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt ) _set_Geodetic_Position( lat, lon, alt ); _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET ); - _set_Runway_altitude( fgGetDouble("/position/ground-elev-m") * SG_METER_TO_FEET ); + double alt_m = alt*SG_FEET_TO_METER; + double groundlevel_m = get_groundlevel_m(lat, lon, alt_m); + _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET ); _set_sin_lat_geocentric( lat_geoc ); _set_cos_lat_geocentric( lat_geoc ); @@ -505,25 +522,6 @@ void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt ) _set_sin_cos_longitude( lon ); _set_sin_cos_latitude( lat ); - - /* Norman's code for slope of the terrain */ - /* needs to be tested -- get it on the HUD and taxi around */ - /* double *tnorm = scenery.cur_normal; - - double sy = sin ( -get_Psi() ) ; - double cy = cos ( -get_Psi() ) ; - - double phitb, thetatb, psitb; - if ( tnorm[1] != 0.0 ) { - psitb = -atan2 ( tnorm[0], tnorm[1] ); - } - if ( tnorm[2] != 0.0 ) { - thetatb = atan2 ( tnorm[0] * cy - tnorm[1] * sy, tnorm[2] ); - phitb = -atan2 ( tnorm[1] * cy + tnorm[0] * sy, tnorm[2] ); - } - - _set_terrain_slope(phitb, thetatb, psitb) - */ } @@ -553,7 +551,9 @@ void FGInterface::_updateGeocentricPosition( double lat_geoc, double lon, _set_Geodetic_Position( lat_geod, lon, alt ); _set_Sea_level_radius( sl_radius2 * SG_METER_TO_FEET ); - _set_Runway_altitude( fgGetDouble("/position/ground-elev-m") * SG_METER_TO_FEET ); + double alt_m = alt*SG_FEET_TO_METER; + double groundlevel_m = get_groundlevel_m(lat_geod, lon, alt_m); + _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET ); _set_sin_lat_geocentric( lat_geoc ); _set_cos_lat_geocentric( lat_geoc ); @@ -561,25 +561,6 @@ void FGInterface::_updateGeocentricPosition( double lat_geoc, double lon, _set_sin_cos_longitude( lon ); _set_sin_cos_latitude( lat_geod ); - - /* Norman's code for slope of the terrain */ - /* needs to be tested -- get it on the HUD and taxi around */ - /* double *tnorm = scenery.cur_normal; - - double sy = sin ( -get_Psi() ) ; - double cy = cos ( -get_Psi() ) ; - - double phitb, thetatb, psitb; - if ( tnorm[1] != 0.0 ) { - psitb = -atan2 ( tnorm[0], tnorm[1] ); - } - if ( tnorm[2] != 0.0 ) { - thetatb = atan2 ( tnorm[0] * cy - tnorm[1] * sy, tnorm[2] ); - phitb = -atan2 ( tnorm[1] * cy + tnorm[0] * sy, tnorm[2] ); - } - - _set_terrain_slope(phitb, thetatb, psitb) - */ } @@ -848,7 +829,7 @@ FGInterface::get_agl_m(double t, const double pt[3], int *type, double *loadCapacity, double *frictionFactor, double *agl) { - return ground_cache.get_agl(t, pt, contact, normal, vel, type, + return ground_cache.get_agl(t, pt, 2.0, contact, normal, vel, type, loadCapacity, frictionFactor, agl); } @@ -861,7 +842,7 @@ FGInterface::get_agl_ft(double t, const double pt[3], // Convert units and do the real work. sgdVec3 pt_m; sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER ); - bool ret = ground_cache.get_agl(t, pt_m, contact, normal, vel, + bool ret = ground_cache.get_agl(t, pt_m, 2.0, contact, normal, vel, type, loadCapacity, frictionFactor, agl); // Convert units back ... sgdScaleVec3( contact, SG_METER_TO_FEET ); @@ -872,6 +853,63 @@ FGInterface::get_agl_ft(double t, const double pt[3], return ret; } +bool +FGInterface::get_agl_m(double t, const double pt[3], double max_altoff, + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) +{ + return ground_cache.get_agl(t, pt, max_altoff, contact, normal, vel, type, + loadCapacity, frictionFactor, agl); +} + +bool +FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff, + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) +{ + // Convert units and do the real work. + sgdVec3 pt_m; + sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER ); + bool ret = ground_cache.get_agl(t, pt_m, SG_FEET_TO_METER * max_altoff, + contact, normal, vel, + type, loadCapacity, frictionFactor, agl); + // Convert units back ... + sgdScaleVec3( contact, SG_METER_TO_FEET ); + sgdScaleVec3( vel, SG_METER_TO_FEET ); + *agl *= SG_METER_TO_FEET; + // FIXME: scale the load limit to something in the english unit system. + // Be careful with the DBL_MAX which is returned by default. + return ret; +} + + +double +FGInterface::get_groundlevel_m(double lat, double lon, double alt) +{ + // First compute the sea level radius, + sgdVec3 pos, cpos; + sgGeodToCart(lat, lon, 0, pos); + double slr = sgdLengthVec3(pos); + // .. then the cartesian position of the given lat/lon/alt. + sgGeodToCart(lat, lon, alt, pos); + + // FIXME: how to handle t - ref_time differences ??? + double ref_time, radius; + // Prepare the ground cache for that position. + if (!is_valid_m(&ref_time, cpos, &radius)) + prepare_ground_cache_m(ref_time, pos, 10); + else if (radius*radius <= sgdDistanceSquaredVec3(pos, cpos)) + prepare_ground_cache_m(ref_time, pos, radius); + + double contact[3], normal[3], vel[3], lc, ff, agl; + int type; + get_agl_m(ref_time, pos, 2.0, contact, normal, vel, &type, &lc, &ff, &agl); + + return sgdLengthVec3(contact) - slr; +} + bool FGInterface::caught_wire_m(double t, const double pt[4][3]) { diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index 36df52e42..5b01cff0a 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -977,6 +977,7 @@ public: } inline double get_Runway_altitude() const { return runway_altitude; } + inline double get_Runway_altitude_m() const { return SG_FEET_TO_METER * runway_altitude; } // inline double get_Runway_latitude() const { return runway_latitude; } // inline void set_Runway_latitude( double lat ) { runway_latitude = lat; } // inline double get_Runway_longitude() const { return runway_longitude; } @@ -1132,6 +1133,15 @@ public: double contact[3], double normal[3], double vel[3], int *type, double *loadCapacity, double *frictionFactor, double *agl); + bool get_agl_m(double t, const double pt[3], double max_altoff, + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl); + bool get_agl_ft(double t, const double pt[3], double max_altoff, + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl); + double get_groundlevel_m(double lat, double lon, double alt); // Return 1 if the hook intersects with a wire. diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 6c2654083..871bb56ea 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -583,10 +583,10 @@ FGGroundCache::get_cat(double t, const double dpt[3], } bool -FGGroundCache::get_agl(double t, const double dpt[3], - double contact[3], double normal[3], double vel[3], - int *type, double *loadCapacity, - double *frictionFactor, double *agl) +FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff, + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) { bool ret = false; @@ -608,9 +608,10 @@ FGGroundCache::get_agl(double t, const double dpt[3], // The search direction sgdVec3 dir; sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] ); + sgdNormaliseVec3( dir ); // Initialize to something sensible - double sqdist = DBL_MAX; + double current_radius = 0.0; size_t sz = triangles.size(); for (size_t i = 0; i < sz; ++i) { @@ -623,28 +624,33 @@ FGGroundCache::get_agl(double t, const double dpt[3], sgdVec3 isecpoint; if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) && sgdPointInTriangle( isecpoint, triangle.vertices ) ) { - - // Check for the closest intersection point. - // FIXME: is this the right one? - SGDfloat newSqdist = sgdDistanceSquaredVec3( isecpoint, pt ); - if ( newSqdist < sqdist ) { - sqdist = newSqdist; - ret = true; - // Save the new potential intersection point. - sgdCopyVec3( contact, isecpoint ); - sgdAddVec3( contact, cache_center ); - // The first three values in the vector are the plane normal. - sgdCopyVec3( normal, triangle.plane ); - // The velocity wrt earth. - /// FIXME: only true for non rotating objects!!!! - sgdCopyVec3( vel, triangle.velocity ); - // Save the ground type. - *type = triangle.type; - // FIXME: figure out how to get that sign ... + // Transform to the wgs system + sgdAddVec3( isecpoint, cache_center ); + // compute the radius, good enough approximation to take the geocentric radius + SGDfloat radius = sgdLengthSquaredVec3(isecpoint); + if (current_radius < radius) { + // Compute the vector from pt to the intersection point ... + sgdVec3 off; + sgdSubVec3(off, pt, isecpoint); + // ... and check if it is too high or not + if (-max_altoff < sgdScalarProductVec3( off, dir )) { + current_radius = radius; + ret = true; + // Save the new potential intersection point. + sgdCopyVec3( contact, isecpoint ); + // The first three values in the vector are the plane normal. + sgdCopyVec3( normal, triangle.plane ); + // The velocity wrt earth. + /// FIXME: only true for non rotating objects!!!! + sgdCopyVec3( vel, triangle.velocity ); + // Save the ground type. + *type = triangle.type; + // FIXME: figure out how to get that sign ... // *agl = sqrt(sqdist); - *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); + *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); // *loadCapacity = DBL_MAX; // *frictionFactor = 1.0; + } } } } diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx index 36f75e5d9..e881463bb 100644 --- a/src/FDM/groundcache.hxx +++ b/src/FDM/groundcache.hxx @@ -56,17 +56,16 @@ public: // Return the altitude above ground below the wgs84 point pt - // Search for the nearest triangle to pt. + // Search for highest triangle not higher than pt + max_altoff. // Return ground properties like the ground type, the maximum load // this kind kind of ground can carry, the friction factor between // 0 and 1 which can be used to model lower friction with wet runways // and finally the altitude above ground. - bool get_agl(double t, const double pt[3], + bool get_agl(double t, const double pt[3], double max_altoff, double contact[3], double normal[3], double vel[3], int *type, double *loadCapacity, double *frictionFactor, double *agl); - // Return 1 if the hook intersects with a wire. // That test is done by checking if the quad spanned by the points pt* // intersects with the line representing the wire. diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index b2e26d21e..f1c50afb8 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -954,7 +955,49 @@ static bool fgSetPosFromNAV( const string& id, const double& freq ) { } } +// Set current_options lon/lat given an aircraft carrier id +static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) { + + // set initial position from runway and heading + Point3D geodPos; + double heading; + sgdVec3 uvw; + if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) { + double lon = geodPos.lon() * SGD_RADIANS_TO_DEGREES; + double lat = geodPos.lat() * SGD_RADIANS_TO_DEGREES; + double alt = geodPos.elev() * SG_METER_TO_FEET; + + SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for " + << carrier << " at lat = " << lat << ", lon = " << lon + << ", alt = " << alt << ", heading = " << heading); + + fgSetDouble("/sim/presets/longitude-deg", lon); + fgSetDouble("/sim/presets/latitude-deg", lat); + fgSetDouble("/sim/presets/altitude-ft", alt); + fgSetDouble("/sim/presets/heading-deg", heading); + fgSetDouble("/position/longitude-deg", lon); + fgSetDouble("/position/latitude-deg", lat); + fgSetDouble("/position/altitude-ft", alt); + fgSetDouble("/orientation/heading-deg", heading); + + fgSetString("/sim/presets/speed-set", "UVW"); + fgSetDouble("/velocities/uBody-fps", uvw[0]); + fgSetDouble("/velocities/vBody-fps", uvw[1]); + fgSetDouble("/velocities/wBody-fps", uvw[2]); + fgSetDouble("/sim/presets/uBody-fps", uvw[0]); + fgSetDouble("/sim/presets/vBody-fps", uvw[1]); + fgSetDouble("/sim/presets/wBody-fps", uvw[2]); + fgSetBool("/sim/presets/onground", true); + + return true; + } else { + SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carroer = " + << carrier ); + return false; + } +} + // Set current_options lon/lat given an airport id and heading (degrees) static bool fgSetPosFromFix( const string& id ) { FGFix fix; @@ -1127,6 +1170,8 @@ bool fgInitPosition() { double vor_freq = fgGetDouble("/sim/presets/vor-freq"); string ndb = fgGetString("/sim/presets/ndb-id"); double ndb_freq = fgGetDouble("/sim/presets/ndb-freq"); + string carrier = fgGetString("/sim/presets/carrier"); + string parkpos = fgGetString("/sim/presets/parkpos"); string fix = fgGetString("/sim/presets/fix"); if ( !set_pos && !apt.empty() && !rwy_no.empty() ) { @@ -1163,6 +1208,13 @@ bool fgInitPosition() { } } + if ( !set_pos && !carrier.empty() ) { + // an aircraft carrier is requested + if ( fgSetPosFromCarrier( carrier, parkpos ) ) { + set_pos = true; + } + } + if ( !set_pos && !fix.empty() ) { // a Fix is requested if ( fgSetPosFromFix( fix ) ) { @@ -1185,7 +1237,7 @@ bool fgInitPosition() { fgGetDouble("/sim/presets/heading-deg") ); // determine if this should be an on-ground or in-air start - if ( fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1 ) { + if ((fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1) && carrier.empty()) { fgSetBool("/sim/presets/onground", false); } else { fgSetBool("/sim/presets/onground", true); diff --git a/src/Main/options.cxx b/src/Main/options.cxx index f6de4c80d..f49c5bf68 100644 --- a/src/Main/options.cxx +++ b/src/Main/options.cxx @@ -597,6 +597,8 @@ clearLocation () fgSetString("/sim/presets/airport-id", ""); fgSetString("/sim/presets/vor-id", ""); fgSetString("/sim/presets/ndb-id", ""); + fgSetString("/sim/presets/carrier", ""); + fgSetString("/sim/presets/parkpos", ""); fgSetString("/sim/presets/fix", ""); } @@ -616,6 +618,21 @@ fgOptNDB( const char * arg ) return FG_OPTIONS_OK; } +static int +fgOptCarrier( const char * arg ) +{ + clearLocation(); + fgSetString("/sim/presets/carrier", arg); + return FG_OPTIONS_OK; +} + +static int +fgOptParkpos( const char * arg ) +{ + fgSetString("/sim/presets/parkpos", arg); + return FG_OPTIONS_OK; +} + static int fgOptFIX( const char * arg ) { @@ -1215,6 +1232,8 @@ struct OptionDesc { {"runway", true, OPTION_STRING, "/sim/presets/runway", false, "", 0 }, {"vor", true, OPTION_FUNC, "", false, "", fgOptVOR }, {"ndb", true, OPTION_FUNC, "", false, "", fgOptNDB }, + {"carrier", true, OPTION_FUNC, "", false, "", fgOptCarrier }, + {"parkpos", true, OPTION_FUNC, "", false, "", fgOptParkpos }, {"fix", true, OPTION_FUNC, "", false, "", fgOptFIX }, {"offset-distance", true, OPTION_DOUBLE, "/sim/presets/offset-distance", false, "", 0 }, {"offset-azimuth", true, OPTION_DOUBLE, "/sim/presets/offset-azimuth", false, "", 0 }, -- 2.39.5