diff --git a/src/Navaids/Makefile.am b/src/Navaids/Makefile.am index cff153f9b..a6201ae7f 100644 --- a/src/Navaids/Makefile.am +++ b/src/Navaids/Makefile.am @@ -7,7 +7,8 @@ libNavaids_a_SOURCES = \ fix.hxx fixlist.hxx fixlist.cxx \ awynet.hxx awynet.cxx \ navrecord.hxx navrecord.cxx \ - navlist.hxx navlist.cxx + navlist.hxx navlist.cxx \ + positioned.hxx positioned.cxx # ils.hxx ilslist.hxx ilslist.cxx \ # mkrbeacons.hxx mkrbeacons.cxx diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx new file mode 100644 index 000000000..2d2309fed --- /dev/null +++ b/src/Navaids/positioned.cxx @@ -0,0 +1,438 @@ +// positioned.cxx - base class for objects which are positioned +// +// Copyright (C) 2008 James Turner +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#include <map> +#include <set> + +#include <simgear/math/sg_geodesy.hxx> + +#include "positioned.hxx" + +typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex; +typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> 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(); + } +}; + +typedef std::set<FGPositioned*, OrderByType> BucketEntry; +typedef std::map<long int, BucketEntry> 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); + global_namedIndex.insert(global_namedIndex.begin(), + std::make_pair(aPos->ident(), aPos)); + + SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos); + it->second.insert(aPos); +} + +static void +removeFromIndices(FGPositioned* aPos) +{ + assert(aPos); + + 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; + } + + SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos); + sit->second.erase(aPos); +} + +static void +spatialFilterInBucket(const SGBucket& aBucket, const 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(); + + for ( ; l != u; ++l) { + if (aFilter(*l)) { + aResult.push_back(*l); + } + } +} + +static void +spatialFind(const SGGeod& aPos, double aRange, + const 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 +} + +class LowerLimitOfType +{ +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(); + } +}; + +static void +spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, FGPositioned::Type aUpper, 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++) { + 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()); + + for ( ; l != u; ++l) { + aResult.push_back(*l); + } + + } // of j-iteration + } // of i-iteration +} + +/** + * Cartesian range predicate. Note that for really long ranges, might need to + * to use geodetic / geocentric distance instead + */ +class RangePredictate +{ +public: + RangePredictate(const Point3D& aOrigin, double aRange) : + mOrigin(aOrigin), + mRangeSquared(aRange * aRange) + { ; } + + bool operator()(const FGPositionedRef& aPos) + { + Point3D p(Point3D::fromSGGeod(aPos->geod())); + bool ok = (mOrigin.distance3Dsquared(p) > mRangeSquared); + if (ok) { + double x = sqrt(mOrigin.distance3Dsquared(p) - mRangeSquared); + x *= SG_METER_TO_NM; + //std::cout << "pos:" << aPos->ident() << " failed range check by " << x << std::endl; + } + return ok; + } + +private: + Point3D mOrigin; + double mRangeSquared; +}; + +static void +filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult) +{ + RangePredictate pred(Point3D::fromSGGeod(aPos), aRange * SG_NM_TO_METER); + FGPositioned::List::iterator newEnd; + newEnd = std::remove_if(aResult.begin(), aResult.end(), pred); + aResult.erase(newEnd, aResult.end()); +} + +class DistanceOrdering +{ +public: + DistanceOrdering(const SGGeod& aPos) : + mPos(Point3D::fromSGGeod(aPos)) + { } + + bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const + { + return mPos.distance3Dsquared(Point3D::fromSGGeod(a->geod())) < + mPos.distance3Dsquared(Point3D::fromSGGeod(b->geod())); + } + +private: + Point3D mPos; +}; + +static void +sortByDistance(const SGGeod& aPos, FGPositioned::List& aResult) +{ + std::sort(aResult.begin(), aResult.end(), DistanceOrdering(aPos)); +} + +static FGPositionedRef +namedFindClosestTyped(const std::string& aIdent, const SGGeod& aOrigin, + FGPositioned::Type aLower, FGPositioned::Type aUpper) +{ + NamedIndexRange range = global_namedIndex.equal_range(aIdent); + if (range.first == range.second) return NULL; + +// 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 - all we care about is the type + FGPositioned::Type ty = range.first->second->type(); + if ((ty < aLower) || (ty > aUpper)) { + return NULL; // type check failed + } + + 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; + Point3D origin(Point3D::fromSGGeod(aOrigin)); + + for (; range.first != range.second; ++range.first) { + // filter by type + FGPositioned::Type ty = range.first->second->type(); + if ((ty < aLower) || (ty > aUpper)) { + continue; + } + + // find distance + Point3D p(Point3D::fromSGGeod(range.first->second->geod())); + double ds = origin.distance3Dsquared(p); + if (ds < minDist) { + minDist = ds; + result = range.first->second; + } + } + + return result; +} + +static FGPositioned::List +spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, const 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; + + // 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); + } + + result.insert(result.end(), hits.begin(), hits.end()); // append + } // of outer loop + + if (result.size() > aN) { + result.resize(aN); // truncate at requested number of matches + } + + sortByDistance(aPos, result); + return result; +} + +/////////////////////////////////////////////////////////////////////////////// + +FGPositioned::FGPositioned() : + mType(INVALID) +{ +} + +FGPositioned::FGPositioned(Type ty, const std::string& aIdent, double aLat, double aLon, double aElev) : + mType(ty), + mIdent(aIdent), + mPosition(SGGeod::fromDegFt(aLon, aLat, aElev)) +{ + //addToIndices(this); + //SGReferenced::get(this); // hold an owning ref, for the moment +} + +FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) : + mType(ty), + mIdent(aIdent), + mPosition(aPos) +{ + //addToIndices(this); + //SGReferenced::get(this); // hold an owning ref, for the moment +} + +FGPositioned::~FGPositioned() +{ + //std::cout << "~FGPositioned:" << mIdent << std::endl; + //removeFromIndices(this); +} + +SGBucket +FGPositioned::bucket() const +{ + return SGBucket(mPosition); +} + +const char* FGPositioned::nameForType(Type aTy) +{ + switch (aTy) { + case FIX: return "fix"; + case VOR: return "VOR"; + case NDB: return "NDB"; + case OM: return "outer-marker"; + case MM: return "middle-marker"; + case IM: return "inner-marker"; + case AIRPORT: return "airport"; + case HELIPORT: return "heliport"; + case SEAPORT: return "seaport"; + case WAYPOINT: return "waypoint"; + default: + return "unknown"; + } +} + +/////////////////////////////////////////////////////////////////////////////// +// search / query functions + +FGPositionedRef +FGPositioned::findClosestWithIdent(const std::string& aIdent, double aLat, double aLon) +{ + return findClosestWithIdent(aIdent, SGGeod::fromDeg(aLon, aLat)); +} + +FGPositionedRef +FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos) +{ + return namedFindClosestTyped(aIdent, aPos, INVALID, LAST_TYPE); +} + +FGPositioned::List +FGPositioned::findWithinRangeByType(const SGGeod& aPos, double aRangeNm, Type aTy) +{ + List result; + spatialFindTyped(aPos, aRangeNm, aTy, aTy, result); + filterListByRange(aPos, aRangeNm, result); + return result; +} + +FGPositioned::List +FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, const Filter& aFilter) +{ + List result; + spatialFind(aPos, aRangeNm, aFilter, result); + filterListByRange(aPos, aRangeNm, result); + return result; +} + +FGPositioned::List +FGPositioned::findAllWithIdent(const std::string& aIdent) +{ + List result; + NamedIndexRange range = global_namedIndex.equal_range(aIdent); + for (; range.first != range.second; ++range.first) { + result.push_back(range.first->second); + } + + return result; +} + +FGPositioned::List +FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, const Filter& aFilter) +{ + return spatialGetClosest(aPos, aN, aCutoffNm, aFilter); +} + diff --git a/src/Navaids/positioned.hxx b/src/Navaids/positioned.hxx new file mode 100644 index 000000000..f6cf3cf37 --- /dev/null +++ b/src/Navaids/positioned.hxx @@ -0,0 +1,142 @@ +// positioned.hxx - base class for objects which are positioned +// +// Copyright (C) 2008 James Turner +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#ifndef FG_POSITIONED_HXX +#define FG_POSITIONED_HXX + +#include <vector> + +#include <simgear/structure/SGSharedPtr.hxx> +#include <simgear/bucket/newbucket.hxx> + +class FGPositioned; + +typedef SGSharedPtr<FGPositioned> FGPositionedRef; + +class FGPositioned : public SGReferenced +{ +public: + + typedef enum { + INVALID = 0, + AIRPORT, + HELIPORT, + SEAPORT, + RUNWAY, + TAXIWAY, + PARK_STAND, + FIX, + VOR, + NDB, + ILS, + LOC, + GS, + OM, + MM, + IM, + DME, + TACAN, + OBSTACLE, + WAYPOINT, // user-defined waypoint + FREQ_GND, + FREQ_TWR, + FREQ_ATIS, + LAST_TYPE + } Type; + + typedef std::vector<FGPositionedRef> List; + + virtual ~FGPositioned(); + + Type type() const + { return mType; } + + const std::string& ident() const + { return mIdent; } + + const SGGeod& geod() const + { return mPosition; } + + SGBucket bucket() const; + + double latitude() const + { return mPosition.getLatitudeDeg(); } + + double longitude() const + { return mPosition.getLongitudeDeg(); } + + double elevation() const + { return mPosition.getElevationFt(); } + + /** + * Predicate class to support custom filtering of FGPositioned queries + */ + class Filter + { + public: + virtual ~Filter() { ; } + + virtual bool pass(FGPositioned* aPos) const = 0; + + bool operator()(FGPositioned* aPos) const + { return pass(aPos); } + }; + + static List findWithinRange(const SGGeod& aPos, double aRangeNm, const Filter& aFilter); + + static List findWithinRangeByType(const SGGeod& aPos, double aRangeNm, Type aTy); + + static FGPositionedRef findClosestWithIdent(const std::string& aIdent, double aLat, double aLon); + + static FGPositionedRef findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos); + + static List findAllWithIdent(const std::string& aIdent); + + /** + * Find the closest N items to a position, which pass the specified filter + * A cutoff range in NM must be specified, to constrain the search acceptably. + * Very large cutoff values will make this slow. + * + * @result The matches (possibly less than N, depending on the filter and cutoff), + * sorted by distance from the search pos + * @param aN - number of matches to find + * @param aCutoffNm - maximum distance to search within, in nautical miles + */ + static List findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, const Filter& aFilter); + + /** + * Debug helper, map a type to a human-readable string + */ + static const char* nameForType(Type aTy); +protected: + FGPositioned(Type ty, const std::string& aIdent, double aLat, double aLon, double aElev); + + FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos); + + SGGeod mPosition; // can't be const right now + +//private: // make private once FGFix and FGRunway are heap-based + FGPositioned(); + + Type mType; + std::string mIdent; +}; + +#endif // of FG_POSITIONED_HXX