Fix flight-plan course and distance computations.

Always use the RoutePath system for course and distance computations
in the flight plan, so that dynamic segments are handled correctly.

src/Autopilot/route_mgr.cxx

@@ -51,6 +51,8 @@
 #include "Navaids/positioned.hxx"
 #include <Navaids/waypoint.hxx>
 #include <Navaids/procedure.hxx>
+#include <Navaids/routePath.hxx>
+
 #include "Airports/airport.hxx"
 #include "Airports/runways.hxx"
 #include <GUI/new_gui.hxx>
@@ -482,11 +484,13 @@ void FGRouteMgr::update( double dt )
     return;
   }
   
-  double courseDeg;
+  // use RoutePath to compute location of active WP
-  double distanceM;
+  RoutePath path(_plan);
-  boost::tie(courseDeg, distanceM) = leg->waypoint()->courseAndDistanceFrom(currentPos);
+  SGGeod wpPos = path.positionForIndex(_plan->currentIndex());
-  
+  double courseDeg, az2, distanceM;
-// update wp0 / wp1 / wp-last
+  SGGeodesy::inverse(currentPos, wpPos, courseDeg, az2, distanceM);
+
+  // update wp0 / wp1 / wp-last
   wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
   wp0->setDoubleValue("true-bearing-deg", courseDeg);
   courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
@@ -507,8 +511,9 @@ void FGRouteMgr::update( double dt )
   
   FlightPlan::Leg* nextLeg = _plan->nextLeg();
   if (nextLeg) {
-    boost::tie(courseDeg, distanceM) = nextLeg->waypoint()->courseAndDistanceFrom(currentPos);
+    wpPos = path.positionForIndex(_plan->currentIndex() + 1);
-     
+    SGGeodesy::inverse(currentPos, wpPos, courseDeg, az2, distanceM);
+
     wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
     wp1->setDoubleValue("true-bearing-deg", courseDeg);
     courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing

src/GUI/WaypointList.cxx

@@ -18,6 +18,7 @@
 #include <Main/fg_props.hxx>
 
 #include <Navaids/positioned.hxx>
+#include <Navaids/routePath.hxx>
 #include <Autopilot/route_mgr.hxx>
 
 // select if the widget grabs keys necessary to fly aircraft from the keyboard,
@@ -78,6 +79,11 @@ public:
     return _fp->legAtIndex(index)->waypoint();
   }
 
+  virtual flightgear::FlightPlan* flightplan() const
+  {
+    return _fp;
+  }
+  
   virtual void deleteAt(unsigned int index)
   {
     _fp->deleteIndex(index);
@@ -372,8 +378,11 @@ void WaypointList::draw( int dx, int dy )
   y -= (_scrollPx % rowHeight); // partially draw the first row
   
   _arrowWidth = legendFont.getStringWidth(">");
+  
+  RoutePath path(_model->flightplan());
+  
   for ( ; row <= final; ++row, y += rowHeight) {
-    drawRow(dx, dy, row, y);
+    drawRow(dx, dy, row, y, path);
   } // of row drawing iteration
   
   glDisable(GL_SCISSOR_TEST);
@@ -392,7 +401,8 @@ void WaypointList::draw( int dx, int dy )
   }
 }
 
-void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
+void WaypointList::drawRow(int dx, int dy, int rowIndex, int y,
+                           const RoutePath& path)
 {
   flightgear::Waypt* wp(_model->waypointAt(rowIndex));
     
@@ -459,18 +469,20 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
   x += 300 + PUSTR_LGAP;
   
   if (_showLatLon) {
-    SGGeod p(wp->position());
+    // only show for non-dynamic waypoints
-    char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S';
+    if (!wp->flag(WPT_DYNAMIC)) {
-    char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W';
+      SGGeod p(wp->position());
-    
+      char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S';
-    ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c",
+      char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W';
-      fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns);
+      
+      ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c",
+        fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns);
+    } else {
+      buffer[0] = 0;
+    }
   } else if (rowIndex > 0) {
-    double courseDeg;
+    double courseDeg = path.computeTrackForIndex(rowIndex);
-    double distanceM;
+    double distanceM = path.computeDistanceForIndex(rowIndex);
-    Waypt* prev = _model->waypointAt(rowIndex - 1);
-    boost::tie(courseDeg, distanceM) = wp->courseAndDistanceFrom(prev->position());
-  
     ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm",
       courseDeg, distanceM * SG_METER_TO_NM);
   }

src/GUI/WaypointList.hxx

@@ -15,9 +15,11 @@
 // forward decls
 class puaScrollBar;
 class SGCallback;
+class RoutePath;
 
 namespace flightgear {
   class Waypt;
+  class FlightPlan;
 }
 
 class WaypointList : public puFrame, public GUI_ID
@@ -76,6 +78,8 @@ public:
     virtual int currentWaypoint() const = 0;
     virtual flightgear::Waypt* waypointAt(unsigned int index) const = 0;
   
+    virtual flightgear::FlightPlan* flightplan() const = 0;
+    
   // update notifications
     virtual void setUpdateCallback(SGCallback* cb) = 0;
   
@@ -90,7 +94,7 @@ public:
 protected:
 
 private:
-  void drawRow(int dx, int dy, int rowIndex, int yOrigin);
+  void drawRow(int dx, int dy, int rowIndex, int yOrigin, const RoutePath& path);
 
   void handleDrag(int x, int y);
   void doDrop(int x, int y);

src/Instrumentation/gps.cxx

@@ -23,6 +23,8 @@
 #include <Navaids/waypoint.hxx>
 #include "Navaids/navrecord.hxx"
 #include "Navaids/FlightPlan.hxx"
+#include <Navaids/routePath.hxx>
+
 #include "Airports/airport.hxx"
 #include "Airports/runways.hxx"
 #include "Autopilot/route_mgr.hxx"
@@ -723,16 +725,18 @@ void GPS::computeTurnData()
   }
   
   WayptRef next = _route->nextLeg()->waypoint();
-  if (next->flag(WPT_DYNAMIC) || !_config.turnAnticipationEnabled()) {
+  if (next->flag(WPT_DYNAMIC) ||
+      !_config.turnAnticipationEnabled() ||
+      next->flag(WPT_OVERFLIGHT))
+  {
     _anticipateTurn = false;
     return;
   }
   
   _turnStartBearing = _desiredCourse;
 // compute next leg course
-  double crs, dist;
+  RoutePath path(_route);
-  boost::tie(crs, dist) = next->courseAndDistanceFrom(_currentWaypt->position());
+  double crs = path.computeTrackForIndex(_route->currentIndex() + 1);
-    
 
 // compute offset bearing
   _turnAngle = crs - _turnStartBearing;
@@ -795,9 +799,14 @@ void GPS::updateRouteData()
 {
   double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
   // walk all waypoints from wp2 to route end, and sum
- // for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) {
+  for (int i=_route->currentIndex()+1; i<_route->numLegs(); ++i) {
-    //totalDistance += _routeMgr->get_waypoint(i).get_distance();
+    FlightPlan::Leg* leg = _route->legAtIndex(i);
-  //}
+    // omit missed-approach waypoints in distance calculation
+    if (leg->waypoint()->flag(WPT_MISS))
+      continue;
+      
+    totalDistance += leg->distanceAlongRoute();
+  }
   
   _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);
   if (_last_speed_kts > 1.0) {

src/Navaids/FlightPlan.cxx

@@ -49,6 +49,7 @@
 #include "Main/fg_props.hxx"
 #include <Navaids/procedure.hxx>
 #include <Navaids/waypoint.hxx>
+#include <Navaids/routePath.hxx>
 
 using std::string;
 using std::vector;
@@ -1186,17 +1187,21 @@ double FlightPlan::Leg::distanceAlongRoute() const
 void FlightPlan::rebuildLegData()
 {
   _totalDistance = 0.0;
+  double totalDistanceIncludingMissed = 0.0;
+  RoutePath path(this);
+  
   int lastLeg = static_cast<int>(_legs.size()) - 1;
   for (int l=0; l<lastLeg; ++l) {
-    Leg* cur = _legs[l];
+    _legs[l]->_courseDeg = path.computeTrackForIndex(l);
-    Leg* next = _legs[l + 1];
+    _legs[l]->_pathDistance = path.computeDistanceForIndex(l) * SG_METER_TO_NM;
+    _legs[l]->_distanceAlongPath = totalDistanceIncludingMissed;
     
-    std::pair<double, double> crsDist =
+    // omit misseed-approach waypoints from total distance calculation
-      next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
+    if (!_legs[l]->waypoint()->flag(WPT_MISS)) {
-    _legs[l]->_courseDeg = crsDist.first;
+      _totalDistance += _legs[l]->_pathDistance;
-    _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
+    }
-    _legs[l]->_distanceAlongPath = _totalDistance;
+    
-    _totalDistance += crsDist.second * SG_METER_TO_NM;
+    totalDistanceIncludingMissed += _legs[l]->_pathDistance;
   } // of legs iteration
   
 // set some data on the final leg
@@ -1205,7 +1210,7 @@ void FlightPlan::rebuildLegData()
     // waypoint
     _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
     _legs[lastLeg]->_pathDistance = 0.0;
-    _legs[lastLeg]->_distanceAlongPath = _totalDistance;
+    _legs[lastLeg]->_distanceAlongPath = totalDistanceIncludingMissed;
   }
 }
   

src/Navaids/route.cxx

@@ -139,18 +139,6 @@ double Waypt::speedMach() const
   assert(_speedRestrict == SPEED_RESTRICT_MACH);
   return speed();
 }
-  
-std::pair<double, double>
-Waypt::courseAndDistanceFrom(const SGGeod& aPos) const
-{
-  if (flag(WPT_DYNAMIC)) {
-    return std::make_pair(0.0, 0.0);
-  }
-  
-  double course, az2, distance;
-  SGGeodesy::inverse(aPos, position(), course, az2, distance);
-  return std::make_pair(course, distance);
-}
 
 double Waypt::magvarDeg() const
 {

src/Navaids/route.hxx

@@ -98,13 +98,7 @@ public:
   
 	RouteBase* owner() const 
 		{ return _owner; }
-  
+         	
-  /**
-   * Return true course (in degrees) and distance (in metres) from the provided
-   * position to this waypoint
-   */
-  virtual std::pair<double, double> courseAndDistanceFrom(const SGGeod& aPos) const;
-                  	
 	virtual SGGeod position() const = 0;
 	
 	/**

src/Navaids/routePath.cxx

@@ -210,18 +210,6 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const
   return r;
 }
 
-/**
- * the path context holds the state of of an imaginary aircraft traversing
- * the route, and limits the rate at which heading / altitude / position can
- * change
- */
-class RoutePath::PathCtx
-{
-public:
-  SGGeod pos;
-  double heading;
-};
-
 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
 {
   if ((index < 0) || (index >= (int) _waypts.size())) {
@@ -300,7 +288,21 @@ bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
     SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
     return true;
   } else if (w->type() == "vectors"){
-    return false;
+    // return position of next point (which is presumably static fix/wpt)
+    // however, a missed approach might end with VECTORS, so tolerate that case
+    if (index + 1 >= _waypts.size()) {
+      SG_LOG(SG_NAVAID, SG_INFO, "route ends with VECTORS, no position");
+      return false;
+    }
+    
+    WayptRef nextWp = _waypts[index+1];
+    if (nextWp->flag(WPT_DYNAMIC)) {
+      SG_LOG(SG_NAVAID, SG_INFO, "dynamic WP following VECTORS, no position");
+      return false;
+    }
+    
+    r = nextWp->position();
+    return true;
   } else if (w->type() == "hold") {
     r = w->position();
     return true;
@@ -310,6 +312,39 @@ bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
   return false;
 }
 
+double RoutePath::computeDistanceForIndex(int index) const
+{
+  if ((index < 0) || (index >= (int) _waypts.size())) {
+    throw sg_range_exception("waypt index out of range",
+                             "RoutePath::computeDistanceForIndex");
+  }
+  
+  if (index + 1 >= (int) _waypts.size()) {
+    // final waypoint, distance is 0
+    return 0.0;
+  }
+  
+  WayptRef w = _waypts[index],
+    nextWp = _waypts[index+1];
+  
+  // common case, both waypoints are static
+  if (!w->flag(WPT_DYNAMIC) && !nextWp->flag(WPT_DYNAMIC)) {
+    return SGGeodesy::distanceM(w->position(), nextWp->position());
+  }
+  
+  
+  SGGeod wPos, nextPos;
+  bool ok = computedPositionForIndex(index, wPos),
+    nextOk = computedPositionForIndex(index + 1, nextPos);
+  if (ok && nextOk) {
+    return SGGeodesy::distanceM(wPos, nextPos);
+  }
+  
+  SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computeDistanceForIndex: unhandled arrangement:"
+         << w->type() << " followed by " << nextWp->type());
+  return 0.0;
+}
+
 double RoutePath::computeAltitudeForIndex(int index) const
 {
   if ((index < 0) || (index >= (int) _waypts.size())) {
@@ -382,17 +417,21 @@ double RoutePath::computeTrackForIndex(int index) const
     return rwy->headingDeg();
   }
   
-// course based upon previous and current pos
+// final waypoint, use inbound course
-  SGGeod pos, prevPos;
+  if (index + 1 >= _waypts.size()) {
+    return computeTrackForIndex(index - 1);
+  }
   
+// course based upon current and next pos
+  SGGeod pos, nextPos;
   if (!computedPositionForIndex(index, pos) ||
-      !computedPositionForIndex(index - 1, prevPos))
+      !computedPositionForIndex(index + 1, nextPos))
   {
     SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints");
     throw sg_range_exception("unable to compute position for waypoints");
   }
 
-  return SGGeodesy::courseDeg(prevPos, pos);
+  return SGGeodesy::courseDeg(pos, nextPos);
 }
 
 double RoutePath::distanceForClimb(double climbFt) const
@@ -412,4 +451,3 @@ double RoutePath::magVarFor(const SGGeod& geod) const
   double jd = globals->get_time_params()->getJD();
   return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
 }
-

src/Navaids/routePath.hxx

@@ -44,6 +44,9 @@ public:
   
   SGGeod positionForIndex(int index) const;
   
+  double computeDistanceForIndex(int index) const;
+  double computeTrackForIndex(int index) const;
+
 private:
   void commonInit();
   
@@ -53,7 +56,6 @@ private:
   
   bool computedPositionForIndex(int index, SGGeod& pos) const;
   double computeAltitudeForIndex(int index) const;
-  double computeTrackForIndex(int index) const;
   
   void interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const;
   

src/Scripting/NasalPositioned.cxx

@@ -2299,11 +2299,12 @@ static naRef f_leg_courseAndDistanceFrom(naContext c, naRef me, int argc, naRef*
     
     SGGeod pos;
     geodFromArgs(args, 0, argc, pos);
-    
+  
-    double courseDeg;
+    RoutePath path(leg->owner());
-    double distanceM;
+    SGGeod wpPos = path.positionForIndex(leg->index());
-    boost::tie(courseDeg, distanceM) = leg->waypoint()->courseAndDistanceFrom(pos);
+    double courseDeg, az2, distanceM;
-    
+    SGGeodesy::inverse(pos, wpPos, courseDeg, az2, distanceM);
+  
     naRef result = naNewVector(c);
     naVec_append(result, naNum(courseDeg));
     naVec_append(result, naNum(distanceM * SG_METER_TO_NM));