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/tuple/tuple.hpp>
36 #include <Main/globals.hxx>
37 #include <Navaids/positioned.hxx>
38 #include <Navaids/waypoint.hxx>
45 #define DEBUG_AWY_SEARCH 1
50 Airway::Network* Airway::static_lowLevel = NULL;
51 Airway::Network* Airway::static_highLevel = NULL;
53 //////////////////////////////////////////////////////////////////////////////
56 * information about an edge in the network.
57 * Some of this information is computed lazily
59 class AdjacentWaypoint
62 AdjacentWaypoint(const FGPositionedRef& aOrigin,
63 const FGPositionedRef& aDest, Airway* aWay);
65 double distanceM() const;
67 const FGPositionedRef& other(const FGPositionedRef& aEnd) const;
69 const FGPositionedRef origin;
70 const FGPositionedRef destination;
74 void validate() const;
75 mutable double _distanceM;
78 class AStarOpenNode : public SGReferenced
81 AStarOpenNode(FGPositionedRef aNode, double aLegDist,
83 FGPositionedRef aDest, AStarOpenNode* aPrev) :
88 distanceFromStart = aLegDist;
90 distanceFromStart += previous->distanceFromStart;
93 directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
96 virtual ~AStarOpenNode()
100 FGPositionedRef node;
102 SGSharedPtr<AStarOpenNode> previous;
103 double distanceFromStart; // aka 'g(x)'
104 double directDistanceToDestination; // aka 'h(x)'
109 double totalCost() const {
110 return distanceFromStart + directDistanceToDestination;
114 typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
116 ////////////////////////////////////////////////////////////////////////////
118 Airway::Network* Airway::lowLevel()
120 return static_lowLevel;
123 Airway::Network* Airway::highLevel()
125 return static_highLevel;
128 Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
130 _topAltitudeFt(aTop),
131 _bottomAltitudeFt(aBottom)
137 static_lowLevel = new Network;
138 static_highLevel = new Network;
140 SGPath path( globals->get_fg_root() );
141 path.append( "Navaids/awy.dat" );
143 std::string identStart, identEnd, name;
144 double latStart, lonStart, latEnd, lonEnd;
146 //int airwayIndex = 0;
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()));
154 // toss the first two lines of the file
158 // read in each remaining line of the file
162 if (identStart == "99") {
166 in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
169 // type = 1; low-altitude
170 // type = 2; high-altitude
171 Network* net = (type == 1) ? static_lowLevel : static_highLevel;
173 SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
174 endPos(SGGeod::fromDeg(lonEnd, latEnd));
176 Airway* awy = net->findAirway(name, top, base);
177 net->addEdge(awy, startPos, identStart, endPos, identEnd);
178 } // of file line iteration
181 Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
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));
192 void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos,
193 const std::string& aStartIdent,
194 const SGGeod& aEndPos, const std::string& aEndIdent)
196 FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
197 FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
200 SG_LOG(SG_GENERAL, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
201 start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
206 SG_LOG(SG_GENERAL, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
207 end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
211 AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay);
212 _graph.insert(make_pair(start, edge));
213 _graph.insert(make_pair(end, edge));
216 //////////////////////////////////////////////////////////////////////////////
218 bool Airway::Network::inNetwork(const FGPositioned* aPos) const
220 FGPositioned* pos = const_cast<FGPositioned*>(aPos);
221 return (_graph.find(pos) != _graph.end());
224 bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
227 if (!aFrom || !aTo) {
228 throw sg_exception("invalid waypoints to route between");
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.
235 FGPositionedRef from, to;
236 bool exactTo, exactFrom;
237 boost::tie(from, exactFrom) = findClosestNode(aFrom);
238 boost::tie(to, exactTo) = findClosestNode(aTo);
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());
245 bool ok = search2(from, to, aPath);
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());
266 std::pair<FGPositionedRef, bool>
267 Airway::Network::findClosestNode(WayptRef aRef)
269 return findClosestNode(aRef->position());
272 class InAirwayFilter : public FGPositioned::Filter
275 InAirwayFilter(Airway::Network* aNet) :
279 virtual bool pass(FGPositioned* aPos) const
281 return _net->inNetwork(aPos);
284 virtual FGPositioned::Type minType() const
285 { return FGPositioned::WAYPOINT; }
287 virtual FGPositioned::Type maxType() const
288 { return FGPositioned::NDB; }
291 Airway::Network* _net;
294 std::pair<FGPositionedRef, bool>
295 Airway::Network::findClosestNode(const SGGeod& aGeod)
297 InAirwayFilter f(this);
298 FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
301 if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
302 exact = true; // within 100 metres, let's call that exact
305 return make_pair(r, exact);
308 /////////////////////////////////////////////////////////////////////////////
310 typedef vector<AStarOpenNodeRef> OpenNodeHeap;
312 static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
314 // count the route length, and hence pre-size aRoute
316 AStarOpenNodeRef n = aNode;
317 for (; n != NULL; ++count, n = n->previous) {;}
318 aRoute.resize(count);
320 // run over the route, creating waypoints
321 for (n = aNode; n; n=n->previous) {
322 aRoute[--count] = new NavaidWaypoint(n->node, n->airway);
327 * Inefficent (linear) helper to find an open node in the heap
329 static AStarOpenNodeRef
330 findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
332 for (unsigned int i=0; i<aHeap.size(); ++i) {
333 if (aHeap[i]->node == aPos) {
344 bool operator()(AStarOpenNode* a, AStarOpenNode* b)
346 return a->totalCost() > b->totalCost();
350 bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
354 typedef set<FGPositioned*> ClosedNodeSet;
355 typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> AdjacencyMapRange;
357 OpenNodeHeap openNodes;
358 ClosedNodeSet closedNodes;
361 openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL));
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);
371 // SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
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.
376 buildWaypoints(x, aRoute);
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
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);
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);
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);
413 } // of neighbour iteration
414 } // of open node iteration
416 SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route");
420 //////////////////////////////////////////////////////////////////////////////
421 // AdjacentWaypoint definitions
423 AdjacentWaypoint::AdjacentWaypoint(
424 const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) :
433 const FGPositionedRef&
434 AdjacentWaypoint::other(const FGPositionedRef& aEnd) const
436 return (aEnd == origin) ? destination : origin;
439 double AdjacentWaypoint::distanceM() const
445 void AdjacentWaypoint::validate() const
447 if (_distanceM > 0.0) {
448 return; // already validated
451 _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod());
454 } // of namespace flightgear