Rewrite the spatial index to use a sparse octree on the cartesian coordinates of items.

* Fixes errors selecting 'nearest' items, including duplicates
* Stable behaviour around the poles and dateline
* Decently efficient, even for 'everything within 500nm' queries

src/Instrumentation/testgps.cxx

@@ -72,6 +72,21 @@ int main(int argc, char* argv[])
   
   SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
   
+  const FGAirport* egph = fgFindAirportID("EGPH");
+  SG_LOG(SG_GENERAL, SG_ALERT, "egph: cart location:" << egph->cart());
+  
+  FGAirport::AirportFilter af;
+  FGPositioned::List l = FGPositioned::findClosestN(egph->geod(), 20, 2000.0, &af);
+  for (unsigned int i=0; i<l.size(); ++i) {
+    SG_LOG(SG_GENERAL, SG_ALERT, "\t" << l[i]->ident() << "/" << l[i]->name());
+  }
+  
+  //l = FGPositioned::findWithinRange(egph->geod(), 500.0, &af);
+  //for (unsigned int i=0; i<l.size(); ++i) {
+  //  SG_LOG(SG_GENERAL, SG_ALERT, "\t" << l[i]->ident() << "/" << l[i]->name());
+  //}
+  
+
   FGRouteMgr* rm = new FGRouteMgr;
   globals->add_subsystem( "route-manager", rm );
   
@@ -84,7 +99,7 @@ int main(int argc, char* argv[])
   GPS* gps = new GPS(nd);
   globals->add_subsystem("gps", gps);
   
-  const FGAirport* egph = fgFindAirportID("EGPH");
+  
   testSetPosition(egph->geod());
   
   // startup the route manager

src/Navaids/positioned.cxx

@@ -25,9 +25,7 @@
 #include <map>
 #include <set>
 #include <algorithm> // for sort
-#include <locale> // for char-traits toupper
+#include <queue>
-
-#include <iostream>
 
 #include <boost/algorithm/string/case_conv.hpp>
 #include <boost/algorithm/string/predicate.hpp>
@@ -36,6 +34,7 @@
 #include <simgear/timing/timestamp.hxx>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/structure/exception.hxx>
+#include <simgear/math/SGBox.hxx>
 
 #include "positioned.hxx"
 
@@ -45,62 +44,337 @@ typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::co
 using std::lower_bound;
 using std::upper_bound;
 
-/**
- * Order positioned elements by type, then pointer address. This allows us to
- * use range searches (lower_ and upper_bound) to grab items of a particular
- * type out of bucket efficently.
- */
-class OrderByType
-{
-public:
-  bool operator()(const FGPositioned* a, const FGPositioned* b) const
-  {
-    if (a->type() == b->type()) return a < b;
-    return a->type() < b->type();
-  }
-};
-
-class LowerLimitOfType
-{
-public:
-  bool operator()(const FGPositioned* a, const FGPositioned::Type b) const
-  {
-    return a->type() < b;
-  }
-
-  bool operator()(const FGPositioned::Type a, const FGPositioned* b) const
-  {
-    return a < b->type();
-  }
-
-  // The operator below is required by VS2005 in debug mode
-  bool operator()(const FGPositioned* a, const FGPositioned* b) const
-  {
-    return a->type() < b->type();
-  }
-};
-
-
-typedef std::set<FGPositioned*, OrderByType> BucketEntry;
-typedef std::map<long int, BucketEntry> SpatialPositionedIndex;
-
 static NamedPositionedIndex global_identIndex;
 static NamedPositionedIndex global_nameIndex;
-static SpatialPositionedIndex global_spatialIndex;
 
-SpatialPositionedIndex::iterator
+//////////////////////////////////////////////////////////////////////////////
-bucketEntryForPositioned(FGPositioned* aPos)
+
+namespace Octree
 {
-  int bucketIndex = aPos->bucket().gen_index();
+
-  SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex);
+const double LEAF_SIZE = SG_NM_TO_METER * 8.0;
-  if (it != global_spatialIndex.end()) {
+const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE;
-    return it;
+
+typedef SGBox<double> SGBoxd;
+
+template<typename T1, typename T2>
+inline bool
+intersects(const SGVec3<T1>& v, const SGBox<T2>& box)
+{
+  if (v[0] < box.getMin()[0])
+    return false;
+  if (box.getMax()[0] < v[0])
+    return false;
+  if (v[1] < box.getMin()[1])
+    return false;
+  if (box.getMax()[1] < v[1])
+    return false;
+  if (v[2] < box.getMin()[2])
+    return false;
+  if (box.getMax()[2] < v[2])
+    return false;
+  return true;
 }
 
-  // create a new BucketEntry
+/**
-  return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry()));
+ * Decorate an object with a double value, and use that value to order 
+ * items, for the purpoises of the STL algorithms
+ */
+template <class T>
+class Ordered
+{
+public:
+    Ordered(const T& v, double x) :
+        _order(x),
+        _inner(v)
+    {
     }
     
+    Ordered(const Ordered<T>& a) :
+        _order(a._order),
+        _inner(a._inner)
+    {
+    }
+    
+    Ordered<T>& operator=(const Ordered<T>& a)
+    {
+        _order = a._order;
+        _inner = a._inner;
+        return *this;
+    }
+    
+    bool operator<(const Ordered<T>& other) const
+    {
+        return _order < other._order;
+    }
+    
+    bool operator>(const Ordered<T>& other) const
+    {
+        return _order > other._order;
+    }
+    
+    const T& get() const
+        { return _inner; }
+    
+    double order() const
+        { return _order; }
+    
+private:    
+    double _order;
+    T _inner;
+};
+
+class Node;
+typedef Ordered<Node*> OrderedNode;
+typedef std::greater<OrderedNode> FNPQCompare; 
+
+/**
+ * the priority queue is fundamental to our search algorithm. When searching,
+ * we know the front of the queue is the nearest unexpanded node (to the search
+ * location). The default STL pqueue returns the 'largest' item from top(), so
+ * to get the smallest, we need to replace the default Compare functor (less<>)
+ * with greater<>.
+ */
+typedef std::priority_queue<OrderedNode, std::vector<OrderedNode>, FNPQCompare> FindNearestPQueue;
+
+typedef Ordered<FGPositioned*> OrderedPositioned;
+typedef std::vector<OrderedPositioned> FindNearestResults;
+
+Node* global_spatialOctree = NULL;
+
+/**
+ * Octree node base class, tracks its bounding box and provides various
+ * queries relating to it
+ */
+class Node
+{
+public:
+    bool contains(const SGVec3d& aPos) const
+    {
+        return intersects(aPos, _box);
+    }
+
+    double distSqrToNearest(const SGVec3d& aPos) const
+    {
+        return distSqr(aPos, getClosestPoint(aPos));
+    }
+    
+    virtual void insert(FGPositioned* aP) = 0;
+    
+    SGVec3d getClosestPoint(const SGVec3d& aPos) const
+    {
+      SGVec3d r;
+      
+      for (unsigned int i=0;i<3; ++i) {
+        if (aPos[i] < _box.getMin()[i]) {
+          r[i] = _box.getMin()[i];
+        } else if (aPos[i] > _box.getMax()[i]) {
+          r[i] = _box.getMax()[i];
+        } else {
+          r[i] = aPos[i];
+        }
+      } // of axis iteration
+      
+      return r;
+    }
+    
+    virtual void visit(const SGVec3d& aPos, double aCutoff, 
+      FGPositioned::Filter* aFilter, 
+      FindNearestResults& aResults, FindNearestPQueue&) = 0;
+protected:
+    Node(const SGBoxd &aBox) :
+        _box(aBox)
+    {
+    }
+    
+    const SGBoxd _box;
+};
+
+class Leaf : public Node
+{
+public:
+    Leaf(const SGBoxd& aBox) :
+        Node(aBox)
+    {
+    }
+    
+    const FGPositioned::List& members() const
+    { return _members; }
+    
+    virtual void insert(FGPositioned* aP)
+    {
+        _members.push_back(aP);
+    }
+    
+    virtual void visit(const SGVec3d& aPos, double aCutoff, 
+      FGPositioned::Filter* aFilter, 
+      FindNearestResults& aResults, FindNearestPQueue&)
+    {
+        std::vector<Ordered<FGPositioned*> > results;
+        for (unsigned int i=0; i<_members.size(); ++i) {
+            FGPositioned* p = _members[i];
+            double d2 = distSqr(aPos, p->cart());
+            if (d2 > aCutoff) {
+                continue;
+            }
+            
+            if (aFilter) {
+              if (aFilter->hasTypeRange() && !aFilter->passType(p->type())) {
+                continue;
+              }
+      
+              if (!aFilter->pass(p)) {
+                continue;
+              }
+            } // of have a filter
+
+            aResults.push_back(OrderedPositioned(p, d2));
+        }
+      }
+private:
+    FGPositioned::List _members;
+};
+
+class Branch : public Node
+{
+public:
+    Branch(const SGBoxd& aBox) :
+        Node(aBox)
+    {
+        memset(children, 0, sizeof(Node*) * 8);
+    }
+    
+    virtual void insert(FGPositioned* aP)
+    {
+        SGVec3d cart(aP->cart());
+        assert(contains(cart));
+        int childIndex = 0;
+        
+        SGVec3d center(_box.getCenter());
+    // tests must match indices in SGbox::getCorner
+        if (cart.x() < center.x()) {
+            childIndex += 1;
+        }
+        
+        if (cart.y() < center.y()) {
+            childIndex += 2;
+        }
+        
+        if (cart.z() < center.z()) {
+            childIndex += 4;
+        }
+        
+        Node* child = children[childIndex];
+        if (!child) { // lazy building of children
+            SGBoxd cb(boxForChild(childIndex));            
+            double d2 = dot(cb.getSize(), cb.getSize());
+            if (d2 < LEAF_SIZE_SQR) {
+                child = new Leaf(cb);
+            } else {
+                child = new Branch(cb);
+            }
+            
+            children[childIndex] = child; 
+        }
+        
+        child->insert(aP);
+    }
+    
+    virtual void visit(const SGVec3d& aPos, double aCutoff, 
+      FGPositioned::Filter*, 
+      FindNearestResults&, FindNearestPQueue& aQ)
+    {    
+        for (unsigned int i=0; i<8; ++i) {
+            if (!children[i]) {
+                continue;
+            }
+            
+            double d2 = children[i]->distSqrToNearest(aPos);
+            if (d2 > aCutoff) {
+                continue; // exceeded cutoff
+            }
+            
+            aQ.push(Ordered<Node*>(children[i], d2));
+        } // of child iteration
+    }
+    
+    
+private:
+    /**
+     * Return the box for a child touching the specified corner
+     */
+    SGBoxd boxForChild(unsigned int aCorner) const
+    {
+        SGBoxd r(_box.getCenter());
+        r.expandBy(_box.getCorner(aCorner));
+        return r;
+    }
+    
+    Node* children[8];
+};
+
+void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
+{
+    aResults.clear();
+    FindNearestPQueue pq;
+    FindNearestResults results;
+    pq.push(Ordered<Node*>(global_spatialOctree, 0));
+    double cut = aCutoffM * aCutoffM;
+    
+    while (aResults.size() < aN) {
+        if (pq.empty()) {
+            break;
+        }
+        
+        Node* nd = pq.top().get();
+        pq.pop();
+        
+        nd->visit(aPos, cut, aFilter, results, pq);
+    } // of queue iteration
+    
+  // sort by distance
+    std::sort(results.begin(), results.end());
+    // depending on leaf population, we may have (slighty) more results
+    // than requested
+    unsigned int numResults = std::min((unsigned int) results.size(), aN);
+
+  // copy results out
+    aResults.resize(numResults);
+    for (unsigned int r=0; r<numResults; ++r) {
+      aResults[r] = results[r].get();
+    }
+}
+
+void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
+{
+    aResults.clear();
+    FindNearestPQueue pq;
+    FindNearestResults results;
+    pq.push(Ordered<Node*>(global_spatialOctree, 0));
+    double rng = aRangeM * aRangeM;
+    
+    while (!pq.empty()) {
+        Node* nd = pq.top().get();
+        pq.pop();
+        
+        nd->visit(aPos, rng, aFilter, results, pq);
+    } // of queue iteration
+    
+  // sort by distance
+    std::sort(results.begin(), results.end());
+    unsigned int numResults = results.size();
+
+  // copy results out
+    aResults.resize(numResults);
+    for (unsigned int r=0; r<numResults; ++r) {
+      aResults[r] = results[r].get();
+    }
+}
+
+} // of namespace Octree
+
+//////////////////////////////////////////////////////////////////////////////
+
 static void
 addToIndices(FGPositioned* aPos)
 {
@@ -115,9 +389,12 @@ addToIndices(FGPositioned* aPos)
                              std::make_pair(aPos->name(), aPos));
   }
 
-  
+  if (!Octree::global_spatialOctree) {
-  SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos);
+    double RADIUS_EARTH_M = 7000 * 1000.0; // 7000km is plenty
-  it->second.insert(aPos);
+    SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M);
+    Octree::global_spatialOctree = new Octree::Branch(SGBox<double>(-earthExtent, earthExtent));
+  }
+  Octree::global_spatialOctree->insert(aPos);
 }
 
 static void
@@ -148,88 +425,6 @@ removeFromIndices(FGPositioned* aPos)
       ++it;
     } // of multimap walk
   }
-
-  SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos);
-  sit->second.erase(aPos);
-}
-
-static void
-spatialFilterInBucket(const SGBucket& aBucket, FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
-{
-  SpatialPositionedIndex::const_iterator it;
-  it = global_spatialIndex.find(aBucket.gen_index());
-  if (it == global_spatialIndex.end()) {
-    return;
-  }
-  
-  BucketEntry::const_iterator l = it->second.begin();
-  BucketEntry::const_iterator u = it->second.end();
-
-  if (!aFilter) { // pass everything
-    aResult.insert(aResult.end(), l, u);
-    return;
-  }
-
-  if (aFilter->hasTypeRange()) {
-    // avoid many calls to the filter hook
-    l = lower_bound(it->second.begin(), it->second.end(), aFilter->minType(), LowerLimitOfType());
-    u = upper_bound(l, it->second.end(), aFilter->maxType(), LowerLimitOfType());
-  }
-
-  for ( ; l != u; ++l) {
-    if ((*aFilter)(*l)) {
-      aResult.push_back(*l);
-    }
-  }
-}
-
-static void
-spatialFind(const SGGeod& aPos, double aRange, 
-  FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
-{
-  SGBucket buck(aPos);
-  double lat = aPos.getLatitudeDeg(),
-    lon = aPos.getLongitudeDeg();
-  
-  int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
-  int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
-    
-  // loop over bucket range 
-  for ( int i=-bx; i<=bx; i++) {
-    for ( int j=-by; j<=by; j++) {
-      spatialFilterInBucket(sgBucketOffset(lon, lat, i, j), aFilter, aResult);
-    } // of j-iteration
-  } // of i-iteration  
-}
-
-/**
- */
-class RangePredictate
-{
-public:
-  RangePredictate(const SGGeod& aOrigin, double aRange) :
-    mOrigin(SGVec3d::fromGeod(aOrigin)),
-    mRangeSqr(aRange * aRange)
-  { ; }
-  
-  bool operator()(const FGPositionedRef& aPos)
-  {
-    double dSqr = distSqr(aPos->cart(), mOrigin);
-    return (dSqr > mRangeSqr);
-  }
-  
-private:
-  SGVec3d mOrigin;
-  double mRangeSqr;
-};
-
-static void
-filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult)
-{
-  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());
 }
 
 class DistanceOrdering
@@ -317,51 +512,6 @@ namedFindClosest(const NamedPositionedIndex& aIndex, const std::string& aName,
   return result;
 }
 
-static FGPositioned::List
-spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, FGPositioned::Filter* aFilter)
-{
-  FGPositioned::List result;
-  int radius = 1; // start at 1, radius 0 is handled explicitly
-  SGBucket buck;
-  double lat = aPos.getLatitudeDeg(),
-    lon = aPos.getLongitudeDeg();
-  // final cutoff is in metres, and scaled to account for testing the corners
-  // of the 'box' instead of the centre of each edge
-  double cutoffM = aCutoffNm * SG_NM_TO_METER * 1.5;
-  
-  // base case, simplifes loop to do it seperately here
-  spatialFilterInBucket(sgBucketOffset(lon, lat, 0, 0), aFilter, result);
-
-  for (;result.size() < aN; ++radius) {
-    // cutoff check
-    double az1, az2, d1, d2;
-    SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, -radius, -radius).get_center(), az1, az2, d1);
-    SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, radius, radius).get_center(), az1, az2, d2);  
-      
-    if ((d1 > cutoffM) && (d2 > cutoffM)) {
-      //std::cerr << "spatialGetClosest terminating due to range cutoff" << std::endl;
-      break;
-    }
-    
-    FGPositioned::List hits;
-    for ( int i=-radius; i<=radius; i++) {
-      spatialFilterInBucket(sgBucketOffset(lon, lat, i, -radius), aFilter, hits);
-      spatialFilterInBucket(sgBucketOffset(lon, lat, -radius, i), aFilter, hits);
-      spatialFilterInBucket(sgBucketOffset(lon, lat, i, radius), aFilter, hits);
-      spatialFilterInBucket(sgBucketOffset(lon, lat, radius, i), aFilter, hits);
-    }
-
-    result.insert(result.end(), hits.begin(), hits.end()); // append
-  } // of outer loop
-  
-  sortByDistance(aPos, result);
-  if (result.size() > aN) {
-    result.resize(aN); // truncate at requested number of matches
-  }
-  
-  return result;
-}
-
 //////////////////////////////////////////////////////////////////////////////
 
 class OrderByName
@@ -598,6 +748,7 @@ FGPositioned::Type FGPositioned::typeFromName(const std::string& aName)
   // aliases
     {"waypoint", WAYPOINT},
     {"apt", AIRPORT},
+    {"arpt", AIRPORT},
     {"any", INVALID},
     {"all", INVALID},
     
@@ -656,8 +807,8 @@ FGPositioned::List
 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 {
   List result;
-  spatialFind(aPos, aRangeNm, aFilter, result);
+  Octree::findAllWithinRange(SGVec3d::fromGeod(aPos), 
-  filterListByRange(aPos, aRangeNm, result);
+    aRangeNm * SG_NM_TO_METER, aFilter, result);
   return result;
 }
 
@@ -676,7 +827,7 @@ FGPositioned::findAllWithNameSortedByRange(const std::string& aName, const SGGeo
 FGPositionedRef
 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 {
-   FGPositioned::List l(spatialGetClosest(aPos, 1, aCutoffNm, aFilter));
+   List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
    if (l.empty()) {
       return NULL;
    }
@@ -688,7 +839,9 @@ FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 FGPositioned::List
 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 {
-  return spatialGetClosest(aPos, aN, aCutoffNm, aFilter);
+  List result;
+  Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result);
+  return result;
 }
 
 FGPositionedRef
@@ -789,8 +942,8 @@ findClosestWithPartial(const SGGeod& aPos, FGPositioned::Filter* aFilter, int aO
 {
   // why aOffset +2 ? at offset=3, we want the fourth search result, but also
   // to know if the fifth result exists (to set aNext flag for iterative APIs)
-  FGPositioned::List matches = 
+  FGPositioned::List matches;
-    spatialGetClosest(aPos, aOffset + 2, 1000.0, aFilter);
+  Octree::findNearestN(SGVec3d::fromGeod(aPos), aOffset + 2, 1000 * SG_NM_TO_METER, aFilter, matches);
   
   if ((int) matches.size() <= aOffset) {
     SG_LOG(SG_GENERAL, SG_INFO, "findClosestWithPartial, couldn't match enough with prefix");