X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2Fairways.cxx;h=0000cdfa0652ed24a9a877027f53195f0710b825;hb=5ccc83566785c9b5b75e8d03579dbd1aa45d7237;hp=5bdbb2beec022b579c36f099e26159965b9df2b9;hpb=622f71c01c70a81db94a7e670953645d0c9129e5;p=flightgear.git diff --git a/src/Navaids/airways.cxx b/src/Navaids/airways.cxx index 5bdbb2bee..0000cdfa0 100644 --- a/src/Navaids/airways.cxx +++ b/src/Navaids/airways.cxx @@ -31,55 +31,31 @@ #include #include +#include #include #include
#include #include +#include using std::make_pair; using std::string; using std::set; using std::vector; -#define DEBUG_AWY_SEARCH 1 +//#define DEBUG_AWY_SEARCH 1 namespace flightgear { -Airway::Network* Airway::static_lowLevel = NULL; -Airway::Network* Airway::static_highLevel = NULL; - ////////////////////////////////////////////////////////////////////////////// -/** - * information about an edge in the network. - * Some of this information is computed lazily - */ -class AdjacentWaypoint -{ -public: - AdjacentWaypoint(const FGPositionedRef& aOrigin, - const FGPositionedRef& aDest, Airway* aWay); - - double distanceM() const; - - const FGPositionedRef& other(const FGPositionedRef& aEnd) const; - - const FGPositionedRef origin; - const FGPositionedRef destination; - const Airway* airway; - -private: - void validate() const; - mutable double _distanceM; -}; - class AStarOpenNode : public SGReferenced { public: AStarOpenNode(FGPositionedRef aNode, double aLegDist, - Airway* aAirway, + int aAirway, FGPositionedRef aDest, AStarOpenNode* aPrev) : node(aNode), airway(aAirway), @@ -98,7 +74,7 @@ public: } FGPositionedRef node; - Airway* airway; + int airway; SGSharedPtr previous; double distanceFromStart; // aka 'g(x)' double directDistanceToDestination; // aka 'h(x)' @@ -117,11 +93,24 @@ typedef SGSharedPtr AStarOpenNodeRef; Airway::Network* Airway::lowLevel() { + static Network* static_lowLevel = NULL; + + if (!static_lowLevel) { + static_lowLevel = new Network; + static_lowLevel->_networkID = 1; + } + return static_lowLevel; } Airway::Network* Airway::highLevel() { + static Network* static_highLevel = NULL; + if (!static_highLevel) { + static_highLevel = new Network; + static_highLevel->_networkID = 2; + } + return static_highLevel; } @@ -132,14 +121,8 @@ Airway::Airway(const std::string& aIdent, double aTop, double aBottom) : { } -void Airway::load() +void Airway::load(const SGPath& path) { - static_lowLevel = new Network; - static_highLevel = new Network; - - SGPath path( globals->get_fg_root() ); - path.append( "Navaids/awy.dat" ); - std::string identStart, identEnd, name; double latStart, lonStart, latEnd, lonEnd; int type, base, top; @@ -148,7 +131,7 @@ void Airway::load() sg_gzifstream in( path.str() ); if ( !in.is_open() ) { - SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() ); + SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() ); throw sg_io_exception("Could not open airways data", sg_location(path.str())); } // toss the first two lines of the file @@ -168,28 +151,22 @@ void Airway::load() // type = 1; low-altitude // type = 2; high-altitude - Network* net = (type == 1) ? static_lowLevel : static_highLevel; + Network* net = (type == 1) ? lowLevel() : highLevel(); SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)), endPos(SGGeod::fromDeg(lonEnd, latEnd)); - Airway* awy = net->findAirway(name, top, base); + int awy = net->findAirway(name, top, base); net->addEdge(awy, startPos, identStart, endPos, identEnd); } // of file line iteration } -Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase) +int Airway::Network::findAirway(const std::string& aName, double aTop, double aBase) { - AirwayDict::iterator it = _airways.find(aName); - if (it == _airways.end()) { - Airway* awy = new Airway(aName, aTop, aBase); - it = _airways.insert(it, make_pair(aName, awy)); - } - - return it->second; + return NavDataCache::instance()->findAirway(_networkID, aName); } -void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos, +void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos, const std::string& aStartIdent, const SGGeod& aEndPos, const std::string& aEndIdent) { @@ -197,28 +174,39 @@ void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos, FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos); if (!start) { - SG_LOG(SG_GENERAL, SG_INFO, "unknown airways start pt: '" << aStartIdent << "'"); + SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'"); start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos); return; } if (!end) { - SG_LOG(SG_GENERAL, SG_INFO, "unknown airways end pt: '" << aEndIdent << "'"); + SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'"); end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos); return; } - AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay); - _graph.insert(make_pair(start, edge)); - _graph.insert(make_pair(end, edge)); + NavDataCache::instance()->insertEdge(_networkID, aWay, start->guid(), end->guid()); } ////////////////////////////////////////////////////////////////////////////// -bool Airway::Network::inNetwork(const FGPositioned* aPos) const +static double headingDiffDeg(double a, double b) { - FGPositioned* pos = const_cast(aPos); - return (_graph.find(pos) != _graph.end()); + double rawDiff = b - a; + SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0); + return rawDiff; +} + +bool Airway::Network::inNetwork(PositionedID posID) const +{ + NetworkMembershipDict::iterator it = _inNetworkCache.find(posID); + if (it != _inNetworkCache.end()) { + return it->second; // cached, easy + } + + bool r = NavDataCache::instance()->isInAirwayNetwork(_networkID, posID); + _inNetworkCache.insert(it, std::make_pair(posID, r)); + return r; } bool Airway::Network::route(WayptRef aFrom, WayptRef aTo, @@ -238,8 +226,8 @@ bool Airway::Network::route(WayptRef aFrom, WayptRef aTo, boost::tie(to, exactTo) = findClosestNode(aTo); #ifdef DEBUG_AWY_SEARCH - SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name()); - SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name()); + SG_LOG(SG_NAVAID, SG_INFO, "from:" << from->ident() << "/" << from->name()); + SG_LOG(SG_NAVAID, SG_INFO, "to:" << to->ident() << "/" << to->name()); #endif bool ok = search2(from, to, aPath); @@ -247,19 +235,46 @@ bool Airway::Network::route(WayptRef aFrom, WayptRef aTo, return false; } - if (exactTo) { + return cleanGeneratedPath(aFrom, aTo, aPath, exactTo, exactFrom); +} + +bool Airway::Network::cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath, + bool exactTo, bool exactFrom) +{ + // path cleaning phase : various cases to handle here. + // if either the TO or FROM waypoints were 'exact', i.e part of the enroute + // structure, we don't want to duplicate them. This happens frequently with + // published SIDs and STARs. + // secondly, if the waypoints are NOT on the enroute structure, the course to + // them may be a significant dog-leg. Check how the leg course deviates + // from the direct course FROM->TO, and delete the first/last leg if it's more + // than 90 degrees out. + // note we delete a maximum of one leg, and no more. This is a heuristic - we + // could check the next (previous) legs, but at some point we'll end up + // deleting too much. + + const double MAX_DOG_LEG = 90.0; + double enrouteCourse = SGGeodesy::courseDeg(aFrom->position(), aTo->position()), + finalLegCourse = SGGeodesy::courseDeg(aPath.back()->position(), aTo->position()); + + bool isDogLeg = fabs(headingDiffDeg(enrouteCourse, finalLegCourse)) > MAX_DOG_LEG; + if (exactTo || isDogLeg) { aPath.pop_back(); } - if (exactFrom) { - // edge case - if from and to are equal, which can happen, don't - // crash here. This happens routing EGPH -> EGCC; 'DCS' is common - // to the EGPH departure and EGCC STAR. - if (!aPath.empty()) { - aPath.erase(aPath.begin()); - } + // edge case - if from and to are equal, which can happen, don't + // crash here. This happens routing EGPH -> EGCC; 'DCS' is common + // to the EGPH departure and EGCC STAR. + if (aPath.empty()) { + return true; } + double initialLegCourse = SGGeodesy::courseDeg(aFrom->position(), aPath.front()->position()); + isDogLeg = fabs(headingDiffDeg(enrouteCourse, initialLegCourse)) > MAX_DOG_LEG; + if (exactFrom || isDogLeg) { + aPath.erase(aPath.begin()); + } + return true; } @@ -278,7 +293,7 @@ public: virtual bool pass(FGPositioned* aPos) const { - return _net->inNetwork(aPos); + return _net->inNetwork(aPos->guid()); } virtual FGPositioned::Type minType() const @@ -319,7 +334,7 @@ static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute) // run over the route, creating waypoints for (n = aNode; n; n=n->previous) { - aRoute[--count] = new NavaidWaypoint(n->node, n->airway); + aRoute[--count] = new NavaidWaypoint(n->node, NULL); } } @@ -349,16 +364,14 @@ public: bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest, WayptVec& aRoute) -{ - - typedef set ClosedNodeSet; - typedef std::pair AdjacencyMapRange; +{ + typedef set ClosedNodeSet; OpenNodeHeap openNodes; ClosedNodeSet closedNodes; HeapOrder ordering; - openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL)); + openNodes.push_back(new AStarOpenNode(aStart, 0.0, 0, aDest, NULL)); // A* open node iteration while (!openNodes.empty()) { @@ -366,9 +379,11 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest, AStarOpenNodeRef x = openNodes.back(); FGPositioned* xp = x->node; openNodes.pop_back(); - closedNodes.insert(xp); - - // SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost()); + closedNodes.insert(xp->guid()); + +#ifdef DEBUG_AWY_SEARCH + SG_LOG(SG_NAVAID, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost()); +#endif // check if xp is the goal; if so we're done, since there cannot be an open // node with lower f(x) value. @@ -378,77 +393,48 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest, } // adjacent (neighbour) iteration - AdjacencyMapRange r(_graph.equal_range(xp)); - for (; r.first != r.second; ++r.first) { - AdjacentWaypoint* adj(r.first->second); - FGPositioned* yp = adj->other(xp); - if (closedNodes.count(yp)) { + NavDataCache* cache = NavDataCache::instance(); + BOOST_FOREACH(AirwayEdge other, cache->airwayEdgesFrom(_networkID, xp->guid())) { + if (closedNodes.count(other.second)) { continue; // closed, ignore } + FGPositioned* yp = cache->loadById(other.second); + double edgeDistanceM = SGGeodesy::distanceM(xp->geod(), yp->geod()); AStarOpenNodeRef y = findInOpen(openNodes, yp); if (y) { // already open - double g = x->distanceFromStart + adj->distanceM(); + double g = x->distanceFromStart + edgeDistanceM; if (g > y->distanceFromStart) { // worse path, ignore - //SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() << - // " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g); +#ifdef DEBUG_AWY_SEARCH + SG_LOG(SG_NAVAID, SG_INFO, "\tabandoning " << yp->ident() << + " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g); +#endif continue; } // we need to update y. Unfortunately this means rebuilding the heap, // since y's score can change arbitrarily - //SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g); +#ifdef DEBUG_AWY_SEARCH + SG_LOG(SG_NAVAID, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g); +#endif y->previous = x; y->distanceFromStart = g; - y->airway = (Airway*) adj->airway; + y->airway = other.first; std::make_heap(openNodes.begin(), openNodes.end(), ordering); } else { // not open, insert a new node for y into the heap - y = new AStarOpenNode(yp, adj->distanceM(), - (Airway*) adj->airway, aDest, x); - //SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost()); + y = new AStarOpenNode(yp, edgeDistanceM, other.first, aDest, x); +#ifdef DEBUG_AWY_SEARCH + SG_LOG(SG_NAVAID, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost()); +#endif openNodes.push_back(y); std::push_heap(openNodes.begin(), openNodes.end(), ordering); } } // of neighbour iteration } // of open node iteration - SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route"); + SG_LOG(SG_NAVAID, SG_INFO, "A* failed to find route"); return false; } -////////////////////////////////////////////////////////////////////////////// -// AdjacentWaypoint definitions - -AdjacentWaypoint::AdjacentWaypoint( - const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) : - origin(aOrigin), - destination(aDest), - airway(aWay), - _distanceM(-1.0) -{ - -} - -const FGPositionedRef& -AdjacentWaypoint::other(const FGPositionedRef& aEnd) const -{ - return (aEnd == origin) ? destination : origin; -} - -double AdjacentWaypoint::distanceM() const -{ - validate(); - return _distanceM; -} - -void AdjacentWaypoint::validate() const -{ - if (_distanceM > 0.0) { - return; // already validated - } - - _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod()); -} - } // of namespace flightgear