Bug #1166, slow POI parsing.

[flightgear.git] / src / Navaids / routePath.cxx

diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx
index 6353fb5b678711d5815de93ca7778dd4deb92b6c..afbd628542644387cefbe0fc46ac57a68f2a1bb7 100644 (file)
--- 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::normalizeAngle(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) {
+    lon = SGMiscd::normalizeAngle(lon);
+    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;