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 },