X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2Fpositioned.cxx;h=bdc7d17a55bcb596cc07706e712a8b64a4d9d205;hb=9575783491783bfdb6f6dcbfa59ba750a3cf0da6;hp=d1e34373e0017c1e2d2cd9ab68192f9b264c67af;hpb=f9de92f53db91c45e4bd885ba606749e9597fdbb;p=flightgear.git diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx index d1e34373e..bdc7d17a5 100644 --- a/src/Navaids/positioned.cxx +++ b/src/Navaids/positioned.cxx @@ -22,459 +22,156 @@ # include "config.h" #endif +#include "positioned.hxx" + #include #include #include // for sort -#include // for char-traits toupper +#include +#include -#include +#include +#include -#include #include +#include +#include +#include +#include -#include "positioned.hxx" - -typedef std::multimap NamedPositionedIndex; -typedef std::pair NamedIndexRange; - -using std::lower_bound; -using std::upper_bound; +#include "Navaids/PositionedOctree.hxx" -/** - * 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::string; +using namespace flightgear; -class LowerLimitOfType +static void validateSGGeod(const SGGeod& geod) { -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 + if (SGMisc::isNaN(geod.getLatitudeDeg()) || + SGMisc::isNaN(geod.getLongitudeDeg())) { - return a < b->type(); + throw sg_range_exception("position is invalid, NaNs"); } -}; - - -typedef std::set BucketEntry; -typedef std::map SpatialPositionedIndex; +} -static NamedPositionedIndex global_namedIndex; -static SpatialPositionedIndex global_spatialIndex; -SpatialPositionedIndex::iterator -bucketEntryForPositioned(FGPositioned* aPos) -{ - 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); +FGPositioned::FGPositioned(PositionedID aGuid, Type ty, const std::string& aIdent, const SGGeod& aPos) : + mGuid(aGuid), + mPosition(aPos), + mCart(SGVec3d::fromGeod(mPosition)), + mType(ty), + mIdent(aIdent) +{ } -static void -removeFromIndices(FGPositioned* aPos) +FGPositioned::~FGPositioned() { - 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); +// std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl; } -static void -spatialFilterInBucket(const SGBucket& aBucket, FGPositioned::Filter* aFilter, FGPositioned::List& aResult) +FGPositioned* +FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos) { - SpatialPositionedIndex::const_iterator it; - it = global_spatialIndex.find(aBucket.gen_index()); - if (it == global_spatialIndex.end()) { - return; + NavDataCache* cache = NavDataCache::instance(); + TypeFilter filter(WAYPOINT); + FGPositionedList existing = cache->findAllWithIdent(aIdent, &filter, true); + if (!existing.empty()) { + SG_LOG(SG_NAVAID, SG_WARN, "attempt to insert duplicate WAYPOINT:" << aIdent); + return existing.front().ptr(); } - BucketEntry::const_iterator l = it->second.begin(); - BucketEntry::const_iterator u = it->second.end(); - - if (!aFilter) { // pass everything - aResult.insert(aResult.end(), l, u); - return; - } - - if (aFilter->hasTypeRange()) { - // avoid many calls to the filter hook - l = lower_bound(it->second.begin(), it->second.end(), aFilter->minType(), LowerLimitOfType()); - u = upper_bound(l, it->second.end(), aFilter->maxType(), LowerLimitOfType()); - } - - for ( ; l != u; ++l) { - if ((*aFilter)(*l)) { - aResult.push_back(*l); - } - } + PositionedID id = cache->createPOI(WAYPOINT, aIdent, aPos); + return cache->loadById(id); } -static void -spatialFind(const SGGeod& aPos, double aRange, - FGPositioned::Filter* aFilter, FGPositioned::List& aResult) +void FGPositioned::deleteUserWaypoint(const std::string& aIdent) { - 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 + NavDataCache* cache = NavDataCache::instance(); + cache->removePOI(WAYPOINT, aIdent); } -/** - */ -class RangePredictate -{ -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); - } - -private: - SGVec3d mOrigin; - double mRangeSqr; -}; -static void -filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult) +const SGVec3d& +FGPositioned::cart() const { - 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()); + return mCart; } -class DistanceOrdering +FGPositioned::Type FGPositioned::typeFromName(const std::string& aName) { -public: - DistanceOrdering(const SGGeod& aPos) : - mPos(SGVec3d::fromGeod(aPos)) - { } - - bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const - { - double dA = distSqr(a->cart(), mPos), - dB = distSqr(b->cart(), mPos); - return dA < dB; + if (aName.empty() || (aName == "")) { + return INVALID; } -private: - SGVec3d mPos; -}; - -static void -sortByDistance(const SGGeod& aPos, FGPositioned::List& aResult) -{ - std::sort(aResult.begin(), aResult.end(), DistanceOrdering(aPos)); -} - -static FGPositionedRef -namedFindClosest(const std::string& aIdent, const SGGeod& aOrigin, FGPositioned::Filter* aFilter) -{ - NamedIndexRange range = global_namedIndex.equal_range(aIdent); - if (range.first == range.second) { - return NULL; - } + typedef struct { + const char* _name; + Type _ty; + } NameTypeEntry; -// common case, only one result. looks a bit ugly because these are -// sequential iterators, not random-access ones - NamedPositionedIndex::const_iterator check = range.first; - if (++check == range.second) { - // excellent, only one match in the range - FGPositioned* r = range.first->second; - if (aFilter) { - if (aFilter->hasTypeRange() && !aFilter->passType(r->type())) { - return NULL; - } + const NameTypeEntry names[] = { + {"airport", AIRPORT}, + {"vor", VOR}, + {"loc", LOC}, + {"ils", ILS}, + {"gs", GS}, + {"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}, + {"runway", RUNWAY}, + {"helipad", HELIPAD}, + {"country", COUNTRY}, + {"city", CITY}, + {"town", TOWN}, + {"village", VILLAGE}, - if (!aFilter->pass(r)) { - return NULL; - } - } // of have a filter - - return r; - } // 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)); - - for (; it != range.second; ++it) { - FGPositioned::Type ty = range.first->second->type(); - if (aFilter) { - if (aFilter->hasTypeRange() && !aFilter->passType(ty)) { - continue; - } - - if (!aFilter->pass(range.first->second)) { - continue; - } - } + // aliases + {"localizer", LOC}, + {"gnd", FREQ_GROUND}, + {"twr", FREQ_TOWER}, + {"waypoint", WAYPOINT}, + {"apt", AIRPORT}, + {"arpt", AIRPORT}, + {"rwy", RUNWAY}, + {"any", INVALID}, + {"all", INVALID}, - // find distance - double d2 = distSqr(cartOrigin, it->second->cart()); - if (d2 < minDist) { - minDist = d2; - result = it->second; - } - } + {NULL, INVALID} + }; - return result; -} - -static FGPositioned::List -spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, FGPositioned::Filter* aFilter) -{ - 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; + std::string lowerName(boost::to_lower_copy(aName)); - // 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 ((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); + for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) { + if (::strcmp(n->_name, lowerName.c_str()) == 0) { + return n->_ty; } - - 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 } - return result; -} - -////////////////////////////////////////////////////////////////////////////// - -class OrderByName -{ -public: - bool operator()(FGPositioned* a, FGPositioned* b) const - { - return a->name() < b->name(); - } -}; - -/** - * 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) -{ - 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; - - 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; - } - } - - matches.push_back(it->second); - } - - // sort alphabetically on name - std::sort(matches.begin(), matches.end(), OrderByName()); - - // 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 - - // nasty code to avoid excessive string copying and allocations. - // 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 - for (unsigned int i=0; iname().size(); - int icaoLength = matches[i]->ident().size(); - char* entry = new char[nameLength + 11]; - char* dst = entry; - *dst++ = ' '; - memcpy(dst, matches[i]->name().c_str(), nameLength); - dst += nameLength; - *dst++ = ' '; - *dst++ = ' '; - *dst++ = ' '; - *dst++ = '('; - memcpy(dst, matches[i]->ident().c_str(), icaoLength); - dst += icaoLength; - *dst++ = ')'; - *dst++ = 0; - result[i] = entry; - } - - return result; -} - -/////////////////////////////////////////////////////////////////////////////// - -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); -} - -/////////////////////////////////////////////////////////////////////////////// - -FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos, bool aIndexed) : - mType(ty), - mPosition(aPos), - mIdent(aIdent) -{ - SGReferenced::get(this); // hold an owning ref, for the moment - - if (aIndexed) { - assert(ty != TAXIWAY); - addToIndices(this); - } -} - -FGPositioned::~FGPositioned() -{ - //std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl; - removeFromIndices(this); -} - -SGBucket -FGPositioned::bucket() const -{ - return SGBucket(mPosition); -} - -SGVec3d -FGPositioned::cart() const -{ - return SGVec3d::fromGeod(mPosition); + SG_LOG(SG_NAVAID, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName); + return INVALID; } const char* FGPositioned::nameForType(Type aTy) { switch (aTy) { case RUNWAY: return "runway"; + case HELIPAD: return "helipad"; case TAXIWAY: return "taxiway"; - case PARK_STAND: return "parking stand"; + case PAVEMENT: return "pavement"; + case PARKING: return "parking stand"; case FIX: return "fix"; case VOR: return "VOR"; case NDB: return "NDB"; case ILS: return "ILS"; - case LOC: return "localiser"; + case LOC: return "localizer"; case GS: return "glideslope"; case OM: return "outer-marker"; case MM: return "middle-marker"; @@ -485,6 +182,18 @@ 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"; + case TAXI_NODE: return "taxi-node"; + case COUNTRY: return "country"; + case CITY: return "city"; + case TOWN: return "town"; + case VILLAGE: return "village"; default: return "unknown"; } @@ -496,79 +205,168 @@ const char* FGPositioned::nameForType(Type aTy) FGPositionedRef FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter) { - return namedFindClosest(aIdent, aPos, aFilter); + validateSGGeod(aPos); + return NavDataCache::instance()->findClosestWithIdent(aIdent, aPos, aFilter); } -FGPositioned::List +FGPositionedRef +FGPositioned::findFirstWithIdent(const std::string& aIdent, Filter* aFilter) +{ + if (aIdent.empty()) { + return NULL; + } + + FGPositionedList r = + NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, true); + if (r.empty()) { + return NULL; + } + + return r.front(); +} + +FGPositionedList FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter) { - List result; - spatialFind(aPos, aRangeNm, aFilter, result); - filterListByRange(aPos, aRangeNm, result); + validateSGGeod(aPos); + + FGPositionedList result; + Octree::findAllWithinRange(SGVec3d::fromGeod(aPos), + aRangeNm * SG_NM_TO_METER, aFilter, result, 0xffffff); return result; } -FGPositioned::List -FGPositioned::findAllWithIdentSortedByRange(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter) +FGPositionedList +FGPositioned::findWithinRangePartial(const SGGeod& aPos, double aRangeNm, Filter* aFilter, bool& aPartial) { - 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); - } + validateSGGeod(aPos); - sortByDistance(aPos, result); + int limitMsec = 32; + FGPositionedList result; + aPartial = Octree::findAllWithinRange(SGVec3d::fromGeod(aPos), + aRangeNm * SG_NM_TO_METER, aFilter, result, + limitMsec); return result; } +FGPositionedList +FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact) +{ + return NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, aExact); +} + +FGPositionedList +FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact) +{ + return NavDataCache::instance()->findAllWithName(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); + + FGPositionedList l(findClosestN(aPos, 1, aCutoffNm, aFilter)); + if (l.empty()) { + return NULL; + } + + assert(l.size() == 1); + return l.front(); } -FGPositioned::List +FGPositionedList FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter) { - return spatialGetClosest(aPos, aN, aCutoffNm, aFilter); + validateSGGeod(aPos); + + FGPositionedList result; + int limitMsec = 0xffff; + Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result, limitMsec); + return result; } -FGPositionedRef -FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter) +FGPositionedList +FGPositioned::findClosestNPartial(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter, bool &aPartial) { - 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; - } + validateSGGeod(aPos); - if (aFilter) { - if (aFilter->hasTypeRange() && !aFilter->passType(candidate->type())) { - continue; - } - - if(!aFilter->pass(candidate)) { - continue; - } - } + FGPositionedList result; + int limitMsec = 32; + aPartial = Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result, + limitMsec); + return result; +} + +void +FGPositioned::sortByRange(FGPositionedList& aResult, const SGGeod& aPos) +{ + validateSGGeod(aPos); - if (!aCur) { - return candidate; - } + SGVec3d cartPos(SGVec3d::fromGeod(aPos)); +// computer ordering values + Octree::FindNearestResults r; + FGPositionedList::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()); + +// convert to a plain list + unsigned int count = aResult.size(); + for (unsigned int i=0; i(mPosition) = newPos; + const_cast(mCart) = SGVec3d::fromGeod(newPos); +} + +//------------------------------------------------------------------------------ +FGPositionedRef FGPositioned::loadByIdImpl(PositionedID id) +{ + return flightgear::NavDataCache::instance()->loadById(id); +} + +FGPositioned::TypeFilter::TypeFilter(Type aTy) : + mMinType(aTy), + mMaxType(aTy) +{ + addType(aTy); +} + +void FGPositioned::TypeFilter::addType(Type aTy) +{ + if (aTy == INVALID) { + return; } - return NULL; // fell out, no match in range + types.push_back(aTy); + mMinType = std::min(mMinType, aTy); + mMaxType = std::max(mMaxType, aTy); +} + +bool +FGPositioned::TypeFilter::pass(FGPositioned* aPos) const +{ + if (types.empty()) { + return true; + } + + std::vector::const_iterator it = types.begin(), + end = types.end(); + for (; it != end; ++it) { + if (aPos->type() == *it) { + return true; + } + } + + return false; }