src/Navaids/airways.cxx

   1 // airways.cxx - storage of airways network, and routing between nodes
   2 // Written by James Turner, started 2009.
   3 //
   4 // Copyright (C) 2009  Curtis L. Olson
   5 //
   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.
  10 //
  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.
  15 //
  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.
  19
  20 #ifndef HAVE_CONFIG_H
  21 # include "config.h"
  22 #endif
  23
  24 #include "airways.hxx"
  25
  26 #include <algorithm>
  27 #include <set>
  28
  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>
  33
  34 #include <boost/foreach.hpp>
  35 #include <boost/tuple/tuple.hpp>
  36
  37 #include <Main/globals.hxx>
  38 #include <Navaids/positioned.hxx>
  39 #include <Navaids/waypoint.hxx>
  40 #include <Navaids/NavDataCache.hxx>
  41
  42 using std::make_pair;
  43 using std::string;
  44 using std::set;
  45 using std::vector;
  46
  47 //#define DEBUG_AWY_SEARCH 1
  48
  49 namespace flightgear
  50 {
  51
  52 //////////////////////////////////////////////////////////////////////////////
  53
  54 class AStarOpenNode : public SGReferenced
  55 {
  56 public:
  57   AStarOpenNode(FGPositionedRef aNode, double aLegDist,
  58     int aAirway,
  59     FGPositionedRef aDest, AStarOpenNode* aPrev) :
  60     node(aNode),
  61     airway(aAirway),
  62     previous(aPrev)
  63   {
  64     distanceFromStart = aLegDist;
  65     if (previous) {
  66       distanceFromStart +=  previous->distanceFromStart;
  67     }
  68
  69                 directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
  70   }
  71
  72   virtual ~AStarOpenNode()
  73   {
  74   }
  75
  76   FGPositionedRef node;
  77   int airway;
  78   SGSharedPtr<AStarOpenNode> previous;
  79   double distanceFromStart; // aka 'g(x)'
  80   double directDistanceToDestination; // aka 'h(x)'
  81
  82   /**
  83          * aka 'f(x)'
  84          */
  85         double totalCost() const {
  86                 return distanceFromStart + directDistanceToDestination;
  87         }
  88 };
  89
  90 typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
  91
  92 ////////////////////////////////////////////////////////////////////////////
  93
  94 Airway::Network* Airway::lowLevel()
  95 {
  96   static Network* static_lowLevel = NULL;
  97
  98   if (!static_lowLevel) {
  99     static_lowLevel = new Network;
 100     static_lowLevel->_networkID = 1;
 101   }
 102
 103   return static_lowLevel;
 104 }
 105
 106 Airway::Network* Airway::highLevel()
 107 {
 108   static Network* static_highLevel = NULL;
 109   if (!static_highLevel) {
 110     static_highLevel = new Network;
 111     static_highLevel->_networkID = 2;
 112   }
 113
 114   return static_highLevel;
 115 }
 116
 117 Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
 118   _ident(aIdent),
 119   _topAltitudeFt(aTop),
 120   _bottomAltitudeFt(aBottom)
 121 {
 122 }
 123
 124 void Airway::load(const SGPath& path)
 125 {
 126   std::string identStart, identEnd, name;
 127   double latStart, lonStart, latEnd, lonEnd;
 128   int type, base, top;
 129   //int airwayIndex = 0;
 130   //FGNode *n;
 131
 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()));
 136   }
 137 // toss the first two lines of the file
 138   in >> skipeol;
 139   in >> skipeol;
 140
 141 // read in each remaining line of the file
 142   while (!in.eof()) {
 143     in >> identStart;
 144
 145     if (identStart == "99") {
 146       break;
 147     }
 148
 149     in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
 150     in >> skipeol;
 151
 152     // type = 1; low-altitude
 153     // type = 2; high-altitude
 154     Network* net = (type == 1) ? lowLevel() : highLevel();
 155
 156     SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
 157       endPos(SGGeod::fromDeg(lonEnd, latEnd));
 158
 159     int awy = net->findAirway(name, top, base);
 160     net->addEdge(awy, startPos, identStart, endPos, identEnd);
 161   } // of file line iteration
 162 }
 163
 164 WayptVec::const_iterator Airway::find(WayptRef wpt) const
 165 {
 166     WayptVec::const_iterator it;
 167     for (it = _elements.begin(); it != _elements.end(); ++it) {
 168         if (wpt->matches(*it)) {
 169             return it;
 170         }
 171     }
 172
 173     return it;
 174 }
 175
 176 bool Airway::canVia(const WayptRef& from, const WayptRef& to) const
 177 {
 178     WayptVec::const_iterator fit = find(from);
 179     WayptVec::const_iterator tit = find(to);
 180
 181     if ((fit == _elements.end()) || (tit == _elements.end())) {
 182         return false;
 183     }
 184
 185     return true;
 186 }
 187
 188 WayptVec Airway::via(const WayptRef& from, const WayptRef& to) const
 189 {
 190     WayptVec v;
 191     WayptVec::const_iterator fit = find(from);
 192     WayptVec::const_iterator tit = find(to);
 193
 194     if ((fit == _elements.end()) || (tit == _elements.end())) {
 195         throw sg_exception("bad VIA transition points");
 196     }
 197
 198     if (fit == tit) {
 199         // will cause duplicate point but that seems better than
 200         // return an empty
 201         v.push_back(*tit);
 202         return v;
 203     }
 204
 205     // establish the ordering of the transitions, i.e are we moving forward or
 206     // backard along the airway.
 207     if (fit < tit) {
 208         // forward progression
 209         for (++fit; fit != tit; ++fit) {
 210             v.push_back(*fit);
 211         }
 212     } else {
 213         // reverse progression
 214         for (--fit; fit != tit; --fit) {
 215             v.push_back(*fit);
 216         }
 217     }
 218
 219     v.push_back(*tit);
 220     return v;
 221 }
 222
 223 bool Airway::containsNavaid(const FGPositionedRef &navaid) const
 224 {
 225     return find(new NavaidWaypoint(navaid, NULL)) != _elements.end();
 226 }
 227
 228 int Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
 229 {
 230   return NavDataCache::instance()->findAirway(_networkID, aName);
 231 }
 232
 233 Airway* Airway::findByIdent(const std::string& aIdent)
 234 {
 235     NavDataCache* ndc = NavDataCache::instance();
 236
 237     int id = ndc->findAirway(0, aIdent);
 238
 239     PositionedIDVec pts = ndc->airwayWaypts(id);
 240     Airway* awy = new Airway(aIdent, 0, 0);
 241
 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);
 247     }
 248
 249     return awy;
 250 }
 251
 252 WayptRef Airway::findEnroute(const std::string &aIdent) const
 253 {
 254     WayptVec::const_iterator it;
 255     for (it = _elements.begin(); it != _elements.end(); ++it) {
 256         if ((*it)->ident() == aIdent) {
 257             return *it;
 258         }
 259     }
 260
 261     return WayptRef();
 262 }
 263
 264 void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
 265   const std::string& aStartIdent,
 266   const SGGeod& aEndPos, const std::string& aEndIdent)
 267 {
 268   FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
 269   FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
 270
 271   if (!start) {
 272     SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
 273     start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
 274     return;
 275   }
 276
 277   if (!end) {
 278     SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
 279     end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
 280     return;
 281   }
 282
 283   NavDataCache::instance()->insertEdge(_networkID, aWay, start->guid(), end->guid());
 284 }
 285
 286 //////////////////////////////////////////////////////////////////////////////
 287
 288 static double headingDiffDeg(double a, double b)
 289 {
 290     double rawDiff = b - a;
 291     SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0);
 292     return rawDiff;
 293 }
 294
 295 bool Airway::Network::inNetwork(PositionedID posID) const
 296 {
 297   NetworkMembershipDict::iterator it = _inNetworkCache.find(posID);
 298   if (it != _inNetworkCache.end()) {
 299     return it->second; // cached, easy
 300   }
 301
 302   bool r =  NavDataCache::instance()->isInAirwayNetwork(_networkID, posID);
 303   _inNetworkCache.insert(it, std::make_pair(posID, r));
 304   return r;
 305 }
 306
 307 bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
 308   WayptVec& aPath)
 309 {
 310   if (!aFrom || !aTo) {
 311     throw sg_exception("invalid waypoints to route between");
 312   }
 313
 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.
 317
 318   FGPositionedRef from, to;
 319   bool exactTo, exactFrom;
 320   boost::tie(from, exactFrom) = findClosestNode(aFrom);
 321   boost::tie(to, exactTo) = findClosestNode(aTo);
 322
 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());
 326 #endif
 327
 328   bool ok = search2(from, to, aPath);
 329   if (!ok) {
 330     return false;
 331   }
 332
 333   return cleanGeneratedPath(aFrom, aTo, aPath, exactTo, exactFrom);
 334 }
 335
 336 bool Airway::Network::cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath,
 337                                 bool exactTo, bool exactFrom)
 338 {
 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.
 350
 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());
 354
 355   bool isDogLeg = fabs(headingDiffDeg(enrouteCourse, finalLegCourse)) > MAX_DOG_LEG;
 356   if (exactTo || isDogLeg) {
 357     aPath.pop_back();
 358   }
 359
 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.
 363   if (aPath.empty()) {
 364     return true;
 365   }
 366
 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());
 371   }
 372
 373   return true;
 374 }
 375
 376 std::pair<FGPositionedRef, bool>
 377 Airway::Network::findClosestNode(WayptRef aRef)
 378 {
 379   return findClosestNode(aRef->position());
 380 }
 381
 382 class InAirwayFilter : public FGPositioned::Filter
 383 {
 384 public:
 385   InAirwayFilter(Airway::Network* aNet) :
 386     _net(aNet)
 387   { ; }
 388
 389   virtual bool pass(FGPositioned* aPos) const
 390   {
 391     return _net->inNetwork(aPos->guid());
 392   }
 393
 394   virtual FGPositioned::Type minType() const
 395   { return FGPositioned::WAYPOINT; }
 396
 397   virtual FGPositioned::Type maxType() const
 398   { return FGPositioned::NDB; }
 399
 400 private:
 401   Airway::Network* _net;
 402 };
 403
 404 std::pair<FGPositionedRef, bool>
 405 Airway::Network::findClosestNode(const SGGeod& aGeod)
 406 {
 407   InAirwayFilter f(this);
 408   FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
 409   bool exact = false;
 410
 411   if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
 412     exact = true; // within 100 metres, let's call that exact
 413   }
 414
 415   return make_pair(r, exact);
 416 }
 417
 418 /////////////////////////////////////////////////////////////////////////////
 419
 420 typedef vector<AStarOpenNodeRef> OpenNodeHeap;
 421
 422 static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
 423 {
 424 // count the route length, and hence pre-size aRoute
 425   int count = 0;
 426   AStarOpenNodeRef n = aNode;
 427   for (; n != NULL; ++count, n = n->previous) {;}
 428   aRoute.resize(count);
 429
 430 // run over the route, creating waypoints
 431   for (n = aNode; n; n=n->previous) {
 432     aRoute[--count] = new NavaidWaypoint(n->node, NULL);
 433   }
 434 }
 435
 436 /**
 437  * Inefficent (linear) helper to find an open node in the heap
 438  */
 439 static AStarOpenNodeRef
 440 findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
 441 {
 442   for (unsigned int i=0; i<aHeap.size(); ++i) {
 443     if (aHeap[i]->node == aPos) {
 444       return aHeap[i];
 445     }
 446   }
 447
 448   return NULL;
 449 }
 450
 451 class HeapOrder
 452 {
 453 public:
 454   bool operator()(AStarOpenNode* a, AStarOpenNode* b)
 455   {
 456     return a->totalCost() > b->totalCost();
 457   }
 458 };
 459
 460 bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
 461   WayptVec& aRoute)
 462 {
 463   typedef set<PositionedID> ClosedNodeSet;
 464
 465   OpenNodeHeap openNodes;
 466   ClosedNodeSet closedNodes;
 467   HeapOrder ordering;
 468
 469   openNodes.push_back(new AStarOpenNode(aStart, 0.0, 0, aDest, NULL));
 470
 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());
 478
 479 #ifdef DEBUG_AWY_SEARCH
 480     SG_LOG(SG_NAVAID, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
 481 #endif
 482
 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.
 485     if (xp == aDest) {
 486       buildWaypoints(x, aRoute);
 487       return true;
 488     }
 489
 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
 495       }
 496
 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);
 507 #endif
 508           continue;
 509         }
 510
 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);
 515 #endif
 516         y->previous = x;
 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());
 524 #endif
 525         openNodes.push_back(y);
 526         std::push_heap(openNodes.begin(), openNodes.end(), ordering);
 527       }
 528     } // of neighbour iteration
 529   } // of open node iteration
 530
 531   SG_LOG(SG_NAVAID, SG_INFO, "A* failed to find route");
 532   return false;
 533 }
 534
 535 } // of namespace flightgear