From 5ff8311acca298b5c46809f2c8d773139c012826 Mon Sep 17 00:00:00 2001
From: James Turner <jmt@Bishop.local>
Date: Mon, 31 Dec 2012 17:21:05 +0000
Subject: [PATCH] Fix route-path display of long legs.

When leg spans more than a threshold (currently 0.5 degrees) of longitude, interpolate the actual path flown by the GPS/RM, which is a true great-circle. Previous we rendered a Rhumb line which does not agree at all. (Especially noticeable in the MapWidget and NavDisplay code, both of which use RoutePath to tesselate the route before rendering)
---
 src/Navaids/routePath.cxx | 60 +++++++++++++++++++++++++++++++++++----
 src/Navaids/routePath.hxx |  2 ++
 2 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx
index 6353fb5b6..e17c8c462 100644
--- a/src/Navaids/routePath.cxx
+++ b/src/Navaids/routePath.cxx
@@ -52,6 +52,25 @@ double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc&
   return SGMiscd::min(dp1Nm, dp2Nm);
 }
 
+// http://williams.best.vwh.net/avform.htm#Int
+double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
+{
+#if 0
+Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:
+
+lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2)
+     -sin(lat2)*cos(lat1)*sin(lon-lon1))/(cos(lat1)*cos(lat2)*sin(lon1-lon2)))
+#endif
+  double lonDiff = a.getLongitudeRad() - b.getLongitudeRad();
+  double cosLat1 = cos(a.getLatitudeRad()),
+      cosLat2 = cos(b.getLatitudeRad());
+  double x = sin(a.getLatitudeRad()) * cosLat2 * sin(lon - b.getLongitudeRad());
+  double y = sin(b.getLatitudeRad()) * cosLat1 * sin(lon - a.getLongitudeRad());
+  double denom = cosLat1 * cosLat2 * sin(lonDiff);
+  double lat = atan((x - y) / denom);
+  return lat;
+}
+
 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
   _waypts(wpts)
 {
@@ -89,17 +108,24 @@ SGGeodVec RoutePath::pathForIndex(int index) const
   }
     
   SGGeodVec r;
-  SGGeod pos;
-  if (!computedPositionForIndex(index-1, pos)) {
+  SGGeod from, to;
+  if (!computedPositionForIndex(index-1, from)) {
     return SGGeodVec();
   }
   
-  r.push_back(pos);
-  if (!computedPositionForIndex(index, pos)) {
+  r.push_back(from);
+  if (!computedPositionForIndex(index, to)) {
     return SGGeodVec();
   }
   
-  r.push_back(pos);
+  // compute rounding offset, we want to round towards the direction of travel
+  // which depends on the east/west sign of the longitude change
+  double lonDelta = to.getLongitudeDeg() - from.getLongitudeDeg();
+  if (fabs(lonDelta) > 0.5) {
+    interpolateGreatCircle(from, to, r);
+  }
+  
+  r.push_back(to);
   
   if (_waypts[index]->type() == "runway") {
     // runways get an extra point, at the end. this is particularly
@@ -111,6 +137,30 @@ SGGeodVec RoutePath::pathForIndex(int index) const
   return r;
 }
 
+void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
+{
+  SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
+    gcTo = SGGeoc::fromGeod(aTo);
+  
+  double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
+  if (fabs(lonDelta) < 1e-3) {
+    return;
+  }
+  
+  lonDelta = SGMiscd::normalizeAngle2(lonDelta);
+
+  int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
+  double lonStep = (lonDelta / steps);
+  
+  double lon = gcFrom.getLongitudeRad() + lonStep;
+  for (int s=0; s < (steps - 1); ++s) {
+    double lat = latitudeForGCLongitude(gcFrom, gcTo, lon);
+    r.push_back(SGGeod::fromGeoc(SGGeoc::fromRadM(lon, lat, SGGeodesy::EQURAD)));
+    //SG_LOG(SG_GENERAL, SG_INFO, "lon:" << lon * SG_RADIANS_TO_DEGREES << " gives lat " << lat * SG_RADIANS_TO_DEGREES);
+    lon += lonStep;
+  }
+}
+
 SGGeod RoutePath::positionForIndex(int index) const
 {
   SGGeod r;
diff --git a/src/Navaids/routePath.hxx b/src/Navaids/routePath.hxx
index 97513a70d..778e0d00e 100644
--- a/src/Navaids/routePath.hxx
+++ b/src/Navaids/routePath.hxx
@@ -55,6 +55,8 @@ private:
   double computeAltitudeForIndex(int index) const;
   double computeTrackForIndex(int index) const;
   
+  void interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const;
+  
   /**
    * Find the distance (in Nm) to climb/descend a height in feet
    */
-- 
2.39.5