X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2Fairways.cxx;h=0000cdfa0652ed24a9a877027f53195f0710b825;hb=4b573ebd13bbf301795f57293a0e190d0993ea47;hp=e43687b1d6975a8d35b92d5b40b3919a747afc57;hpb=9b900e94304b95337e2731946525cde4ef377da9;p=flightgear.git

diff --git a/src/Navaids/airways.cxx b/src/Navaids/airways.cxx
index e43687b1d..0000cdfa0 100644
--- a/src/Navaids/airways.cxx
+++ b/src/Navaids/airways.cxx
@@ -131,7 +131,7 @@ void Airway::load(const SGPath& path)
 
   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
@@ -174,13 +174,13 @@ void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
   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;
   }
@@ -190,6 +190,13 @@ void Airway::Network::addEdge(int aWay, const SGGeod& aStartPos,
 
 //////////////////////////////////////////////////////////////////////////////
 
+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);
@@ -219,8 +226,8 @@ 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);
@@ -228,19 +235,46 @@ bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
     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;
 }
 
@@ -348,7 +382,7 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
     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
@@ -373,7 +407,7 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
         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;
@@ -382,7 +416,7 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
       // 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;
@@ -391,7 +425,7 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
       } 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);
@@ -399,7 +433,7 @@ bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
     } // 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;
 }