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/tuple/tuple.hpp>
  35
  36 #include <Main/globals.hxx>
  37 #include <Navaids/positioned.hxx>
  38 #include <Navaids/waypoint.hxx>
  39
  40 using std::make_pair;
  41 using std::string;
  42 using std::set;
  43 using std::vector;
  44
  45 #define DEBUG_AWY_SEARCH 1
  46
  47 namespace flightgear
  48 {
  49
  50 Airway::Network* Airway::static_lowLevel = NULL;
  51 Airway::Network* Airway::static_highLevel = NULL;
  52
  53 //////////////////////////////////////////////////////////////////////////////
  54
  55 /**
  56  * information about an edge in the network.
  57  * Some of this information is computed lazily
  58  */
  59 class AdjacentWaypoint
  60 {
  61 public:
  62   AdjacentWaypoint(const FGPositionedRef& aOrigin,
  63     const FGPositionedRef& aDest, Airway* aWay);
  64
  65   double distanceM() const;
  66
  67   const FGPositionedRef& other(const FGPositionedRef& aEnd) const;
  68
  69   const FGPositionedRef origin;
  70   const FGPositionedRef destination;
  71   const Airway* airway;
  72
  73 private:
  74   void validate() const;
  75   mutable double _distanceM;
  76 };
  77
  78 class AStarOpenNode : public SGReferenced
  79 {
  80 public:
  81   AStarOpenNode(FGPositionedRef aNode, double aLegDist,
  82     Airway* aAirway,
  83     FGPositionedRef aDest, AStarOpenNode* aPrev) :
  84     node(aNode),
  85     airway(aAirway),
  86     previous(aPrev)
  87   {
  88     distanceFromStart = aLegDist;
  89     if (previous) {
  90       distanceFromStart +=  previous->distanceFromStart;
  91     }
  92
  93                 directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
  94   }
  95
  96   virtual ~AStarOpenNode()
  97   {
  98   }
  99
 100   FGPositionedRef node;
 101   Airway* airway;
 102   SGSharedPtr<AStarOpenNode> previous;
 103   double distanceFromStart; // aka 'g(x)'
 104   double directDistanceToDestination; // aka 'h(x)'
 105
 106   /**
 107          * aka 'f(x)'
 108          */
 109         double totalCost() const {
 110                 return distanceFromStart + directDistanceToDestination;
 111         }
 112 };
 113
 114 typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
 115
 116 ////////////////////////////////////////////////////////////////////////////
 117
 118 Airway::Network* Airway::lowLevel()
 119 {
 120   return static_lowLevel;
 121 }
 122
 123 Airway::Network* Airway::highLevel()
 124 {
 125   return static_highLevel;
 126 }
 127
 128 Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
 129   _ident(aIdent),
 130   _topAltitudeFt(aTop),
 131   _bottomAltitudeFt(aBottom)
 132 {
 133 }
 134
 135 void Airway::load()
 136 {
 137   static_lowLevel = new Network;
 138   static_highLevel = new Network;
 139
 140   SGPath path( globals->get_fg_root() );
 141   path.append( "Navaids/awy.dat" );
 142
 143   std::string identStart, identEnd, name;
 144   double latStart, lonStart, latEnd, lonEnd;
 145   int type, base, top;
 146   //int airwayIndex = 0;
 147   //FGNode *n;
 148
 149   sg_gzifstream in( path.str() );
 150   if ( !in.is_open() ) {
 151     SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
 152     throw sg_io_exception("Could not open airways data", sg_location(path.str()));
 153   }
 154 // toss the first two lines of the file
 155   in >> skipeol;
 156   in >> skipeol;
 157
 158 // read in each remaining line of the file
 159   while (!in.eof()) {
 160     in >> identStart;
 161
 162     if (identStart == "99") {
 163       break;
 164     }
 165
 166     in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
 167     in >> skipeol;
 168
 169     // type = 1; low-altitude
 170     // type = 2; high-altitude
 171     Network* net = (type == 1) ? static_lowLevel : static_highLevel;
 172
 173     SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
 174       endPos(SGGeod::fromDeg(lonEnd, latEnd));
 175
 176     Airway* awy = net->findAirway(name, top, base);
 177     net->addEdge(awy, startPos, identStart, endPos, identEnd);
 178   } // of file line iteration
 179 }
 180
 181 Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
 182 {
 183   AirwayDict::iterator it = _airways.find(aName);
 184   if (it == _airways.end()) {
 185     Airway* awy = new Airway(aName, aTop, aBase);
 186     it = _airways.insert(it, make_pair(aName, awy));
 187   }
 188
 189   return it->second;
 190 }
 191
 192 void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos,
 193   const std::string& aStartIdent,
 194   const SGGeod& aEndPos, const std::string& aEndIdent)
 195 {
 196   FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
 197   FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
 198
 199   if (!start) {
 200     SG_LOG(SG_GENERAL, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
 201     start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
 202     return;
 203   }
 204
 205   if (!end) {
 206     SG_LOG(SG_GENERAL, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
 207     end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
 208     return;
 209   }
 210
 211   AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay);
 212   _graph.insert(make_pair(start, edge));
 213   _graph.insert(make_pair(end, edge));
 214 }
 215
 216 //////////////////////////////////////////////////////////////////////////////
 217
 218 bool Airway::Network::inNetwork(const FGPositioned* aPos) const
 219 {
 220   FGPositioned* pos = const_cast<FGPositioned*>(aPos);
 221   return (_graph.find(pos) != _graph.end());
 222 }
 223
 224 bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
 225   WayptVec& aPath)
 226 {
 227   if (!aFrom || !aTo) {
 228     throw sg_exception("invalid waypoints to route between");
 229   }
 230
 231 // find closest nodes on the graph to from/to
 232 // if argument waypoints are directly on the graph (which is frequently the
 233 // case), note this so we don't duplicate them in the output.
 234
 235   FGPositionedRef from, to;
 236   bool exactTo, exactFrom;
 237   boost::tie(from, exactFrom) = findClosestNode(aFrom);
 238   boost::tie(to, exactTo) = findClosestNode(aTo);
 239
 240 #ifdef DEBUG_AWY_SEARCH
 241   SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name());
 242   SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name());
 243 #endif
 244
 245   bool ok = search2(from, to, aPath);
 246   if (!ok) {
 247     return false;
 248   }
 249
 250   if (exactTo) {
 251     aPath.pop_back();
 252   }
 253
 254   if (exactFrom) {
 255     // edge case - if from and to are equal, which can happen, don't
 256     // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
 257     // to the EGPH departure and EGCC STAR.
 258     if (!aPath.empty()) {
 259       aPath.erase(aPath.begin());
 260     }
 261   }
 262
 263   return true;
 264 }
 265
 266 std::pair<FGPositionedRef, bool>
 267 Airway::Network::findClosestNode(WayptRef aRef)
 268 {
 269   return findClosestNode(aRef->position());
 270 }
 271
 272 class InAirwayFilter : public FGPositioned::Filter
 273 {
 274 public:
 275   InAirwayFilter(Airway::Network* aNet) :
 276     _net(aNet)
 277   { ; }
 278
 279   virtual bool pass(FGPositioned* aPos) const
 280   {
 281     return _net->inNetwork(aPos);
 282   }
 283
 284   virtual FGPositioned::Type minType() const
 285   { return FGPositioned::WAYPOINT; }
 286
 287   virtual FGPositioned::Type maxType() const
 288   { return FGPositioned::NDB; }
 289
 290 private:
 291   Airway::Network* _net;
 292 };
 293
 294 std::pair<FGPositionedRef, bool>
 295 Airway::Network::findClosestNode(const SGGeod& aGeod)
 296 {
 297   InAirwayFilter f(this);
 298   FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
 299   bool exact = false;
 300
 301   if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
 302     exact = true; // within 100 metres, let's call that exact
 303   }
 304
 305   return make_pair(r, exact);
 306 }
 307
 308 /////////////////////////////////////////////////////////////////////////////
 309
 310 typedef vector<AStarOpenNodeRef> OpenNodeHeap;
 311
 312 static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
 313 {
 314 // count the route length, and hence pre-size aRoute
 315   int count = 0;
 316   AStarOpenNodeRef n = aNode;
 317   for (; n != NULL; ++count, n = n->previous) {;}
 318   aRoute.resize(count);
 319
 320 // run over the route, creating waypoints
 321   for (n = aNode; n; n=n->previous) {
 322     aRoute[--count] = new NavaidWaypoint(n->node, n->airway);
 323   }
 324 }
 325
 326 /**
 327  * Inefficent (linear) helper to find an open node in the heap
 328  */
 329 static AStarOpenNodeRef
 330 findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
 331 {
 332   for (unsigned int i=0; i<aHeap.size(); ++i) {
 333     if (aHeap[i]->node == aPos) {
 334       return aHeap[i];
 335     }
 336   }
 337
 338   return NULL;
 339 }
 340
 341 class HeapOrder
 342 {
 343 public:
 344   bool operator()(AStarOpenNode* a, AStarOpenNode* b)
 345   {
 346     return a->totalCost() > b->totalCost();
 347   }
 348 };
 349
 350 bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
 351   WayptVec& aRoute)
 352 {
 353
 354   typedef set<FGPositioned*> ClosedNodeSet;
 355   typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> AdjacencyMapRange;
 356
 357   OpenNodeHeap openNodes;
 358   ClosedNodeSet closedNodes;
 359   HeapOrder ordering;
 360
 361   openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL));
 362
 363 // A* open node iteration
 364   while (!openNodes.empty()) {
 365     std::pop_heap(openNodes.begin(), openNodes.end(), ordering);
 366     AStarOpenNodeRef x = openNodes.back();
 367     FGPositioned* xp = x->node;
 368     openNodes.pop_back();
 369     closedNodes.insert(xp);
 370
 371   //  SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
 372
 373   // check if xp is the goal; if so we're done, since there cannot be an open
 374   // node with lower f(x) value.
 375     if (xp == aDest) {
 376       buildWaypoints(x, aRoute);
 377       return true;
 378     }
 379
 380   // adjacent (neighbour) iteration
 381     AdjacencyMapRange r(_graph.equal_range(xp));
 382     for (; r.first != r.second; ++r.first) {
 383       AdjacentWaypoint* adj(r.first->second);
 384       FGPositioned* yp = adj->other(xp);
 385       if (closedNodes.count(yp)) {
 386         continue; // closed, ignore
 387       }
 388
 389       AStarOpenNodeRef y = findInOpen(openNodes, yp);
 390       if (y) { // already open
 391         double g = x->distanceFromStart + adj->distanceM();
 392         if (g > y->distanceFromStart) {
 393           // worse path, ignore
 394           //SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() <<
 395           // " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
 396           continue;
 397         }
 398
 399       // we need to update y. Unfortunately this means rebuilding the heap,
 400       // since y's score can change arbitrarily
 401         //SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
 402         y->previous = x;
 403         y->distanceFromStart = g;
 404         y->airway = (Airway*) adj->airway;
 405         std::make_heap(openNodes.begin(), openNodes.end(), ordering);
 406       } else { // not open, insert a new node for y into the heap
 407         y = new AStarOpenNode(yp, adj->distanceM(),
 408           (Airway*) adj->airway, aDest, x);
 409         //SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
 410         openNodes.push_back(y);
 411         std::push_heap(openNodes.begin(), openNodes.end(), ordering);
 412       }
 413     } // of neighbour iteration
 414   } // of open node iteration
 415
 416   SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route");
 417   return false;
 418 }
 419
 420 //////////////////////////////////////////////////////////////////////////////
 421 // AdjacentWaypoint definitions
 422
 423 AdjacentWaypoint::AdjacentWaypoint(
 424   const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) :
 425   origin(aOrigin),
 426   destination(aDest),
 427   airway(aWay),
 428   _distanceM(-1.0)
 429 {
 430
 431 }
 432
 433 const FGPositionedRef&
 434 AdjacentWaypoint::other(const FGPositionedRef& aEnd) const
 435 {
 436   return (aEnd == origin) ? destination : origin;
 437 }
 438
 439 double AdjacentWaypoint::distanceM() const
 440 {
 441   validate();
 442   return _distanceM;
 443 }
 444
 445 void AdjacentWaypoint::validate() const
 446 {
 447   if (_distanceM > 0.0) {
 448     return; // already validated
 449   }
 450
 451   _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod());
 452 }
 453
 454 } // of namespace flightgear