X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2Fpositioned.cxx;h=7aa183eba02be4a831f782f39b433c4871b210fc;hb=4928886e562c04bf8a8497a6b4f70aba793e20c2;hp=b01418a17367a1814dea1b8cbd5427f80192d2e7;hpb=030742fa4ae7d99facb517381da71fa404c4ee2b;p=flightgear.git diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx index b01418a17..7aa183eba 100644 --- a/src/Navaids/positioned.cxx +++ b/src/Navaids/positioned.cxx @@ -22,320 +22,400 @@ # include "config.h" #endif +#include "positioned.hxx" + #include #include #include // for sort -#include // for char-traits toupper +#include +#include + +#include +#include -#include +#include // for osg::isNaN -#include #include +#include +#include +#include +#include +#include +#include -#include "positioned.hxx" +#include "PositionedBinding.hxx" +#include "Airports/simple.hxx" +#include "Main/fg_props.hxx" typedef std::multimap NamedPositionedIndex; typedef std::pair NamedIndexRange; -/** - * Order positioned elements by type, then pointer address. This allows us to - * use range searches (lower_ and upper_bound) to grab items of a particular - * type out of bucket efficently. - */ -class OrderByType -{ -public: - bool operator()(const FGPositioned* a, const FGPositioned* b) const - { - if (a->type() == b->type()) return a < b; - return a->type() < b->type(); - } -}; +using std::lower_bound; +using std::upper_bound; -typedef std::set BucketEntry; -typedef std::map SpatialPositionedIndex; +static NamedPositionedIndex global_identIndex; +static NamedPositionedIndex global_nameIndex; -static NamedPositionedIndex global_namedIndex; -static SpatialPositionedIndex global_spatialIndex; +////////////////////////////////////////////////////////////////////////////// -SpatialPositionedIndex::iterator -bucketEntryForPositioned(FGPositioned* aPos) +namespace Octree { - int bucketIndex = aPos->bucket().gen_index(); - SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex); - if (it != global_spatialIndex.end()) { - return it; - } - - // create a new BucketEntry - return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry())); -} -static void -addToIndices(FGPositioned* aPos) -{ - assert(aPos); - if (!aPos->ident().empty()) { - global_namedIndex.insert(global_namedIndex.begin(), - std::make_pair(aPos->ident(), aPos)); - } - - SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos); - it->second.insert(aPos); -} +const double LEAF_SIZE = SG_NM_TO_METER * 8.0; +const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE; -static void -removeFromIndices(FGPositioned* aPos) +/** + * Decorate an object with a double value, and use that value to order + * items, for the purpoises of the STL algorithms + */ +template +class Ordered { - assert(aPos); - - if (!aPos->ident().empty()) { - NamedPositionedIndex::iterator it = global_namedIndex.find(aPos->ident()); - while (it != global_namedIndex.end() && (it->first == aPos->ident())) { - if (it->second == aPos) { - global_namedIndex.erase(it); - break; - } - - ++it; - } // of multimap walk - } - - SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos); - sit->second.erase(aPos); -} +public: + Ordered(const T& v, double x) : + _order(x), + _inner(v) + { + } + + Ordered(const Ordered& a) : + _order(a._order), + _inner(a._inner) + { + } + + Ordered& operator=(const Ordered& a) + { + _order = a._order; + _inner = a._inner; + return *this; + } + + bool operator<(const Ordered& other) const + { + return _order < other._order; + } + + bool operator>(const Ordered& other) const + { + return _order > other._order; + } + + const T& get() const + { return _inner; } + + double order() const + { return _order; } + +private: + double _order; + T _inner; +}; -static void -spatialFilterInBucket(const SGBucket& aBucket, FGPositioned::Filter* aFilter, FGPositioned::List& aResult) -{ - SpatialPositionedIndex::const_iterator it; - it = global_spatialIndex.find(aBucket.gen_index()); - if (it == global_spatialIndex.end()) { - return; - } - - BucketEntry::const_iterator l = it->second.begin(); - BucketEntry::const_iterator u = it->second.end(); +class Node; +typedef Ordered OrderedNode; +typedef std::greater FNPQCompare; - if (!aFilter) { // pass everything - aResult.insert(aResult.end(), l, u); - return; - } +/** + * the priority queue is fundamental to our search algorithm. When searching, + * we know the front of the queue is the nearest unexpanded node (to the search + * location). The default STL pqueue returns the 'largest' item from top(), so + * to get the smallest, we need to replace the default Compare functor (less<>) + * with greater<>. + */ +typedef std::priority_queue, FNPQCompare> FindNearestPQueue; - for ( ; l != u; ++l) { - if ((*aFilter)(*l)) { - aResult.push_back(*l); - } - } -} +typedef Ordered OrderedPositioned; +typedef std::vector FindNearestResults; -static void -spatialFind(const SGGeod& aPos, double aRange, - FGPositioned::Filter* aFilter, FGPositioned::List& aResult) -{ - SGBucket buck(aPos); - double lat = aPos.getLatitudeDeg(), - lon = aPos.getLongitudeDeg(); - - int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2); - int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 ); - - // loop over bucket range - for ( int i=-bx; i<=bx; i++) { - for ( int j=-by; j<=by; j++) { - spatialFilterInBucket(sgBucketOffset(lon, lat, i, j), aFilter, aResult); - } // of j-iteration - } // of i-iteration -} +Node* global_spatialOctree = NULL; -/* -class LowerLimitOfType +/** + * Octree node base class, tracks its bounding box and provides various + * queries relating to it + */ +class Node { public: - bool operator()(const FGPositioned* a, const FGPositioned::Type b) const - { - return a->type() < b; - } - - bool operator()(const FGPositioned::Type a, const FGPositioned* b) const - { - return a < b->type(); - } -}; + bool contains(const SGVec3d& aPos) const + { + return intersects(aPos, _box); + } + double distSqrToNearest(const SGVec3d& aPos) const + { + return distSqr(aPos, _box.getClosestPoint(aPos)); + } + + virtual void insert(FGPositioned* aP) = 0; + + virtual void visit(const SGVec3d& aPos, double aCutoff, + FGPositioned::Filter* aFilter, + FindNearestResults& aResults, FindNearestPQueue&) = 0; +protected: + Node(const SGBoxd &aBox) : + _box(aBox) + { + } + + const SGBoxd _box; +}; -static void -spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, FGPositioned::Type aUpper, FGPositioned::List& aResult) +class Leaf : public Node { - SGBucket buck(aPos); - double lat = aPos.getLatitudeDeg(), - lon = aPos.getLongitudeDeg(); - - int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2); - int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 ); +public: + Leaf(const SGBoxd& aBox) : + Node(aBox) + { + } - // loop over bucket range - for ( int i=-bx; i<=bx; i++) { - for ( int j=-by; j<=by; j++) { - buck = sgBucketOffset(lon, lat, i, j); - - SpatialPositionedIndex::const_iterator it; - it = global_spatialIndex.find(buck.gen_index()); - if (it == global_spatialIndex.end()) { - continue; - } - - BucketEntry::const_iterator l = std::lower_bound(it->second.begin(), it->second.end(), aLower, LowerLimitOfType()); - BucketEntry::const_iterator u = std::upper_bound(l, it->second.end(), aUpper, LowerLimitOfType()); + const FGPositioned::List& members() const + { return _members; } + + virtual void insert(FGPositioned* aP) + { + _members.push_back(aP); + } + + virtual void visit(const SGVec3d& aPos, double aCutoff, + FGPositioned::Filter* aFilter, + FindNearestResults& aResults, FindNearestPQueue&) + { + int previousResultsSize = aResults.size(); + int addedCount = 0; + + for (unsigned int i=0; i<_members.size(); ++i) { + FGPositioned* p = _members[i]; + double d2 = distSqr(aPos, p->cart()); + if (d2 > aCutoff) { + continue; + } + + if (aFilter) { + if (aFilter->hasTypeRange() && !aFilter->passType(p->type())) { + continue; + } - for ( ; l != u; ++l) { - aResult.push_back(*l); + if (!aFilter->pass(p)) { + continue; + } + } // of have a filter + + ++addedCount; + aResults.push_back(OrderedPositioned(p, d2)); + } + + if (addedCount == 0) { + return; + } + + // keep aResults sorted + // sort the new items, usually just one or two items + std::sort(aResults.begin() + previousResultsSize, aResults.end()); + + // merge the two sorted ranges together - in linear time + std::inplace_merge(aResults.begin(), + aResults.begin() + previousResultsSize, aResults.end()); } - - } // of j-iteration - } // of i-iteration -} -*/ +private: + FGPositioned::List _members; +}; -/** - */ -class RangePredictate +class Branch : public Node { public: - RangePredictate(const SGGeod& aOrigin, double aRange) : - mOrigin(SGVec3d::fromGeod(aOrigin)), - mRangeSqr(aRange * aRange) - { ; } - - bool operator()(const FGPositionedRef& aPos) - { - double dSqr = distSqr(aPos->cart(), mOrigin); - return (dSqr > mRangeSqr); - } - + Branch(const SGBoxd& aBox) : + Node(aBox) + { + memset(children, 0, sizeof(Node*) * 8); + } + + virtual void insert(FGPositioned* aP) + { + SGVec3d cart(aP->cart()); + assert(contains(cart)); + int childIndex = 0; + + SGVec3d center(_box.getCenter()); + // tests must match indices in SGbox::getCorner + if (cart.x() < center.x()) { + childIndex += 1; + } + + if (cart.y() < center.y()) { + childIndex += 2; + } + + if (cart.z() < center.z()) { + childIndex += 4; + } + + Node* child = children[childIndex]; + if (!child) { // lazy building of children + SGBoxd cb(boxForChild(childIndex)); + double d2 = dot(cb.getSize(), cb.getSize()); + if (d2 < LEAF_SIZE_SQR) { + child = new Leaf(cb); + } else { + child = new Branch(cb); + } + + children[childIndex] = child; + } + + child->insert(aP); + } + + virtual void visit(const SGVec3d& aPos, double aCutoff, + FGPositioned::Filter*, + FindNearestResults&, FindNearestPQueue& aQ) + { + for (unsigned int i=0; i<8; ++i) { + if (!children[i]) { + continue; + } + + double d2 = children[i]->distSqrToNearest(aPos); + if (d2 > aCutoff) { + continue; // exceeded cutoff + } + + aQ.push(Ordered(children[i], d2)); + } // of child iteration + } + + private: - SGVec3d mOrigin; - double mRangeSqr; + /** + * Return the box for a child touching the specified corner + */ + SGBoxd boxForChild(unsigned int aCorner) const + { + SGBoxd r(_box.getCenter()); + r.expandBy(_box.getCorner(aCorner)); + return r; + } + + Node* children[8]; }; -static void -filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult) +void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults) { - RangePredictate pred(aPos, aRange * SG_NM_TO_METER); - FGPositioned::List::iterator newEnd; - newEnd = std::remove_if(aResult.begin(), aResult.end(), pred); - aResult.erase(newEnd, aResult.end()); + aResults.clear(); + FindNearestPQueue pq; + FindNearestResults results; + pq.push(Ordered(global_spatialOctree, 0)); + double cut = aCutoffM * aCutoffM; + + while (!pq.empty()) { + if (!results.empty()) { + // terminate the search if we have sufficent results, and we are + // sure no node still on the queue contains a closer match + double furthestResultOrder = results.back().order(); + if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) { + break; + } + } + + Node* nd = pq.top().get(); + pq.pop(); + + nd->visit(aPos, cut, aFilter, results, pq); + } // of queue iteration + + // depending on leaf population, we may have (slighty) more results + // than requested + unsigned int numResults = std::min((unsigned int) results.size(), aN); + // copy results out + aResults.resize(numResults); + for (unsigned int r=0; rcart(), mPos), - dB = distSqr(b->cart(), mPos); - return dA < dB; - } + aResults.clear(); + FindNearestPQueue pq; + FindNearestResults results; + pq.push(Ordered(global_spatialOctree, 0)); + double rng = aRangeM * aRangeM; + + while (!pq.empty()) { + Node* nd = pq.top().get(); + pq.pop(); + + nd->visit(aPos, rng, aFilter, results, pq); + } // of queue iteration + + unsigned int numResults = results.size(); + // copy results out + aResults.resize(numResults); + for (unsigned int r=0; rpass(range.first->second)) { - return NULL; // type check failed - } + assert(aPos); + if (!aPos->ident().empty()) { + std::string u(boost::to_upper_copy(aPos->ident())); - return range.first->second; - } // of short-circuit logic for single-element range - -// multiple matches, we need to actually check the distance to each one - double minDist = HUGE_VAL; - FGPositionedRef result; - NamedPositionedIndex::const_iterator it = range.first; - SGVec3d cartOrigin(SGVec3d::fromGeod(aOrigin)); + global_identIndex.insert(global_identIndex.begin(), + std::make_pair(u, aPos)); + } - for (; it != range.second; ++it) { - if (aFilter && !aFilter->pass(range.first->second)) { - continue; - } + if (!aPos->name().empty()) { + std::string u(boost::to_upper_copy(aPos->name())); - // find distance - double d2 = distSqr(cartOrigin, it->second->cart()); - if (d2 < minDist) { - minDist = d2; - result = it->second; - } + global_nameIndex.insert(global_nameIndex.begin(), + std::make_pair(u, aPos)); } - - return result; + + if (!Octree::global_spatialOctree) { + double RADIUS_EARTH_M = 7000 * 1000.0; // 7000km is plenty + SGVec3d earthExtent(RADIUS_EARTH_M, RADIUS_EARTH_M, RADIUS_EARTH_M); + Octree::global_spatialOctree = new Octree::Branch(SGBox(-earthExtent, earthExtent)); + } + Octree::global_spatialOctree->insert(aPos); } -static FGPositioned::List -spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, FGPositioned::Filter* aFilter) +static void +removeFromIndices(FGPositioned* aPos) { - FGPositioned::List result; - int radius = 1; // start at 1, radius 0 is handled explicitly - SGBucket buck; - double lat = aPos.getLatitudeDeg(), - lon = aPos.getLongitudeDeg(); - // final cutoff is in metres, and scaled to account for testing the corners - // of the 'box' instead of the centre of each edge - double cutoffM = aCutoffNm * SG_NM_TO_METER * 1.5; + assert(aPos); - // base case, simplifes loop to do it seperately here - spatialFilterInBucket(sgBucketOffset(lon, lat, 0, 0), aFilter, result); - - for (;result.size() < aN; ++radius) { - // cutoff check - double az1, az2, d1, d2; - SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, -radius, -radius).get_center(), az1, az2, d1); - SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, radius, radius).get_center(), az1, az2, d2); + if (!aPos->ident().empty()) { + std::string u(boost::to_upper_copy(aPos->ident())); + NamedPositionedIndex::iterator it = global_identIndex.find(u); + while (it != global_identIndex.end() && (it->first == u)) { + if (it->second == aPos) { + global_identIndex.erase(it); + break; + } - if ((d1 > cutoffM) && (d2 > cutoffM)) { - //std::cerr << "spatialGetClosest terminating due to range cutoff" << std::endl; - break; - } - - FGPositioned::List hits; - for ( int i=-radius; i<=radius; i++) { - spatialFilterInBucket(sgBucketOffset(lon, lat, i, -radius), aFilter, hits); - spatialFilterInBucket(sgBucketOffset(lon, lat, -radius, i), aFilter, hits); - spatialFilterInBucket(sgBucketOffset(lon, lat, i, radius), aFilter, hits); - spatialFilterInBucket(sgBucketOffset(lon, lat, radius, i), aFilter, hits); - } - - result.insert(result.end(), hits.begin(), hits.end()); // append - } // of outer loop - - sortByDistance(aPos, result); - if (result.size() > aN) { - result.resize(aN); // truncate at requested number of matches + ++it; + } // of multimap walk } - return result; + if (!aPos->name().empty()) { + std::string u(boost::to_upper_copy(aPos->name())); + NamedPositionedIndex::iterator it = global_nameIndex.find(u); + while (it != global_nameIndex.end() && (it->first == u)) { + if (it->second == aPos) { + global_nameIndex.erase(it); + break; + } + + ++it; + } // of multimap walk + } } ////////////////////////////////////////////////////////////////////////////// @@ -349,50 +429,51 @@ public: } }; -/** - * A special purpose helper (imported by FGAirport::searchNamesAndIdents) to - * implement the AirportList dialog. It's unfortunate that it needs to reside - * here, but for now it's least ugly solution. - */ -char** searchAirportNamesAndIdents(const std::string& aFilter) +void findInIndex(NamedPositionedIndex& aIndex, const std::string& aFind, std::vector& aResult) { - const std::ctype &ct = std::use_facet >(std::locale()); - std::string filter(aFilter); - bool hasFilter = !filter.empty(); - if (hasFilter) { - ct.toupper((char *)filter.data(), (char *)filter.data() + filter.size()); - } - - NamedPositionedIndex::const_iterator it = global_namedIndex.begin(); - NamedPositionedIndex::const_iterator end = global_namedIndex.end(); - - // note this is a vector of raw pointers, not smart pointers, because it - // may get very large and smart-pointer-atomicity-locking then becomes a - // bottleneck for this case. - std::vector matches; - std::string upper; - + NamedPositionedIndex::const_iterator it = aIndex.begin(); + NamedPositionedIndex::const_iterator end = aIndex.end(); + + bool haveFilter = !aFind.empty(); + for (; it != end; ++it) { FGPositioned::Type ty = it->second->type(); if ((ty < FGPositioned::AIRPORT) || (ty > FGPositioned::SEAPORT)) { continue; } - if (hasFilter && (it->second->ident().find(aFilter) == std::string::npos)) { - upper = it->second->name(); // string copy, sadly - ct.toupper((char *)upper.data(), (char *)upper.data() + upper.size()); - if (upper.find(aFilter) == std::string::npos) { - continue; - } + if (haveFilter && it->first.find(aFind) == std::string::npos) { + continue; } - matches.push_back(it->second); + aResult.push_back(it->second); + } // of index iteration +} + +/** + * A special purpose helper (imported by FGAirport::searchNamesAndIdents) to + * implement the AirportList dialog. It's unfortunate that it needs to reside + * here, but for now it's least ugly solution. + */ +char** searchAirportNamesAndIdents(const std::string& aFilter) +{ +// note this is a vector of raw pointers, not smart pointers, because it +// may get very large and smart-pointer-atomicity-locking then becomes a +// bottleneck for this case. + std::vector matches; + if (!aFilter.empty()) { + std::string filter = boost::to_upper_copy(aFilter); + findInIndex(global_identIndex, filter, matches); + findInIndex(global_nameIndex, filter, matches); + } else { + + findInIndex(global_identIndex, std::string(), matches); } - // sort alphabetically on name +// sort alphabetically on name std::sort(matches.begin(), matches.end(), OrderByName()); - // convert results to format comptible with puaList +// convert results to format comptible with puaList unsigned int numMatches = matches.size(); char** result = new char*[numMatches + 1]; result[numMatches] = NULL; // end-of-list marker @@ -401,13 +482,14 @@ char** searchAirportNamesAndIdents(const std::string& aFilter) // We format results as follows (note whitespace!): // ' name-of-airport-chars (ident)' // so the total length is: - // 1 + strlen(name) + 4 + 4 (for the ident) + 1 + 1 (for the null) - // which gives a grand total of 11 + the length of the name. - // note the ident is sometimes only three letters for non-ICAO small strips + // 1 + strlen(name) + 4 + strlen(icao) + 1 + 1 (for the null) + // which gives a grand total of 7 + name-length + icao-length. + // note the ident can be three letters (non-ICAO local strip), four + // (default ICAO) or more (extended format ICAO) for (unsigned int i=0; iname().size(); int icaoLength = matches[i]->ident().size(); - char* entry = new char[nameLength + 11]; + char* entry = new char[7 + nameLength + icaoLength]; char* dst = entry; *dst++ = ' '; memcpy(dst, matches[i]->name().c_str(), nameLength); @@ -426,17 +508,89 @@ char** searchAirportNamesAndIdents(const std::string& aFilter) return result; } +static void validateSGGeod(const SGGeod& geod) +{ + if (osg::isNaN(geod.getLatitudeDeg()) || + osg::isNaN(geod.getLongitudeDeg())) + { + throw sg_range_exception("position is invalid, NaNs"); + } +} + /////////////////////////////////////////////////////////////////////////////// -FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos, bool aIndexed) : - mType(ty), +bool +FGPositioned::Filter::hasTypeRange() const +{ + assert(minType() <= maxType()); + return (minType() != INVALID) && (maxType() != INVALID); +} + +bool +FGPositioned::Filter::passType(Type aTy) const +{ + assert(hasTypeRange()); + return (minType() <= aTy) && (maxType() >= aTy); +} + +static FGPositioned::List +findAll(const NamedPositionedIndex& aIndex, + const std::string& aName, + FGPositioned::Filter* aFilter, + bool aExact) +{ + FGPositioned::List result; + if (aName.empty()) { + return result; + } + + std::string name = boost::to_upper_copy(aName); + NamedPositionedIndex::const_iterator upperBound; + + if (aExact) { + upperBound = aIndex.upper_bound(name); + } else { + std::string upperBoundId = name; + upperBoundId[upperBoundId.size()-1]++; + upperBound = aIndex.lower_bound(upperBoundId); + } + + NamedPositionedIndex::const_iterator it = aIndex.lower_bound(name); + + for (; it != upperBound; ++it) { + FGPositionedRef candidate = it->second; + if (aFilter) { + if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) { + continue; + } + + if (!aFilter->pass(candidate)) { + continue; + } + } + + result.push_back(candidate); + } + + return result; +} + +/////////////////////////////////////////////////////////////////////////////// + +FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) : mPosition(aPos), + mType(ty), mIdent(aIdent) { +} + +void FGPositioned::init(bool aIndexed) +{ SGReferenced::get(this); // hold an owning ref, for the moment + mCart = SGVec3d::fromGeod(mPosition); if (aIndexed) { - assert(ty != TAXIWAY); + assert(mType != TAXIWAY && mType != PAVEMENT); addToIndices(this); } } @@ -447,16 +601,67 @@ FGPositioned::~FGPositioned() removeFromIndices(this); } -SGBucket -FGPositioned::bucket() const +FGPositioned* +FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos) { - return SGBucket(mPosition); + FGPositioned* wpt = new FGPositioned(WAYPOINT, aIdent, aPos); + wpt->init(true); + return wpt; } -SGVec3d +const SGVec3d& FGPositioned::cart() const { - return SGVec3d::fromGeod(mPosition); + return mCart; +} + +FGPositioned::Type FGPositioned::typeFromName(const std::string& aName) +{ + if (aName.empty() || (aName == "")) { + return INVALID; + } + + typedef struct { + const char* _name; + Type _ty; + } NameTypeEntry; + + const NameTypeEntry names[] = { + {"airport", AIRPORT}, + {"vor", VOR}, + {"ndb", NDB}, + {"wpt", WAYPOINT}, + {"fix", FIX}, + {"tacan", TACAN}, + {"dme", DME}, + {"atis", FREQ_ATIS}, + {"awos", FREQ_AWOS}, + {"tower", FREQ_TOWER}, + {"ground", FREQ_GROUND}, + {"approach", FREQ_APP_DEP}, + {"departure", FREQ_APP_DEP}, + // aliases + {"gnd", FREQ_GROUND}, + {"twr", FREQ_TOWER}, + {"waypoint", WAYPOINT}, + {"apt", AIRPORT}, + {"arpt", AIRPORT}, + {"any", INVALID}, + {"all", INVALID}, + + {NULL, INVALID} + }; + + std::string lowerName(boost::to_lower_copy(aName)); + + for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) { + if (::strcmp(n->_name, lowerName.c_str()) == 0) { + return n->_ty; + } + } + + SG_LOG(SG_GENERAL, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName); + return INVALID; } const char* FGPositioned::nameForType(Type aTy) @@ -464,6 +669,7 @@ const char* FGPositioned::nameForType(Type aTy) switch (aTy) { case RUNWAY: return "runway"; case TAXIWAY: return "taxiway"; + case PAVEMENT: return "pavement"; case PARK_STAND: return "parking stand"; case FIX: return "fix"; case VOR: return "VOR"; @@ -480,84 +686,297 @@ const char* FGPositioned::nameForType(Type aTy) case WAYPOINT: return "waypoint"; case DME: return "dme"; case TACAN: return "tacan"; + case FREQ_TOWER: return "tower"; + case FREQ_ATIS: return "atis"; + case FREQ_AWOS: return "awos"; + case FREQ_GROUND: return "ground"; + case FREQ_CLEARANCE: return "clearance"; + case FREQ_UNICOM: return "unicom"; + case FREQ_APP_DEP: return "approach-departure"; default: return "unknown"; } } +flightgear::PositionedBinding* +FGPositioned::createBinding(SGPropertyNode* node) const +{ + return new flightgear::PositionedBinding(this, node); +} + /////////////////////////////////////////////////////////////////////////////// // search / query functions FGPositionedRef FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter) { - return namedFindClosest(aIdent, aPos, aFilter); + validateSGGeod(aPos); + + FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true)); + if (r.empty()) { + return FGPositionedRef(); + } + + sortByRange(r, aPos); + return r.front(); } FGPositioned::List FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter) { + validateSGGeod(aPos); + List result; - spatialFind(aPos, aRangeNm, aFilter, result); - filterListByRange(aPos, aRangeNm, result); + Octree::findAllWithinRange(SGVec3d::fromGeod(aPos), + aRangeNm * SG_NM_TO_METER, aFilter, result); return result; } FGPositioned::List -FGPositioned::findAllWithIdentSortedByRange(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter) +FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact) { - List result; - NamedIndexRange range = global_namedIndex.equal_range(aIdent); - for (; range.first != range.second; ++range.first) { - if (aFilter && !aFilter->pass(range.first->second)) { - continue; - } - - result.push_back(range.first->second); - } - - sortByDistance(aPos, result); - return result; + return findAll(global_identIndex, aIdent, aFilter, aExact); +} + +FGPositioned::List +FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact) +{ + return findAll(global_nameIndex, aName, aFilter, aExact); } FGPositionedRef FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter) { - FGPositioned::List l(spatialGetClosest(aPos, 1, aCutoffNm, aFilter)); - if (l.empty()) { - return NULL; - } - - assert(l.size() == 1); - return l.front(); + validateSGGeod(aPos); + + List l(findClosestN(aPos, 1, aCutoffNm, aFilter)); + if (l.empty()) { + return NULL; + } + + assert(l.size() == 1); + return l.front(); } FGPositioned::List FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter) { - return spatialGetClosest(aPos, aN, aCutoffNm, aFilter); + validateSGGeod(aPos); + + List result; + Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result); + return result; } FGPositionedRef FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter) { - NamedIndexRange range = global_namedIndex.equal_range(aId); - for (; range.first != range.second; ++range.first) { - FGPositionedRef candidate = range.first->second; - if (aCur == candidate) { - aCur = NULL; // found our start point, next match will pass - continue; - } - - if (aFilter && !aFilter->pass(candidate)) { - continue; - } + if (aId.empty()) { + return NULL; + } - if (!aCur) { - return candidate; + std::string id(boost::to_upper_copy(aId)); + + // It is essential to bound our search, to avoid iterating all the way to the end of the database. + // Do this by generating a second ID with the final character incremented by 1. + // e.g., if the partial ID is "KI", we wish to search "KIxxx" but not "KJ". + std::string upperBoundId = id; + upperBoundId[upperBoundId.size()-1]++; + NamedPositionedIndex::const_iterator upperBound = global_identIndex.lower_bound(upperBoundId); + + NamedIndexRange range = global_identIndex.equal_range(id); + while (range.first != upperBound) { + for (; range.first != range.second; ++range.first) { + FGPositionedRef candidate = range.first->second; + if (aCur == candidate) { + aCur = NULL; // found our start point, next match will pass + continue; + } + + if (aFilter) { + if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) { + continue; + } + + if (!aFilter->pass(candidate)) { + continue; + } + } + + if (!aCur) { + return candidate; + } } + + // Unable to match the filter with this range - try the next range. + range = global_identIndex.equal_range(range.second->first); } + + return NULL; // Reached the end of the valid sequence with no match. +} + +void +FGPositioned::sortByRange(List& aResult, const SGGeod& aPos) +{ + validateSGGeod(aPos); + + SGVec3d cartPos(SGVec3d::fromGeod(aPos)); +// computer ordering values + Octree::FindNearestResults r; + List::iterator it = aResult.begin(), lend = aResult.end(); + for (; it != lend; ++it) { + double d2 = distSqr((*it)->cart(), cartPos); + r.push_back(Octree::OrderedPositioned(*it, d2)); + } + +// sort + std::sort(r.begin(), r.end()); - return NULL; // fell out, no match in range +// convert to a plain list + unsigned int count = aResult.size(); + for (unsigned int i=0; igetStringValue("type", "any")); + FGPositioned::Type ty = FGPositioned::typeFromName(sty); + double minRunwayLenFt = arg->getDoubleValue("min-runway-length-ft", -1.0); + + if ((ty == FGPositioned::AIRPORT) && (minRunwayLenFt > 0.0)) { + return new FGAirport::HardSurfaceFilter(minRunwayLenFt); + } else if (ty != FGPositioned::INVALID) { + FGPositioned::TypeFilter* tf = new FGPositioned::TypeFilter(ty); + + for (int t=1; arg->hasChild("type", t); ++t) { + sty = arg->getChild("type", t)->getStringValue(); + tf->addType(FGPositioned::typeFromName(sty)); + } + + return tf; + } + + return NULL; +} + +static SGGeod commandSearchPos(const SGPropertyNode* arg) +{ + if (arg->hasChild("longitude-deg") && arg->hasChild("latitude-deg")) { + return SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"), + arg->getDoubleValue("latitude-deg")); + } + + // use current viewer/aircraft position + return SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"), + fgGetDouble("/position/latitude-deg")); +} + +void commandClearExisting(const SGPropertyNode* arg) +{ + if (arg->getBoolValue("clear", true)) { + // delete all existing result children from their parent + string resultPath = arg->getStringValue("results"); + SGPropertyNode* n = fgGetNode(resultPath.c_str(), 0, true); + SGPropertyNode* pr = n->getParent(); + pr->removeChildren(n->getName(), false /* keep=false, i.e delete nodes */); + } +} + +bool commandFindClosest(const SGPropertyNode* arg) +{ + int n = arg->getIntValue("max-results", 1); + if ((n < 1) || (n > 100)) { + SG_LOG(SG_GENERAL, SG_WARN, "commandFindClosest: max-results invalid:" << n); + return false; + } + + string resultPath = arg->getStringValue("results"); + if (resultPath.empty()) { + SG_LOG(SG_GENERAL, SG_WARN, "commandFindClosest: no results path defined"); + return false; + } + + std::auto_ptr filt(createSearchFilter(arg)); +// cap search range, since huge ranges will overload everything + double cutoff = arg->getDoubleValue("cutoff-nm", 400.0); + SG_CLAMP_RANGE(cutoff, 0.0, 1000.0); + + SGGeod pos = commandSearchPos(arg); + commandClearExisting(arg); + + FGPositioned::List results = FGPositioned::findClosestN(pos, n, cutoff, filt.get()); + for (unsigned int i=0; igetStringValue("results"); + if (resultPath.empty()) { + SG_LOG(SG_GENERAL, SG_WARN, "commandFindByIdent: no results path defined"); + return false; + } + + std::auto_ptr filt(createSearchFilter(arg)); + SGGeod pos = commandSearchPos(arg); + commandClearExisting(arg); + + FGPositioned::List results; + bool exact = arg->getBoolValue("exact", true); + if (arg->hasChild("name")) { + results = FGPositioned::findAllWithName(arg->getStringValue("name"), filt.get(), exact); + } else if (arg->hasChild("ident")) { + results = FGPositioned::findAllWithIdent(arg->getStringValue("ident"), filt.get(), exact); + } else { + SG_LOG(SG_GENERAL, SG_WARN, "commandFindByIdent: no search term defined"); + return false; + } + + bool orderByRange = arg->getBoolValue("order-by-distance", true); + if (orderByRange) { + FGPositioned::sortByRange(results, pos); + } + + for (unsigned int i=0; iaddCommand("find-nearest", commandFindClosest); + SGCommandMgr::instance()->addCommand("find-by-ident", commandFindByIdent); +} + +FGPositioned::TypeFilter::TypeFilter(Type aTy) +{ + types.push_back(aTy); +} + +void FGPositioned::TypeFilter::addType(Type aTy) +{ + types.push_back(aTy); +} + +bool +FGPositioned::TypeFilter::pass(FGPositioned* aPos) const +{ + std::vector::const_iterator it = types.begin(), + end = types.end(); + for (; it != end; ++it) { + return aPos->type() == *it; + } + + return false; }