James Turner:

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.

src/Autopilot/route_mgr.cxx

@@ -281,24 +281,24 @@ bool FGRouteMgr::build() {
 }
 
 
-int FGRouteMgr::new_waypoint( const string& target, int n ) {
+void FGRouteMgr::new_waypoint( const string& target, int n ) {
-    SGWayPoint *wp = 0;
+    SGWayPoint* wp = make_waypoint( target );
-    int type = make_waypoint( &wp, target );
+    if (!wp) {
-
+        return;
-    if (wp) {
+    }
-        add_waypoint( *wp, n );
+    
-        delete wp;
+    add_waypoint( *wp, n );
-
+    delete wp;
-        if ( !near_ground() )
+
-            fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
+    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 new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target );
-        return 1;
+    }    
-    }
 
-    // check for airport id
+    SGGeod basePosition;
-    const FGAirport *apt = fgFindAirportID( target );
+    if (route->size() > 0) {
-    if (apt) {
+        SGWayPoint wp = get_waypoint(route->size()-1);
-        SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (airport) = " << target );
+        basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat());
-        *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();
     } else {
-        lat = fgGetNode("/position/latitude-deg")->getDoubleValue();
+        // route is empty, use current position
-        lon = fgGetNode("/position/longitude-deg")->getDoubleValue();
+        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 ) ) {
+    FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition);
-        SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (fix) = " << target );
+    if (!p) {
-        *wp = new SGWayPoint( f->get_lon(), f->get_lat(), alt, SGWayPoint::WGS84, target );
+        SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target);
-        return 3;
+        return NULL;
     }
-
+    
-    // Try finding a nav matching the ID
+    return new SGWayPoint(p->longitude(), p->latitude(), p->elevation(), SGWayPoint::WGS84, target);
-    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;
 }
 
 

src/Autopilot/route_mgr.hxx

@@ -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 );
 

src/Navaids/positioned.cxx

@@ -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()));
+    double d, az1, az2;
-    bool ok = (mOrigin.distance3Dsquared(p) > mRangeSquared);
+    SGGeodesy::inverse(aPos->geod(), mOrigin, az1, az2, d);
-    if (ok) {
+    return (d > mRange);
-      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;
   }
   
 private:
-  Point3D mOrigin;
+  SGGeod mOrigin;
-  double mRangeSquared;
+  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())) < 
+    double dA, dB, az1, az2;
-      mPos.distance3Dsquared(Point3D::fromSGGeod(b->geod()));
+    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));
+  NamedPositionedIndex::const_iterator it = range.first;
-  
+    
-  for (; range.first != range.second; ++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 d, az1, az2;
-    double ds = origin.distance3Dsquared(p);
+    SGGeodesy::inverse(aOrigin, it->second->geod(), az2, az2, d);
-    if (ds < minDist) {
+    if (d < minDist) {
-      minDist = ds;
+      minDist = d;
-      result = range.first->second;
+      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);
+  addToIndices(this);
-  //SGReferenced::get(this); // hold an owning ref, for the moment
+  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);
+  addToIndices(this);
-  //SGReferenced::get(this); // hold an owning ref, for the moment
+  SGReferenced::get(this); // hold an owning ref, for the moment
 }
 
 FGPositioned::~FGPositioned()
 {
-  //std::cout << "~FGPositioned:" << mIdent << std::endl;
+  removeFromIndices(this);
-  //removeFromIndices(this);
 }
 
 SGBucket