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