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 int Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
166 return NavDataCache::instance()->findAirway(_networkID, aName);
169 void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
170 const std::string& aStartIdent,
171 const SGGeod& aEndPos, const std::string& aEndIdent)
173 FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
174 FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
177 SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
178 start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
183 SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
184 end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
188 NavDataCache::instance()->insertEdge(_networkID, aWay, start->guid(), end->guid());
191 //////////////////////////////////////////////////////////////////////////////
193 static double headingDiffDeg(double a, double b)
195 double rawDiff = b - a;
196 SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0);
200 bool Airway::Network::inNetwork(PositionedID posID) const
202 NetworkMembershipDict::iterator it = _inNetworkCache.find(posID);
203 if (it != _inNetworkCache.end()) {
204 return it->second; // cached, easy
207 bool r = NavDataCache::instance()->isInAirwayNetwork(_networkID, posID);
208 _inNetworkCache.insert(it, std::make_pair(posID, r));
212 bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
215 if (!aFrom || !aTo) {
216 throw sg_exception("invalid waypoints to route between");
219 // find closest nodes on the graph to from/to
220 // if argument waypoints are directly on the graph (which is frequently the
221 // case), note this so we don't duplicate them in the output.
223 FGPositionedRef from, to;
224 bool exactTo, exactFrom;
225 boost::tie(from, exactFrom) = findClosestNode(aFrom);
226 boost::tie(to, exactTo) = findClosestNode(aTo);
228 #ifdef DEBUG_AWY_SEARCH
229 SG_LOG(SG_NAVAID, SG_INFO, "from:" << from->ident() << "/" << from->name());
230 SG_LOG(SG_NAVAID, SG_INFO, "to:" << to->ident() << "/" << to->name());
233 bool ok = search2(from, to, aPath);
238 return cleanGeneratedPath(aFrom, aTo, aPath, exactTo, exactFrom);
241 bool Airway::Network::cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath,
242 bool exactTo, bool exactFrom)
244 // path cleaning phase : various cases to handle here.
245 // if either the TO or FROM waypoints were 'exact', i.e part of the enroute
246 // structure, we don't want to duplicate them. This happens frequently with
247 // published SIDs and STARs.
248 // secondly, if the waypoints are NOT on the enroute structure, the course to
249 // them may be a significant dog-leg. Check how the leg course deviates
250 // from the direct course FROM->TO, and delete the first/last leg if it's more
251 // than 90 degrees out.
252 // note we delete a maximum of one leg, and no more. This is a heuristic - we
253 // could check the next (previous) legs, but at some point we'll end up
254 // deleting too much.
256 const double MAX_DOG_LEG = 90.0;
257 double enrouteCourse = SGGeodesy::courseDeg(aFrom->position(), aTo->position()),
258 finalLegCourse = SGGeodesy::courseDeg(aPath.back()->position(), aTo->position());
260 bool isDogLeg = fabs(headingDiffDeg(enrouteCourse, finalLegCourse)) > MAX_DOG_LEG;
261 if (exactTo || isDogLeg) {
265 // edge case - if from and to are equal, which can happen, don't
266 // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
267 // to the EGPH departure and EGCC STAR.
272 double initialLegCourse = SGGeodesy::courseDeg(aFrom->position(), aPath.front()->position());
273 isDogLeg = fabs(headingDiffDeg(enrouteCourse, initialLegCourse)) > MAX_DOG_LEG;
274 if (exactFrom || isDogLeg) {
275 aPath.erase(aPath.begin());
281 std::pair<FGPositionedRef, bool>
282 Airway::Network::findClosestNode(WayptRef aRef)
284 return findClosestNode(aRef->position());
287 class InAirwayFilter : public FGPositioned::Filter
290 InAirwayFilter(Airway::Network* aNet) :
294 virtual bool pass(FGPositioned* aPos) const
296 return _net->inNetwork(aPos->guid());
299 virtual FGPositioned::Type minType() const
300 { return FGPositioned::WAYPOINT; }
302 virtual FGPositioned::Type maxType() const
303 { return FGPositioned::NDB; }
306 Airway::Network* _net;
309 std::pair<FGPositionedRef, bool>
310 Airway::Network::findClosestNode(const SGGeod& aGeod)
312 InAirwayFilter f(this);
313 FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
316 if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
317 exact = true; // within 100 metres, let's call that exact
320 return make_pair(r, exact);
323 /////////////////////////////////////////////////////////////////////////////
325 typedef vector<AStarOpenNodeRef> OpenNodeHeap;
327 static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
329 // count the route length, and hence pre-size aRoute
331 AStarOpenNodeRef n = aNode;
332 for (; n != NULL; ++count, n = n->previous) {;}
333 aRoute.resize(count);
335 // run over the route, creating waypoints
336 for (n = aNode; n; n=n->previous) {
337 aRoute[--count] = new NavaidWaypoint(n->node, NULL);
342 * Inefficent (linear) helper to find an open node in the heap
344 static AStarOpenNodeRef
345 findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
347 for (unsigned int i=0; i<aHeap.size(); ++i) {
348 if (aHeap[i]->node == aPos) {
359 bool operator()(AStarOpenNode* a, AStarOpenNode* b)
361 return a->totalCost() > b->totalCost();
365 bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
368 typedef set<PositionedID> ClosedNodeSet;
370 OpenNodeHeap openNodes;
371 ClosedNodeSet closedNodes;
374 openNodes.push_back(new AStarOpenNode(aStart, 0.0, 0, aDest, NULL));
376 // A* open node iteration
377 while (!openNodes.empty()) {
378 std::pop_heap(openNodes.begin(), openNodes.end(), ordering);
379 AStarOpenNodeRef x = openNodes.back();
380 FGPositioned* xp = x->node;
381 openNodes.pop_back();
382 closedNodes.insert(xp->guid());
384 #ifdef DEBUG_AWY_SEARCH
385 SG_LOG(SG_NAVAID, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
388 // check if xp is the goal; if so we're done, since there cannot be an open
389 // node with lower f(x) value.
391 buildWaypoints(x, aRoute);
395 // adjacent (neighbour) iteration
396 NavDataCache* cache = NavDataCache::instance();
397 BOOST_FOREACH(AirwayEdge other, cache->airwayEdgesFrom(_networkID, xp->guid())) {
398 if (closedNodes.count(other.second)) {
399 continue; // closed, ignore
402 FGPositioned* yp = cache->loadById(other.second);
403 double edgeDistanceM = SGGeodesy::distanceM(xp->geod(), yp->geod());
404 AStarOpenNodeRef y = findInOpen(openNodes, yp);
405 if (y) { // already open
406 double g = x->distanceFromStart + edgeDistanceM;
407 if (g > y->distanceFromStart) {
408 // worse path, ignore
409 #ifdef DEBUG_AWY_SEARCH
410 SG_LOG(SG_NAVAID, SG_INFO, "\tabandoning " << yp->ident() <<
411 " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
416 // we need to update y. Unfortunately this means rebuilding the heap,
417 // since y's score can change arbitrarily
418 #ifdef DEBUG_AWY_SEARCH
419 SG_LOG(SG_NAVAID, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
422 y->distanceFromStart = g;
423 y->airway = other.first;
424 std::make_heap(openNodes.begin(), openNodes.end(), ordering);
425 } else { // not open, insert a new node for y into the heap
426 y = new AStarOpenNode(yp, edgeDistanceM, other.first, aDest, x);
427 #ifdef DEBUG_AWY_SEARCH
428 SG_LOG(SG_NAVAID, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
430 openNodes.push_back(y);
431 std::push_heap(openNodes.begin(), openNodes.end(), ordering);
433 } // of neighbour iteration
434 } // of open node iteration
436 SG_LOG(SG_NAVAID, SG_INFO, "A* failed to find route");
440 } // of namespace flightgear