1 // airways.cxx - storage of airways network, and routing between nodes
2 // Written by James Turner, started 2009.
4 // Copyright (C) 2009 Curtis L. Olson
6 // This program is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU General Public License as
8 // published by the Free Software Foundation; either version 2 of the
9 // License, or (at your option) any later version.
11 // This program is distributed in the hope that it will be useful, but
12 // WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Public License for more details.
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
24 #include "airways.hxx"
29 #include <simgear/sg_inlines.h>
30 #include <simgear/structure/exception.hxx>
31 #include <simgear/misc/sgstream.hxx>
32 #include <simgear/misc/sg_path.hxx>
34 #include <boost/foreach.hpp>
35 #include <boost/tuple/tuple.hpp>
37 #include <Main/globals.hxx>
38 #include <Navaids/positioned.hxx>
39 #include <Navaids/waypoint.hxx>
40 #include <Navaids/NavDataCache.hxx>
47 //#define DEBUG_AWY_SEARCH 1
52 //////////////////////////////////////////////////////////////////////////////
54 class AStarOpenNode : public SGReferenced
57 AStarOpenNode(FGPositionedRef aNode, double aLegDist,
59 FGPositionedRef aDest, AStarOpenNode* aPrev) :
64 distanceFromStart = aLegDist;
66 distanceFromStart += previous->distanceFromStart;
69 directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
72 virtual ~AStarOpenNode()
78 SGSharedPtr<AStarOpenNode> previous;
79 double distanceFromStart; // aka 'g(x)'
80 double directDistanceToDestination; // aka 'h(x)'
85 double totalCost() const {
86 return distanceFromStart + directDistanceToDestination;
90 typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
92 ////////////////////////////////////////////////////////////////////////////
94 Airway::Network* Airway::lowLevel()
96 static Network* static_lowLevel = NULL;
98 if (!static_lowLevel) {
99 static_lowLevel = new Network;
100 static_lowLevel->_networkID = 1;
103 return static_lowLevel;
106 Airway::Network* Airway::highLevel()
108 static Network* static_highLevel = NULL;
109 if (!static_highLevel) {
110 static_highLevel = new Network;
111 static_highLevel->_networkID = 2;
114 return static_highLevel;
117 Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
119 _topAltitudeFt(aTop),
120 _bottomAltitudeFt(aBottom)
124 void Airway::load(const SGPath& path)
126 std::string identStart, identEnd, name;
127 double latStart, lonStart, latEnd, lonEnd;
129 //int airwayIndex = 0;
132 sg_gzifstream in( path.str() );
133 if ( !in.is_open() ) {
134 SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() );
135 throw sg_io_exception("Could not open airways data", sg_location(path.str()));
137 // toss the first two lines of the file
141 // read in each remaining line of the file
145 if (identStart == "99") {
149 in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
152 // type = 1; low-altitude
153 // type = 2; high-altitude
154 Network* net = (type == 1) ? lowLevel() : highLevel();
156 SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
157 endPos(SGGeod::fromDeg(lonEnd, latEnd));
159 int awy = net->findAirway(name, top, base);
160 net->addEdge(awy, startPos, identStart, endPos, identEnd);
161 } // of file line iteration
164 WayptVec::const_iterator Airway::find(WayptRef wpt) const
166 WayptVec::const_iterator it;
167 for (it = _elements.begin(); it != _elements.end(); ++it) {
168 if (wpt->matches(*it)) {
176 bool Airway::canVia(const WayptRef& from, const WayptRef& to) const
178 WayptVec::const_iterator fit = find(from);
179 WayptVec::const_iterator tit = find(to);
181 if ((fit == _elements.end()) || (tit == _elements.end())) {
188 WayptVec Airway::via(const WayptRef& from, const WayptRef& to) const
191 WayptVec::const_iterator fit = find(from);
192 WayptVec::const_iterator tit = find(to);
194 if ((fit == _elements.end()) || (tit == _elements.end())) {
195 throw sg_exception("bad VIA transition points");
199 // will cause duplicate point but that seems better than
205 // establish the ordering of the transitions, i.e are we moving forward or
206 // backard along the airway.
208 // forward progression
209 for (++fit; fit != tit; ++fit) {
213 // reverse progression
214 for (--fit; fit != tit; --fit) {
223 bool Airway::containsNavaid(const FGPositionedRef &navaid) const
225 return find(new NavaidWaypoint(navaid, NULL)) != _elements.end();
228 int Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
230 return NavDataCache::instance()->findAirway(_networkID, aName);
233 Airway* Airway::findByIdent(const std::string& aIdent)
235 NavDataCache* ndc = NavDataCache::instance();
237 int id = ndc->findAirway(0, aIdent);
239 PositionedIDVec pts = ndc->airwayWaypts(id);
240 Airway* awy = new Airway(aIdent, 0, 0);
242 PositionedIDVec::const_iterator it;
243 for (it = pts.begin(); it != pts.end(); ++it) {
244 FGPositionedRef pos = ndc->loadById(*it);
245 WayptRef w = new NavaidWaypoint(pos, NULL);
246 awy->_elements.push_back(w);
252 WayptRef Airway::findEnroute(const std::string &aIdent) const
254 WayptVec::const_iterator it;
255 for (it = _elements.begin(); it != _elements.end(); ++it) {
256 if ((*it)->ident() == aIdent) {
264 void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
265 const std::string& aStartIdent,
266 const SGGeod& aEndPos, const std::string& aEndIdent)
268 FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
269 FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
272 SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
273 start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
278 SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
279 end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
283 NavDataCache::instance()->insertEdge(_networkID, aWay, start->guid(), end->guid());
286 //////////////////////////////////////////////////////////////////////////////
288 static double headingDiffDeg(double a, double b)
290 double rawDiff = b - a;
291 SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0);
295 bool Airway::Network::inNetwork(PositionedID posID) const
297 NetworkMembershipDict::iterator it = _inNetworkCache.find(posID);
298 if (it != _inNetworkCache.end()) {
299 return it->second; // cached, easy
302 bool r = NavDataCache::instance()->isInAirwayNetwork(_networkID, posID);
303 _inNetworkCache.insert(it, std::make_pair(posID, r));
307 bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
310 if (!aFrom || !aTo) {
311 throw sg_exception("invalid waypoints to route between");
314 // find closest nodes on the graph to from/to
315 // if argument waypoints are directly on the graph (which is frequently the
316 // case), note this so we don't duplicate them in the output.
318 FGPositionedRef from, to;
319 bool exactTo, exactFrom;
320 boost::tie(from, exactFrom) = findClosestNode(aFrom);
321 boost::tie(to, exactTo) = findClosestNode(aTo);
323 #ifdef DEBUG_AWY_SEARCH
324 SG_LOG(SG_NAVAID, SG_INFO, "from:" << from->ident() << "/" << from->name());
325 SG_LOG(SG_NAVAID, SG_INFO, "to:" << to->ident() << "/" << to->name());
328 bool ok = search2(from, to, aPath);
333 return cleanGeneratedPath(aFrom, aTo, aPath, exactTo, exactFrom);
336 bool Airway::Network::cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath,
337 bool exactTo, bool exactFrom)
339 // path cleaning phase : various cases to handle here.
340 // if either the TO or FROM waypoints were 'exact', i.e part of the enroute
341 // structure, we don't want to duplicate them. This happens frequently with
342 // published SIDs and STARs.
343 // secondly, if the waypoints are NOT on the enroute structure, the course to
344 // them may be a significant dog-leg. Check how the leg course deviates
345 // from the direct course FROM->TO, and delete the first/last leg if it's more
346 // than 90 degrees out.
347 // note we delete a maximum of one leg, and no more. This is a heuristic - we
348 // could check the next (previous) legs, but at some point we'll end up
349 // deleting too much.
351 const double MAX_DOG_LEG = 90.0;
352 double enrouteCourse = SGGeodesy::courseDeg(aFrom->position(), aTo->position()),
353 finalLegCourse = SGGeodesy::courseDeg(aPath.back()->position(), aTo->position());
355 bool isDogLeg = fabs(headingDiffDeg(enrouteCourse, finalLegCourse)) > MAX_DOG_LEG;
356 if (exactTo || isDogLeg) {
360 // edge case - if from and to are equal, which can happen, don't
361 // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
362 // to the EGPH departure and EGCC STAR.
367 double initialLegCourse = SGGeodesy::courseDeg(aFrom->position(), aPath.front()->position());
368 isDogLeg = fabs(headingDiffDeg(enrouteCourse, initialLegCourse)) > MAX_DOG_LEG;
369 if (exactFrom || isDogLeg) {
370 aPath.erase(aPath.begin());
376 std::pair<FGPositionedRef, bool>
377 Airway::Network::findClosestNode(WayptRef aRef)
379 return findClosestNode(aRef->position());
382 class InAirwayFilter : public FGPositioned::Filter
385 InAirwayFilter(Airway::Network* aNet) :
389 virtual bool pass(FGPositioned* aPos) const
391 return _net->inNetwork(aPos->guid());
394 virtual FGPositioned::Type minType() const
395 { return FGPositioned::WAYPOINT; }
397 virtual FGPositioned::Type maxType() const
398 { return FGPositioned::NDB; }
401 Airway::Network* _net;
404 std::pair<FGPositionedRef, bool>
405 Airway::Network::findClosestNode(const SGGeod& aGeod)
407 InAirwayFilter f(this);
408 FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
411 if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
412 exact = true; // within 100 metres, let's call that exact
415 return make_pair(r, exact);
418 /////////////////////////////////////////////////////////////////////////////
420 typedef vector<AStarOpenNodeRef> OpenNodeHeap;
422 static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
424 // count the route length, and hence pre-size aRoute
426 AStarOpenNodeRef n = aNode;
427 for (; n != NULL; ++count, n = n->previous) {;}
428 aRoute.resize(count);
430 // run over the route, creating waypoints
431 for (n = aNode; n; n=n->previous) {
432 aRoute[--count] = new NavaidWaypoint(n->node, NULL);
437 * Inefficent (linear) helper to find an open node in the heap
439 static AStarOpenNodeRef
440 findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
442 for (unsigned int i=0; i<aHeap.size(); ++i) {
443 if (aHeap[i]->node == aPos) {
454 bool operator()(AStarOpenNode* a, AStarOpenNode* b)
456 return a->totalCost() > b->totalCost();
460 bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
463 typedef set<PositionedID> ClosedNodeSet;
465 OpenNodeHeap openNodes;
466 ClosedNodeSet closedNodes;
469 openNodes.push_back(new AStarOpenNode(aStart, 0.0, 0, aDest, NULL));
471 // A* open node iteration
472 while (!openNodes.empty()) {
473 std::pop_heap(openNodes.begin(), openNodes.end(), ordering);
474 AStarOpenNodeRef x = openNodes.back();
475 FGPositioned* xp = x->node;
476 openNodes.pop_back();
477 closedNodes.insert(xp->guid());
479 #ifdef DEBUG_AWY_SEARCH
480 SG_LOG(SG_NAVAID, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
483 // check if xp is the goal; if so we're done, since there cannot be an open
484 // node with lower f(x) value.
486 buildWaypoints(x, aRoute);
490 // adjacent (neighbour) iteration
491 NavDataCache* cache = NavDataCache::instance();
492 BOOST_FOREACH(AirwayEdge other, cache->airwayEdgesFrom(_networkID, xp->guid())) {
493 if (closedNodes.count(other.second)) {
494 continue; // closed, ignore
497 FGPositioned* yp = cache->loadById(other.second);
498 double edgeDistanceM = SGGeodesy::distanceM(xp->geod(), yp->geod());
499 AStarOpenNodeRef y = findInOpen(openNodes, yp);
500 if (y) { // already open
501 double g = x->distanceFromStart + edgeDistanceM;
502 if (g > y->distanceFromStart) {
503 // worse path, ignore
504 #ifdef DEBUG_AWY_SEARCH
505 SG_LOG(SG_NAVAID, SG_INFO, "\tabandoning " << yp->ident() <<
506 " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
511 // we need to update y. Unfortunately this means rebuilding the heap,
512 // since y's score can change arbitrarily
513 #ifdef DEBUG_AWY_SEARCH
514 SG_LOG(SG_NAVAID, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
517 y->distanceFromStart = g;
518 y->airway = other.first;
519 std::make_heap(openNodes.begin(), openNodes.end(), ordering);
520 } else { // not open, insert a new node for y into the heap
521 y = new AStarOpenNode(yp, edgeDistanceM, other.first, aDest, x);
522 #ifdef DEBUG_AWY_SEARCH
523 SG_LOG(SG_NAVAID, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
525 openNodes.push_back(y);
526 std::push_heap(openNodes.begin(), openNodes.end(), ordering);
528 } // of neighbour iteration
529 } // of open node iteration
531 SG_LOG(SG_NAVAID, SG_INFO, "A* failed to find route");
535 } // of namespace flightgear