src/Navaids/routePath.cxx

   1
   2 #ifdef HAVE_CONFIG_H
   3 #  include "config.h"
   4 #endif
   5
   6 #include <Navaids/routePath.hxx>
   7
   8 #include <simgear/structure/exception.hxx>
   9 #include <simgear/magvar/magvar.hxx>
  10 #include <simgear/timing/sg_time.hxx>
  11
  12 #include <Main/globals.hxx>
  13 #include <Airports/runways.hxx>
  14 #include <Navaids/waypoint.hxx>
  15 #include <Navaids/FlightPlan.hxx>
  16 #include <Navaids/positioned.hxx>
  17
  18 namespace flightgear {
  19   bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
  20 }
  21
  22 using namespace flightgear;
  23
  24 // implement Point(s) known distance from a great circle
  25
  26 static double sqr(const double x)
  27 {
  28   return x * x;
  29 }
  30
  31 // http://williams.best.vwh.net/avform.htm#POINTDME
  32 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
  33 {
  34   double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
  35   double bDist = SGGeodesy::distanceRad(a, d);
  36
  37   // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
  38   double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5);
  39
  40   double p = atan2(sin(bDist)*cos(A), cos(bDist));
  41
  42   if (sqr(cos(dist)) > sqr(r)) {
  43     SG_LOG(SG_NAVAID, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
  44     return -1.0;
  45   }
  46
  47   double dp1 = p + acos(cos(dist)/r);
  48   double dp2 = p - acos(cos(dist)/r);
  49
  50   double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
  51   double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
  52
  53   return SGMiscd::min(dp1Nm, dp2Nm);
  54 }
  55
  56 // http://williams.best.vwh.net/avform.htm#Int
  57 double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
  58 {
  59 #if 0
  60 Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:
  61
  62 lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2)
  63      -sin(lat2)*cos(lat1)*sin(lon-lon1))/(cos(lat1)*cos(lat2)*sin(lon1-lon2)))
  64 #endif
  65   double lonDiff = a.getLongitudeRad() - b.getLongitudeRad();
  66   double cosLat1 = cos(a.getLatitudeRad()),
  67       cosLat2 = cos(b.getLatitudeRad());
  68   double x = sin(a.getLatitudeRad()) * cosLat2 * sin(lon - b.getLongitudeRad());
  69   double y = sin(b.getLatitudeRad()) * cosLat1 * sin(lon - a.getLongitudeRad());
  70   double denom = cosLat1 * cosLat2 * sin(lonDiff);
  71   double lat = atan((x - y) / denom);
  72   return lat;
  73 }
  74
  75 static double magVarFor(const SGGeod& geod)
  76 {
  77   double jd = globals->get_time_params()->getJD();
  78   return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
  79 }
  80
  81 class WayptData
  82 {
  83 public:
  84   explicit WayptData(WayptRef w) :
  85     wpt(w),
  86     hasEntry(false),
  87     posValid(false),
  88     legCourseValid(false),
  89     skipped(false),
  90     turnAngle(0.0),
  91     turnRadius(0.0),
  92     pathDistanceM(0.0),
  93     turnPathDistanceM(0.0),
  94     overflightCompensationAngle(0.0),
  95     flyOver(w->flag(WPT_OVERFLIGHT))
  96   {
  97   }
  98
  99   void initPass0()
 100   {
 101     const std::string& ty(wpt->type());
 102     if (wpt->flag(WPT_DYNAMIC)) {
 103       // presumption is that we always overfly such a waypoint
 104       if (ty == "hdgToAlt") {
 105         flyOver = true;
 106       }
 107     } else {
 108       pos = wpt->position();
 109       posValid = true;
 110
 111       if ((ty == "runway") || (ty == "hold")) {
 112         legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
 113         legCourseValid = true;
 114       }
 115
 116       if (ty == "runway") {
 117         FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 118         turnExitPos = rwy->end();
 119       }
 120     } // of static waypt
 121   }
 122
 123   /**
 124    * test if course of this leg can be adjusted or is contrained to an exact value
 125    */
 126   bool isCourseConstrained() const
 127   {
 128     const std::string& ty(wpt->type());
 129     return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept");
 130   }
 131
 132   // compute leg courses for all static legs (both ends are fixed)
 133   void initPass1(const WayptData& previous, WayptData* next)
 134   {
 135     if (wpt->type() == "vectors") {
 136       // relying on the fact vectors will be followed by a static fix/wpt
 137       if (next && next->posValid) {
 138         posValid = true;
 139         pos = next->pos;
 140       }
 141     }
 142
 143     if (posValid && !legCourseValid && previous.posValid) {
 144     // check for duplicate points now
 145       if (previous.wpt->matches(wpt)) {
 146           skipped = true;
 147       }
 148
 149       // we can compute leg course now
 150       legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
 151       legCourseValid = true;
 152     }
 153   }
 154
 155   void computeLegCourse(const WayptData& previous)
 156   {
 157       if (!legCourseValid) {
 158           if (posValid) {
 159               legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
 160               legCourseValid = true;
 161           } else if (isCourseConstrained()) {
 162               double magVar = magVarFor(previous.pos);
 163               legCourseTrue = wpt->headingRadialDeg() + magVar;
 164               legCourseValid = true;
 165
 166           }
 167       }
 168   }
 169
 170   void computeTurn(double radiusM, const WayptData& previous, WayptData& next)
 171   {
 172     assert(!skipped);
 173     assert(legCourseValid && next.legCourseValid);
 174
 175     turnAngle = next.legCourseTrue - legCourseTrue;
 176     SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0);
 177     turnRadius = radiusM;
 178     double p = copysign(90.0, turnAngle);
 179
 180     if (fabs(turnAngle) > 120.0) {
 181         // flyBy logic blows up for sharp turns - due to the tan() term
 182         // heading towards infinity. By converting to flyOver we do something
 183         // closer to what was requested.
 184         flyOver = true;
 185     }
 186
 187     if (flyOver) {
 188       turnEntryPos = pos;
 189       turnCenter = SGGeodesy::direct(pos, legCourseTrue + p, turnRadius);
 190       // use the leg course
 191       turnExitPos = SGGeodesy::direct(turnCenter, next.legCourseTrue - p, turnRadius);
 192
 193       if (!next.wpt->flag(WPT_DYNAMIC)) {
 194         // distance perpendicular to next leg course, after turning
 195         // through turnAngle
 196         double xtk = turnRadius * (1 - cos(turnAngle * SG_DEGREES_TO_RADIANS));
 197
 198         if (next.isCourseConstrained()) {
 199           // next leg course is constrained. We need to swing back onto the
 200           // desired course, using a compensation turn
 201
 202           // compensation angle to turn back on course
 203           double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
 204           theta = copysign(theta, turnAngle);
 205           turnAngle += theta;
 206
 207           // move by the distance to compensate
 208           double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS);
 209           turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourseTrue, d);
 210           overflightCompensationAngle = -theta;
 211
 212           turnPathDistanceM = turnRadius * (fabs(turnAngle) +
 213                                             fabs(overflightCompensationAngle)) * SG_DEGREES_TO_RADIANS;
 214         } else {
 215           // next leg course can be adjusted. increase the turn angle
 216           // and modify the next leg's course accordingly.
 217
 218           // hypotenuse of triangle, opposite edge has length turnRadius
 219           double distAlongPath = std::min(1.0, sin(fabs(turnAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius;
 220           double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - distAlongPath;
 221           double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES;
 222           increaseAngle = copysign(increaseAngle, turnAngle);
 223
 224           turnAngle += increaseAngle;
 225           turnExitPos = SGGeodesy::direct(turnCenter, legCourseTrue + turnAngle - p, turnRadius);
 226
 227           // modify next leg course
 228           next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
 229
 230           turnPathDistanceM = turnRadius * (fabs(turnAngle) * SG_DEGREES_TO_RADIANS);
 231         } // of next leg isn't course constrained
 232       } else {
 233           // next point is dynamic
 234           // no compensation needed
 235           turnPathDistanceM = turnRadius * (fabs(turnAngle) * SG_DEGREES_TO_RADIANS);
 236       }
 237     } else {
 238       hasEntry = true;
 239
 240       double halfAngle = turnAngle * 0.5;
 241
 242       double turnCenterOffset = turnRadius / cos(halfAngle * SG_DEGREES_TO_RADIANS);
 243       turnCenter = SGGeodesy::direct(pos, legCourseTrue + halfAngle + p, turnCenterOffset);
 244
 245       double distAlongPath = turnRadius * tan(fabs(halfAngle) * SG_DEGREES_TO_RADIANS);
 246
 247       turnEntryPos = SGGeodesy::direct(pos, legCourseTrue, -distAlongPath);
 248       turnExitPos = SGGeodesy::direct(pos, next.legCourseTrue, distAlongPath);
 249
 250       turnPathDistanceM = turnRadius * (fabs(halfAngle) * SG_DEGREES_TO_RADIANS);
 251     }
 252   }
 253
 254   double turnDistanceM() const
 255   {
 256       return turnPathDistanceM;
 257   }
 258
 259   void turnEntryPath(SGGeodVec& path) const
 260   {
 261     assert(!flyOver);
 262     if (fabs(turnAngle) < 0.5 ) {
 263       path.push_back(pos);
 264       return;
 265     }
 266
 267     double halfAngle = turnAngle * 0.5;
 268     int steps = std::max(SGMiscd::roundToInt(fabs(halfAngle) / 3.0), 1);
 269     double stepIncrement = halfAngle / steps;
 270     double h = legCourseTrue;
 271     SGGeod p = turnEntryPos;
 272     double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
 273
 274     for (int s=0; s<steps; ++s) {
 275       path.push_back(p);
 276       p = SGGeodesy::direct(p, h, stepDist);
 277       h += stepIncrement;
 278     }
 279
 280     path.push_back(p);
 281   }
 282
 283   void turnExitPath(SGGeodVec& path) const
 284   {
 285     if (fabs(turnAngle) < 0.5) {
 286       path.push_back(turnExitPos);
 287       return;
 288     }
 289
 290     double t = flyOver ? turnAngle : turnAngle * 0.5;
 291     int steps = std::max(SGMiscd::roundToInt(fabs(t) / 3.0), 1);
 292     double stepIncrement = t / steps;
 293
 294     // initial exit heading
 295     double h = legCourseTrue + (flyOver ? 0.0 : (turnAngle * 0.5));
 296     double turnDirOffset = copysign(90.0, turnAngle);
 297
 298     // compute the first point on the exit path. Depends on fly-over vs fly-by
 299     SGGeod p = flyOver ? pos : SGGeodesy::direct(turnCenter, h + turnDirOffset, -turnRadius);
 300     double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
 301
 302     for (int s=0; s<steps; ++s) {
 303       path.push_back(p);
 304       p = SGGeodesy::direct(p, h, stepDist);
 305       h += stepIncrement;
 306     }
 307
 308     // we can use an exact check on the compensation angle, becayse we
 309     // initialise it directly. Depending on the next element we might be
 310     // doing compensation or adjusting the next leg's course; if we did
 311     // adjust the course ecerything 'just works' above.
 312
 313     if (flyOver && (overflightCompensationAngle != 0.0)) {
 314       // skew by compensation angle back
 315       steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
 316
 317       // step in opposite direction to the turn angle to swing back onto
 318       // the next leg course
 319       stepIncrement = overflightCompensationAngle / steps;
 320
 321       for (int s=0; s<steps; ++s) {
 322         path.push_back(p);
 323         p = SGGeodesy::direct(p, h, stepDist);
 324         h += stepIncrement;
 325       }
 326     }
 327
 328     path.push_back(p);
 329   }
 330
 331   SGGeod pointAlongExitPath(double distanceM) const
 332   {
 333       double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
 334       double p = copysign(90, turnAngle);
 335
 336       if (flyOver && (overflightCompensationAngle != 0.0)) {
 337           // figure out if we're in the compensation section
 338           if (theta > turnAngle) {
 339               // compute the compensation turn center - twice the turn radius
 340               // from turnCenter
 341               SGGeod tc2 = SGGeodesy::direct(turnCenter,
 342                                              legCourseTrue - overflightCompensationAngle - p,
 343                                              turnRadius * 2.0);
 344               theta = copysign(theta - turnAngle, overflightCompensationAngle);
 345               return SGGeodesy::direct(tc2,
 346                                        legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
 347           }
 348       }
 349
 350       theta = copysign(theta, turnAngle);
 351       double halfAngle = turnAngle * 0.5;
 352       double inboundCourse = legCourseTrue + (flyOver ? 0.0 : halfAngle);
 353       return SGGeodesy::direct(turnCenter, inboundCourse + theta - p, turnRadius);
 354   }
 355
 356   SGGeod pointAlongEntryPath(double distanceM) const
 357   {
 358     assert(hasEntry);
 359     double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
 360     theta = copysign(theta, turnAngle);
 361     double p = copysign(90, turnAngle);
 362     double course = legCourseTrue - turnAngle + theta;
 363     return SGGeodesy::direct(turnCenter, course - p, turnRadius);
 364   }
 365
 366   WayptRef wpt;
 367   bool hasEntry, posValid, legCourseValid, skipped;
 368   SGGeod pos, turnEntryPos, turnExitPos, turnCenter;
 369   double turnAngle, turnRadius, legCourseTrue;
 370   double pathDistanceM;
 371   double turnPathDistanceM; // for flyBy, this is half the distance; for flyOver it's the completel distance
 372   double overflightCompensationAngle;
 373   bool flyOver;
 374 };
 375
 376 typedef std::vector<WayptData> WayptDataVec;
 377
 378 class PerformanceBracket
 379 {
 380 public:
 381   PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
 382     atOrBelowAltitudeFt(atOrBelow),
 383     climbRateFPM(climb),
 384     descentRateFPM(descent),
 385     speedIASOrMach(speed),
 386     speedIsMach(isMach)
 387   { }
 388
 389   double atOrBelowAltitudeFt;
 390   double climbRateFPM;
 391   double descentRateFPM;
 392   double speedIASOrMach;
 393   bool speedIsMach;
 394 };
 395
 396 typedef std::vector<PerformanceBracket> PerformanceBracketVec;
 397
 398 bool isDescentWaypoint(const WayptRef& wpt)
 399 {
 400   return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
 401 }
 402
 403 class RoutePath::RoutePathPrivate
 404 {
 405 public:
 406   WayptDataVec waypoints;
 407   PerformanceBracketVec perf;
 408
 409
 410   PerformanceBracketVec::const_iterator
 411   findPerformanceBracket(double altFt) const
 412   {
 413     PerformanceBracketVec::const_iterator r;
 414     PerformanceBracketVec::const_iterator result = perf.begin();
 415
 416     for (r = perf.begin(); r != perf.end(); ++r) {
 417       if (r->atOrBelowAltitudeFt > altFt) {
 418         break;
 419       }
 420
 421       result = r;
 422     } // of brackets iteration
 423
 424     return result;
 425   }
 426
 427   void computeDynamicPosition(int index)
 428   {
 429     const WayptData& previous(previousValidWaypoint(index));
 430     WayptRef wpt = waypoints[index].wpt;
 431     assert(previous.posValid);
 432
 433     const std::string& ty(wpt->type());
 434     if (ty == "hdgToAlt") {
 435       HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
 436
 437       double altFt = computeVNAVAltitudeFt(index - 1);
 438       double altChange = h->altitudeFt() - altFt;
 439       PerformanceBracketVec::const_iterator it = findPerformanceBracket(altFt);
 440       double speedMSec = groundSpeedForAltitude(altFt) * SG_KT_TO_MPS;
 441       double timeToChangeSec;
 442
 443       if (isDescentWaypoint(wpt)) {
 444         timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
 445       } else {
 446         timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
 447       }
 448
 449       double distanceM = timeToChangeSec * speedMSec;
 450       double hdg = h->headingDegMagnetic() + magVarFor(previous.pos);
 451       waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM);
 452       waypoints[index].posValid = true;
 453     } else if (ty == "radialIntercept") {
 454       // start from previous.turnExit
 455       RadialIntercept* i = (RadialIntercept*) wpt.get();
 456
 457       SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
 458       SGGeoc navid = SGGeoc::fromGeod(wpt->position());
 459       SGGeoc rGc;
 460       double magVar = magVarFor(previous.pos);
 461
 462       double radial = i->radialDegMagnetic() + magVar;
 463       double track = i->courseDegMagnetic() + magVar;
 464       bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
 465       if (!ok) {
 466         SGGeoc navidAdjusted;
 467         // try pulling backward along the radial in case we're too close.
 468         // suggests bad procedure construction if this is happening!
 469         SGGeodesy::advanceRadM(navid, radial, SG_NM_TO_METER * -10, navidAdjusted);
 470
 471         // try again
 472         ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
 473         if (!ok) {
 474           SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
 475                << previous.turnExitPos << " / " << track << "/" << wpt->position()
 476                << "/" << radial);
 477           waypoints[index].pos = wpt->position(); // horrible fallback
 478
 479         } else {
 480           waypoints[index].pos = SGGeod::fromGeoc(rGc);
 481         }
 482       } else {
 483         waypoints[index].pos = SGGeod::fromGeoc(rGc);
 484       }
 485
 486       waypoints[index].posValid = true;
 487     } else if (ty == "dmeIntercept") {
 488       DMEIntercept* di = (DMEIntercept*) wpt.get();
 489
 490       SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
 491       SGGeoc navid = SGGeoc::fromGeod(wpt->position());
 492       double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
 493       SGGeoc rGc;
 494
 495       SGGeoc bPt;
 496       double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
 497       SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
 498
 499       double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
 500       if (dNm < 0.0) {
 501         SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
 502         waypoints[index].pos = wpt->position(); // horrible fallback
 503       } else {
 504         waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER);
 505       }
 506
 507       waypoints[index].posValid = true;
 508     } else if (ty == "vectors") {
 509       waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos);
 510       waypoints[index].legCourseValid = true;
 511       // no turn data
 512     }
 513   }
 514
 515   double computeVNAVAltitudeFt(int index)
 516   {
 517     WayptRef w = waypoints[index].wpt;
 518     if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
 519       // descent
 520       int next = findNextKnownAltitude(index);
 521       if (next < 0) {
 522         return 0.0;
 523       }
 524
 525       double fixedAlt = altitudeForIndex(next);
 526       double distanceM = distanceBetweenIndices(index, next);
 527       double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
 528       double minutes = (distanceM / speedMSec) / 60.0;
 529
 530       PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
 531       return fixedAlt + (it->descentRateFPM * minutes);
 532
 533     } else {
 534       // climb
 535       int prev = findPreceedingKnownAltitude(index);
 536       if (prev < 0) {
 537         return 0.0;
 538       }
 539
 540       double fixedAlt = altitudeForIndex(prev);
 541       double distanceM = distanceBetweenIndices(prev, index);
 542       double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
 543       double minutes = (distanceM / speedMSec) / 60.0;
 544
 545       PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
 546       return fixedAlt + (it->climbRateFPM * minutes);
 547     }
 548
 549   }
 550
 551   int findPreceedingKnownAltitude(int index) const
 552   {
 553     const WayptData& w(waypoints[index]);
 554     if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
 555       return index;
 556     }
 557
 558     // principal base case is runways.
 559     const std::string& ty(w.wpt->type());
 560     if (ty == "runway") {
 561       return index; // runway always has a known elevation
 562     }
 563
 564     if (index == 0) {
 565       SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceeding altitude value found");
 566       return -1;
 567     }
 568
 569     // recurse earlier in the route
 570     return findPreceedingKnownAltitude(index - 1);
 571   }
 572
 573   int findNextKnownAltitude(int index) const
 574   {
 575     if (index >= waypoints.size()) {
 576       SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
 577       return -1;
 578     }
 579
 580     const WayptData& w(waypoints[index]);
 581     if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
 582       return index;
 583     }
 584
 585     // principal base case is runways.
 586     const std::string& ty(w.wpt->type());
 587     if (ty == "runway") {
 588       return index; // runway always has a known elevation
 589     }
 590
 591     if (index == waypoints.size() - 1) {
 592       SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
 593       return -1;
 594     }
 595
 596     return findNextKnownAltitude(index + 1);
 597   }
 598
 599   double altitudeForIndex(int index) const
 600   {
 601     const WayptData& w(waypoints[index]);
 602     if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
 603       return w.wpt->altitudeFt();
 604     }
 605
 606     const std::string& ty(w.wpt->type());
 607     if (ty == "runway") {
 608       FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
 609       return rwy->threshold().getElevationFt();
 610     }
 611
 612     SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
 613     return 0.0;
 614   }
 615
 616   double groundSpeedForAltitude(double altitude) const
 617   {
 618     // FIXME
 619 #if 0
 620     if (0) {
 621       PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
 622       double mach;
 623
 624       if (it->speedIsMach) {
 625         mach = it->speedIASOrMach; // easy
 626       } else {
 627         const double Cs_0 = 661.4786; // speed of sound at sea level, knots
 628         const double P_0 = 29.92126;
 629         const double P = P_0 * pow(, );
 630         // convert IAS (which we will treat as CAS) to Mach based on altitude
 631       }
 632
 633       double oatK;
 634       double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
 635
 636       double tas = mach * Cs;
 637
 638 #if 0
 639       P_0= 29.92126 "Hg = 1013.25 mB = 2116.2166 lbs/ft^2
 640       P= P_0*(1-6.8755856*10^-6*PA)^5.2558797, pressure altitude, PA<36,089.24ft
 641       CS= 38.967854*sqrt(T+273.15)  where T is the (static/true) OAT in Celsius.
 642
 643       DP=P_0*((1 + 0.2*(IAS/CS_0)^2)^3.5 -1)
 644       M=(5*( (DP/P + 1)^(2/7) -1) )^0.5   (*)
 645       TAS= M*CS
 646 #endif
 647     }
 648 #endif
 649
 650     return 250.0;
 651   }
 652
 653   double distanceBetweenIndices(int from, int to) const
 654   {
 655     double total = 0.0;
 656
 657     for (int i=from+1; i<= to; ++i) {
 658       total += waypoints[i].pathDistanceM;
 659     }
 660
 661     return total;
 662   }
 663
 664   void initPerfData()
 665   {
 666     // assume category C/D aircraft for now
 667     perf.push_back(PerformanceBracket(4000, 1800, 1800, 180));
 668     perf.push_back(PerformanceBracket(10000, 1800, 1800, 230));
 669     perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
 670     perf.push_back(PerformanceBracket(60000, 800, 1200, 0.85, true /* is Mach */));
 671   }
 672
 673     const WayptData& previousValidWaypoint(int index) const
 674     {
 675         assert(index > 0);
 676         if (waypoints[index-1].skipped) {
 677             return waypoints[index-2];
 678         }
 679
 680         return waypoints[index-1];
 681     }
 682
 683 }; // of RoutePathPrivate class
 684
 685 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
 686   d(new RoutePathPrivate)
 687 {
 688   WayptVec::const_iterator it;
 689   for (it = wpts.begin(); it != wpts.end(); ++it) {
 690     d->waypoints.push_back(WayptData(*it));
 691   }
 692   commonInit();
 693 }
 694
 695 RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
 696   d(new RoutePathPrivate)
 697 {
 698   for (int l=0; l<fp->numLegs(); ++l) {
 699      d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint()));
 700   }
 701   commonInit();
 702 }
 703
 704 void RoutePath::commonInit()
 705 {
 706   _pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn
 707
 708   d->initPerfData();
 709
 710   WayptDataVec::iterator it;
 711   for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
 712     it->initPass0();
 713   }
 714
 715   for (unsigned int i=1; i<d->waypoints.size(); ++i) {
 716     WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : 0;
 717     d->waypoints[i].initPass1(d->waypoints[i-1], nextPtr);
 718   }
 719
 720   for (unsigned int i=1; i<d->waypoints.size(); ++i) {
 721       if (d->waypoints[i].skipped) {
 722           continue;
 723       }
 724
 725     const WayptData& prev(d->previousValidWaypoint(i));
 726     d->waypoints[i].computeLegCourse(prev);
 727     d->computeDynamicPosition(i);
 728
 729     double alt = 0.0; // FIXME
 730     double gs = d->groundSpeedForAltitude(alt);
 731     double radiusM = ((360.0 / _pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();
 732
 733     if (i < (d->waypoints.size() - 1)) {
 734         int nextIndex = i + 1;
 735         if (d->waypoints[nextIndex].skipped) {
 736             nextIndex++;
 737         }
 738         WayptData& next(d->waypoints[nextIndex]);
 739         next.computeLegCourse(d->waypoints[i]);
 740
 741       if (next.legCourseValid) {
 742         d->waypoints[i].computeTurn(radiusM, prev, next);
 743       } else {
 744         // next waypoint has indeterminate course. Let's create a sharp turn
 745         // this can happen when the following point is ATC vectors, for example.
 746         d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
 747         d->waypoints[i].turnExitPos = d->waypoints[i].pos;
 748       }
 749     } else {
 750       // final waypt, fix up some data
 751       d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
 752       d->waypoints[i].turnExitPos = d->waypoints[i].pos;
 753     }
 754
 755     // now turn is computed, can resolve distances
 756     d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
 757   }
 758 }
 759
 760 SGGeodVec RoutePath::pathForIndex(int index) const
 761 {
 762   const WayptData& w(d->waypoints[index]);
 763   const std::string& ty(w.wpt->type());
 764   SGGeodVec r;
 765   if (index == 0) {
 766     // common case where we do want to show something for first waypoint
 767     if (ty == "runway") {
 768       FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
 769       r.push_back(rwy->geod());
 770       r.push_back(rwy->end());
 771     }
 772
 773     return r;
 774   }
 775
 776     if (d->waypoints[index].skipped) {
 777         return SGGeodVec();
 778     }
 779
 780   if (ty == "vectors") {
 781       // ideally we'd show a stippled line to connect the route?
 782     return SGGeodVec();
 783   }
 784
 785   if (ty== "hold") {
 786     return pathForHold((Hold*) d->waypoints[index].wpt.get());
 787   }
 788
 789   const WayptData& prev(d->previousValidWaypoint(index));
 790   prev.turnExitPath(r);
 791
 792   SGGeod from = prev.turnExitPos,
 793     to = w.turnEntryPos;
 794
 795   // compute rounding offset, we want to round towards the direction of travel
 796   // which depends on the east/west sign of the longitude change
 797   double lonDelta = to.getLongitudeDeg() - from.getLongitudeDeg();
 798   if (fabs(lonDelta) > 0.5) {
 799     interpolateGreatCircle(from, to, r);
 800   }
 801
 802   if (w.flyOver) {
 803     r.push_back(w.pos);
 804   } else {
 805     // flyBy
 806     w.turnEntryPath(r);
 807   }
 808
 809   if (ty == "runway") {
 810     // runways get an extra point, at the end. this is particularly
 811     // important so missed approach segments draw correctly
 812     FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
 813     r.push_back(rwy->end());
 814   }
 815
 816   return r;
 817 }
 818
 819 void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
 820 {
 821   SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
 822     gcTo = SGGeoc::fromGeod(aTo);
 823
 824   double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
 825   if (fabs(lonDelta) < 1e-3) {
 826     return;
 827   }
 828
 829   lonDelta = SGMiscd::normalizeAngle(lonDelta);
 830   int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
 831   double lonStep = (lonDelta / steps);
 832
 833   double lon = gcFrom.getLongitudeRad() + lonStep;
 834   for (int s=0; s < (steps - 1); ++s) {
 835     lon = SGMiscd::normalizeAngle(lon);
 836     double lat = latitudeForGCLongitude(gcFrom, gcTo, lon);
 837     r.push_back(SGGeod::fromGeoc(SGGeoc::fromRadM(lon, lat, SGGeodesy::EQURAD)));
 838     //SG_LOG(SG_GENERAL, SG_INFO, "lon:" << lon * SG_RADIANS_TO_DEGREES << " gives lat " << lat * SG_RADIANS_TO_DEGREES);
 839     lon += lonStep;
 840   }
 841 }
 842
 843 SGGeod RoutePath::positionForIndex(int index) const
 844 {
 845   return d->waypoints[index].pos;
 846 }
 847
 848 SGGeodVec RoutePath::pathForHold(Hold* hold) const
 849 {
 850   int turnSteps = 16;
 851   double hdg = hold->inboundRadial();
 852   double turnDelta = 180.0 / turnSteps;
 853   double altFt = 0.0; // FIXME
 854   double gsKts = d->groundSpeedForAltitude(altFt);
 855
 856   SGGeodVec r;
 857   double az2;
 858   double stepTime = turnDelta / _pathTurnRate; // in seconds
 859   double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
 860   double legDist = hold->isDistance() ?
 861     hold->timeOrDistance()
 862     : gsKts * (hold->timeOrDistance() / 3600.0);
 863   legDist *= SG_NM_TO_METER;
 864
 865   if (hold->isLeftHanded()) {
 866     turnDelta = -turnDelta;
 867   }
 868   SGGeod pos = hold->position();
 869   r.push_back(pos);
 870
 871   // turn+leg sides are a mirror
 872   for (int j=0; j < 2; ++j) {
 873   // turn
 874     for (int i=0;i<turnSteps; ++i) {
 875       hdg += turnDelta;
 876       SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
 877       r.push_back(pos);
 878     }
 879
 880   // leg
 881     SGGeodesy::direct(pos, hdg, legDist, pos, az2);
 882     r.push_back(pos);
 883   } // of leg+turn duplication
 884
 885   return r;
 886 }
 887
 888 double RoutePath::computeDistanceForIndex(int index) const
 889 {
 890   if ((index < 0) || (index >= (int) d->waypoints.size())) {
 891     throw sg_range_exception("waypt index out of range",
 892                              "RoutePath::computeDistanceForIndex");
 893   }
 894
 895   if (index == 0) {
 896     // first waypoint, distance is 0
 897     return 0.0;
 898   }
 899
 900     if (d->waypoints[index].skipped) {
 901         return 0.0;
 902     }
 903
 904     const WayptData& prev(d->previousValidWaypoint(index));
 905   double dist = SGGeodesy::distanceM(prev.turnExitPos,
 906                               d->waypoints[index].turnEntryPos);
 907   dist += prev.turnDistanceM();
 908
 909   if (!d->waypoints[index].flyOver) {
 910     // add entry distance
 911     dist += d->waypoints[index].turnDistanceM();
 912   }
 913
 914   return dist;
 915 }
 916
 917 double RoutePath::trackForIndex(int index) const
 918 {
 919     if (d->waypoints[index].skipped)
 920         return trackForIndex(index - 1);
 921   return d->waypoints[index].legCourseTrue;
 922 }
 923
 924 double RoutePath::distanceForIndex(int index) const
 925 {
 926   return d->waypoints[index].pathDistanceM;
 927 }
 928
 929 double RoutePath::distanceBetweenIndices(int from, int to) const
 930 {
 931   return d->distanceBetweenIndices(from, to);
 932 }
 933
 934 SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
 935 {
 936     int sz = (int) d->waypoints.size();
 937     if (index < 0) {
 938         index = sz - 1; // map negative values to end of the route
 939     }
 940
 941     if ((index < 0) || (index >= sz)) {
 942         throw sg_range_exception("waypt index out of range",
 943                                  "RoutePath::positionForDistanceFrom");
 944     }
 945
 946     // find the actual leg we're within
 947     if (distanceM < 0.0) {
 948         // scan backwards
 949         while ((index > 0) && (distanceM < 0.0)) {
 950             --index;
 951             distanceM += d->waypoints[index].pathDistanceM;
 952         }
 953
 954         if (distanceM < 0.0) {
 955             // still negative, return route start
 956             return d->waypoints[0].pos;
 957         }
 958
 959     } else {
 960         // scan forwards
 961         int nextIndex = index + 1;
 962         while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
 963             distanceM -= d->waypoints[nextIndex].pathDistanceM;
 964             index = nextIndex++;
 965         }
 966     }
 967
 968     if ((index + 1) >= sz) {
 969         // past route end, just return final position
 970         return d->waypoints[sz - 1].pos;
 971     }
 972
 973     const WayptData& wpt(d->waypoints[index]);
 974     const WayptData& next(d->waypoints[index+1]);
 975
 976     if (wpt.turnPathDistanceM > distanceM) {
 977         // on the exit path of current wpt
 978         return wpt.pointAlongExitPath(distanceM);
 979     } else {
 980         distanceM -= wpt.turnPathDistanceM;
 981     }
 982
 983     double corePathDistance = next.pathDistanceM - next.turnPathDistanceM;
 984     if (next.hasEntry && (distanceM > corePathDistance)) {
 985         // on the entry path of next waypoint
 986         return next.pointAlongEntryPath(distanceM - corePathDistance);
 987     }
 988
 989     // linear between turn exit and turn entry points
 990     return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
 991 }