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 SGGeod turnCenterOverflight(const SGGeod& pt, double inHeadingDeg,
  82                            double turnAngleDeg, double turnRadiusM)
  83 {
  84     double p = copysign(90.0, turnAngleDeg);
  85     return SGGeodesy::direct(pt, inHeadingDeg + p, turnRadiusM);
  86 }
  87
  88 SGGeod turnCenterFlyBy(const SGGeod& pt, double inHeadingDeg,
  89                            double turnAngleDeg, double turnRadiusM)
  90 {
  91     double halfAngle = turnAngleDeg * 0.5;
  92     double turnCenterOffset = turnRadiusM / cos(halfAngle * SG_DEGREES_TO_RADIANS);
  93     double p = copysign(90.0, turnAngleDeg);
  94     return SGGeodesy::direct(pt, inHeadingDeg + halfAngle + p, turnCenterOffset);
  95 }
  96
  97 SGGeod turnCenterFromExit(const SGGeod& pt, double outHeadingDeg,
  98                        double turnAngleDeg, double turnRadiusM)
  99 {
 100     double p = copysign(90.0, turnAngleDeg);
 101     return SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);
 102 }
 103
 104 struct TurnInfo
 105 {
 106     TurnInfo() : valid(false),
 107     inboundCourseDeg(0.0),
 108     turnAngleDeg(0.0) { }
 109
 110     bool valid;
 111     SGGeod turnCenter;
 112     double inboundCourseDeg;
 113     double turnAngleDeg;
 114 };
 115
 116 /**
 117  * given a turn exit position and heading, and an arbitrary origin postion,
 118  * compute the turn center / angle the matches. Certain configurations may
 119  * fail, especially if the origin is less than two turn radii from the exit pos.
 120  */
 121 TurnInfo turnCenterAndAngleFromExit(const SGGeod& pt, double outHeadingDeg,
 122                                 double turnRadiusM, const SGGeod&origin)
 123 {
 124     double bearingToExit = SGGeodesy::courseDeg(origin, pt);
 125     // not the final turn angle, but we need to know which side of the
 126     // exit course we're on, to decide if it's a left-hand or right-hand turn
 127     double turnAngle = outHeadingDeg - bearingToExit;
 128     SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0);
 129     double p = copysign(90.0, turnAngle);
 130
 131     TurnInfo r;
 132     r.turnCenter = SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);
 133
 134     double courseToTC, distanceToTC, az2;
 135     SGGeodesy::inverse(origin, r.turnCenter, courseToTC, az2, distanceToTC);
 136     if (distanceToTC < turnRadiusM) {
 137         SG_LOG(SG_NAVAID, SG_WARN, "turnCenterAndAngleFromExit: origin point to close to turn center");
 138         return r;
 139     }
 140
 141     // find additional course angle away from the exit pos to intersect
 142     // the turn circle.
 143     double theta = asin(turnRadiusM / distanceToTC) * SG_RADIANS_TO_DEGREES;
 144     // invert angle sign so we increase the turn angle
 145     theta = -copysign(theta, turnAngle);
 146
 147     r.inboundCourseDeg = courseToTC + theta;
 148     SG_NORMALIZE_RANGE(r.inboundCourseDeg, 0.0, 360.0);
 149
 150     // turn angle must have same direction (sign) as turnAngle above, even if
 151     // the turn radius means the sign would cross over (happens if origin point
 152     // is close by
 153     r.turnAngleDeg = outHeadingDeg - r.inboundCourseDeg;
 154     if (r.turnAngleDeg > 0.0) {
 155         if (turnAngle < 0.0) r.turnAngleDeg -= 360.0;
 156     } else {
 157         if (turnAngle > 0.0) r.turnAngleDeg += 360.0;
 158     }
 159
 160     r.valid = true;
 161     return r;
 162 }
 163
 164 class WayptData
 165 {
 166 public:
 167   explicit WayptData(WayptRef w) :
 168     wpt(w),
 169     hasEntry(false),
 170     posValid(false),
 171     legCourseValid(false),
 172     skipped(false),
 173     turnEntryAngle(0.0),
 174     turnExitAngle(0.0),
 175     turnRadius(0.0),
 176     pathDistanceM(0.0),
 177     turnPathDistanceM(0.0),
 178     overflightCompensationAngle(0.0),
 179     flyOver(w->flag(WPT_OVERFLIGHT))
 180   {
 181   }
 182
 183   void initPass0()
 184   {
 185     const std::string& ty(wpt->type());
 186     if (wpt->flag(WPT_DYNAMIC)) {
 187       // presumption is that we always overfly such a waypoint
 188       if (ty == "hdgToAlt") {
 189         flyOver = true;
 190       }
 191     } else {
 192       pos = wpt->position();
 193       posValid = true;
 194
 195       if (ty == "hold") {
 196         legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
 197         legCourseValid = true;
 198       }
 199     } // of static waypt
 200   }
 201
 202   /**
 203    * test if course of this leg can be adjusted or is contrained to an exact value
 204    */
 205   bool isCourseConstrained() const
 206   {
 207     const std::string& ty(wpt->type());
 208     return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept");
 209   }
 210
 211   // compute leg courses for all static legs (both ends are fixed)
 212   void initPass1(const WayptData& previous, WayptData* next)
 213   {
 214     if (wpt->type() == "vectors") {
 215       // relying on the fact vectors will be followed by a static fix/wpt
 216       if (next && next->posValid) {
 217         posValid = true;
 218         pos = next->pos;
 219       }
 220     }
 221
 222     if (posValid && !legCourseValid && previous.posValid) {
 223     // check for duplicate points now
 224       if (previous.wpt->matches(wpt)) {
 225           skipped = true;
 226       }
 227
 228       // we can compute leg course now
 229         if (previous.wpt->type() == "runway") {
 230             // use the runway departure end pos
 231             FGRunway* rwy = static_cast<RunwayWaypt*>(previous.wpt.get())->runway();
 232             legCourseTrue = SGGeodesy::courseDeg(rwy->end(), pos);
 233         } else if (wpt->type() != "runway") {
 234             // need to wait to compute runway leg course
 235             legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
 236             legCourseValid = true;
 237         }
 238     }
 239   }
 240
 241   void computeLegCourse(const WayptData& previous, double radiusM)
 242   {
 243       if (legCourseValid) {
 244           return;
 245       }
 246
 247       if (wpt->type() == "hold") {
 248
 249       } else if (wpt->type() == "runway") {
 250           FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 251           flyOver = true;
 252
 253           TurnInfo ti = turnCenterAndAngleFromExit(rwy->threshold(),
 254                                                    rwy->headingDeg(),
 255                                                    radiusM, previous.pos);
 256           if (ti.valid) {
 257               legCourseTrue = ti.inboundCourseDeg;
 258               turnEntryAngle = ti.turnAngleDeg;
 259               turnEntryCenter = ti.turnCenter;
 260               turnRadius = radiusM;
 261               hasEntry = true;
 262               turnEntryPos = pointOnEntryTurnFromHeading(ti.inboundCourseDeg);
 263           } else {
 264               // couldn't compute entry, never mind
 265               legCourseTrue = SGGeodesy::courseDeg(previous.pos, rwy->threshold());
 266           }
 267           legCourseValid = true;
 268       } else {
 269           if (posValid) {
 270               legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
 271               legCourseValid = true;
 272           } else if (isCourseConstrained()) {
 273               double magVar = magVarFor(previous.pos);
 274               legCourseTrue = wpt->headingRadialDeg() + magVar;
 275               legCourseValid = true;
 276           } // of pos not valid
 277       } // of neither hold nor runway
 278   }
 279
 280     SGGeod pointOnEntryTurnFromHeading(double headingDeg) const
 281     {
 282         assert(hasEntry);
 283         double p = copysign(90.0, turnEntryAngle);
 284         return SGGeodesy::direct(turnEntryCenter, headingDeg - p, turnRadius);
 285     }
 286
 287     SGGeod pointOnExitTurnFromHeading(double headingDeg) const
 288     {
 289         double p = copysign(90.0, turnExitAngle);
 290         return SGGeodesy::direct(turnExitCenter, headingDeg - p, turnRadius);
 291     }
 292
 293     double pathDistanceForTurnAngle(double angleDeg) const
 294     {
 295         return turnRadius * fabs(angleDeg) * SG_DEGREES_TO_RADIANS;
 296     }
 297
 298   void computeTurn(double radiusM, bool constrainLegCourse, WayptData& next)
 299   {
 300     assert(!skipped);
 301     assert(next.legCourseValid);
 302       bool isRunway = (wpt->type() == "runway");
 303
 304       if (legCourseValid) {
 305           if (isRunway) {
 306               FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 307               turnExitAngle = next.legCourseTrue - rwy->headingDeg();
 308           } else {
 309               turnExitAngle = next.legCourseTrue - legCourseTrue;
 310           }
 311       } else {
 312           // happens for first leg
 313           legCourseValid = true;
 314           if (isRunway) {
 315               FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 316               turnExitAngle = next.legCourseTrue - rwy->headingDeg();
 317               legCourseTrue = rwy->headingDeg();
 318               flyOver = true;
 319           } else {
 320               turnExitAngle = 0.0;
 321           }
 322       }
 323
 324     SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
 325     turnRadius = radiusM;
 326
 327     if (!flyOver && fabs(turnExitAngle) > 120.0) {
 328         // flyBy logic blows up for sharp turns - due to the tan() term
 329         // heading towards infinity. By converting to flyOver we do something
 330         // closer to what was requested.
 331         flyOver = true;
 332     }
 333
 334     if (flyOver) {
 335         if (isRunway) {
 336             FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 337             turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius);
 338         } else {
 339             turnEntryPos = pos;
 340             turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);
 341
 342         }
 343       turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
 344
 345       if (!next.wpt->flag(WPT_DYNAMIC)) {
 346         // distance perpendicular to next leg course, after turning
 347         // through turnAngle
 348         double xtk = turnRadius * (1 - cos(turnExitAngle * SG_DEGREES_TO_RADIANS));
 349
 350         if (constrainLegCourse || next.isCourseConstrained()) {
 351           // next leg course is constrained. We need to swing back onto the
 352           // desired course, using a compensation turn
 353
 354           // compensation angle to turn back on course
 355           double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
 356           theta = copysign(theta, turnExitAngle);
 357           turnExitAngle += theta;
 358
 359           // move by the distance to compensate
 360           double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS);
 361           turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourseTrue, d);
 362           overflightCompensationAngle = -theta;
 363
 364             // sign of angles will differ, so compute distances seperately
 365             turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle) +
 366                 pathDistanceForTurnAngle(overflightCompensationAngle);
 367         } else {
 368           // next leg course can be adjusted. increase the turn angle
 369           // and modify the next leg's course accordingly.
 370
 371           // hypotenuse of triangle, opposite edge has length turnRadius
 372           double distAlongPath = std::min(1.0, sin(fabs(turnExitAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius;
 373           double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - distAlongPath;
 374           double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES;
 375           increaseAngle = copysign(increaseAngle, turnExitAngle);
 376
 377           turnExitAngle += increaseAngle;
 378           turnExitPos = pointOnExitTurnFromHeading(legCourseTrue + turnExitAngle);
 379           // modify next leg course
 380           next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
 381           turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
 382         } // of next leg isn't course constrained
 383       } else {
 384           // next point is dynamic
 385           // no compensation needed
 386           turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
 387       }
 388     } else {
 389       hasEntry = true;
 390         turnEntryCenter = turnCenterFlyBy(pos, legCourseTrue, turnExitAngle, turnRadius);
 391
 392         turnExitAngle = turnExitAngle * 0.5;
 393         turnEntryAngle = turnExitAngle;
 394         turnExitCenter = turnEntryCenter; // important that these match
 395
 396       turnEntryPos = pointOnEntryTurnFromHeading(legCourseTrue);
 397       turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
 398       turnPathDistanceM = pathDistanceForTurnAngle(turnEntryAngle);
 399     }
 400   }
 401
 402   double turnDistanceM() const
 403   {
 404       return turnPathDistanceM;
 405   }
 406
 407   void turnEntryPath(SGGeodVec& path) const
 408   {
 409     if (!hasEntry || fabs(turnEntryAngle) < 0.5 ) {
 410       path.push_back(pos);
 411       return;
 412     }
 413
 414     int steps = std::max(SGMiscd::roundToInt(fabs(turnEntryAngle) / 3.0), 1);
 415     double stepIncrement = turnEntryAngle / steps;
 416     double h = legCourseTrue;
 417     for (int s=0; s<steps; ++s) {
 418         path.push_back(pointOnEntryTurnFromHeading(h));
 419         h += stepIncrement;
 420     }
 421   }
 422
 423   void turnExitPath(SGGeodVec& path) const
 424   {
 425     if (fabs(turnExitAngle) < 0.5) {
 426       path.push_back(turnExitPos);
 427       return;
 428     }
 429
 430     int steps = std::max(SGMiscd::roundToInt(fabs(turnExitAngle) / 3.0), 1);
 431     double stepIncrement = turnExitAngle / steps;
 432
 433     // initial exit heading
 434       double h = legCourseTrue + (flyOver ? 0.0 : turnEntryAngle);
 435       if (wpt->type() == "runway") {
 436           FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
 437           h = rwy->headingDeg();
 438       }
 439
 440     for (int s=0; s<steps; ++s) {
 441         path.push_back(pointOnExitTurnFromHeading(h));
 442         h += stepIncrement;
 443     }
 444
 445     // we can use an exact check on the compensation angle, because we
 446     // initialise it directly. Depending on the next element we might be
 447     // doing compensation or adjusting the next leg's course; if we did
 448     // adjust the course everything 'just works' above.
 449
 450     if (flyOver && (overflightCompensationAngle != 0.0)) {
 451       // skew by compensation angle back
 452       steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
 453
 454       // step in opposite direction to the turn angle to swing back onto
 455       // the next leg course
 456       stepIncrement = overflightCompensationAngle / steps;
 457
 458         SGGeod p = path.back();
 459         double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
 460
 461       for (int s=0; s<steps; ++s) {
 462           h += stepIncrement;
 463         p = SGGeodesy::direct(p, h, stepDist);
 464           path.push_back(p);
 465
 466       }
 467     } // of overflight compensation turn
 468   }
 469
 470   SGGeod pointAlongExitPath(double distanceM) const
 471   {
 472       double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
 473       double p = copysign(90, turnExitAngle);
 474
 475       if (flyOver && (overflightCompensationAngle != 0.0)) {
 476           // figure out if we're in the compensation section
 477           if (theta > turnExitAngle) {
 478               // compute the compensation turn center - twice the turn radius
 479               // from turnCenter
 480               SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
 481                                              legCourseTrue - overflightCompensationAngle - p,
 482                                              turnRadius * 2.0);
 483               theta = copysign(theta - turnExitAngle, overflightCompensationAngle);
 484               return SGGeodesy::direct(tc2,
 485                                        legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
 486           }
 487       }
 488
 489       theta = copysign(theta, turnExitAngle);
 490       double inboundCourse = legCourseTrue + (flyOver ? 0.0 : turnExitAngle);
 491       return pointOnExitTurnFromHeading(inboundCourse + theta);
 492   }
 493
 494   SGGeod pointAlongEntryPath(double distanceM) const
 495   {
 496     assert(hasEntry);
 497     double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
 498       theta = copysign(theta, turnEntryAngle);
 499       return pointOnEntryTurnFromHeading(legCourseTrue + theta);
 500   }
 501
 502   WayptRef wpt;
 503   bool hasEntry, posValid, legCourseValid, skipped;
 504   SGGeod pos, turnEntryPos, turnExitPos, turnEntryCenter, turnExitCenter;
 505   double turnEntryAngle, turnExitAngle, turnRadius, legCourseTrue;
 506   double pathDistanceM;
 507   double turnPathDistanceM; // for flyBy, this is half the distance; for flyOver it's the complete distance
 508   double overflightCompensationAngle;
 509   bool flyOver;
 510 };
 511
 512 typedef std::vector<WayptData> WayptDataVec;
 513
 514 class PerformanceBracket
 515 {
 516 public:
 517   PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
 518     atOrBelowAltitudeFt(atOrBelow),
 519     climbRateFPM(climb),
 520     descentRateFPM(descent),
 521     speedIASOrMach(speed),
 522     speedIsMach(isMach)
 523   { }
 524
 525   double atOrBelowAltitudeFt;
 526   double climbRateFPM;
 527   double descentRateFPM;
 528   double speedIASOrMach;
 529   bool speedIsMach;
 530 };
 531
 532 typedef std::vector<PerformanceBracket> PerformanceBracketVec;
 533
 534 bool isDescentWaypoint(const WayptRef& wpt)
 535 {
 536   return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
 537 }
 538
 539 class RoutePath::RoutePathPrivate
 540 {
 541 public:
 542     WayptDataVec waypoints;
 543
 544     char aircraftCategory;
 545     PerformanceBracketVec perf;
 546     double pathTurnRate;
 547     bool constrainLegCourses;
 548
 549   PerformanceBracketVec::const_iterator
 550   findPerformanceBracket(double altFt) const
 551   {
 552     PerformanceBracketVec::const_iterator r;
 553     PerformanceBracketVec::const_iterator result = perf.begin();
 554
 555     for (r = perf.begin(); r != perf.end(); ++r) {
 556       if (r->atOrBelowAltitudeFt > altFt) {
 557         break;
 558       }
 559
 560       result = r;
 561     } // of brackets iteration
 562
 563     return result;
 564   }
 565
 566   void computeDynamicPosition(int index)
 567   {
 568     const WayptData& previous(previousValidWaypoint(index));
 569     WayptRef wpt = waypoints[index].wpt;
 570     assert(previous.posValid);
 571
 572     const std::string& ty(wpt->type());
 573     if (ty == "hdgToAlt") {
 574       HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
 575
 576       double altFt = computeVNAVAltitudeFt(index - 1);
 577       double altChange = h->altitudeFt() - altFt;
 578       PerformanceBracketVec::const_iterator it = findPerformanceBracket(altFt);
 579       double speedMSec = groundSpeedForAltitude(altFt) * SG_KT_TO_MPS;
 580       double timeToChangeSec;
 581
 582       if (isDescentWaypoint(wpt)) {
 583         timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
 584       } else {
 585         timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
 586       }
 587
 588       double distanceM = timeToChangeSec * speedMSec;
 589       double hdg = h->headingDegMagnetic() + magVarFor(previous.pos);
 590       waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM);
 591       waypoints[index].posValid = true;
 592     } else if (ty == "radialIntercept") {
 593       // start from previous.turnExit
 594       RadialIntercept* i = (RadialIntercept*) wpt.get();
 595
 596       SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
 597       SGGeoc navid = SGGeoc::fromGeod(wpt->position());
 598       SGGeoc rGc;
 599       double magVar = magVarFor(previous.pos);
 600
 601       double radial = i->radialDegMagnetic() + magVar;
 602       double track = i->courseDegMagnetic() + magVar;
 603       bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
 604       if (!ok) {
 605         SGGeoc navidAdjusted;
 606         // try pulling backward along the radial in case we're too close.
 607         // suggests bad procedure construction if this is happening!
 608         SGGeodesy::advanceRadM(navid, radial, SG_NM_TO_METER * -10, navidAdjusted);
 609
 610         // try again
 611         ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
 612         if (!ok) {
 613           SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
 614                << previous.turnExitPos << " / " << track << "/" << wpt->position()
 615                << "/" << radial);
 616           waypoints[index].pos = wpt->position(); // horrible fallback
 617
 618         } else {
 619           waypoints[index].pos = SGGeod::fromGeoc(rGc);
 620         }
 621       } else {
 622         waypoints[index].pos = SGGeod::fromGeoc(rGc);
 623       }
 624
 625       waypoints[index].posValid = true;
 626     } else if (ty == "dmeIntercept") {
 627       DMEIntercept* di = (DMEIntercept*) wpt.get();
 628
 629       SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
 630       SGGeoc navid = SGGeoc::fromGeod(wpt->position());
 631       double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
 632       SGGeoc rGc;
 633
 634       SGGeoc bPt;
 635       double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
 636       SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
 637
 638       double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
 639       if (dNm < 0.0) {
 640         SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
 641         waypoints[index].pos = wpt->position(); // horrible fallback
 642       } else {
 643         waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER);
 644       }
 645
 646       waypoints[index].posValid = true;
 647     } else if (ty == "vectors") {
 648       waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos);
 649       waypoints[index].legCourseValid = true;
 650       // no turn data
 651     }
 652   }
 653
 654   double computeVNAVAltitudeFt(int index)
 655   {
 656     WayptRef w = waypoints[index].wpt;
 657     if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
 658       // descent
 659       int next = findNextKnownAltitude(index);
 660       if (next < 0) {
 661         return 0.0;
 662       }
 663
 664       double fixedAlt = altitudeForIndex(next);
 665       double distanceM = distanceBetweenIndices(index, next);
 666       double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
 667       double minutes = (distanceM / speedMSec) / 60.0;
 668
 669       PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
 670       return fixedAlt + (it->descentRateFPM * minutes);
 671
 672     } else {
 673       // climb
 674       int prev = findPreceedingKnownAltitude(index);
 675       if (prev < 0) {
 676         return 0.0;
 677       }
 678
 679       double fixedAlt = altitudeForIndex(prev);
 680       double distanceM = distanceBetweenIndices(prev, index);
 681       double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
 682       double minutes = (distanceM / speedMSec) / 60.0;
 683
 684       PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
 685       return fixedAlt + (it->climbRateFPM * minutes);
 686     }
 687
 688   }
 689
 690   int findPreceedingKnownAltitude(int index) const
 691   {
 692     const WayptData& w(waypoints[index]);
 693     if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
 694       return index;
 695     }
 696
 697     // principal base case is runways.
 698     const std::string& ty(w.wpt->type());
 699     if (ty == "runway") {
 700       return index; // runway always has a known elevation
 701     }
 702
 703     if (index == 0) {
 704       SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceding altitude value found");
 705       return -1;
 706     }
 707
 708     // recurse earlier in the route
 709     return findPreceedingKnownAltitude(index - 1);
 710   }
 711
 712   int findNextKnownAltitude(unsigned int index) const
 713   {
 714     if (index >= waypoints.size()) {
 715       SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
 716       return -1;
 717     }
 718
 719     const WayptData& w(waypoints[index]);
 720     if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
 721       return index;
 722     }
 723
 724     // principal base case is runways.
 725     const std::string& ty(w.wpt->type());
 726     if (ty == "runway") {
 727       return index; // runway always has a known elevation
 728     }
 729
 730     if (index == waypoints.size() - 1) {
 731       SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
 732       return -1;
 733     }
 734
 735     return findNextKnownAltitude(index + 1);
 736   }
 737
 738   double altitudeForIndex(int index) const
 739   {
 740     const WayptData& w(waypoints[index]);
 741     if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
 742       return w.wpt->altitudeFt();
 743     }
 744
 745     const std::string& ty(w.wpt->type());
 746     if (ty == "runway") {
 747       FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
 748       return rwy->threshold().getElevationFt();
 749     }
 750
 751     SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
 752     return 0.0;
 753   }
 754
 755   double groundSpeedForAltitude(double altitude) const
 756   {
 757       PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
 758       if (it->speedIsMach) {
 759           return 300.0;
 760       } else {
 761           // FIXME - convert IAS to ground-speed, using standard atmosphere / temperature model
 762           return it->speedIASOrMach;
 763       }
 764
 765 #if 0
 766     if (0) {
 767       double mach;
 768
 769       if (it->speedIsMach) {
 770         mach = it->speedIASOrMach; // easy
 771       } else {
 772         const double Cs_0 = 661.4786; // speed of sound at sea level, knots
 773         const double P_0 = 29.92126;
 774         const double P = P_0 * pow(, );
 775         // convert IAS (which we will treat as CAS) to Mach based on altitude
 776       }
 777
 778       double oatK;
 779       double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
 780
 781       double tas = mach * Cs;
 782
 783 /*
 784       P_0= 29.92126 "Hg = 1013.25 mB = 2116.2166 lbs/ft^2
 785       P= P_0*(1-6.8755856*10^-6*PA)^5.2558797, pressure altitude, PA<36,089.24ft
 786       CS= 38.967854*sqrt(T+273.15)  where T is the (static/true) OAT in Celsius.
 787
 788       DP=P_0*((1 + 0.2*(IAS/CS_0)^2)^3.5 -1)
 789       M=(5*( (DP/P + 1)^(2/7) -1) )^0.5   (*)
 790       TAS= M*CS
 791 */
 792     }
 793 #endif
 794   }
 795
 796   double distanceBetweenIndices(int from, int to) const
 797   {
 798     double total = 0.0;
 799
 800     for (int i=from+1; i<= to; ++i) {
 801       total += waypoints[i].pathDistanceM;
 802     }
 803
 804     return total;
 805   }
 806
 807   void initPerfData()
 808   {
 809       pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn
 810       switch (aircraftCategory) {
 811       case ICAO_AIRCRAFT_CATEGORY_A:
 812           perf.push_back(PerformanceBracket(4000, 600, 1200, 75));
 813           perf.push_back(PerformanceBracket(10000, 600, 1200, 140));
 814           break;
 815
 816       case ICAO_AIRCRAFT_CATEGORY_B:
 817           perf.push_back(PerformanceBracket(4000, 100, 1200, 100));
 818           perf.push_back(PerformanceBracket(10000, 800, 1200, 160));
 819           perf.push_back(PerformanceBracket(18000, 600, 1800, 200));
 820           break;
 821
 822       case ICAO_AIRCRAFT_CATEGORY_C:
 823           perf.push_back(PerformanceBracket(4000, 1800, 1800, 150));
 824           perf.push_back(PerformanceBracket(10000, 1800, 1800, 200));
 825           perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
 826           perf.push_back(PerformanceBracket(60000, 800, 1200, 0.80, true /* is Mach */));
 827           break;
 828
 829       case ICAO_AIRCRAFT_CATEGORY_D:
 830       case ICAO_AIRCRAFT_CATEGORY_E:
 831       default:
 832
 833           perf.push_back(PerformanceBracket(4000, 1800, 1800, 180));
 834           perf.push_back(PerformanceBracket(10000, 1800, 1800, 230));
 835           perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
 836           perf.push_back(PerformanceBracket(60000, 800, 1200, 0.87, true /* is Mach */));
 837           break;
 838       }
 839   }
 840
 841     const WayptData& previousValidWaypoint(int index) const
 842     {
 843         assert(index > 0);
 844         if (waypoints[index-1].skipped) {
 845             return waypoints[index-2];
 846         }
 847
 848         return waypoints[index-1];
 849     }
 850
 851 }; // of RoutePathPrivate class
 852
 853 RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
 854   d(new RoutePathPrivate)
 855 {
 856     for (int l=0; l<fp->numLegs(); ++l) {
 857         d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint()));
 858     }
 859
 860     d->aircraftCategory = fp->icaoAircraftCategory()[0];
 861     d->constrainLegCourses = fp->followLegTrackToFixes();
 862     commonInit();
 863 }
 864
 865 RoutePath::~RoutePath()
 866 {
 867 }
 868
 869 void RoutePath::commonInit()
 870 {
 871   d->initPerfData();
 872
 873   WayptDataVec::iterator it;
 874   for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
 875     it->initPass0();
 876   }
 877
 878   for (unsigned int i=1; i<d->waypoints.size(); ++i) {
 879     WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : 0;
 880     d->waypoints[i].initPass1(d->waypoints[i-1], nextPtr);
 881   }
 882
 883   for (unsigned int i=0; i<d->waypoints.size(); ++i) {
 884       if (d->waypoints[i].skipped) {
 885           continue;
 886       }
 887
 888       double alt = 0.0; // FIXME
 889       double gs = d->groundSpeedForAltitude(alt);
 890       double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();
 891
 892       if (i > 0) {
 893           const WayptData& prev(d->previousValidWaypoint(i));
 894           d->waypoints[i].computeLegCourse(prev, radiusM);
 895           d->computeDynamicPosition(i);
 896       }
 897
 898     if (i < (d->waypoints.size() - 1)) {
 899         int nextIndex = i + 1;
 900         if (d->waypoints[nextIndex].skipped) {
 901             nextIndex++;
 902         }
 903         WayptData& next(d->waypoints[nextIndex]);
 904         next.computeLegCourse(d->waypoints[i], radiusM);
 905
 906       if (next.legCourseValid) {
 907         d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, next);
 908       } else {
 909         // next waypoint has indeterminate course. Let's create a sharp turn
 910         // this can happen when the following point is ATC vectors, for example.
 911         d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
 912         d->waypoints[i].turnExitPos = d->waypoints[i].pos;
 913       }
 914     } else {
 915       // final waypt, fix up some data
 916       d->waypoints[i].turnExitPos = d->waypoints[i].pos;
 917     }
 918
 919     // now turn is computed, can resolve distances
 920     d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
 921   }
 922 }
 923
 924 SGGeodVec RoutePath::pathForIndex(int index) const
 925 {
 926   const WayptData& w(d->waypoints[index]);
 927   const std::string& ty(w.wpt->type());
 928   SGGeodVec r;
 929
 930     if (d->waypoints[index].skipped) {
 931         return SGGeodVec();
 932     }
 933
 934   if (ty == "vectors") {
 935       // ideally we'd show a stippled line to connect the route?
 936     return SGGeodVec();
 937   }
 938
 939   if (ty== "hold") {
 940     return pathForHold((Hold*) d->waypoints[index].wpt.get());
 941   }
 942
 943     if (index > 0) {
 944       const WayptData& prev(d->previousValidWaypoint(index));
 945       prev.turnExitPath(r);
 946
 947       SGGeod from = prev.turnExitPos,
 948         to = w.turnEntryPos;
 949
 950       // compute rounding offset, we want to round towards the direction of travel
 951       // which depends on the east/west sign of the longitude change
 952       double lonDelta = to.getLongitudeDeg() - from.getLongitudeDeg();
 953       if (fabs(lonDelta) > 0.5) {
 954         interpolateGreatCircle(from, to, r);
 955       }
 956     } // of have previous waypoint
 957
 958     w.turnEntryPath(r);
 959
 960   if (ty == "runway") {
 961     // runways get an extra point, at the end. this is particularly
 962     // important so missed approach segments draw correctly
 963     FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
 964     r.push_back(rwy->end());
 965   }
 966
 967   return r;
 968 }
 969
 970 void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
 971 {
 972   SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
 973     gcTo = SGGeoc::fromGeod(aTo);
 974
 975   double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
 976   if (fabs(lonDelta) < 1e-3) {
 977     return;
 978   }
 979
 980   lonDelta = SGMiscd::normalizeAngle(lonDelta);
 981   int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
 982   double lonStep = (lonDelta / steps);
 983
 984   double lon = gcFrom.getLongitudeRad() + lonStep;
 985   for (int s=0; s < (steps - 1); ++s) {
 986     lon = SGMiscd::normalizeAngle(lon);
 987     double lat = latitudeForGCLongitude(gcFrom, gcTo, lon);
 988     r.push_back(SGGeod::fromGeoc(SGGeoc::fromRadM(lon, lat, SGGeodesy::EQURAD)));
 989     //SG_LOG(SG_GENERAL, SG_INFO, "lon:" << lon * SG_RADIANS_TO_DEGREES << " gives lat " << lat * SG_RADIANS_TO_DEGREES);
 990     lon += lonStep;
 991   }
 992 }
 993
 994 SGGeod RoutePath::positionForIndex(int index) const
 995 {
 996   return d->waypoints[index].pos;
 997 }
 998
 999 SGGeodVec RoutePath::pathForHold(Hold* hold) const
1000 {
1001   int turnSteps = 16;
1002   double hdg = hold->inboundRadial();
1003   double turnDelta = 180.0 / turnSteps;
1004   double altFt = 0.0; // FIXME
1005   double gsKts = d->groundSpeedForAltitude(altFt);
1006
1007   SGGeodVec r;
1008   double az2;
1009   double stepTime = turnDelta / d->pathTurnRate; // in seconds
1010   double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
1011   double legDist = hold->isDistance() ?
1012     hold->timeOrDistance()
1013     : gsKts * (hold->timeOrDistance() / 3600.0);
1014   legDist *= SG_NM_TO_METER;
1015
1016   if (hold->isLeftHanded()) {
1017     turnDelta = -turnDelta;
1018   }
1019   SGGeod pos = hold->position();
1020   r.push_back(pos);
1021
1022   // turn+leg sides are a mirror
1023   for (int j=0; j < 2; ++j) {
1024   // turn
1025     for (int i=0;i<turnSteps; ++i) {
1026       hdg += turnDelta;
1027       SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
1028       r.push_back(pos);
1029     }
1030
1031   // leg
1032     SGGeodesy::direct(pos, hdg, legDist, pos, az2);
1033     r.push_back(pos);
1034   } // of leg+turn duplication
1035
1036   return r;
1037 }
1038
1039 double RoutePath::computeDistanceForIndex(int index) const
1040 {
1041   if ((index < 0) || (index >= (int) d->waypoints.size())) {
1042     throw sg_range_exception("waypt index out of range",
1043                              "RoutePath::computeDistanceForIndex");
1044   }
1045
1046   if (index == 0) {
1047     // first waypoint, distance is 0
1048     return 0.0;
1049   }
1050
1051     if (d->waypoints[index].skipped) {
1052         return 0.0;
1053     }
1054
1055     const WayptData& prev(d->previousValidWaypoint(index));
1056   double dist = SGGeodesy::distanceM(prev.turnExitPos,
1057                               d->waypoints[index].turnEntryPos);
1058   dist += prev.turnDistanceM();
1059
1060   if (!d->waypoints[index].flyOver) {
1061     // add entry distance
1062     dist += d->waypoints[index].turnDistanceM();
1063   }
1064
1065   return dist;
1066 }
1067
1068 double RoutePath::trackForIndex(int index) const
1069 {
1070     if (d->waypoints[index].skipped)
1071         return trackForIndex(index - 1);
1072   return d->waypoints[index].legCourseTrue;
1073 }
1074
1075 double RoutePath::distanceForIndex(int index) const
1076 {
1077   return d->waypoints[index].pathDistanceM;
1078 }
1079
1080 double RoutePath::distanceBetweenIndices(int from, int to) const
1081 {
1082   return d->distanceBetweenIndices(from, to);
1083 }
1084
1085 SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
1086 {
1087     int sz = (int) d->waypoints.size();
1088     if (index < 0) {
1089         index = sz - 1; // map negative values to end of the route
1090     }
1091
1092     if ((index < 0) || (index >= sz)) {
1093         throw sg_range_exception("waypt index out of range",
1094                                  "RoutePath::positionForDistanceFrom");
1095     }
1096
1097     // find the actual leg we're within
1098     if (distanceM < 0.0) {
1099         // scan backwards
1100         while ((index > 0) && (distanceM < 0.0)) {
1101             // we are looking at index n, say 4, but with a negative distance.
1102             // we want to look at index n-1 (so, 3), and see if this makes
1103             // distance positive. We need to offset by distance from 3 -> 4,
1104             // which is waypoint 4's path distance.
1105             distanceM += d->waypoints[index].pathDistanceM;
1106             --index;
1107         }
1108
1109         if (distanceM < 0.0) {
1110             // still negative, return route start
1111             return d->waypoints[0].pos;
1112         }
1113
1114     } else {
1115         // scan forwards
1116         int nextIndex = index + 1;
1117         while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
1118             distanceM -= d->waypoints[nextIndex].pathDistanceM;
1119             index = nextIndex++;
1120         }
1121     }
1122
1123     if ((index + 1) >= sz) {
1124         // past route end, just return final position
1125         return d->waypoints[sz - 1].pos;
1126     }
1127
1128     const WayptData& wpt(d->waypoints[index]);
1129     const WayptData& next(d->waypoints[index+1]);
1130
1131     if (wpt.turnPathDistanceM > distanceM) {
1132         // on the exit path of current wpt
1133         return wpt.pointAlongExitPath(distanceM);
1134     } else {
1135         distanceM -= wpt.turnPathDistanceM;
1136     }
1137
1138     double corePathDistance = next.pathDistanceM - next.turnPathDistanceM;
1139     if (next.hasEntry && (distanceM > corePathDistance)) {
1140         // on the entry path of next waypoint
1141         return next.pointAlongEntryPath(distanceM - corePathDistance);
1142     }
1143
1144     // linear between turn exit and turn entry points
1145     return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
1146 }