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
FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
if (!start) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "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_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
+ SG_LOG(SG_NAVAID, SG_DEBUG, "unknown airways end pt: '" << aEndIdent << "'");
end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
return;
}
//////////////////////////////////////////////////////////////////////////////
+static double headingDiffDeg(double a, double b)
+{
+ 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);
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;
}
closedNodes.insert(xp->guid());
#ifdef DEBUG_AWY_SEARCH
- SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
+ 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
if (g > y->distanceFromStart) {
// worse path, ignore
#ifdef DEBUG_AWY_SEARCH
- SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() <<
+ 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
#ifdef DEBUG_AWY_SEARCH
- SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
+ SG_LOG(SG_NAVAID, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
#endif
y->previous = x;
y->distanceFromStart = g;
} else { // not open, insert a new node for y into the heap
y = new AStarOpenNode(yp, edgeDistanceM, other.first, aDest, x);
#ifdef DEBUG_AWY_SEARCH
- SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
+ 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;
}
projects / flightgear.git / blobdiff