6 #include <Navaids/routePath.hxx>
8 #include <simgear/structure/exception.hxx>
9 #include <simgear/magvar/magvar.hxx>
10 #include <simgear/timing/sg_time.hxx>
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>
18 namespace flightgear {
19 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
22 using namespace flightgear;
24 // implement Point(s) known distance from a great circle
26 static double sqr(const double x)
31 // http://williams.best.vwh.net/avform.htm#POINTDME
32 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
34 double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
35 double bDist = SGGeodesy::distanceRad(a, d);
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);
40 double p = atan2(sin(bDist)*cos(A), cos(bDist));
42 if (sqr(cos(dist)) > sqr(r)) {
43 SG_LOG(SG_NAVAID, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
47 double dp1 = p + acos(cos(dist)/r);
48 double dp2 = p - acos(cos(dist)/r);
50 double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
51 double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
53 return SGMiscd::min(dp1Nm, dp2Nm);
56 // http://williams.best.vwh.net/avform.htm#Int
57 double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
60 Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:
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)))
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);
75 static double magVarFor(const SGGeod& geod)
77 double jd = globals->get_time_params()->getJD();
78 return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
84 explicit WayptData(WayptRef w) :
88 legCourseValid(false),
93 turnPathDistanceM(0.0),
94 overflightCompensationAngle(0.0),
95 flyOver(w->flag(WPT_OVERFLIGHT))
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") {
108 pos = wpt->position();
111 if ((ty == "runway") || (ty == "hold")) {
112 legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
113 legCourseValid = true;
116 if (ty == "runway") {
117 FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
118 turnExitPos = rwy->end();
124 * test if course of this leg can be adjusted or is contrained to an exact value
126 bool isCourseConstrained() const
128 const std::string& ty(wpt->type());
129 return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept");
132 // compute leg courses for all static legs (both ends are fixed)
133 void initPass1(const WayptData& previous, WayptData* next)
135 if (wpt->type() == "vectors") {
136 // relying on the fact vectors will be followed by a static fix/wpt
137 if (next && next->posValid) {
143 if (posValid && !legCourseValid && previous.posValid) {
144 // check for duplicate points now
145 if (previous.wpt->matches(wpt)) {
149 // we can compute leg course now
150 legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
151 legCourseValid = true;
155 void computeLegCourse(const WayptData& previous)
157 if (!legCourseValid) {
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;
170 void computeTurn(double radiusM, const WayptData& previous, WayptData& next)
173 assert(legCourseValid && next.legCourseValid);
175 turnAngle = next.legCourseTrue - legCourseTrue;
176 SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0);
177 turnRadius = radiusM;
178 double p = copysign(90.0, turnAngle);
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.
189 turnCenter = SGGeodesy::direct(pos, legCourseTrue + p, turnRadius);
190 // use the leg course
191 turnExitPos = SGGeodesy::direct(turnCenter, next.legCourseTrue - p, turnRadius);
193 if (!next.wpt->flag(WPT_DYNAMIC)) {
194 // distance perpendicular to next leg course, after turning
196 double xtk = turnRadius * (1 - cos(turnAngle * SG_DEGREES_TO_RADIANS));
198 if (next.isCourseConstrained()) {
199 // next leg course is constrained. We need to swing back onto the
200 // desired course, using a compensation turn
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);
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;
212 turnPathDistanceM = turnRadius * (fabs(turnAngle) +
213 fabs(overflightCompensationAngle)) * SG_DEGREES_TO_RADIANS;
215 // next leg course can be adjusted. increase the turn angle
216 // and modify the next leg's course accordingly.
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);
224 turnAngle += increaseAngle;
225 turnExitPos = SGGeodesy::direct(turnCenter, legCourseTrue + turnAngle - p, turnRadius);
227 // modify next leg course
228 next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
230 turnPathDistanceM = turnRadius * (fabs(turnAngle) * SG_DEGREES_TO_RADIANS);
231 } // of next leg isn't course constrained
233 // next point is dynamic
234 // no compensation needed
235 turnPathDistanceM = turnRadius * (fabs(turnAngle) * SG_DEGREES_TO_RADIANS);
240 double halfAngle = turnAngle * 0.5;
242 double turnCenterOffset = turnRadius / cos(halfAngle * SG_DEGREES_TO_RADIANS);
243 turnCenter = SGGeodesy::direct(pos, legCourseTrue + halfAngle + p, turnCenterOffset);
245 double distAlongPath = turnRadius * tan(fabs(halfAngle) * SG_DEGREES_TO_RADIANS);
247 turnEntryPos = SGGeodesy::direct(pos, legCourseTrue, -distAlongPath);
248 turnExitPos = SGGeodesy::direct(pos, next.legCourseTrue, distAlongPath);
250 turnPathDistanceM = turnRadius * (fabs(halfAngle) * SG_DEGREES_TO_RADIANS);
254 double turnDistanceM() const
256 return turnPathDistanceM;
259 void turnEntryPath(SGGeodVec& path) const
262 if (fabs(turnAngle) < 0.5 ) {
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;
274 for (int s=0; s<steps; ++s) {
276 p = SGGeodesy::direct(p, h, stepDist);
283 void turnExitPath(SGGeodVec& path) const
285 if (fabs(turnAngle) < 0.5) {
286 path.push_back(turnExitPos);
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;
294 // initial exit heading
295 double h = legCourseTrue + (flyOver ? 0.0 : (turnAngle * 0.5));
296 double turnDirOffset = copysign(90.0, turnAngle);
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;
302 for (int s=0; s<steps; ++s) {
304 p = SGGeodesy::direct(p, h, stepDist);
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.
313 if (flyOver && (overflightCompensationAngle != 0.0)) {
314 // skew by compensation angle back
315 steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
317 // step in opposite direction to the turn angle to swing back onto
318 // the next leg course
319 stepIncrement = overflightCompensationAngle / steps;
321 for (int s=0; s<steps; ++s) {
323 p = SGGeodesy::direct(p, h, stepDist);
331 SGGeod pointAlongExitPath(double distanceM) const
333 double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
334 double p = copysign(90, turnAngle);
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
341 SGGeod tc2 = SGGeodesy::direct(turnCenter,
342 legCourseTrue - overflightCompensationAngle - p,
344 theta = copysign(theta - turnAngle, overflightCompensationAngle);
345 return SGGeodesy::direct(tc2,
346 legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
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);
356 SGGeod pointAlongEntryPath(double distanceM) const
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);
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;
376 typedef std::vector<WayptData> WayptDataVec;
378 class PerformanceBracket
381 PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
382 atOrBelowAltitudeFt(atOrBelow),
384 descentRateFPM(descent),
385 speedIASOrMach(speed),
389 double atOrBelowAltitudeFt;
391 double descentRateFPM;
392 double speedIASOrMach;
396 typedef std::vector<PerformanceBracket> PerformanceBracketVec;
398 bool isDescentWaypoint(const WayptRef& wpt)
400 return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
403 class RoutePath::RoutePathPrivate
406 WayptDataVec waypoints;
407 PerformanceBracketVec perf;
410 PerformanceBracketVec::const_iterator
411 findPerformanceBracket(double altFt) const
413 PerformanceBracketVec::const_iterator r;
414 PerformanceBracketVec::const_iterator result = perf.begin();
416 for (r = perf.begin(); r != perf.end(); ++r) {
417 if (r->atOrBelowAltitudeFt > altFt) {
422 } // of brackets iteration
427 void computeDynamicPosition(int index)
429 const WayptData& previous(previousValidWaypoint(index));
430 WayptRef wpt = waypoints[index].wpt;
431 assert(previous.posValid);
433 const std::string& ty(wpt->type());
434 if (ty == "hdgToAlt") {
435 HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
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;
443 if (isDescentWaypoint(wpt)) {
444 timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
446 timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
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();
457 SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
458 SGGeoc navid = SGGeoc::fromGeod(wpt->position());
460 double magVar = magVarFor(previous.pos);
462 double radial = i->radialDegMagnetic() + magVar;
463 double track = i->courseDegMagnetic() + magVar;
464 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
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);
472 ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
474 SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
475 << previous.turnExitPos << " / " << track << "/" << wpt->position()
477 waypoints[index].pos = wpt->position(); // horrible fallback
480 waypoints[index].pos = SGGeod::fromGeoc(rGc);
483 waypoints[index].pos = SGGeod::fromGeoc(rGc);
486 waypoints[index].posValid = true;
487 } else if (ty == "dmeIntercept") {
488 DMEIntercept* di = (DMEIntercept*) wpt.get();
490 SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
491 SGGeoc navid = SGGeoc::fromGeod(wpt->position());
492 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
496 double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
497 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
499 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
501 SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
502 waypoints[index].pos = wpt->position(); // horrible fallback
504 waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER);
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;
515 double computeVNAVAltitudeFt(int index)
517 WayptRef w = waypoints[index].wpt;
518 if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
520 int next = findNextKnownAltitude(index);
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;
530 PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
531 return fixedAlt + (it->descentRateFPM * minutes);
535 int prev = findPreceedingKnownAltitude(index);
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;
545 PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
546 return fixedAlt + (it->climbRateFPM * minutes);
551 int findPreceedingKnownAltitude(int index) const
553 const WayptData& w(waypoints[index]);
554 if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
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
565 SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceeding altitude value found");
569 // recurse earlier in the route
570 return findPreceedingKnownAltitude(index - 1);
573 int findNextKnownAltitude(int index) const
575 if (index >= waypoints.size()) {
576 SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
580 const WayptData& w(waypoints[index]);
581 if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
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
591 if (index == waypoints.size() - 1) {
592 SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
596 return findNextKnownAltitude(index + 1);
599 double altitudeForIndex(int index) const
601 const WayptData& w(waypoints[index]);
602 if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
603 return w.wpt->altitudeFt();
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();
612 SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
616 double groundSpeedForAltitude(double altitude) const
621 PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
624 if (it->speedIsMach) {
625 mach = it->speedIASOrMach; // easy
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
634 double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
636 double tas = mach * Cs;
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.
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 (*)
653 double distanceBetweenIndices(int from, int to) const
657 for (int i=from+1; i<= to; ++i) {
658 total += waypoints[i].pathDistanceM;
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 */));
673 const WayptData& previousValidWaypoint(int index) const
676 if (waypoints[index-1].skipped) {
677 return waypoints[index-2];
680 return waypoints[index-1];
683 }; // of RoutePathPrivate class
685 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
686 d(new RoutePathPrivate)
688 WayptVec::const_iterator it;
689 for (it = wpts.begin(); it != wpts.end(); ++it) {
690 d->waypoints.push_back(WayptData(*it));
695 RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
696 d(new RoutePathPrivate)
698 for (int l=0; l<fp->numLegs(); ++l) {
699 d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint()));
704 void RoutePath::commonInit()
706 _pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn
710 WayptDataVec::iterator it;
711 for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
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);
720 for (unsigned int i=1; i<d->waypoints.size(); ++i) {
721 if (d->waypoints[i].skipped) {
725 const WayptData& prev(d->previousValidWaypoint(i));
726 d->waypoints[i].computeLegCourse(prev);
727 d->computeDynamicPosition(i);
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();
733 if (i < (d->waypoints.size() - 1)) {
734 int nextIndex = i + 1;
735 if (d->waypoints[nextIndex].skipped) {
738 WayptData& next(d->waypoints[nextIndex]);
739 next.computeLegCourse(d->waypoints[i]);
741 if (next.legCourseValid) {
742 d->waypoints[i].computeTurn(radiusM, prev, next);
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;
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;
755 // now turn is computed, can resolve distances
756 d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
760 SGGeodVec RoutePath::pathForIndex(int index) const
762 const WayptData& w(d->waypoints[index]);
763 const std::string& ty(w.wpt->type());
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());
776 if (d->waypoints[index].skipped) {
780 if (ty == "vectors") {
781 // ideally we'd show a stippled line to connect the route?
786 return pathForHold((Hold*) d->waypoints[index].wpt.get());
789 const WayptData& prev(d->previousValidWaypoint(index));
790 prev.turnExitPath(r);
792 SGGeod from = prev.turnExitPos,
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);
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());
819 void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
821 SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
822 gcTo = SGGeoc::fromGeod(aTo);
824 double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
825 if (fabs(lonDelta) < 1e-3) {
829 lonDelta = SGMiscd::normalizeAngle(lonDelta);
830 int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
831 double lonStep = (lonDelta / steps);
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);
843 SGGeod RoutePath::positionForIndex(int index) const
845 return d->waypoints[index].pos;
848 SGGeodVec RoutePath::pathForHold(Hold* hold) const
851 double hdg = hold->inboundRadial();
852 double turnDelta = 180.0 / turnSteps;
853 double altFt = 0.0; // FIXME
854 double gsKts = d->groundSpeedForAltitude(altFt);
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;
865 if (hold->isLeftHanded()) {
866 turnDelta = -turnDelta;
868 SGGeod pos = hold->position();
871 // turn+leg sides are a mirror
872 for (int j=0; j < 2; ++j) {
874 for (int i=0;i<turnSteps; ++i) {
876 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
881 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
883 } // of leg+turn duplication
888 double RoutePath::computeDistanceForIndex(int index) const
890 if ((index < 0) || (index >= (int) d->waypoints.size())) {
891 throw sg_range_exception("waypt index out of range",
892 "RoutePath::computeDistanceForIndex");
896 // first waypoint, distance is 0
900 if (d->waypoints[index].skipped) {
904 const WayptData& prev(d->previousValidWaypoint(index));
905 double dist = SGGeodesy::distanceM(prev.turnExitPos,
906 d->waypoints[index].turnEntryPos);
907 dist += prev.turnDistanceM();
909 if (!d->waypoints[index].flyOver) {
910 // add entry distance
911 dist += d->waypoints[index].turnDistanceM();
917 double RoutePath::trackForIndex(int index) const
919 if (d->waypoints[index].skipped)
920 return trackForIndex(index - 1);
921 return d->waypoints[index].legCourseTrue;
924 double RoutePath::distanceForIndex(int index) const
926 return d->waypoints[index].pathDistanceM;
929 double RoutePath::distanceBetweenIndices(int from, int to) const
931 return d->distanceBetweenIndices(from, to);
934 SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
936 int sz = (int) d->waypoints.size();
938 index = sz - 1; // map negative values to end of the route
941 if ((index < 0) || (index >= sz)) {
942 throw sg_range_exception("waypt index out of range",
943 "RoutePath::positionForDistanceFrom");
946 // find the actual leg we're within
947 if (distanceM < 0.0) {
949 while ((index > 0) && (distanceM < 0.0)) {
951 distanceM += d->waypoints[index].pathDistanceM;
954 if (distanceM < 0.0) {
955 // still negative, return route start
956 return d->waypoints[0].pos;
961 int nextIndex = index + 1;
962 while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
963 distanceM -= d->waypoints[nextIndex].pathDistanceM;
968 if ((index + 1) >= sz) {
969 // past route end, just return final position
970 return d->waypoints[sz - 1].pos;
973 const WayptData& wpt(d->waypoints[index]);
974 const WayptData& next(d->waypoints[index+1]);
976 if (wpt.turnPathDistanceM > distanceM) {
977 // on the exit path of current wpt
978 return wpt.pointAlongExitPath(distanceM);
980 distanceM -= wpt.turnPathDistanceM;
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);
989 // linear between turn exit and turn entry points
990 return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);