fly/flightgear
1
0
Fork 0
Code Activity

James Turner:

Browse source 
Attached patch updates the route-manager to use FGPositioned to search
for waypoints, instead of a manual airport / fix / navaid search. This
is good because it's now using the 'strictly closest' match, rather
than arbitrarily picking a distant fix over a nearby navaid. In my
case, the TLA VOR is significant to several EGPH procedures, but also
happens to be the ident of a fix a long, long way away.

Also updates the FGPositioned class to stop using Point3D, partly
because it's deprecated and partly because I had misunderstood the
interface and was using it wrong. For now, all FGPositioned distance
checks use SGGeodesy::inverse, which is accurate but inefficient. Once
FGPositioned queries are used for something on a hot path, I'll
probably store the cartesian position as well as the geodetic, to make
these checks fast.
This commit is contained in:
fredb 2008-12-03 20:03:46 +00:00
parent 7ad55be8f6
commit f3b110e565
3 changed files with 66 additions and 99 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

87
src/Autopilot/route_mgr.cxx
View file

@ -281,24 +281,24 @@ bool FGRouteMgr::build() {
}
int FGRouteMgr::new_waypoint( const string& target, int n ) {
SGWayPoint *wp = 0;
int type = make_waypoint( &wp, target );
if (wp) {
add_waypoint( *wp, n );
delete wp;
if ( !near_ground() )
fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
void FGRouteMgr::new_waypoint( const string& target, int n ) {
SGWayPoint* wp = make_waypoint( target );
if (!wp) {
return;
}
add_waypoint( *wp, n );
delete wp;
if ( !near_ground() ) {
fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
}
return type;
}
int FGRouteMgr::make_waypoint( SGWayPoint **wp, const string& tgt ) {
SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
string target = tgt;
// make upper case
for (unsigned int i = 0; i < target.size(); i++)
if (target[i] >= 'a' && target[i] <= 'z')
@ -321,57 +321,28 @@ int FGRouteMgr::make_waypoint( SGWayPoint **wp, const string& tgt ) {
double lat = atof( target.c_str() + pos + 1);
SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint lon = " << lon << ", lat = " << lat );
*wp = new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target );
return 1;
}
return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target );
}
// check for airport id
const FGAirport *apt = fgFindAirportID( target );
if (apt) {
SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (airport) = " << target );
*wp = new SGWayPoint( apt->getLongitude(), apt->getLatitude(), alt, SGWayPoint::WGS84, target );
return 2;
}
double lat, lon;
// The base lon/lat are determined by the last WP,
// or the current pos if the WP list is empty.
const int wps = this->size();
if (wps > 0) {
SGWayPoint wp = get_waypoint(wps-1);
lat = wp.get_target_lat();
lon = wp.get_target_lon();
SGGeod basePosition;
if (route->size() > 0) {
SGWayPoint wp = get_waypoint(route->size()-1);
basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat());
} else {
lat = fgGetNode("/position/latitude-deg")->getDoubleValue();
lon = fgGetNode("/position/longitude-deg")->getDoubleValue();
// route is empty, use current position
basePosition = SGGeod::fromDeg(
fgGetNode("/position/longitude-deg")->getDoubleValue(),
fgGetNode("/position/latitude-deg")->getDoubleValue());
}
// check for fix id
FGFix* f;
double heading;
double dist;
if ( globals->get_fixlist()->query_and_offset( target, lon, lat, 0, f, &heading, &dist ) ) {
SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (fix) = " << target );
*wp = new SGWayPoint( f->get_lon(), f->get_lat(), alt, SGWayPoint::WGS84, target );
return 3;
FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition);
if (!p) {
SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target);
return NULL;
}
// Try finding a nav matching the ID
lat *= SGD_DEGREES_TO_RADIANS;
lon *= SGD_DEGREES_TO_RADIANS;
SG_LOG( SG_GENERAL, SG_INFO, "Looking for nav " << target << " at " << lon << " " << lat);
if (FGNavRecord* nav = globals->get_navlist()->findByIdent(target.c_str(), lon, lat)) {
SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (nav) = " << target );
*wp = new SGWayPoint( nav->get_lon(), nav->get_lat(), alt, SGWayPoint::WGS84, target );
return 4;
}
// unknown target
return 0;
return new SGWayPoint(p->longitude(), p->latitude(), p->elevation(), SGWayPoint::WGS84, target);
}

4
src/Autopilot/route_mgr.hxx
View file

@ -93,7 +93,7 @@ private:
SGPropertyNode_ptr mirror;
bool altitude_set;
int make_waypoint( SGWayPoint **wp, const string& target );
SGWayPoint* make_waypoint(const string& target);
void update_mirror();
bool near_ground();
@ -110,7 +110,7 @@ public:
bool build ();
int new_waypoint( const string& tgt_alt, int n = -1 );
void new_waypoint( const string& tgt_alt, int n = -1 );
void add_waypoint( const SGWayPoint& wp, int n = -1 );
SGWayPoint pop_waypoint( int i = 0 );

74
src/Navaids/positioned.cxx
View file

@ -182,38 +182,31 @@ spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, F
}
/**
* Cartesian range predicate. Note that for really long ranges, might need to
* to use geodetic / geocentric distance instead
*/
class RangePredictate
{
public:
RangePredictate(const Point3D& aOrigin, double aRange) :
RangePredictate(const SGGeod& aOrigin, double aRange) :
mOrigin(aOrigin),
mRangeSquared(aRange * aRange)
mRange(aRange)
{ ; }
bool operator()(const FGPositionedRef& aPos)
{
Point3D p(Point3D::fromSGGeod(aPos->geod()));
bool ok = (mOrigin.distance3Dsquared(p) > mRangeSquared);
if (ok) {
double x = sqrt(mOrigin.distance3Dsquared(p) - mRangeSquared);
x *= SG_METER_TO_NM;
//std::cout << "pos:" << aPos->ident() << " failed range check by " << x << std::endl;
}
return ok;
double d, az1, az2;
SGGeodesy::inverse(aPos->geod(), mOrigin, az1, az2, d);
return (d > mRange);
}
private:
Point3D mOrigin;
double mRangeSquared;
SGGeod mOrigin;
double mRange;
};
static void
filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult)
{
RangePredictate pred(Point3D::fromSGGeod(aPos), aRange * SG_NM_TO_METER);
RangePredictate pred(aPos, aRange * SG_NM_TO_METER);
FGPositioned::List::iterator newEnd;
newEnd = std::remove_if(aResult.begin(), aResult.end(), pred);
aResult.erase(newEnd, aResult.end());
@ -223,17 +216,19 @@ class DistanceOrdering
{
public:
DistanceOrdering(const SGGeod& aPos) :
mPos(Point3D::fromSGGeod(aPos))
mPos(aPos)
{ }
bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const
{
return mPos.distance3Dsquared(Point3D::fromSGGeod(a->geod())) <
mPos.distance3Dsquared(Point3D::fromSGGeod(b->geod()));
double dA, dB, az1, az2;
SGGeodesy::inverse(mPos, a->geod(), az1, az2, dA);
SGGeodesy::inverse(mPos, b->geod(), az1, az2, dB);
return dA < dB;
}
private:
Point3D mPos;
SGGeod mPos;
};
static void
@ -247,7 +242,9 @@ namedFindClosestTyped(const std::string& aIdent, const SGGeod& aOrigin,
FGPositioned::Type aLower, FGPositioned::Type aUpper)
{
NamedIndexRange range = global_namedIndex.equal_range(aIdent);
if (range.first == range.second) return NULL;
if (range.first == range.second) {
return NULL;
}
// common case, only one result. looks a bit ugly because these are
// sequential iterators, not random-access ones
@ -265,21 +262,21 @@ namedFindClosestTyped(const std::string& aIdent, const SGGeod& aOrigin,
// multiple matches, we need to actually check the distance to each one
double minDist = HUGE_VAL;
FGPositionedRef result;
Point3D origin(Point3D::fromSGGeod(aOrigin));
for (; range.first != range.second; ++range.first) {
NamedPositionedIndex::const_iterator it = range.first;
for (; it != range.second; ++it) {
// filter by type
FGPositioned::Type ty = range.first->second->type();
FGPositioned::Type ty = it->second->type();
if ((ty < aLower) || (ty > aUpper)) {
continue;
}
// find distance
Point3D p(Point3D::fromSGGeod(range.first->second->geod()));
double ds = origin.distance3Dsquared(p);
if (ds < minDist) {
minDist = ds;
result = range.first->second;
double d, az1, az2;
SGGeodesy::inverse(aOrigin, it->second->geod(), az2, az2, d);
if (d < minDist) {
minDist = d;
result = it->second;
}
}
@ -340,26 +337,25 @@ FGPositioned::FGPositioned() :
FGPositioned::FGPositioned(Type ty, const std::string& aIdent, double aLat, double aLon, double aElev) :
mType(ty),
mIdent(aIdent),
mPosition(SGGeod::fromDegFt(aLon, aLat, aElev))
mPosition(SGGeod::fromDegFt(aLon, aLat, aElev)),
mIdent(aIdent)
{
//addToIndices(this);
//SGReferenced::get(this); // hold an owning ref, for the moment
addToIndices(this);
SGReferenced::get(this); // hold an owning ref, for the moment
}
FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) :
mType(ty),
mIdent(aIdent),
mPosition(aPos)
mPosition(aPos),
mIdent(aIdent)
{
//addToIndices(this);
//SGReferenced::get(this); // hold an owning ref, for the moment
addToIndices(this);
SGReferenced::get(this); // hold an owning ref, for the moment
}
FGPositioned::~FGPositioned()
{
//std::cout << "~FGPositioned:" << mIdent << std::endl;
//removeFromIndices(this);
removeFromIndices(this);
}
SGBucket