src/Navaids/positioned.cxx

   1 // positioned.cxx - base class for objects which are positioned
   2 //
   3 // Copyright (C) 2008 James Turner
   4 //
   5 // This program is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU General Public License as
   7 // published by the Free Software Foundation; either version 2 of the
   8 // License, or (at your option) any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful, but
  11 // WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  18 //
  19 // $Id$
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include "config.h"
  23 #endif
  24
  25 #include "positioned.hxx"
  26
  27 #include <map>
  28 #include <set>
  29 #include <algorithm> // for sort
  30 #include <queue>
  31
  32 #include <boost/algorithm/string/case_conv.hpp>
  33 #include <boost/algorithm/string/predicate.hpp>
  34
  35 #include <osg/Math> // for osg::isNaN
  36
  37 #include <simgear/timing/timestamp.hxx>
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/structure/exception.hxx>
  40 #include <simgear/math/SGGeometry.hxx>
  41
  42
  43
  44 typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex;
  45 typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> NamedIndexRange;
  46
  47 using std::lower_bound;
  48 using std::upper_bound;
  49
  50 static NamedPositionedIndex global_identIndex;
  51 static NamedPositionedIndex global_nameIndex;
  52
  53 //////////////////////////////////////////////////////////////////////////////
  54
  55 namespace Octree
  56 {
  57
  58 const double LEAF_SIZE = SG_NM_TO_METER * 8.0;
  59 const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE;
  60
  61 /**
  62  * Decorate an object with a double value, and use that value to order
  63  * items, for the purpoises of the STL algorithms
  64  */
  65 template <class T>
  66 class Ordered
  67 {
  68 public:
  69     Ordered(const T& v, double x) :
  70         _order(x),
  71         _inner(v)
  72     {
  73     }
  74
  75     Ordered(const Ordered<T>& a) :
  76         _order(a._order),
  77         _inner(a._inner)
  78     {
  79     }
  80
  81     Ordered<T>& operator=(const Ordered<T>& a)
  82     {
  83         _order = a._order;
  84         _inner = a._inner;
  85         return *this;
  86     }
  87
  88     bool operator<(const Ordered<T>& other) const
  89     {
  90         return _order < other._order;
  91     }
  92
  93     bool operator>(const Ordered<T>& other) const
  94     {
  95         return _order > other._order;
  96     }
  97
  98     const T& get() const
  99         { return _inner; }
 100
 101     double order() const
 102         { return _order; }
 103
 104 private:
 105     double _order;
 106     T _inner;
 107 };
 108
 109 class Node;
 110 typedef Ordered<Node*> OrderedNode;
 111 typedef std::greater<OrderedNode> FNPQCompare;
 112
 113 /**
 114  * the priority queue is fundamental to our search algorithm. When searching,
 115  * we know the front of the queue is the nearest unexpanded node (to the search
 116  * location). The default STL pqueue returns the 'largest' item from top(), so
 117  * to get the smallest, we need to replace the default Compare functor (less<>)
 118  * with greater<>.
 119  */
 120 typedef std::priority_queue<OrderedNode, std::vector<OrderedNode>, FNPQCompare> FindNearestPQueue;
 121
 122 typedef Ordered<FGPositioned*> OrderedPositioned;
 123 typedef std::vector<OrderedPositioned> FindNearestResults;
 124
 125 Node* global_spatialOctree = NULL;
 126
 127 /**
 128  * Octree node base class, tracks its bounding box and provides various
 129  * queries relating to it
 130  */
 131 class Node
 132 {
 133 public:
 134     bool contains(const SGVec3d& aPos) const
 135     {
 136         return intersects(aPos, _box);
 137     }
 138
 139     double distSqrToNearest(const SGVec3d& aPos) const
 140     {
 141         return distSqr(aPos, _box.getClosestPoint(aPos));
 142     }
 143
 144     virtual void insert(FGPositioned* aP) = 0;
 145
 146     virtual void visit(const SGVec3d& aPos, double aCutoff,
 147       FGPositioned::Filter* aFilter,
 148       FindNearestResults& aResults, FindNearestPQueue&) = 0;
 149 protected:
 150     Node(const SGBoxd &aBox) :
 151         _box(aBox)
 152     {
 153     }
 154
 155     const SGBoxd _box;
 156 };
 157
 158 class Leaf : public Node
 159 {
 160 public:
 161     Leaf(const SGBoxd& aBox) :
 162         Node(aBox)
 163     {
 164     }
 165
 166     const FGPositioned::List& members() const
 167     { return _members; }
 168
 169     virtual void insert(FGPositioned* aP)
 170     {
 171         _members.push_back(aP);
 172     }
 173
 174     virtual void visit(const SGVec3d& aPos, double aCutoff,
 175       FGPositioned::Filter* aFilter,
 176       FindNearestResults& aResults, FindNearestPQueue&)
 177     {
 178         int previousResultsSize = aResults.size();
 179         int addedCount = 0;
 180
 181         for (unsigned int i=0; i<_members.size(); ++i) {
 182             FGPositioned* p = _members[i];
 183             double d2 = distSqr(aPos, p->cart());
 184             if (d2 > aCutoff) {
 185                 continue;
 186             }
 187
 188             if (aFilter) {
 189               if (aFilter->hasTypeRange() && !aFilter->passType(p->type())) {
 190                 continue;
 191               }
 192
 193               if (!aFilter->pass(p)) {
 194                 continue;
 195               }
 196             } // of have a filter
 197
 198             ++addedCount;
 199             aResults.push_back(OrderedPositioned(p, d2));
 200         }
 201
 202         if (addedCount == 0) {
 203           return;
 204         }
 205
 206       // keep aResults sorted
 207         // sort the new items, usually just one or two items
 208         std::sort(aResults.begin() + previousResultsSize, aResults.end());
 209
 210         // merge the two sorted ranges together - in linear time
 211         std::inplace_merge(aResults.begin(),
 212           aResults.begin() + previousResultsSize, aResults.end());
 213       }
 214 private:
 215     FGPositioned::List _members;
 216 };
 217
 218 class Branch : public Node
 219 {
 220 public:
 221     Branch(const SGBoxd& aBox) :
 222         Node(aBox)
 223     {
 224         memset(children, 0, sizeof(Node*) * 8);
 225     }
 226
 227     virtual void insert(FGPositioned* aP)
 228     {
 229         SGVec3d cart(aP->cart());
 230         assert(contains(cart));
 231         int childIndex = 0;
 232
 233         SGVec3d center(_box.getCenter());
 234     // tests must match indices in SGbox::getCorner
 235         if (cart.x() < center.x()) {
 236             childIndex += 1;
 237         }
 238
 239         if (cart.y() < center.y()) {
 240             childIndex += 2;
 241         }
 242
 243         if (cart.z() < center.z()) {
 244             childIndex += 4;
 245         }
 246
 247         Node* child = children[childIndex];
 248         if (!child) { // lazy building of children
 249             SGBoxd cb(boxForChild(childIndex));
 250             double d2 = dot(cb.getSize(), cb.getSize());
 251             if (d2 < LEAF_SIZE_SQR) {
 252                 child = new Leaf(cb);
 253             } else {
 254                 child = new Branch(cb);
 255             }
 256
 257             children[childIndex] = child;
 258         }
 259
 260         child->insert(aP);
 261     }
 262
 263     virtual void visit(const SGVec3d& aPos, double aCutoff,
 264       FGPositioned::Filter*,
 265       FindNearestResults&, FindNearestPQueue& aQ)
 266     {
 267         for (unsigned int i=0; i<8; ++i) {
 268             if (!children[i]) {
 269                 continue;
 270             }
 271
 272             double d2 = children[i]->distSqrToNearest(aPos);
 273             if (d2 > aCutoff) {
 274                 continue; // exceeded cutoff
 275             }
 276
 277             aQ.push(Ordered<Node*>(children[i], d2));
 278         } // of child iteration
 279     }
 280
 281
 282 private:
 283     /**
 284      * Return the box for a child touching the specified corner
 285      */
 286     SGBoxd boxForChild(unsigned int aCorner) const
 287     {
 288         SGBoxd r(_box.getCenter());
 289         r.expandBy(_box.getCorner(aCorner));
 290         return r;
 291     }
 292
 293     Node* children[8];
 294 };
 295
 296 void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
 297 {
 298     aResults.clear();
 299     FindNearestPQueue pq;
 300     FindNearestResults results;
 301     pq.push(Ordered<Node*>(global_spatialOctree, 0));
 302     double cut = aCutoffM * aCutoffM;
 303
 304     while (!pq.empty()) {
 305         if (!results.empty()) {
 306           // terminate the search if we have sufficent results, and we are
 307           // sure no node still on the queue contains a closer match
 308           double furthestResultOrder = results.back().order();
 309           if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
 310             break;
 311           }
 312         }
 313
 314         Node* nd = pq.top().get();
 315         pq.pop();
 316
 317         nd->visit(aPos, cut, aFilter, results, pq);
 318     } // of queue iteration
 319
 320     // depending on leaf population, we may have (slighty) more results
 321     // than requested
 322     unsigned int numResults = std::min((unsigned int) results.size(), aN);
 323   // copy results out
 324     aResults.resize(numResults);
 325     for (unsigned int r=0; r<numResults; ++r) {
 326       aResults[r] = results[r].get();
 327     }
 328 }
 329
 330 void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
 331 {
 332     aResults.clear();
 333     FindNearestPQueue pq;
 334     FindNearestResults results;
 335     pq.push(Ordered<Node*>(global_spatialOctree, 0));
 336     double rng = aRangeM * aRangeM;
 337
 338     while (!pq.empty()) {
 339         Node* nd = pq.top().get();
 340         pq.pop();
 341
 342         nd->visit(aPos, rng, aFilter, results, pq);
 343     } // of queue iteration
 344
 345     unsigned int numResults = results.size();
 346   // copy results out
 347     aResults.resize(numResults);
 348     for (unsigned int r=0; r<numResults; ++r) {
 349       aResults[r] = results[r].get();
 350     }
 351 }
 352
 353 } // of namespace Octree
 354
 355 //////////////////////////////////////////////////////////////////////////////
 356
 357 static void
 358 addToIndices(FGPositioned* aPos)
 359 {
 360   assert(aPos);
 361   if (!aPos->ident().empty()) {
 362     std::string u(boost::to_upper_copy(aPos->ident()));
 363
 364     global_identIndex.insert(global_identIndex.begin(),
 365       std::make_pair(u, aPos));
 366   }
 367
 368   if (!aPos->name().empty()) {
 369     std::string u(boost::to_upper_copy(aPos->name()));
 370
 371     global_nameIndex.insert(global_nameIndex.begin(),
 372                              std::make_pair(u, aPos));
 373   }
 374
 375   if (!Octree::global_spatialOctree) {
 376     double RADIUS_EARTH_M = 7000 * 1000.0; // 7000km is plenty
 377     SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M);
 378     Octree::global_spatialOctree = new Octree::Branch(SGBox<double>(-earthExtent, earthExtent));
 379   }
 380   Octree::global_spatialOctree->insert(aPos);
 381 }
 382
 383 static void
 384 removeFromIndices(FGPositioned* aPos)
 385 {
 386   assert(aPos);
 387
 388   if (!aPos->ident().empty()) {
 389     std::string u(boost::to_upper_copy(aPos->ident()));
 390     NamedPositionedIndex::iterator it = global_identIndex.find(u);
 391     while (it != global_identIndex.end() && (it->first == u)) {
 392       if (it->second == aPos) {
 393         global_identIndex.erase(it);
 394         break;
 395       }
 396
 397       ++it;
 398     } // of multimap walk
 399   }
 400
 401   if (!aPos->name().empty()) {
 402     std::string u(boost::to_upper_copy(aPos->name()));
 403     NamedPositionedIndex::iterator it = global_nameIndex.find(u);
 404     while (it != global_nameIndex.end() && (it->first == u)) {
 405       if (it->second == aPos) {
 406         global_nameIndex.erase(it);
 407         break;
 408       }
 409
 410       ++it;
 411     } // of multimap walk
 412   }
 413 }
 414
 415 //////////////////////////////////////////////////////////////////////////////
 416
 417 class OrderByName
 418 {
 419 public:
 420   bool operator()(FGPositioned* a, FGPositioned* b) const
 421   {
 422     return a->name() < b->name();
 423   }
 424 };
 425
 426 void findInIndex(NamedPositionedIndex& aIndex, const std::string& aFind, std::vector<FGPositioned*>& aResult)
 427 {
 428   NamedPositionedIndex::const_iterator it = aIndex.begin();
 429   NamedPositionedIndex::const_iterator end = aIndex.end();
 430
 431   bool haveFilter = !aFind.empty();
 432
 433   for (; it != end; ++it) {
 434     FGPositioned::Type ty = it->second->type();
 435     if ((ty < FGPositioned::AIRPORT) || (ty > FGPositioned::SEAPORT)) {
 436       continue;
 437     }
 438
 439     if (haveFilter && it->first.find(aFind) == std::string::npos) {
 440       continue;
 441     }
 442
 443     aResult.push_back(it->second);
 444   } // of index iteration
 445 }
 446
 447 /**
 448  * A special purpose helper (imported by FGAirport::searchNamesAndIdents) to
 449  * implement the AirportList dialog. It's unfortunate that it needs to reside
 450  * here, but for now it's least ugly solution.
 451  */
 452 char** searchAirportNamesAndIdents(const std::string& aFilter)
 453 {
 454 // note this is a vector of raw pointers, not smart pointers, because it
 455 // may get very large and smart-pointer-atomicity-locking then becomes a
 456 // bottleneck for this case.
 457   std::vector<FGPositioned*> matches;
 458   if (!aFilter.empty()) {
 459     std::string filter = boost::to_upper_copy(aFilter);
 460     findInIndex(global_identIndex, filter, matches);
 461     findInIndex(global_nameIndex, filter, matches);
 462   } else {
 463
 464     findInIndex(global_identIndex, std::string(), matches);
 465   }
 466
 467 // sort alphabetically on name
 468   std::sort(matches.begin(), matches.end(), OrderByName());
 469
 470 // convert results to format comptible with puaList
 471   unsigned int numMatches = matches.size();
 472   char** result = new char*[numMatches + 1];
 473   result[numMatches] = NULL; // end-of-list marker
 474
 475   // nasty code to avoid excessive string copying and allocations.
 476   // We format results as follows (note whitespace!):
 477   //   ' name-of-airport-chars   (ident)'
 478   // so the total length is:
 479   //    1 + strlen(name) + 4 + strlen(icao) + 1 + 1 (for the null)
 480   // which gives a grand total of 7 + name-length + icao-length.
 481   // note the ident can be three letters (non-ICAO local strip), four
 482   // (default ICAO) or more (extended format ICAO)
 483   for (unsigned int i=0; i<numMatches; ++i) {
 484     int nameLength = matches[i]->name().size();
 485     int icaoLength = matches[i]->ident().size();
 486     char* entry = new char[7 + nameLength + icaoLength];
 487     char* dst = entry;
 488     *dst++ = ' ';
 489     memcpy(dst, matches[i]->name().c_str(), nameLength);
 490     dst += nameLength;
 491     *dst++ = ' ';
 492     *dst++ = ' ';
 493     *dst++ = ' ';
 494     *dst++ = '(';
 495     memcpy(dst, matches[i]->ident().c_str(), icaoLength);
 496     dst += icaoLength;
 497     *dst++ = ')';
 498     *dst++ = 0;
 499     result[i] = entry;
 500   }
 501
 502   return result;
 503 }
 504
 505 static void validateSGGeod(const SGGeod& geod)
 506 {
 507   if (osg::isNaN(geod.getLatitudeDeg()) ||
 508       osg::isNaN(geod.getLongitudeDeg()))
 509   {
 510     throw sg_range_exception("position is invalid, NaNs");
 511   }
 512 }
 513
 514 ///////////////////////////////////////////////////////////////////////////////
 515
 516 bool
 517 FGPositioned::Filter::hasTypeRange() const
 518 {
 519   assert(minType() <= maxType());
 520   return (minType() != INVALID) && (maxType() != INVALID);
 521 }
 522
 523 bool
 524 FGPositioned::Filter::passType(Type aTy) const
 525 {
 526   assert(hasTypeRange());
 527   return (minType() <= aTy) && (maxType() >= aTy);
 528 }
 529
 530 static FGPositioned::List
 531 findAll(const NamedPositionedIndex& aIndex,
 532                              const std::string& aName,
 533                              FGPositioned::Filter* aFilter,
 534                              bool aExact)
 535 {
 536   FGPositioned::List result;
 537   if (aName.empty()) {
 538     return result;
 539   }
 540
 541   std::string name = boost::to_upper_copy(aName);
 542   NamedPositionedIndex::const_iterator upperBound;
 543
 544   if (aExact) {
 545     upperBound = aIndex.upper_bound(name);
 546   } else {
 547     std::string upperBoundId = name;
 548     upperBoundId[upperBoundId.size()-1]++;
 549     upperBound = aIndex.lower_bound(upperBoundId);
 550   }
 551
 552   NamedPositionedIndex::const_iterator it = aIndex.lower_bound(name);
 553
 554   for (; it != upperBound; ++it) {
 555     FGPositionedRef candidate = it->second;
 556     if (aFilter) {
 557       if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) {
 558         continue;
 559       }
 560
 561       if (!aFilter->pass(candidate)) {
 562         continue;
 563       }
 564     }
 565
 566     result.push_back(candidate);
 567   }
 568
 569   return result;
 570 }
 571
 572 ///////////////////////////////////////////////////////////////////////////////
 573
 574 FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) :
 575   mPosition(aPos),
 576   mType(ty),
 577   mIdent(aIdent)
 578 {
 579 }
 580
 581 void FGPositioned::init(bool aIndexed)
 582 {
 583   SGReferenced::get(this); // hold an owning ref, for the moment
 584   mCart = SGVec3d::fromGeod(mPosition);
 585
 586   if (aIndexed) {
 587     assert(mType != TAXIWAY && mType != PAVEMENT);
 588     addToIndices(this);
 589   }
 590 }
 591
 592 FGPositioned::~FGPositioned()
 593 {
 594   //std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl;
 595   removeFromIndices(this);
 596 }
 597
 598 FGPositioned*
 599 FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos)
 600 {
 601   FGPositioned* wpt = new FGPositioned(WAYPOINT, aIdent, aPos);
 602   wpt->init(true);
 603   return wpt;
 604 }
 605
 606 const SGVec3d&
 607 FGPositioned::cart() const
 608 {
 609   return mCart;
 610 }
 611
 612 FGPositioned::Type FGPositioned::typeFromName(const std::string& aName)
 613 {
 614   if (aName.empty() || (aName == "")) {
 615     return INVALID;
 616   }
 617
 618   typedef struct {
 619     const char* _name;
 620     Type _ty;
 621   } NameTypeEntry;
 622
 623   const NameTypeEntry names[] = {
 624     {"airport", AIRPORT},
 625     {"vor", VOR},
 626     {"ndb", NDB},
 627     {"wpt", WAYPOINT},
 628     {"fix", FIX},
 629     {"tacan", TACAN},
 630     {"dme", DME},
 631   // aliases
 632     {"waypoint", WAYPOINT},
 633     {"apt", AIRPORT},
 634     {"arpt", AIRPORT},
 635     {"any", INVALID},
 636     {"all", INVALID},
 637
 638     {NULL, INVALID}
 639   };
 640
 641   std::string lowerName(boost::to_lower_copy(aName));
 642
 643   for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) {
 644     if (::strcmp(n->_name, lowerName.c_str()) == 0) {
 645       return n->_ty;
 646     }
 647   }
 648
 649   SG_LOG(SG_GENERAL, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName);
 650   return INVALID;
 651 }
 652
 653 const char* FGPositioned::nameForType(Type aTy)
 654 {
 655  switch (aTy) {
 656  case RUNWAY: return "runway";
 657  case TAXIWAY: return "taxiway";
 658  case PAVEMENT: return "pavement";
 659  case PARK_STAND: return "parking stand";
 660  case FIX: return "fix";
 661  case VOR: return "VOR";
 662  case NDB: return "NDB";
 663  case ILS: return "ILS";
 664  case LOC: return "localiser";
 665  case GS: return "glideslope";
 666  case OM: return "outer-marker";
 667  case MM: return "middle-marker";
 668  case IM: return "inner-marker";
 669  case AIRPORT: return "airport";
 670  case HELIPORT: return "heliport";
 671  case SEAPORT: return "seaport";
 672  case WAYPOINT: return "waypoint";
 673  case DME: return "dme";
 674  case TACAN: return "tacan";
 675  default:
 676   return "unknown";
 677  }
 678 }
 679
 680 ///////////////////////////////////////////////////////////////////////////////
 681 // search / query functions
 682
 683 FGPositionedRef
 684 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 685 {
 686   validateSGGeod(aPos);
 687
 688   FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true));
 689   if (r.empty()) {
 690     return FGPositionedRef();
 691   }
 692
 693   sortByRange(r, aPos);
 694   return r.front();
 695 }
 696
 697 FGPositioned::List
 698 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 699 {
 700   validateSGGeod(aPos);
 701
 702   List result;
 703   Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
 704     aRangeNm * SG_NM_TO_METER, aFilter, result);
 705   return result;
 706 }
 707
 708 FGPositioned::List
 709 FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
 710 {
 711   return findAll(global_identIndex, aIdent, aFilter, aExact);
 712 }
 713
 714 FGPositioned::List
 715 FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
 716 {
 717   return findAll(global_nameIndex, aName, aFilter, aExact);
 718 }
 719
 720 FGPositionedRef
 721 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 722 {
 723   validateSGGeod(aPos);
 724
 725   List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
 726   if (l.empty()) {
 727     return NULL;
 728   }
 729
 730   assert(l.size() == 1);
 731   return l.front();
 732 }
 733
 734 FGPositioned::List
 735 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 736 {
 737   validateSGGeod(aPos);
 738
 739   List result;
 740   Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result);
 741   return result;
 742 }
 743
 744 FGPositionedRef
 745 FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
 746 {
 747   if (aId.empty()) {
 748     return NULL;
 749   }
 750
 751   std::string id(boost::to_upper_copy(aId));
 752
 753   // It is essential to bound our search, to avoid iterating all the way to the end of the database.
 754   // Do this by generating a second ID with the final character incremented by 1.
 755   // e.g., if the partial ID is "KI", we wish to search "KIxxx" but not "KJ".
 756   std::string upperBoundId = id;
 757   upperBoundId[upperBoundId.size()-1]++;
 758   NamedPositionedIndex::const_iterator upperBound = global_identIndex.lower_bound(upperBoundId);
 759
 760   NamedIndexRange range = global_identIndex.equal_range(id);
 761   while (range.first != upperBound) {
 762     for (; range.first != range.second; ++range.first) {
 763       FGPositionedRef candidate = range.first->second;
 764       if (aCur == candidate) {
 765         aCur = NULL; // found our start point, next match will pass
 766         continue;
 767       }
 768
 769       if (aFilter) {
 770         if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) {
 771           continue;
 772         }
 773
 774         if (!aFilter->pass(candidate)) {
 775           continue;
 776         }
 777       }
 778
 779       if (!aCur) {
 780         return candidate;
 781       }
 782     }
 783
 784     // Unable to match the filter with this range - try the next range.
 785     range = global_identIndex.equal_range(range.second->first);
 786   }
 787
 788   return NULL; // Reached the end of the valid sequence with no match.
 789 }
 790
 791 void
 792 FGPositioned::sortByRange(List& aResult, const SGGeod& aPos)
 793 {
 794   validateSGGeod(aPos);
 795
 796   SGVec3d cartPos(SGVec3d::fromGeod(aPos));
 797 // computer ordering values
 798   Octree::FindNearestResults r;
 799   List::iterator it = aResult.begin(), lend = aResult.end();
 800   for (; it != lend; ++it) {
 801     double d2 = distSqr((*it)->cart(), cartPos);
 802     r.push_back(Octree::OrderedPositioned(*it, d2));
 803   }
 804
 805 // sort
 806   std::sort(r.begin(), r.end());
 807
 808 // convert to a plain list
 809   unsigned int count = aResult.size();
 810   for (unsigned int i=0; i<count; ++i) {
 811     aResult[i] = r[i].get();
 812   }
 813 }