From d0e95037666c16676a09997020a469cfa2c5634a Mon Sep 17 00:00:00 2001
From: James Turner <jmt@Bishop.local>
Date: Sun, 30 Dec 2012 16:12:11 +0000
Subject: [PATCH] Fix auto-routing by airways.

When the from and to points are not on the enroute structure, check the initial and final legs for large turns (more than 90-degrees) from the overall route course. If found, kill the leg, to generate more natural route and avoid very sharp turns in the terminal area.
---
 src/Navaids/airways.cxx | 50 ++++++++++++++++++++++++++++++++++-------
 src/Navaids/airways.hxx |  3 +++
 2 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/src/Navaids/airways.cxx b/src/Navaids/airways.cxx
index 70ff237c7..0000cdfa0 100644
--- a/src/Navaids/airways.cxx
+++ b/src/Navaids/airways.cxx
@@ -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);
@@ -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;
 }
 
diff --git a/src/Navaids/airways.hxx b/src/Navaids/airways.hxx
index 514cb6187..6942f3074 100644
--- a/src/Navaids/airways.hxx
+++ b/src/Navaids/airways.hxx
@@ -71,6 +71,9 @@ public:
   
     int findAirway(const std::string& aName, double aTop, double aBase);
 
+    bool cleanGeneratedPath(WayptRef aFrom, WayptRef aTo, WayptVec& aPath,
+                            bool exactTo, bool exactFrom);
+      
     bool search2(FGPositionedRef aStart, FGPositionedRef aDest, WayptVec& aRoute);
   
     /**