Fix route-path bugs:

- accurate fly-over / fly-by computations
 - parse additional LevelD XML elements
 - path vector contains curves for turns

Remove dead code.

src/GUI/WaypointList.cxx

@@ -481,8 +481,8 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y,
       buffer[0] = 0;
     }
   } else if (rowIndex > 0) {
-    double courseDeg = path.computeTrackForIndex(rowIndex);
+    double courseDeg = path.trackForIndex(rowIndex);
-    double distanceM = path.computeDistanceForIndex(rowIndex);
+    double distanceM = path.distanceForIndex(rowIndex);
     ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm",
       courseDeg, distanceM * SG_METER_TO_NM);
   }
@@ -491,11 +491,18 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y,
   x += 100 + PUSTR_LGAP;
   
   if (wp->altitudeRestriction() != RESTRICT_NONE) {
+    char aboveAtBelow = ' ';
+    if (wp->altitudeRestriction() == RESTRICT_ABOVE) {
+      aboveAtBelow = 'A';
+    } else if (wp->altitudeRestriction() == RESTRICT_BELOW) {
+      aboveAtBelow = 'B';
+    }
+    
     int altHundredFt = (wp->altitudeFt() + 50) / 100; // round to nearest 100ft
     if (altHundredFt < 100) {
-      count = ::snprintf(buffer, 128, "%d'", altHundredFt * 100);
+      count = ::snprintf(buffer, 128, "%d'%c", altHundredFt * 100, aboveAtBelow);
     } else { // display as a flight-level
-      count = ::snprintf(buffer, 128, "FL%d", altHundredFt);
+      count = ::snprintf(buffer, 128, "FL%d%c", altHundredFt, aboveAtBelow);
     }
     
     f->drawString(buffer, x, yy);

src/Instrumentation/gps.cxx

@@ -736,7 +736,7 @@ void GPS::computeTurnData()
   _turnStartBearing = _desiredCourse;
 // compute next leg course
   RoutePath path(_route);
-  double crs = path.computeTrackForIndex(_route->currentIndex() + 1);
+  double crs = path.trackForIndex(_route->currentIndex() + 1);
 
 // compute offset bearing
   _turnAngle = crs - _turnStartBearing;
@@ -805,7 +805,7 @@ void GPS::updateRouteData()
     if (leg->waypoint()->flag(WPT_MISS))
       continue;
       
-    totalDistance += leg->distanceAlongRoute();
+    totalDistance += leg->distanceNm();
   }
   
   _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);

src/Instrumentation/rnav_waypt_controller.cxx

@@ -82,8 +82,9 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double
   }
   */
   
-  double ang1 = SGMiscd::normalizeAngle2(crs13-crs12);
+  // normalise to -pi .. pi range
-  double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
+  double ang1 = SGMiscd::normalizeAngle(crs13-crs12);
+  double ang2 = SGMiscd::normalizeAngle(crs21-crs23);
     
   if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
     SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: infinity of intersections");
@@ -91,7 +92,14 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double
   }
   
   if ((sin(ang1)*sin(ang2))<0.0) {
-    SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous");
+    SG_LOG(SG_INSTR, SG_INFO, "deg crs12:" << crs12 * SG_RADIANS_TO_DEGREES);
+    SG_LOG(SG_INSTR, SG_INFO, "deg crs21:" << crs21 * SG_RADIANS_TO_DEGREES);
+    
+    SG_LOG(SG_INSTR, SG_INFO, "crs13 - crs12 in deg:" << (crs13 - crs12) * SG_RADIANS_TO_DEGREES);
+    SG_LOG(SG_INSTR, SG_INFO, "crs21 - crs23 in deg:" << (crs21 - crs23) * SG_RADIANS_TO_DEGREES);
+    
+    SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous:"
+           << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2));
     return false;
   }
   

src/Navaids/FlightPlan.cxx

@@ -1190,10 +1190,9 @@ void FlightPlan::rebuildLegData()
   double totalDistanceIncludingMissed = 0.0;
   RoutePath path(this);
   
-  int lastLeg = static_cast<int>(_legs.size()) - 1;
+  for (unsigned int l=0; l<_legs.size(); ++l) {
-  for (int l=0; l<lastLeg; ++l) {
+    _legs[l]->_courseDeg = path.trackForIndex(l);
-    _legs[l]->_courseDeg = path.computeTrackForIndex(l);
+    _legs[l]->_pathDistance = path.distanceForIndex(l) * SG_METER_TO_NM;
-    _legs[l]->_pathDistance = path.computeDistanceForIndex(l) * SG_METER_TO_NM;
     _legs[l]->_distanceAlongPath = totalDistanceIncludingMissed;
     
     // omit misseed-approach waypoints from total distance calculation
@@ -1204,14 +1203,6 @@ void FlightPlan::rebuildLegData()
     totalDistanceIncludingMissed += _legs[l]->_pathDistance;
   } // of legs iteration
   
-// set some data on the final leg
-  if (lastLeg > 0) {
-    // keep the same course as the final leg, when passing the final
-    // waypoint
-    _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
-    _legs[lastLeg]->_pathDistance = 0.0;
-    _legs[lastLeg]->_distanceAlongPath = totalDistanceIncludingMissed;
-  }
 }
   
 SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const

src/Navaids/LevelDXML.cxx

@@ -70,6 +70,7 @@ void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts)
     _altRestrict = RESTRICT_NONE;
     _altitude = 0.0;
     _overflightWaypt = false; // default to Fly-by
+    _courseFlag = false; // default to heading
   } else if (tag == "Approach") {
     _ident = atts.getValue("Name");
     _waypoints.clear();
@@ -194,8 +195,10 @@ void NavdataVisitor::endElement(const char* name)
     _holdRighthanded = (_text == "Right");
   } else if (tag == "Hld_td_value") {
     _holdTD = atof(_text.c_str());
+  } else if (tag == "Hdg_Crs") {
+    _courseFlag = atoi(_text.c_str());
   } else if (tag == "Hdg_Crs_value") {
-    _course = atof(_text.c_str());
+    _courseOrHeading = atof(_text.c_str());
   } else if (tag == "DMEtoIntercept") {
     _dmeDistance = atof(_text.c_str());
   } else if (tag == "RadialtoIntercept") {
@@ -203,6 +206,11 @@ void NavdataVisitor::endElement(const char* name)
   } else if (tag == "Flytype") {
     // values are 'Fly-by' and 'Fly-over'
     _overflightWaypt = (_text == "Fly-over");
+  } else if ((tag == "AltitudeCons") ||
+             (tag == "BankLimit") ||
+             (tag == "Sp_Turn"))
+  {
+    // ignored but don't warn
   } else {
     SG_LOG(SG_IO, SG_INFO, "unrecognized Level-D XML element:" << tag);
   }
@@ -242,16 +250,16 @@ Waypt* NavdataVisitor::buildWaypoint(RouteBase* owner)
     wp = new ATCVectors(owner, _airport);
   } else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) {
     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
-    wp = new RadialIntercept(owner, _wayptName, pos, _course, _radial);
+    wp = new RadialIntercept(owner, _wayptName, pos, _courseOrHeading, _radial);
   } else if (_wayptType == "DmeIntc") {
     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
-    wp = new DMEIntercept(owner, _wayptName, pos, _course, _dmeDistance);
+    wp = new DMEIntercept(owner, _wayptName, pos, _courseOrHeading, _dmeDistance);
   } else if (_wayptType == "ConstHdgtoAlt") {
-    wp = new HeadingToAltitude(owner, _wayptName, _course);
+    wp = new HeadingToAltitude(owner, _wayptName, _courseOrHeading);
   } else if (_wayptType == "PBD") {
     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)), pos2;
     double az2;
-    SGGeodesy::direct(pos, _course, _dmeDistance, pos2, az2);
+    SGGeodesy::direct(pos, _courseOrHeading, _dmeDistance, pos2, az2);
     wp = new BasicWaypt(pos2, _wayptName, owner);
   } else {
     SG_LOG(SG_NAVAID, SG_ALERT, "implement waypoint type:" << _wayptType);

src/Navaids/LevelDXML.hxx

@@ -59,7 +59,8 @@ private:
   bool _holdRighthanded;
   bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds
   
-  double _course, _radial, _dmeDistance;
+  double _courseOrHeading, _radial, _dmeDistance;
+  bool _courseFlag;
 };
 
 }

src/Navaids/routePath.cxx

@@ -28,6 +28,7 @@ static double sqr(const double x)
   return x * x;
 }
 
+// http://williams.best.vwh.net/avform.htm#POINTDME
 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
 {
   double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
@@ -71,26 +72,559 @@ lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2)
   return lat;
 }
 
-RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
+static double magVarFor(const SGGeod& geod)
-  _waypts(wpts)
 {
+  double jd = globals->get_time_params()->getJD();
+  return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
+}
+
+class WayptData
+{
+public:
+  explicit WayptData(WayptRef w) :
+    wpt(w),
+    hasEntry(false),
+    posValid(false),
+    legCourseValid(false),
+    turnAngle(0.0),
+    turnRadius(0.0),
+    pathDistanceM(0.0),
+    overflightCompensationAngle(0.0),
+    flyOver(w->flag(WPT_OVERFLIGHT))
+  {
+  }
+  
+  void initPass0()
+  {
+    const std::string& ty(wpt->type());
+    if (wpt->flag(WPT_DYNAMIC)) {
+      if ((ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept")) {
+        legCourse = wpt->headingRadialDeg();
+        legCourseValid = true;
+      }
+      
+      // presumtpion is that we always overfly such a waypoint
+      if (ty == "hdgToAlt") {
+        flyOver = true;
+      }
+    } else {
+      pos = wpt->position();
+      posValid = true;
+      
+      if ((ty == "runway") || (ty == "hold")) {
+        legCourse = wpt->headingRadialDeg();
+        legCourseValid = true;
+      }
+      
+      if (ty == "runway") {
+        FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
+        turnExitPos = rwy->end();
+      }
+    } // of static waypt
+  }
+  
+  // compute leg courses for all static legs (both ends are fixed)
+  void initPass1(const WayptData& previous, WayptData* next)
+  {
+    if (wpt->type() == "vectors") {
+      // relying on the fact vectors will be followed by a static fix/wpt
+      if (next && next->posValid) {
+        posValid = true;
+        pos = next->pos;
+      }
+    }
+    
+    if (posValid && !legCourseValid && previous.posValid) {
+      // we can compute leg course now
+      legCourse = SGGeodesy::courseDeg(previous.pos, pos);
+      legCourseValid = true;
+    }
+  }
+  
+  void computeTurn(double radiusM, const WayptData& previous, const WayptData& next)
+  {
+    assert(legCourseValid && next.legCourseValid);
+    
+    turnAngle = next.legCourse - legCourse;
+    SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0);
+    turnRadius = radiusM;
+    
+    double p = copysign(90.0, turnAngle);
+
+    if (flyOver) {
+      turnEntryPos = pos;
+      turnCenter = SGGeodesy::direct(pos, legCourse + p, turnRadius);
+      // use the leg course
+      turnExitPos = SGGeodesy::direct(turnCenter, next.legCourse - p, turnRadius);
+      
+      if (!next.wpt->flag(WPT_DYNAMIC)) {
+        // distance perpendicular to next leg course, after turning
+        // through turnAngle
+        double xtk = turnRadius * (1 - cos(turnAngle * SG_DEGREES_TO_RADIANS));
+        
+        // compensation angle to turn back on course
+        double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
+        theta = copysign(theta, turnAngle);
+        turnAngle += theta;
+      
+        // move by the distance to compensate
+        double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS);
+        turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourse, d);
+        
+        overflightCompensationAngle = -theta;
+      }
+    } else {
+      hasEntry = true;
+
+      double halfAngle = turnAngle * 0.5;
+      
+      double turnCenterOffset = turnRadius / cos(halfAngle * SG_DEGREES_TO_RADIANS);
+      turnCenter = SGGeodesy::direct(pos, legCourse + halfAngle + p, turnCenterOffset);
+      
+      double entryExitDistanceAlongPath = turnRadius * tan(fabs(halfAngle) * SG_DEGREES_TO_RADIANS);
+      
+      turnEntryPos = SGGeodesy::direct(pos, legCourse, -entryExitDistanceAlongPath);
+      turnExitPos = SGGeodesy::direct(pos, next.legCourse, entryExitDistanceAlongPath);
+    }
+  }
+  
+  double turnDistanceM() const
+  {
+    return (fabs(turnAngle * SG_DEGREES_TO_RADIANS) / SG_PI * 2.0) * turnRadius;
+  }
+  
+  void turnEntryPath(SGGeodVec& path) const
+  {
+    assert(!flyOver);
+    if (fabs(turnAngle) < 0.5 ) {
+      path.push_back(pos);
+      return;
+    }
+
+    double halfAngle = turnAngle * 0.5;
+    int steps = std::max(SGMiscd::roundToInt(fabs(halfAngle) / 3.0), 1);
+    double stepIncrement = halfAngle / steps;
+    double h = legCourse;
+    SGGeod p = turnEntryPos;
+    double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
+    
+    for (int s=0; s<steps; ++s) {
+      path.push_back(p);
+      p = SGGeodesy::direct(p, h, stepDist);
+      h += stepIncrement;
+    }
+    
+    path.push_back(p);
+  }
+  
+  void turnExitPath(SGGeodVec& path) const
+  {
+    if (fabs(turnAngle) < 0.5) {
+      path.push_back(turnExitPos);
+      return;
+    }
+
+    double t = flyOver ? turnAngle : turnAngle * 0.5;
+    int steps = std::max(SGMiscd::roundToInt(fabs(t) / 3.0), 1);
+    double stepIncrement = t / steps;
+    
+    // initial exit heading
+    double h = legCourse + (flyOver ? 0.0 : (turnAngle * 0.5));
+    double turnDirOffset = copysign(90.0, turnAngle);
+    
+    // compute the first point on the exit path. Depends on fly-over vs fly-by
+    SGGeod p = flyOver ? pos : SGGeodesy::direct(turnCenter, h + turnDirOffset, -turnRadius);
+    double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
+    
+    for (int s=0; s<steps; ++s) {
+      path.push_back(p);
+      p = SGGeodesy::direct(p, h, stepDist);
+      h += stepIncrement;
+    }
+    
+    if (flyOver) {
+      // skew by compensation angle back
+      steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
+      
+      // step in opposite direction to the turn angle to swing back onto
+      // the next leg course
+      stepIncrement = overflightCompensationAngle / steps;
+      
+      for (int s=0; s<steps; ++s) {
+        path.push_back(p);
+        p = SGGeodesy::direct(p, h, stepDist);
+        h += stepIncrement;
+      }
+    }
+    
+    path.push_back(p);
+  }
+  
+  WayptRef wpt;
+  bool hasEntry, posValid, legCourseValid;
+  SGGeod pos, turnEntryPos, turnExitPos, turnCenter;
+  double turnAngle, turnRadius, legCourse;
+  double pathDistanceM;
+  double overflightCompensationAngle;
+  bool flyOver;
+};
+
+typedef std::vector<WayptData> WayptDataVec;
+
+class PerformanceBracket
+{
+public:
+  PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
+    atOrBelowAltitudeFt(atOrBelow),
+    climbRateFPM(climb),
+    descentRateFPM(descent),
+    speedIASOrMach(speed),
+    speedIsMach(isMach)
+  { }
+  
+  double atOrBelowAltitudeFt;
+  double climbRateFPM;
+  double descentRateFPM;
+  double speedIASOrMach;
+  bool speedIsMach;
+};
+
+typedef std::vector<PerformanceBracket> PerformanceBracketVec;
+
+bool isDescentWaypoint(const WayptRef& wpt)
+{
+  return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
+}
+
+class RoutePath::RoutePathPrivate
+{
+public:
+  WayptDataVec waypoints;
+  PerformanceBracketVec perf;
+  
+  
+  PerformanceBracketVec::const_iterator
+  findPerformanceBracket(double altFt) const
+  {
+    PerformanceBracketVec::const_iterator r;
+    PerformanceBracketVec::const_iterator result = perf.begin();
+
+    for (r = perf.begin(); r != perf.end(); ++r) {
+      if (r->atOrBelowAltitudeFt > altFt) {
+        break;
+      }
+      
+      result = r;
+    } // of brackets iteration
+    
+    return result;
+  }
+  
+  void computeDynamicPosition(int index)
+  {
+    const WayptData& previous(waypoints[index-1]);
+    WayptRef wpt = waypoints[index].wpt;
+    
+    assert(previous.posValid);
+    const std::string& ty(wpt->type());
+    if (ty == "hdgToAlt") {
+      HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
+      
+      double altFt = computeVNAVAltitudeFt(index - 1);
+      double altChange = h->altitudeFt() - altFt;
+      PerformanceBracketVec::const_iterator it = findPerformanceBracket(altFt);
+      double speedMSec = groundSpeedForAltitude(altFt) * SG_KT_TO_MPS;
+      double timeToChangeSec;
+      
+      if (isDescentWaypoint(wpt)) {
+        timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
+      } else {
+        timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
+      }
+
+      double distanceM = timeToChangeSec * speedMSec;
+      double hdg = h->headingDegMagnetic() + magVarFor(previous.pos);
+      waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM);
+      waypoints[index].posValid = true;
+    } else if (ty == "radialIntercept") {
+      // start from previous.turnExit
+      RadialIntercept* i = (RadialIntercept*) wpt.get();
+      
+      SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
+      SGGeoc navid = SGGeoc::fromGeod(wpt->position());
+      SGGeoc rGc;
+      double magVar = magVarFor(previous.pos);
+      
+      double radial = i->radialDegMagnetic() + magVar;
+      double track = i->courseDegMagnetic() + magVar;
+      bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
+      if (!ok) {
+        SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
+               << previous.turnExitPos << " / " << track << "/" << wpt->position()
+               << "/" << radial);
+        waypoints[index].pos = wpt->position(); // horrible fallback
+      } else {
+        waypoints[index].pos = SGGeod::fromGeoc(rGc);
+      }
+      
+      waypoints[index].posValid = true;
+    } else if (ty == "dmeIntercept") {
+      DMEIntercept* di = (DMEIntercept*) wpt.get();
+      
+      SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
+      SGGeoc navid = SGGeoc::fromGeod(wpt->position());
+      double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
+      SGGeoc rGc;
+      
+      SGGeoc bPt;
+      double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
+      SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
+      
+      double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
+      if (dNm < 0.0) {
+        SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
+        waypoints[index].pos = wpt->position(); // horrible fallback
+      } else {
+        waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER);
+      }
+      
+      waypoints[index].posValid = true;
+    } else if (ty == "vectors") {
+      waypoints[index].legCourse = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos);
+      waypoints[index].legCourseValid = true;
+      // no turn data
+    }
+  }
+  
+  double computeVNAVAltitudeFt(int index)
+  {
+    WayptRef w = waypoints[index].wpt;
+    if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
+      // descent
+      int next = findNextKnownAltitude(index);
+      if (next < 0) {
+        return 0.0;
+      }
+      
+      double fixedAlt = altitudeForIndex(next);
+      double distanceM = distanceBetweenIndices(index, next);
+      double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
+      double minutes = (distanceM / speedMSec) / 60.0;
+      
+      PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
+      return fixedAlt + (it->descentRateFPM * minutes);
+
+    } else {
+      // climb
+      int prev = findPreceedingKnownAltitude(index);
+      if (prev < 0) {
+        return 0.0;
+      }
+      
+      double fixedAlt = altitudeForIndex(prev);
+      double distanceM = distanceBetweenIndices(prev, index);
+      double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
+      double minutes = (distanceM / speedMSec) / 60.0;
+      
+      PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
+      return fixedAlt + (it->climbRateFPM * minutes);
+    }
+    
+  }
+  
+  int findPreceedingKnownAltitude(int index) const
+  {
+    const WayptData& w(waypoints[index]);
+    if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
+      return index;
+    }
+    
+    // principal base case is runways.
+    const std::string& ty(w.wpt->type());
+    if (ty == "runway") {
+      return index; // runway always has a known elevation
+    }
+    
+    if (index == 0) {
+      SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceeding altitude value found");
+      return -1;
+    }
+    
+    // recurse earlier in the route
+    return findPreceedingKnownAltitude(index - 1);
+  }
+  
+  int findNextKnownAltitude(int index) const
+  {
+    if (index >= waypoints.size()) {
+      SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
+      return -1;
+    }
+    
+    const WayptData& w(waypoints[index]);
+    if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
+      return index;
+    }
+    
+    // principal base case is runways.
+    const std::string& ty(w.wpt->type());
+    if (ty == "runway") {
+      return index; // runway always has a known elevation
+    }
+    
+    if (index == waypoints.size() - 1) {
+      SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
+      return -1;
+    }
+    
+    return findNextKnownAltitude(index + 1);
+  }
+  
+  double altitudeForIndex(int index) const
+  {
+    const WayptData& w(waypoints[index]);
+    if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
+      return w.wpt->altitudeFt();
+    }
+    
+    const std::string& ty(w.wpt->type());
+    if (ty == "runway") {
+      FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
+      return rwy->threshold().getElevationFt();
+    }
+    
+    SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
+    return 0.0;
+  }
+  
+  double groundSpeedForAltitude(double altitude) const
+  {
+    // FIXME
+#if 0
+    if (0) {
+      PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
+      double mach;
+      
+      if (it->speedIsMach) {
+        mach = it->speedIASOrMach; // easy
+      } else {
+        const double Cs_0 = 661.4786; // speed of sound at sea level, knots
+        const double P_0 = 29.92126;
+        const double P = P_0 * pow(, );
+        // convert IAS (which we will treat as CAS) to Mach based on altitude
+      }
+      
+      double oatK;
+      double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
+      
+      double tas = mach * Cs;
+      
+#if 0
+      P_0= 29.92126 "Hg = 1013.25 mB = 2116.2166 lbs/ft^2
+      P= P_0*(1-6.8755856*10^-6*PA)^5.2558797, pressure altitude, PA<36,089.24ft
+      CS= 38.967854*sqrt(T+273.15)  where T is the (static/true) OAT in Celsius.
+      
+      DP=P_0*((1 + 0.2*(IAS/CS_0)^2)^3.5 -1)
+      M=(5*( (DP/P + 1)^(2/7) -1) )^0.5   (*)
+      TAS= M*CS
+#endif
+    }
+#endif
+    
+    return 250.0;
+  }
+
+  double distanceBetweenIndices(int from, int to) const
+  {
+    double total = 0.0;
+    
+    for (int i=from+1; i<= to; ++i) {
+      total += waypoints[i].pathDistanceM;
+    }
+    
+    return total;
+  }
+  
+  void initPerfData()
+  {
+    // assume category C/D aircraft for now
+    perf.push_back(PerformanceBracket(4000, 1800, 1800, 180));
+    perf.push_back(PerformanceBracket(10000, 1800, 1800, 230));
+    perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
+    perf.push_back(PerformanceBracket(60000, 800, 1200, 0.85, true /* is Mach */));
+  }
+
+}; // of RoutePathPrivate class
+
+RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
+  d(new RoutePathPrivate)
+{
+  WayptVec::const_iterator it;
+  for (it = wpts.begin(); it != wpts.end(); ++it) {
+    d->waypoints.push_back(WayptData(*it));
+  }
   commonInit();
 }
 
-RoutePath::RoutePath(const flightgear::FlightPlan* fp)
+RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
+  d(new RoutePathPrivate)
 {
   for (int l=0; l<fp->numLegs(); ++l) {
-    _waypts.push_back(fp->legAtIndex(l)->waypoint());
+     d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint()));
   }
   commonInit();
 }
 
 void RoutePath::commonInit()
 {
-  _pathClimbFPM = 1200;
+  _pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn
-  _pathDescentFPM = 800;
+ 
-  _pathIAS = 190; 
+  d->initPerfData();
-  _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn  
+  
+  WayptDataVec::iterator it;
+  for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
+    it->initPass0();
+  }
+  
+  for (unsigned int i=1; i<d->waypoints.size(); ++i) {
+    WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : 0;
+    d->waypoints[i].initPass1(d->waypoints[i-1], nextPtr);
+  }
+  
+  for (unsigned int i=1; i<d->waypoints.size(); ++i) {
+    d->computeDynamicPosition(i);
+    
+    const WayptData& prev(d->waypoints[i-1]);
+
+    double alt = 0.0; // FIXME
+    double gs = d->groundSpeedForAltitude(alt);
+    double radiusM = ((360.0 / _pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();
+    
+    if (i < (d->waypoints.size() - 1)) {
+      WayptData& next(d->waypoints[i+1]);
+      if (!next.legCourseValid && next.posValid) {
+        // compute leg course now our own position is valid
+        next.legCourse = SGGeodesy::courseDeg(d->waypoints[i].pos, next.pos);
+        next.legCourseValid = true;
+      }
+      
+      if (next.legCourseValid) {
+        d->waypoints[i].computeTurn(radiusM, prev, next);
+      } else {
+        // next waypoint has indeterminate course. Let's create a sharp turn
+        // this can happen when the following point is ATC vectors, for example.
+        d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
+        d->waypoints[i].turnExitPos = d->waypoints[i].pos;
+      }
+    } else {
+      // final waypt, fix up some data
+      d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
+      d->waypoints[i].turnExitPos = d->waypoints[i].pos;
+    }
+    
+    // now turn is computed, can resolve distances
+    d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
+  }
 }
 
 SGGeodVec RoutePath::pathForIndex(int index) const
@@ -99,24 +633,21 @@ SGGeodVec RoutePath::pathForIndex(int index) const
     return SGGeodVec(); // no path for first waypoint
   }
   
-  if (_waypts[index]->type() == "vectors") {
+  const WayptData& w(d->waypoints[index]);
+  const std::string& ty(w.wpt->type());
+  if (ty == "vectors") {
     return SGGeodVec(); // empty
   }
   
-  if (_waypts[index]->type() == "hold") {
+  if (ty== "hold") {
-    return pathForHold((Hold*) _waypts[index].get());
+    return pathForHold((Hold*) d->waypoints[index].wpt.get());
   }
   
   SGGeodVec r;
-  SGGeod from, to;
+  d->waypoints[index - 1].turnExitPath(r);
-  if (!computedPositionForIndex(index-1, from)) {
-    return SGGeodVec();
-  }
   
-  r.push_back(from);
+  SGGeod from = d->waypoints[index - 1].turnExitPos,
-  if (!computedPositionForIndex(index, to)) {
+    to = w.turnEntryPos;
-    return SGGeodVec();
-  }
   
   // compute rounding offset, we want to round towards the direction of travel
   // which depends on the east/west sign of the longitude change
@@ -125,12 +656,17 @@ SGGeodVec RoutePath::pathForIndex(int index) const
     interpolateGreatCircle(from, to, r);
   }
   
-  r.push_back(to);
+  if (w.flyOver) {
+    r.push_back(w.pos);
+  } else {
+    // flyBy
+    w.turnEntryPath(r);
+  }
   
-  if (_waypts[index]->type() == "runway") {
+  if (ty == "runway") {
     // runways get an extra point, at the end. this is particularly
     // important so missed approach segments draw correctly
-    FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
+    FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
     r.push_back(rwy->end());
   }
   
@@ -163,13 +699,7 @@ void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, S
 
 SGGeod RoutePath::positionForIndex(int index) const
 {
-  SGGeod r;
+  return d->waypoints[index].pos;
-  bool ok = computedPositionForIndex(index, r);
-  if (!ok) {
-    return SGGeod();
-  }
-  
-  return r;
 }
 
 SGGeodVec RoutePath::pathForHold(Hold* hold) const
@@ -177,14 +707,16 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const
   int turnSteps = 16;
   double hdg = hold->inboundRadial();
   double turnDelta = 180.0 / turnSteps;
+  double altFt = 0.0; // FIXME
+  double gsKts = d->groundSpeedForAltitude(altFt);
   
   SGGeodVec r;
   double az2;
   double stepTime = turnDelta / _pathTurnRate; // in seconds
-  double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
+  double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
   double legDist = hold->isDistance() ? 
     hold->timeOrDistance() 
-    : _pathIAS * (hold->timeOrDistance() / 3600.0);
+    : gsKts * (hold->timeOrDistance() / 3600.0);
   legDist *= SG_NM_TO_METER;
   
   if (hold->isLeftHanded()) {
@@ -210,244 +742,48 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const
   return r;
 }
 
-bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
-{
-  if ((index < 0) || (index >= (int) _waypts.size())) {
-    throw sg_range_exception("waypt index out of range", 
-      "RoutePath::computedPositionForIndex");
-  }
-
-  WayptRef w = _waypts[index];
-  if (!w->flag(WPT_DYNAMIC)) {
-    r = w->position();
-    return true;
-  }
-  
-  if (w->type() == "radialIntercept") {
-    // radial intersection along track
-    SGGeod prev;
-    if (!computedPositionForIndex(index - 1, prev)) {
-      return false;
-    }
-  
-    SGGeoc prevGc = SGGeoc::fromGeod(prev);
-    SGGeoc navid = SGGeoc::fromGeod(w->position());
-    SGGeoc rGc;
-    double magVar = magVarFor(prev);
-    
-    RadialIntercept* i = (RadialIntercept*) w.get();
-    double radial = i->radialDegMagnetic() + magVar;
-    double track = i->courseDegMagnetic() + magVar;
-    bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
-    if (!ok) {
-      return false;
-    }
-    
-    r = SGGeod::fromGeoc(rGc);
-    return true;
-  } else if (w->type() == "dmeIntercept") {
-    // find the point along the DME track, from prev, that is the correct distance
-    // from the DME
-    SGGeod prev;
-    if (!computedPositionForIndex(index - 1, prev)) {
-      return false;
-    }
-    
-    DMEIntercept* di = (DMEIntercept*) w.get();
-    
-    SGGeoc prevGc = SGGeoc::fromGeod(prev);
-    SGGeoc navid = SGGeoc::fromGeod(w->position());
-    double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
-    SGGeoc rGc;
-
-    SGGeoc bPt;
-    double crs = di->courseDegMagnetic() + magVarFor(prev);
-    SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
-
-    double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
-    if (dNm < 0.0) {
-      return false;
-    }
-    
-    double az2;
-    SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
-    return true;
-  } else if (w->type() == "hdgToAlt") {
-    HeadingToAltitude* h = (HeadingToAltitude*) w.get();
-    double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
-    double d = distanceForClimb(climb);
-
-    SGGeod prevPos;
-    if (!computedPositionForIndex(index - 1, prevPos)) {
-      return false;
-    }
-    
-    double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
-    
-    double az2;
-    SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
-    return true;
-  } else if (w->type() == "vectors"){
-    // 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;
-  }
-  
-  SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
-  return false;
-}
-
 double RoutePath::computeDistanceForIndex(int index) const
 {
-  if ((index < 0) || (index >= (int) _waypts.size())) {
+  if ((index < 0) || (index >= (int) d->waypoints.size())) {
     throw sg_range_exception("waypt index out of range",
                              "RoutePath::computeDistanceForIndex");
   }
   
-  if (index + 1 >= (int) _waypts.size()) {
+  if (index == 0) {
-    // final waypoint, distance is 0
+    // first waypoint, distance is 0
     return 0.0;
   }
   
-  WayptRef w = _waypts[index],
+  double dist = SGGeodesy::distanceM(d->waypoints[index-1].turnExitPos,
-    nextWp = _waypts[index+1];
+                              d->waypoints[index].turnEntryPos);
-  
+  if (d->waypoints[index-1].flyOver) {
-  // common case, both waypoints are static
+    // all the turn distance counts towards this leg
-  if (!w->flag(WPT_DYNAMIC) && !nextWp->flag(WPT_DYNAMIC)) {
+    dist += d->waypoints[index-1].turnDistanceM();
-    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())) {
-    throw sg_range_exception("waypt index out of range", 
-      "RoutePath::computeAltitudeForIndex");
-  }
-  
-  WayptRef w = _waypts[index];
-  if (w->altitudeRestriction() != RESTRICT_NONE) {
-    return w->altitudeFt(); // easy!
-  }
-  
-  if (w->type() == "runway") {
-    FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
-    return rwy->threshold().getElevationFt();
-  } else if ((w->type() == "hold") || (w->type() == "vectors")) {
-    // pretend we don't change altitude in holds/vectoring
-    return computeAltitudeForIndex(index - 1);
-  }
-
-  double prevAlt = computeAltitudeForIndex(index - 1);
-// find distance to previous, and hence climb/descent 
-  SGGeod pos, prevPos;
-  
-  if (!computedPositionForIndex(index, pos) ||
-      !computedPositionForIndex(index - 1, prevPos))
-  {
-    SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints");
-    throw sg_range_exception("unable to compute position for waypoints");
-  }
-  
-  double d = SGGeodesy::distanceNm(prevPos, pos);
-  double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
-  
-  double deltaFt; // change in altitude in feet
-  if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
-    deltaFt = -_pathDescentFPM * tMinutes;
   } else {
-    deltaFt = _pathClimbFPM * tMinutes;
+    // add half of turn distance
+    dist += d->waypoints[index-1].turnDistanceM() * 0.5;
   }
   
-  return prevAlt + deltaFt;
+  if (!d->waypoints[index].flyOver) {
+    // add half turn distance
+    dist += d->waypoints[index].turnDistanceM() * 0.5;
   }
   
-double RoutePath::computeTrackForIndex(int index) const
+  return dist;
+}
+
+double RoutePath::trackForIndex(int index) const
 {
-  if ((index < 0) || (index >= (int) _waypts.size())) {
+  return d->waypoints[index].legCourse;
-    throw sg_range_exception("waypt index out of range", 
-      "RoutePath::computeTrackForIndex");
 }
 
-  WayptRef w = _waypts[index];
+double RoutePath::distanceForIndex(int index) const
-  if (w->type() == "radialIntercept") {
-    RadialIntercept* r = (RadialIntercept*) w.get();
-    return r->courseDegMagnetic();
-  } else if (w->type() == "dmeIntercept") {
-    DMEIntercept* d = (DMEIntercept*) w.get();
-    return d->courseDegMagnetic();
-  } else if (w->type() == "hdgToAlt") {
-    HeadingToAltitude* h = (HeadingToAltitude*) w.get();
-    return h->headingDegMagnetic();
-  } else if (w->type() == "hold") {
-    Hold* h = (Hold*) w.get();
-    return h->inboundRadial();
-  } else if (w->type() == "vectors") {
-    SG_LOG(SG_NAVAID, SG_WARN, "asked for track from VECTORS");
-    throw sg_range_exception("asked for track from vectors waypt");
-  } else if (w->type() == "runway") {
-    FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
-    return rwy->headingDeg();
-  }
-  
-// final waypoint, use inbound course
-  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, nextPos))
 {
-    SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints");
+  return d->waypoints[index].pathDistanceM;
-    throw sg_range_exception("unable to compute position for waypoints");
 }
 
-  return SGGeodesy::courseDeg(pos, nextPos);
+double RoutePath::distanceBetweenIndices(int from, int to) const
-}
-
-double RoutePath::distanceForClimb(double climbFt) const
 {
-  double t = 0.0; // in seconds
+  return d->distanceBetweenIndices(from, to);
-  if (climbFt > 0.0) {
-    t = (climbFt / _pathClimbFPM) * 60.0;
-  } else if (climbFt < 0.0) {
-    t = (climbFt / _pathDescentFPM) * 60.0;
 }
 
-  return _pathIAS * (t / 3600.0);
-}
-
-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,33 +44,27 @@ public:
   
   SGGeod positionForIndex(int index) const;
   
-  double computeDistanceForIndex(int index) const;
+  double trackForIndex(int index) const;
-  double computeTrackForIndex(int index) const;
   
+  double distanceForIndex(int index) const;
+  
+  double distanceBetweenIndices(int from, int to) const;
 private:
+  class RoutePathPrivate;
+  
   void commonInit();
   
-  class PathCtx;
+  double computeDistanceForIndex(int index) const;
     
   SGGeodVec pathForHold(flightgear::Hold* hold) const;
   
-  bool computedPositionForIndex(int index, SGGeod& pos) const;
-  double computeAltitudeForIndex(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
-   */
-  double distanceForClimb(double climbFt) const;
   
-  double magVarFor(const SGGeod& gd) const; 
+  RoutePathPrivate* d;
   
-  flightgear::WayptVec _waypts;
   
-  int _pathClimbFPM; ///< climb-rate to use for pathing
-  int _pathDescentFPM; ///< descent rate to use (feet-per-minute)
-  int _pathIAS; ///< IAS (knots) to use for pathing
   double _pathTurnRate; ///< degrees-per-second, defaults to 3, i.e 180 in a minute
 };
 

src/Navaids/waypoint.hxx

@@ -273,6 +273,8 @@ public:
   double radialDegMagnetic() const
     { return _radial; }
 
+  virtual double headingRadialDeg() const
+  { return courseDegMagnetic(); }
 private:
   std::string _ident;
   SGGeod _pos;