#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/sg_path.hxx>
+#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <Main/globals.hxx>
#include <Navaids/positioned.hxx>
#include <Navaids/waypoint.hxx>
+#include <Navaids/NavDataCache.hxx>
using std::make_pair;
using std::string;
using std::set;
using std::vector;
-#define DEBUG_AWY_SEARCH 1
+//#define DEBUG_AWY_SEARCH 1
namespace flightgear
{
-Airway::Network* Airway::static_lowLevel = NULL;
-Airway::Network* Airway::static_highLevel = NULL;
-
//////////////////////////////////////////////////////////////////////////////
-/**
- * information about an edge in the network.
- * Some of this information is computed lazily
- */
-class AdjacentWaypoint
-{
-public:
- AdjacentWaypoint(const FGPositionedRef& aOrigin,
- const FGPositionedRef& aDest, Airway* aWay);
-
- double distanceM() const;
-
- const FGPositionedRef& other(const FGPositionedRef& aEnd) const;
-
- const FGPositionedRef origin;
- const FGPositionedRef destination;
- const Airway* airway;
-
-private:
- void validate() const;
- mutable double _distanceM;
-};
-
class AStarOpenNode : public SGReferenced
{
public:
AStarOpenNode(FGPositionedRef aNode, double aLegDist,
- Airway* aAirway,
+ int aAirway,
FGPositionedRef aDest, AStarOpenNode* aPrev) :
node(aNode),
airway(aAirway),
}
FGPositionedRef node;
- Airway* airway;
+ int airway;
SGSharedPtr<AStarOpenNode> previous;
double distanceFromStart; // aka 'g(x)'
double directDistanceToDestination; // aka 'h(x)'
Airway::Network* Airway::lowLevel()
{
+ static Network* static_lowLevel = NULL;
+
+ if (!static_lowLevel) {
+ static_lowLevel = new Network;
+ static_lowLevel->_networkID = 1;
+ }
+
return static_lowLevel;
}
Airway::Network* Airway::highLevel()
{
+ static Network* static_highLevel = NULL;
+ if (!static_highLevel) {
+ static_highLevel = new Network;
+ static_highLevel->_networkID = 2;
+ }
+
return static_highLevel;
}
{
}
-void Airway::load()
+void Airway::load(const SGPath& path)
{
- static_lowLevel = new Network;
- static_highLevel = new Network;
-
- SGPath path( globals->get_fg_root() );
- path.append( "Navaids/awy.dat" );
-
std::string identStart, identEnd, name;
double latStart, lonStart, latEnd, lonEnd;
int type, base, top;
sg_gzifstream in( path.str() );
if ( !in.is_open() ) {
- SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
+ SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() );
throw sg_io_exception("Could not open airways data", sg_location(path.str()));
}
// toss the first two lines of the file
// type = 1; low-altitude
// type = 2; high-altitude
- Network* net = (type == 1) ? static_lowLevel : static_highLevel;
+ Network* net = (type == 1) ? lowLevel() : highLevel();
SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
endPos(SGGeod::fromDeg(lonEnd, latEnd));
- Airway* awy = net->findAirway(name, top, base);
+ int awy = net->findAirway(name, top, base);
net->addEdge(awy, startPos, identStart, endPos, identEnd);
} // of file line iteration
}
-Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
+int Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
{
- AirwayDict::iterator it = _airways.find(aName);
- if (it == _airways.end()) {
- Airway* awy = new Airway(aName, aTop, aBase);
- it = _airways.insert(it, make_pair(aName, awy));
- }
-
- return it->second;
+ return NavDataCache::instance()->findAirway(_networkID, aName);
}
-void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos,
+void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
const std::string& aStartIdent,
const SGGeod& aEndPos, const std::string& aEndIdent)
{
FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
if (!start) {
- SG_LOG(SG_GENERAL, SG_INFO, "unknown airways start pt: '" << aStartIdent << "'");
+ SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways start pt: '" << aStartIdent << "'");
start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
return;
}
if (!end) {
- SG_LOG(SG_GENERAL, SG_INFO, "unknown airways end pt: '" << aEndIdent << "'");
+ SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
return;
}
- AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay);
- _graph.insert(make_pair(start, edge));
- _graph.insert(make_pair(end, edge));
+ NavDataCache::instance()->insertEdge(_networkID, aWay, start->guid(), end->guid());
}
//////////////////////////////////////////////////////////////////////////////
-bool Airway::Network::inNetwork(const FGPositioned* aPos) const
+static double headingDiffDeg(double a, double b)
{
- FGPositioned* pos = const_cast<FGPositioned*>(aPos);
- return (_graph.find(pos) != _graph.end());
+ double rawDiff = b - a;
+ SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0);
+ return rawDiff;
+}
+
+bool Airway::Network::inNetwork(PositionedID posID) const
+{
+ NetworkMembershipDict::iterator it = _inNetworkCache.find(posID);
+ if (it != _inNetworkCache.end()) {
+ return it->second; // cached, easy
+ }
+
+ bool r = NavDataCache::instance()->isInAirwayNetwork(_networkID, posID);
+ _inNetworkCache.insert(it, std::make_pair(posID, r));
+ return r;
}
bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
boost::tie(to, exactTo) = findClosestNode(aTo);
#ifdef DEBUG_AWY_SEARCH
- SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name());
- SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name());
+ SG_LOG(SG_NAVAID, SG_INFO, "from:" << from->ident() << "/" << from->name());
+ SG_LOG(SG_NAVAID, SG_INFO, "to:" << to->ident() << "/" << to->name());
#endif
bool ok = search2(from, to, aPath);
return false;
}
- if (exactTo) {
+ return cleanGeneratedPath(aFrom, aTo, aPath, exactTo, exactFrom);
+}
+
+bool Airway::Network::cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath,
+ bool exactTo, bool exactFrom)
+{
+ // path cleaning phase : various cases to handle here.
+ // if either the TO or FROM waypoints were 'exact', i.e part of the enroute
+ // structure, we don't want to duplicate them. This happens frequently with
+ // published SIDs and STARs.
+ // secondly, if the waypoints are NOT on the enroute structure, the course to
+ // them may be a significant dog-leg. Check how the leg course deviates
+ // from the direct course FROM->TO, and delete the first/last leg if it's more
+ // than 90 degrees out.
+ // note we delete a maximum of one leg, and no more. This is a heuristic - we
+ // could check the next (previous) legs, but at some point we'll end up
+ // deleting too much.
+
+ const double MAX_DOG_LEG = 90.0;
+ double enrouteCourse = SGGeodesy::courseDeg(aFrom->position(), aTo->position()),
+ finalLegCourse = SGGeodesy::courseDeg(aPath.back()->position(), aTo->position());
+
+ bool isDogLeg = fabs(headingDiffDeg(enrouteCourse, finalLegCourse)) > MAX_DOG_LEG;
+ if (exactTo || isDogLeg) {
aPath.pop_back();
}
- if (exactFrom) {
- // edge case - if from and to are equal, which can happen, don't
- // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
- // to the EGPH departure and EGCC STAR.
- if (!aPath.empty()) {
- aPath.erase(aPath.begin());
- }
+ // edge case - if from and to are equal, which can happen, don't
+ // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
+ // to the EGPH departure and EGCC STAR.
+ if (aPath.empty()) {
+ return true;
}
+ double initialLegCourse = SGGeodesy::courseDeg(aFrom->position(), aPath.front()->position());
+ isDogLeg = fabs(headingDiffDeg(enrouteCourse, initialLegCourse)) > MAX_DOG_LEG;
+ if (exactFrom || isDogLeg) {
+ aPath.erase(aPath.begin());
+ }
+
return true;
}
virtual bool pass(FGPositioned* aPos) const
{
- return _net->inNetwork(aPos);
+ return _net->inNetwork(aPos->guid());
}
virtual FGPositioned::Type minType() const
// run over the route, creating waypoints
for (n = aNode; n; n=n->previous) {
- aRoute[--count] = new NavaidWaypoint(n->node, n->airway);
+ aRoute[--count] = new NavaidWaypoint(n->node, NULL);
}
}
bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
WayptVec& aRoute)
-{
-
- typedef set<FGPositioned*> ClosedNodeSet;
- typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> AdjacencyMapRange;
+{
+ typedef set<PositionedID> ClosedNodeSet;
OpenNodeHeap openNodes;
ClosedNodeSet closedNodes;
HeapOrder ordering;
- openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL));
+ openNodes.push_back(new AStarOpenNode(aStart, 0.0, 0, aDest, NULL));
// A* open node iteration
while (!openNodes.empty()) {
AStarOpenNodeRef x = openNodes.back();
FGPositioned* xp = x->node;
openNodes.pop_back();
- closedNodes.insert(xp);
-
- // SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
+ closedNodes.insert(xp->guid());
+
+#ifdef DEBUG_AWY_SEARCH
+ SG_LOG(SG_NAVAID, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
+#endif
// check if xp is the goal; if so we're done, since there cannot be an open
// node with lower f(x) value.
}
// adjacent (neighbour) iteration
- AdjacencyMapRange r(_graph.equal_range(xp));
- for (; r.first != r.second; ++r.first) {
- AdjacentWaypoint* adj(r.first->second);
- FGPositioned* yp = adj->other(xp);
- if (closedNodes.count(yp)) {
+ NavDataCache* cache = NavDataCache::instance();
+ BOOST_FOREACH(AirwayEdge other, cache->airwayEdgesFrom(_networkID, xp->guid())) {
+ if (closedNodes.count(other.second)) {
continue; // closed, ignore
}
+ FGPositioned* yp = cache->loadById(other.second);
+ double edgeDistanceM = SGGeodesy::distanceM(xp->geod(), yp->geod());
AStarOpenNodeRef y = findInOpen(openNodes, yp);
if (y) { // already open
- double g = x->distanceFromStart + adj->distanceM();
+ double g = x->distanceFromStart + edgeDistanceM;
if (g > y->distanceFromStart) {
// worse path, ignore
- //SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() <<
- // " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
+#ifdef DEBUG_AWY_SEARCH
+ SG_LOG(SG_NAVAID, SG_INFO, "\tabandoning " << yp->ident() <<
+ " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
+#endif
continue;
}
// we need to update y. Unfortunately this means rebuilding the heap,
// since y's score can change arbitrarily
- //SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
+#ifdef DEBUG_AWY_SEARCH
+ SG_LOG(SG_NAVAID, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
+#endif
y->previous = x;
y->distanceFromStart = g;
- y->airway = (Airway*) adj->airway;
+ y->airway = other.first;
std::make_heap(openNodes.begin(), openNodes.end(), ordering);
} else { // not open, insert a new node for y into the heap
- y = new AStarOpenNode(yp, adj->distanceM(),
- (Airway*) adj->airway, aDest, x);
- //SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
+ y = new AStarOpenNode(yp, edgeDistanceM, other.first, aDest, x);
+#ifdef DEBUG_AWY_SEARCH
+ SG_LOG(SG_NAVAID, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
+#endif
openNodes.push_back(y);
std::push_heap(openNodes.begin(), openNodes.end(), ordering);
}
} // of neighbour iteration
} // of open node iteration
- SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route");
+ SG_LOG(SG_NAVAID, SG_INFO, "A* failed to find route");
return false;
}
-//////////////////////////////////////////////////////////////////////////////
-// AdjacentWaypoint definitions
-
-AdjacentWaypoint::AdjacentWaypoint(
- const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) :
- origin(aOrigin),
- destination(aDest),
- airway(aWay),
- _distanceM(-1.0)
-{
-
-}
-
-const FGPositionedRef&
-AdjacentWaypoint::other(const FGPositionedRef& aEnd) const
-{
- return (aEnd == origin) ? destination : origin;
-}
-
-double AdjacentWaypoint::distanceM() const
-{
- validate();
- return _distanceM;
-}
-
-void AdjacentWaypoint::validate() const
-{
- if (_distanceM > 0.0) {
- return; // already validated
- }
-
- _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod());
-}
-
} // of namespace flightgear
projects / flightgear.git / blobdiff