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