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