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     {"ndb", NDB},
 633     {"wpt", WAYPOINT},
 634     {"fix", FIX},
 635     {"tacan", TACAN},
 636     {"dme", DME},
 637     {"atis", FREQ_ATIS},
 638     {"awos", FREQ_AWOS},
 639     {"tower", FREQ_TOWER},
 640     {"ground", FREQ_GROUND},
 641     {"approach", FREQ_APP_DEP},
 642     {"departure", FREQ_APP_DEP},
 643   // aliases
 644     {"gnd", FREQ_GROUND},
 645     {"twr", FREQ_TOWER},
 646     {"waypoint", WAYPOINT},
 647     {"apt", AIRPORT},
 648     {"arpt", AIRPORT},
 649     {"any", INVALID},
 650     {"all", INVALID},
 651
 652     {NULL, INVALID}
 653   };
 654
 655   std::string lowerName(boost::to_lower_copy(aName));
 656
 657   for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) {
 658     if (::strcmp(n->_name, lowerName.c_str()) == 0) {
 659       return n->_ty;
 660     }
 661   }
 662
 663   SG_LOG(SG_GENERAL, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName);
 664   return INVALID;
 665 }
 666
 667 const char* FGPositioned::nameForType(Type aTy)
 668 {
 669  switch (aTy) {
 670  case RUNWAY: return "runway";
 671  case TAXIWAY: return "taxiway";
 672  case PAVEMENT: return "pavement";
 673  case PARK_STAND: return "parking stand";
 674  case FIX: return "fix";
 675  case VOR: return "VOR";
 676  case NDB: return "NDB";
 677  case ILS: return "ILS";
 678  case LOC: return "localiser";
 679  case GS: return "glideslope";
 680  case OM: return "outer-marker";
 681  case MM: return "middle-marker";
 682  case IM: return "inner-marker";
 683  case AIRPORT: return "airport";
 684  case HELIPORT: return "heliport";
 685  case SEAPORT: return "seaport";
 686  case WAYPOINT: return "waypoint";
 687  case DME: return "dme";
 688  case TACAN: return "tacan";
 689  case FREQ_TOWER: return "tower";
 690  case FREQ_ATIS: return "atis";
 691  case FREQ_AWOS: return "awos";
 692  case FREQ_GROUND: return "ground";
 693  case FREQ_CLEARANCE: return "clearance";
 694  case FREQ_UNICOM: return "unicom";
 695  case FREQ_APP_DEP: return "approach-departure";
 696  default:
 697   return "unknown";
 698  }
 699 }
 700
 701 flightgear::PositionedBinding*
 702 FGPositioned::createBinding(SGPropertyNode* node) const
 703 {
 704     return new flightgear::PositionedBinding(this, node);
 705 }
 706
 707 ///////////////////////////////////////////////////////////////////////////////
 708 // search / query functions
 709
 710 FGPositionedRef
 711 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 712 {
 713   validateSGGeod(aPos);
 714
 715   FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true));
 716   if (r.empty()) {
 717     return FGPositionedRef();
 718   }
 719
 720   sortByRange(r, aPos);
 721   return r.front();
 722 }
 723
 724 FGPositioned::List
 725 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 726 {
 727   validateSGGeod(aPos);
 728
 729   List result;
 730   Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
 731     aRangeNm * SG_NM_TO_METER, aFilter, result);
 732   return result;
 733 }
 734
 735 FGPositioned::List
 736 FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
 737 {
 738   return findAll(global_identIndex, aIdent, aFilter, aExact);
 739 }
 740
 741 FGPositioned::List
 742 FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
 743 {
 744   return findAll(global_nameIndex, aName, aFilter, aExact);
 745 }
 746
 747 FGPositionedRef
 748 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 749 {
 750   validateSGGeod(aPos);
 751
 752   List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
 753   if (l.empty()) {
 754     return NULL;
 755   }
 756
 757   assert(l.size() == 1);
 758   return l.front();
 759 }
 760
 761 FGPositioned::List
 762 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 763 {
 764   validateSGGeod(aPos);
 765
 766   List result;
 767   Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result);
 768   return result;
 769 }
 770
 771 FGPositionedRef
 772 FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
 773 {
 774   if (aId.empty()) {
 775     return NULL;
 776   }
 777
 778   std::string id(boost::to_upper_copy(aId));
 779
 780   // It is essential to bound our search, to avoid iterating all the way to the end of the database.
 781   // Do this by generating a second ID with the final character incremented by 1.
 782   // e.g., if the partial ID is "KI", we wish to search "KIxxx" but not "KJ".
 783   std::string upperBoundId = id;
 784   upperBoundId[upperBoundId.size()-1]++;
 785   NamedPositionedIndex::const_iterator upperBound = global_identIndex.lower_bound(upperBoundId);
 786
 787   NamedIndexRange range = global_identIndex.equal_range(id);
 788   while (range.first != upperBound) {
 789     for (; range.first != range.second; ++range.first) {
 790       FGPositionedRef candidate = range.first->second;
 791       if (aCur == candidate) {
 792         aCur = NULL; // found our start point, next match will pass
 793         continue;
 794       }
 795
 796       if (aFilter) {
 797         if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) {
 798           continue;
 799         }
 800
 801         if (!aFilter->pass(candidate)) {
 802           continue;
 803         }
 804       }
 805
 806       if (!aCur) {
 807         return candidate;
 808       }
 809     }
 810
 811     // Unable to match the filter with this range - try the next range.
 812     range = global_identIndex.equal_range(range.second->first);
 813   }
 814
 815   return NULL; // Reached the end of the valid sequence with no match.
 816 }
 817
 818 void
 819 FGPositioned::sortByRange(List& aResult, const SGGeod& aPos)
 820 {
 821   validateSGGeod(aPos);
 822
 823   SGVec3d cartPos(SGVec3d::fromGeod(aPos));
 824 // computer ordering values
 825   Octree::FindNearestResults r;
 826   List::iterator it = aResult.begin(), lend = aResult.end();
 827   for (; it != lend; ++it) {
 828     double d2 = distSqr((*it)->cart(), cartPos);
 829     r.push_back(Octree::OrderedPositioned(*it, d2));
 830   }
 831
 832 // sort
 833   std::sort(r.begin(), r.end());
 834
 835 // convert to a plain list
 836   unsigned int count = aResult.size();
 837   for (unsigned int i=0; i<count; ++i) {
 838     aResult[i] = r[i].get();
 839   }
 840 }
 841
 842 FGPositioned::Filter* createSearchFilter(const SGPropertyNode* arg)
 843 {
 844     string sty(arg->getStringValue("type", "any"));
 845     FGPositioned::Type ty = FGPositioned::typeFromName(sty);
 846     double minRunwayLenFt = arg->getDoubleValue("min-runway-length-ft", -1.0);
 847
 848     if ((ty == FGPositioned::AIRPORT) && (minRunwayLenFt > 0.0)) {
 849         return new FGAirport::HardSurfaceFilter(minRunwayLenFt);
 850     } else if (ty != FGPositioned::INVALID) {
 851         FGPositioned::TypeFilter* tf = new FGPositioned::TypeFilter(ty);
 852
 853         for (int t=1; arg->hasChild("type", t); ++t) {
 854             sty = arg->getChild("type", t)->getStringValue();
 855             tf->addType(FGPositioned::typeFromName(sty));
 856         }
 857
 858         return tf;
 859     }
 860
 861     return NULL;
 862 }
 863
 864 static SGGeod commandSearchPos(const SGPropertyNode* arg)
 865 {
 866     if (arg->hasChild("longitude-deg") && arg->hasChild("latitude-deg")) {
 867         return SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
 868             arg->getDoubleValue("latitude-deg"));
 869     }
 870
 871     // use current viewer/aircraft position
 872     return SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"),
 873             fgGetDouble("/position/latitude-deg"));
 874 }
 875
 876 void commandClearExisting(const SGPropertyNode* arg)
 877 {
 878     if (arg->getBoolValue("clear", true)) {
 879     // delete all existing result children from their parent
 880         string resultPath = arg->getStringValue("results");
 881         SGPropertyNode* n = fgGetNode(resultPath.c_str(), 0, true);
 882         SGPropertyNode* pr = n->getParent();
 883         pr->removeChildren(n->getName(), false /* keep=false, i.e delete nodes */);
 884     }
 885 }
 886
 887 bool commandFindClosest(const SGPropertyNode* arg)
 888 {
 889     int n = arg->getIntValue("max-results", 1);
 890     if ((n < 1) || (n > 100)) {
 891         SG_LOG(SG_GENERAL, SG_WARN, "commandFindClosest: max-results invalid:" << n);
 892         return false;
 893     }
 894
 895     string resultPath = arg->getStringValue("results");
 896     if (resultPath.empty()) {
 897         SG_LOG(SG_GENERAL, SG_WARN, "commandFindClosest: no results path defined");
 898         return false;
 899     }
 900
 901     std::auto_ptr<FGPositioned::Filter> filt(createSearchFilter(arg));
 902 // cap search range, since huge ranges will overload everything
 903     double cutoff = arg->getDoubleValue("cutoff-nm", 400.0);
 904     SG_CLAMP_RANGE(cutoff, 0.0, 1000.0);
 905
 906     SGGeod pos = commandSearchPos(arg);
 907     commandClearExisting(arg);
 908
 909     FGPositioned::List results = FGPositioned::findClosestN(pos, n, cutoff, filt.get());
 910     for (unsigned int i=0; i<results.size(); ++i) {
 911        SGPropertyNode* resultsNode = fgGetNode(resultPath.c_str(), i, true);
 912        flightgear::PositionedBinding::bind(results[i], resultsNode);
 913     }
 914
 915     return true;
 916 }
 917
 918 bool commandFindByIdent(const SGPropertyNode* arg)
 919 {
 920     string resultPath = arg->getStringValue("results");
 921     if (resultPath.empty()) {
 922         SG_LOG(SG_GENERAL, SG_WARN, "commandFindByIdent: no results path defined");
 923         return false;
 924     }
 925
 926     std::auto_ptr<FGPositioned::Filter> filt(createSearchFilter(arg));
 927     SGGeod pos = commandSearchPos(arg);
 928     commandClearExisting(arg);
 929
 930     FGPositioned::List results;
 931     bool exact = arg->getBoolValue("exact", true);
 932     if (arg->hasChild("name")) {
 933         results = FGPositioned::findAllWithName(arg->getStringValue("name"), filt.get(), exact);
 934     } else if (arg->hasChild("ident")) {
 935         results = FGPositioned::findAllWithIdent(arg->getStringValue("ident"), filt.get(), exact);
 936     } else {
 937         SG_LOG(SG_GENERAL, SG_WARN, "commandFindByIdent: no search term defined");
 938         return false;
 939     }
 940
 941     bool orderByRange = arg->getBoolValue("order-by-distance", true);
 942     if (orderByRange) {
 943         FGPositioned::sortByRange(results, pos);
 944     }
 945
 946     for (unsigned int i=0; i<results.size(); ++i) {
 947        SGPropertyNode* resultsNode = fgGetNode(resultPath.c_str(), i, true);
 948        flightgear::PositionedBinding::bind(results[i], resultsNode);
 949     }
 950
 951     return true;
 952 }
 953
 954 void
 955 FGPositioned::installCommands()
 956 {
 957     SGCommandMgr::instance()->addCommand("find-nearest", commandFindClosest);
 958     SGCommandMgr::instance()->addCommand("find-by-ident", commandFindByIdent);
 959 }
 960
 961 FGPositioned::TypeFilter::TypeFilter(Type aTy)
 962 {
 963     types.push_back(aTy);
 964 }
 965
 966 void FGPositioned::TypeFilter::addType(Type aTy)
 967 {
 968     types.push_back(aTy);
 969 }
 970
 971 bool
 972 FGPositioned::TypeFilter::pass(FGPositioned* aPos) const
 973 {
 974     std::vector<Type>::const_iterator it = types.begin(),
 975         end = types.end();
 976     for (; it != end; ++it) {
 977         return aPos->type() == *it;
 978     }
 979
 980     return false;
 981 }
 982