1 // positioned.cxx - base class for objects which are positioned
3 // Copyright (C) 2008 James Turner
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.
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.
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.
25 #include "positioned.hxx"
29 #include <algorithm> // for sort
32 #include <boost/algorithm/string/case_conv.hpp>
33 #include <boost/algorithm/string/predicate.hpp>
35 #include <simgear/timing/timestamp.hxx>
36 #include <simgear/debug/logstream.hxx>
37 #include <simgear/structure/exception.hxx>
38 #include <simgear/math/SGGeometry.hxx>
42 typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex;
43 typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> NamedIndexRange;
45 using std::lower_bound;
46 using std::upper_bound;
48 static NamedPositionedIndex global_identIndex;
49 static NamedPositionedIndex global_nameIndex;
51 //////////////////////////////////////////////////////////////////////////////
56 const double LEAF_SIZE = SG_NM_TO_METER * 8.0;
57 const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE;
60 * Decorate an object with a double value, and use that value to order
61 * items, for the purpoises of the STL algorithms
67 Ordered(const T& v, double x) :
73 Ordered(const Ordered<T>& a) :
79 Ordered<T>& operator=(const Ordered<T>& a)
86 bool operator<(const Ordered<T>& other) const
88 return _order < other._order;
91 bool operator>(const Ordered<T>& other) const
93 return _order > other._order;
108 typedef Ordered<Node*> OrderedNode;
109 typedef std::greater<OrderedNode> FNPQCompare;
112 * the priority queue is fundamental to our search algorithm. When searching,
113 * we know the front of the queue is the nearest unexpanded node (to the search
114 * location). The default STL pqueue returns the 'largest' item from top(), so
115 * to get the smallest, we need to replace the default Compare functor (less<>)
118 typedef std::priority_queue<OrderedNode, std::vector<OrderedNode>, FNPQCompare> FindNearestPQueue;
120 typedef Ordered<FGPositioned*> OrderedPositioned;
121 typedef std::vector<OrderedPositioned> FindNearestResults;
123 Node* global_spatialOctree = NULL;
126 * Octree node base class, tracks its bounding box and provides various
127 * queries relating to it
132 bool contains(const SGVec3d& aPos) const
134 return intersects(aPos, _box);
137 double distSqrToNearest(const SGVec3d& aPos) const
139 return distSqr(aPos, _box.getClosestPoint(aPos));
142 virtual void insert(FGPositioned* aP) = 0;
144 virtual void visit(const SGVec3d& aPos, double aCutoff,
145 FGPositioned::Filter* aFilter,
146 FindNearestResults& aResults, FindNearestPQueue&) = 0;
148 Node(const SGBoxd &aBox) :
156 class Leaf : public Node
159 Leaf(const SGBoxd& aBox) :
164 const FGPositioned::List& members() const
167 virtual void insert(FGPositioned* aP)
169 _members.push_back(aP);
172 virtual void visit(const SGVec3d& aPos, double aCutoff,
173 FGPositioned::Filter* aFilter,
174 FindNearestResults& aResults, FindNearestPQueue&)
176 int previousResultsSize = aResults.size();
179 for (unsigned int i=0; i<_members.size(); ++i) {
180 FGPositioned* p = _members[i];
181 double d2 = distSqr(aPos, p->cart());
187 if (aFilter->hasTypeRange() && !aFilter->passType(p->type())) {
191 if (!aFilter->pass(p)) {
194 } // of have a filter
197 aResults.push_back(OrderedPositioned(p, d2));
200 if (addedCount == 0) {
204 // keep aResults sorted
205 // sort the new items, usually just one or two items
206 std::sort(aResults.begin() + previousResultsSize, aResults.end());
208 // merge the two sorted ranges together - in linear time
209 std::inplace_merge(aResults.begin(),
210 aResults.begin() + previousResultsSize, aResults.end());
213 FGPositioned::List _members;
216 class Branch : public Node
219 Branch(const SGBoxd& aBox) :
222 memset(children, 0, sizeof(Node*) * 8);
225 virtual void insert(FGPositioned* aP)
227 SGVec3d cart(aP->cart());
228 assert(contains(cart));
231 SGVec3d center(_box.getCenter());
232 // tests must match indices in SGbox::getCorner
233 if (cart.x() < center.x()) {
237 if (cart.y() < center.y()) {
241 if (cart.z() < center.z()) {
245 Node* child = children[childIndex];
246 if (!child) { // lazy building of children
247 SGBoxd cb(boxForChild(childIndex));
248 double d2 = dot(cb.getSize(), cb.getSize());
249 if (d2 < LEAF_SIZE_SQR) {
250 child = new Leaf(cb);
252 child = new Branch(cb);
255 children[childIndex] = child;
261 virtual void visit(const SGVec3d& aPos, double aCutoff,
262 FGPositioned::Filter*,
263 FindNearestResults&, FindNearestPQueue& aQ)
265 for (unsigned int i=0; i<8; ++i) {
270 double d2 = children[i]->distSqrToNearest(aPos);
272 continue; // exceeded cutoff
275 aQ.push(Ordered<Node*>(children[i], d2));
276 } // of child iteration
282 * Return the box for a child touching the specified corner
284 SGBoxd boxForChild(unsigned int aCorner) const
286 SGBoxd r(_box.getCenter());
287 r.expandBy(_box.getCorner(aCorner));
294 void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
297 FindNearestPQueue pq;
298 FindNearestResults results;
299 pq.push(Ordered<Node*>(global_spatialOctree, 0));
300 double cut = aCutoffM * aCutoffM;
302 while (!pq.empty()) {
303 if (!results.empty()) {
304 // terminate the search if we have sufficent results, and we are
305 // sure no node still on the queue contains a closer match
306 double furthestResultOrder = results.back().order();
307 if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
312 Node* nd = pq.top().get();
315 nd->visit(aPos, cut, aFilter, results, pq);
316 } // of queue iteration
318 // depending on leaf population, we may have (slighty) more results
320 unsigned int numResults = std::min((unsigned int) results.size(), aN);
322 aResults.resize(numResults);
323 for (unsigned int r=0; r<numResults; ++r) {
324 aResults[r] = results[r].get();
328 void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
331 FindNearestPQueue pq;
332 FindNearestResults results;
333 pq.push(Ordered<Node*>(global_spatialOctree, 0));
334 double rng = aRangeM * aRangeM;
336 while (!pq.empty()) {
337 Node* nd = pq.top().get();
340 nd->visit(aPos, rng, aFilter, results, pq);
341 } // of queue iteration
343 unsigned int numResults = results.size();
345 aResults.resize(numResults);
346 for (unsigned int r=0; r<numResults; ++r) {
347 aResults[r] = results[r].get();
351 } // of namespace Octree
353 //////////////////////////////////////////////////////////////////////////////
356 addToIndices(FGPositioned* aPos)
359 if (!aPos->ident().empty()) {
360 global_identIndex.insert(global_identIndex.begin(),
361 std::make_pair(aPos->ident(), aPos));
364 if (!aPos->name().empty()) {
365 global_nameIndex.insert(global_nameIndex.begin(),
366 std::make_pair(aPos->name(), aPos));
369 if (!Octree::global_spatialOctree) {
370 double RADIUS_EARTH_M = 7000 * 1000.0; // 7000km is plenty
371 SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M);
372 Octree::global_spatialOctree = new Octree::Branch(SGBox<double>(-earthExtent, earthExtent));
374 Octree::global_spatialOctree->insert(aPos);
378 removeFromIndices(FGPositioned* aPos)
382 if (!aPos->ident().empty()) {
383 NamedPositionedIndex::iterator it = global_identIndex.find(aPos->ident());
384 while (it != global_identIndex.end() && (it->first == aPos->ident())) {
385 if (it->second == aPos) {
386 global_identIndex.erase(it);
391 } // of multimap walk
394 if (!aPos->name().empty()) {
395 NamedPositionedIndex::iterator it = global_nameIndex.find(aPos->name());
396 while (it != global_nameIndex.end() && (it->first == aPos->name())) {
397 if (it->second == aPos) {
398 global_nameIndex.erase(it);
403 } // of multimap walk
407 //////////////////////////////////////////////////////////////////////////////
412 bool operator()(FGPositioned* a, FGPositioned* b) const
414 return a->name() < b->name();
419 * A special purpose helper (imported by FGAirport::searchNamesAndIdents) to
420 * implement the AirportList dialog. It's unfortunate that it needs to reside
421 * here, but for now it's least ugly solution.
423 char** searchAirportNamesAndIdents(const std::string& aFilter)
425 const std::ctype<char> &ct = std::use_facet<std::ctype<char> >(std::locale());
426 std::string filter(aFilter);
427 bool hasFilter = !filter.empty();
429 ct.toupper((char *)filter.data(), (char *)filter.data() + filter.size());
432 NamedPositionedIndex::const_iterator it = global_identIndex.begin();
433 NamedPositionedIndex::const_iterator end = global_identIndex.end();
435 // note this is a vector of raw pointers, not smart pointers, because it
436 // may get very large and smart-pointer-atomicity-locking then becomes a
437 // bottleneck for this case.
438 std::vector<FGPositioned*> matches;
441 for (; it != end; ++it) {
442 FGPositioned::Type ty = it->second->type();
443 if ((ty < FGPositioned::AIRPORT) || (ty > FGPositioned::SEAPORT)) {
447 if (hasFilter && (it->second->ident().find(aFilter) == std::string::npos)) {
448 upper = it->second->name(); // string copy, sadly
449 ct.toupper((char *)upper.data(), (char *)upper.data() + upper.size());
450 if (upper.find(aFilter) == std::string::npos) {
455 matches.push_back(it->second);
458 // sort alphabetically on name
459 std::sort(matches.begin(), matches.end(), OrderByName());
461 // convert results to format comptible with puaList
462 unsigned int numMatches = matches.size();
463 char** result = new char*[numMatches + 1];
464 result[numMatches] = NULL; // end-of-list marker
466 // nasty code to avoid excessive string copying and allocations.
467 // We format results as follows (note whitespace!):
468 // ' name-of-airport-chars (ident)'
469 // so the total length is:
470 // 1 + strlen(name) + 4 + strlen(icao) + 1 + 1 (for the null)
471 // which gives a grand total of 7 + name-length + icao-length.
472 // note the ident can be three letters (non-ICAO local strip), four
473 // (default ICAO) or more (extended format ICAO)
474 for (unsigned int i=0; i<numMatches; ++i) {
475 int nameLength = matches[i]->name().size();
476 int icaoLength = matches[i]->ident().size();
477 char* entry = new char[7 + nameLength + icaoLength];
480 memcpy(dst, matches[i]->name().c_str(), nameLength);
486 memcpy(dst, matches[i]->ident().c_str(), icaoLength);
496 ///////////////////////////////////////////////////////////////////////////////
499 FGPositioned::Filter::hasTypeRange() const
501 assert(minType() <= maxType());
502 return (minType() != INVALID) && (maxType() != INVALID);
506 FGPositioned::Filter::passType(Type aTy) const
508 assert(hasTypeRange());
509 return (minType() <= aTy) && (maxType() >= aTy);
512 static FGPositioned::List
513 findAll(const NamedPositionedIndex& aIndex,
514 const std::string& aName, FGPositioned::Filter* aFilter)
516 FGPositioned::List result;
521 std::string upperBoundId = aName;
522 upperBoundId[upperBoundId.size()-1]++;
523 NamedPositionedIndex::const_iterator upperBound = aIndex.lower_bound(upperBoundId);
524 NamedPositionedIndex::const_iterator it = aIndex.lower_bound(aName);
526 for (; it != upperBound; ++it) {
527 FGPositionedRef candidate = it->second;
529 if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) {
533 if (!aFilter->pass(candidate)) {
538 result.push_back(candidate);
544 ///////////////////////////////////////////////////////////////////////////////
546 FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) :
553 void FGPositioned::init(bool aIndexed)
555 SGReferenced::get(this); // hold an owning ref, for the moment
556 mCart = SGVec3d::fromGeod(mPosition);
559 assert(mType != TAXIWAY && mType != PAVEMENT);
564 FGPositioned::~FGPositioned()
566 //std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl;
567 removeFromIndices(this);
571 FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos)
573 FGPositioned* wpt = new FGPositioned(WAYPOINT, aIdent, aPos);
579 FGPositioned::cart() const
584 FGPositioned::Type FGPositioned::typeFromName(const std::string& aName)
586 if (aName.empty() || (aName == "")) {
595 const NameTypeEntry names[] = {
596 {"airport", AIRPORT},
604 {"waypoint", WAYPOINT},
613 std::string lowerName(boost::to_lower_copy(aName));
615 for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) {
616 if (::strcmp(n->_name, lowerName.c_str()) == 0) {
621 SG_LOG(SG_GENERAL, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName);
625 const char* FGPositioned::nameForType(Type aTy)
628 case RUNWAY: return "runway";
629 case TAXIWAY: return "taxiway";
630 case PAVEMENT: return "pavement";
631 case PARK_STAND: return "parking stand";
632 case FIX: return "fix";
633 case VOR: return "VOR";
634 case NDB: return "NDB";
635 case ILS: return "ILS";
636 case LOC: return "localiser";
637 case GS: return "glideslope";
638 case OM: return "outer-marker";
639 case MM: return "middle-marker";
640 case IM: return "inner-marker";
641 case AIRPORT: return "airport";
642 case HELIPORT: return "heliport";
643 case SEAPORT: return "seaport";
644 case WAYPOINT: return "waypoint";
645 case DME: return "dme";
646 case TACAN: return "tacan";
652 ///////////////////////////////////////////////////////////////////////////////
653 // search / query functions
656 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
658 FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter));
660 return FGPositionedRef();
663 sortByRange(r, aPos);
668 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
671 Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
672 aRangeNm * SG_NM_TO_METER, aFilter, result);
677 FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter)
679 return findAll(global_identIndex, aIdent, aFilter);
683 FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter)
685 return findAll(global_nameIndex, aName, aFilter);
689 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
691 List l(findClosestN(aPos, 1, aCutoffNm, aFilter));
696 assert(l.size() == 1);
701 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
704 Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result);
709 FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
715 std::string id(boost::to_upper_copy(aId));
717 // It is essential to bound our search, to avoid iterating all the way to the end of the database.
718 // Do this by generating a second ID with the final character incremented by 1.
719 // e.g., if the partial ID is "KI", we wish to search "KIxxx" but not "KJ".
720 std::string upperBoundId = id;
721 upperBoundId[upperBoundId.size()-1]++;
722 NamedPositionedIndex::const_iterator upperBound = global_identIndex.lower_bound(upperBoundId);
724 NamedIndexRange range = global_identIndex.equal_range(id);
725 while (range.first != upperBound) {
726 for (; range.first != range.second; ++range.first) {
727 FGPositionedRef candidate = range.first->second;
728 if (aCur == candidate) {
729 aCur = NULL; // found our start point, next match will pass
734 if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) {
738 if (!aFilter->pass(candidate)) {
748 // Unable to match the filter with this range - try the next range.
749 range = global_identIndex.equal_range(range.second->first);
752 return NULL; // Reached the end of the valid sequence with no match.
756 FGPositioned::sortByRange(List& aResult, const SGGeod& aPos)
758 SGVec3d cartPos(SGVec3d::fromGeod(aPos));
759 // computer ordering values
760 Octree::FindNearestResults r;
761 List::iterator it = aResult.begin(), lend = aResult.end();
762 for (; it != lend; ++it) {
763 double d2 = distSqr((*it)->cart(), cartPos);
764 r.push_back(Octree::OrderedPositioned(*it, d2));
768 std::sort(r.begin(), r.end());
770 // convert to a plain list
771 unsigned int count = aResult.size();
772 for (unsigned int i=0; i<count; ++i) {
773 aResult[i] = r[i].get();