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 {
21 // http://williams.best.vwh.net/avform.htm#Intersection
22 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
24 double crs13 = r1 * SG_DEGREES_TO_RADIANS;
25 double crs23 = r2 * SG_DEGREES_TO_RADIANS;
26 double dst12 = SGGeodesy::distanceRad(a, b);
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)))
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)))
36 double sinLat1 = sin(a.getLatitudeRad());
37 double cosLat1 = cos(a.getLatitudeRad());
38 double sinDst12 = sin(dst12);
39 double cosDst12 = cos(dst12);
41 double crs12 = SGGeodesy::courseRad(a, b),
42 crs21 = SGGeodesy::courseRad(b, a);
45 // normalise to -pi .. pi range
46 double ang1 = SGMiscd::normalizeAngle(crs13-crs12);
47 double ang2 = SGMiscd::normalizeAngle(crs21-crs23);
49 if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
50 SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections");
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));
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))
67 //lon3=mod(lon1-dlon+pi,2*pi)-pi
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));
73 SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);
75 double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));
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);
81 result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
87 using namespace flightgear;
89 // implement Point(s) known distance from a great circle
91 static double sqr(const double x)
96 // http://williams.best.vwh.net/avform.htm#POINTDME
97 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
99 double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
100 double bDist = SGGeodesy::distanceRad(a, d);
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);
105 double p = atan2(sin(bDist)*cos(A), cos(bDist));
107 if (sqr(cos(dist)) > sqr(r)) {
108 SG_LOG(SG_NAVAID, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
112 double dp1 = p + acos(cos(dist)/r);
113 double dp2 = p - acos(cos(dist)/r);
115 double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
116 double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
118 return SGMiscd::min(dp1Nm, dp2Nm);
121 // http://williams.best.vwh.net/avform.htm#Int
122 double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
125 Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:
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)))
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);
140 static double magVarFor(const SGGeod& geod)
142 double jd = globals->get_time_params()->getJD();
143 return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
146 SGGeod turnCenterOverflight(const SGGeod& pt, double inHeadingDeg,
147 double turnAngleDeg, double turnRadiusM)
149 double p = copysign(90.0, turnAngleDeg);
150 return SGGeodesy::direct(pt, inHeadingDeg + p, turnRadiusM);
153 SGGeod turnCenterFlyBy(const SGGeod& pt, double inHeadingDeg,
154 double turnAngleDeg, double turnRadiusM)
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);
162 SGGeod turnCenterFromExit(const SGGeod& pt, double outHeadingDeg,
163 double turnAngleDeg, double turnRadiusM)
165 double p = copysign(90.0, turnAngleDeg);
166 return SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);
171 TurnInfo() : valid(false),
172 inboundCourseDeg(0.0),
173 turnAngleDeg(0.0) { }
177 double inboundCourseDeg;
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.
186 TurnInfo turnCenterAndAngleFromExit(const SGGeod& pt, double outHeadingDeg,
187 double turnRadiusM, const SGGeod&origin)
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);
197 r.turnCenter = SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);
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");
206 // find additional course angle away from the exit pos to intersect
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);
212 r.inboundCourseDeg = courseToTC + theta;
213 SG_NORMALIZE_RANGE(r.inboundCourseDeg, 0.0, 360.0);
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
218 r.turnAngleDeg = outHeadingDeg - r.inboundCourseDeg;
219 if (r.turnAngleDeg > 0.0) {
220 if (turnAngle < 0.0) r.turnAngleDeg -= 360.0;
222 if (turnAngle > 0.0) r.turnAngleDeg += 360.0;
232 explicit WayptData(WayptRef w) :
236 legCourseValid(false),
243 turnPathDistanceM(0.0),
244 overflightCompensationAngle(0.0),
245 flyOver(w->flag(WPT_OVERFLIGHT))
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") {
257 } else if (ty == "discontinuity") {
260 pos = wpt->position();
264 legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
265 legCourseValid = true;
271 * test if course of this leg can be adjusted or is contrained to an exact value
273 bool isCourseConstrained() const
275 const std::string& ty(wpt->type());
276 return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept");
279 // compute leg courses for all static legs (both ends are fixed)
280 void initPass1(const WayptData& previous, WayptData* next)
282 if (wpt->type() == "vectors") {
283 // relying on the fact vectors will be followed by a static fix/wpt
284 if (next && next->posValid) {
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)) {
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;
312 void computeLegCourse(const WayptData& previous, double radiusM)
314 if (legCourseValid) {
318 if ((wpt->type() == "hold") ||
319 (wpt->type() == "discontinuity") ||
320 (wpt->type() == "via"))
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();
328 TurnInfo ti = turnCenterAndAngleFromExit(rwy->threshold(),
330 radiusM, previous.pos);
332 legCourseTrue = ti.inboundCourseDeg;
333 turnEntryAngle = ti.turnAngleDeg;
334 turnEntryCenter = ti.turnCenter;
335 turnRadius = radiusM;
337 turnEntryPos = pointOnEntryTurnFromHeading(ti.inboundCourseDeg);
339 // couldn't compute entry, never mind
340 legCourseTrue = SGGeodesy::courseDeg(previous.pos, rwy->threshold());
342 legCourseValid = true;
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
355 SGGeod pointOnEntryTurnFromHeading(double headingDeg) const
358 double p = copysign(90.0, turnEntryAngle);
359 return SGGeodesy::direct(turnEntryCenter, headingDeg - p, turnRadius);
362 SGGeod pointOnExitTurnFromHeading(double headingDeg) const
364 double p = copysign(90.0, turnExitAngle);
365 return SGGeodesy::direct(turnExitCenter, headingDeg - p, turnRadius);
368 double pathDistanceForTurnAngle(double angleDeg) const
370 return turnRadius * fabs(angleDeg) * SG_DEGREES_TO_RADIANS;
373 void computeTurn(double radiusM, bool constrainLegCourse, WayptData& next)
376 assert(next.legCourseValid);
377 bool isRunway = (wpt->type() == "runway");
379 if (legCourseValid) {
381 FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
382 turnExitAngle = next.legCourseTrue - rwy->headingDeg();
384 turnExitAngle = next.legCourseTrue - legCourseTrue;
387 // happens for first leg
389 legCourseValid = true;
390 FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
391 turnExitAngle = next.legCourseTrue - rwy->headingDeg();
392 legCourseTrue = rwy->headingDeg();
395 // don't set legCourseValid
400 SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
401 turnRadius = radiusM;
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.
412 FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
413 turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius);
416 turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);
419 turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
421 if (!next.wpt->flag(WPT_DYNAMIC)) {
422 // distance perpendicular to next leg course, after turning
424 double xtk = turnRadius * (1 - cos(turnExitAngle * SG_DEGREES_TO_RADIANS));
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
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;
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;
440 // sign of angles will differ, so compute distances seperately
441 turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle) +
442 pathDistanceForTurnAngle(overflightCompensationAngle);
444 // next leg course can be adjusted. increase the turn angle
445 // and modify the next leg's course accordingly.
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);
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
460 // next point is dynamic
461 // no compensation needed
462 turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
466 turnEntryCenter = turnCenterFlyBy(pos, legCourseTrue, turnExitAngle, turnRadius);
468 turnExitAngle = turnExitAngle * 0.5;
469 turnEntryAngle = turnExitAngle;
470 turnExitCenter = turnEntryCenter; // important that these match
472 turnEntryPos = pointOnEntryTurnFromHeading(legCourseTrue);
473 turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
474 turnPathDistanceM = pathDistanceForTurnAngle(turnEntryAngle);
478 double turnDistanceM() const
480 return turnPathDistanceM;
483 void turnEntryPath(SGGeodVec& path) const
485 if (!hasEntry || fabs(turnEntryAngle) < 0.5 ) {
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));
499 void turnExitPath(SGGeodVec& path) const
501 if (fabs(turnExitAngle) < 0.5) {
502 path.push_back(turnExitPos);
506 int steps = std::max(SGMiscd::roundToInt(fabs(turnExitAngle) / 3.0), 1);
507 double stepIncrement = turnExitAngle / steps;
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();
516 for (int s=0; s<steps; ++s) {
517 path.push_back(pointOnExitTurnFromHeading(h));
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.
526 if (flyOver && (overflightCompensationAngle != 0.0)) {
527 // skew by compensation angle back
528 steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
530 // step in opposite direction to the turn angle to swing back onto
531 // the next leg course
532 stepIncrement = overflightCompensationAngle / steps;
534 SGGeod p = path.back();
535 double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;
537 for (int s=0; s<steps; ++s) {
539 p = SGGeodesy::direct(p, h, stepDist);
543 } // of overflight compensation turn
546 SGGeod pointAlongExitPath(double distanceM) const
548 double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
549 double p = copysign(90, turnExitAngle);
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
556 SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
557 legCourseTrue - overflightCompensationAngle - p,
559 theta = copysign(theta - turnExitAngle, overflightCompensationAngle);
560 return SGGeodesy::direct(tc2,
561 legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
565 theta = copysign(theta, turnExitAngle);
566 double inboundCourse = legCourseTrue + (flyOver ? 0.0 : turnExitAngle);
567 return pointOnExitTurnFromHeading(inboundCourse + theta);
570 SGGeod pointAlongEntryPath(double distanceM) const
573 double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
574 theta = copysign(theta, turnEntryAngle);
575 return pointOnEntryTurnFromHeading(legCourseTrue + theta);
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;
588 typedef std::vector<WayptData> WayptDataVec;
590 class PerformanceBracket
593 PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
594 atOrBelowAltitudeFt(atOrBelow),
596 descentRateFPM(descent),
597 speedIASOrMach(speed),
601 double atOrBelowAltitudeFt;
603 double descentRateFPM;
604 double speedIASOrMach;
608 typedef std::vector<PerformanceBracket> PerformanceBracketVec;
610 bool isDescentWaypoint(const WayptRef& wpt)
612 return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
615 class RoutePath::RoutePathPrivate
618 WayptDataVec waypoints;
620 char aircraftCategory;
621 PerformanceBracketVec perf;
623 bool constrainLegCourses;
625 PerformanceBracketVec::const_iterator
626 findPerformanceBracket(double altFt) const
628 PerformanceBracketVec::const_iterator r;
629 PerformanceBracketVec::const_iterator result = perf.begin();
631 for (r = perf.begin(); r != perf.end(); ++r) {
632 if (r->atOrBelowAltitudeFt > altFt) {
637 } // of brackets iteration
642 void computeDynamicPosition(int index)
644 const WayptData& previous(previousValidWaypoint(index));
645 WayptRef wpt = waypoints[index].wpt;
646 assert(previous.posValid);
648 const std::string& ty(wpt->type());
649 if (ty == "hdgToAlt") {
650 HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
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;
658 if (isDescentWaypoint(wpt)) {
659 timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
661 timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
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();
672 SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
673 SGGeoc navid = SGGeoc::fromGeod(wpt->position());
675 double magVar = magVarFor(previous.pos);
677 double radial = i->radialDegMagnetic() + magVar;
678 double track = i->courseDegMagnetic() + magVar;
679 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
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);
687 ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
689 SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
690 << previous.turnExitPos << " / " << track << "/" << wpt->position()
692 waypoints[index].pos = wpt->position(); // horrible fallback
695 waypoints[index].pos = SGGeod::fromGeoc(rGc);
698 waypoints[index].pos = SGGeod::fromGeoc(rGc);
701 waypoints[index].posValid = true;
702 } else if (ty == "dmeIntercept") {
703 DMEIntercept* di = (DMEIntercept*) wpt.get();
705 SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos);
706 SGGeoc navid = SGGeoc::fromGeod(wpt->position());
707 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
711 double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
712 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
714 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
716 SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
717 waypoints[index].pos = wpt->position(); // horrible fallback
719 waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER);
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;
730 double computeVNAVAltitudeFt(int index)
732 WayptRef w = waypoints[index].wpt;
733 if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
735 int next = findNextKnownAltitude(index);
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;
745 PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
746 return fixedAlt + (it->descentRateFPM * minutes);
750 int prev = findPreceedingKnownAltitude(index);
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;
760 PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
761 return fixedAlt + (it->climbRateFPM * minutes);
766 int findPreceedingKnownAltitude(int index) const
768 const WayptData& w(waypoints[index]);
769 if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
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
780 SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceding altitude value found");
784 // recurse earlier in the route
785 return findPreceedingKnownAltitude(index - 1);
788 int findNextKnownAltitude(unsigned int index) const
790 if (index >= waypoints.size()) {
791 SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
795 const WayptData& w(waypoints[index]);
796 if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
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
806 if (index == waypoints.size() - 1) {
807 SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
811 return findNextKnownAltitude(index + 1);
814 double altitudeForIndex(int index) const
816 const WayptData& w(waypoints[index]);
817 if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
818 return w.wpt->altitudeFt();
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();
827 SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
831 double groundSpeedForAltitude(double altitude) const
833 PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
834 if (it->speedIsMach) {
837 // FIXME - convert IAS to ground-speed, using standard atmosphere / temperature model
838 return it->speedIASOrMach;
845 if (it->speedIsMach) {
846 mach = it->speedIASOrMach; // easy
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
855 double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
857 double tas = mach * Cs;
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.
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 (*)
872 double distanceBetweenIndices(int from, int to) const
876 for (int i=from+1; i<= to; ++i) {
877 total += waypoints[i].pathDistanceM;
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));
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));
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 */));
905 case ICAO_AIRCRAFT_CATEGORY_D:
906 case ICAO_AIRCRAFT_CATEGORY_E:
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 */));
917 const WayptData& previousValidWaypoint(int index) const
920 if (waypoints[index-1].skipped) {
921 return waypoints[index-2];
924 return waypoints[index-1];
927 }; // of RoutePathPrivate class
929 RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
930 d(new RoutePathPrivate)
932 for (int l=0; l<fp->numLegs(); ++l) {
933 d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint()));
936 d->aircraftCategory = fp->icaoAircraftCategory()[0];
937 d->constrainLegCourses = fp->followLegTrackToFixes();
941 RoutePath::~RoutePath()
945 void RoutePath::commonInit()
949 WayptDataVec::iterator it;
950 for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
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);
959 for (unsigned int i=0; i<d->waypoints.size(); ++i) {
960 if (d->waypoints[i].skipped) {
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();
969 const WayptData& prev(d->previousValidWaypoint(i));
970 d->waypoints[i].computeLegCourse(prev, radiusM);
971 d->computeDynamicPosition(i);
974 if (i < (d->waypoints.size() - 1)) {
975 int nextIndex = i + 1;
976 if (d->waypoints[nextIndex].skipped) {
979 WayptData& next(d->waypoints[nextIndex]);
980 next.computeLegCourse(d->waypoints[i], radiusM);
982 if (next.legCourseValid) {
983 d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, next);
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;
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;
996 // now turn is computed, can resolve distances
997 d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
1001 SGGeodVec RoutePath::pathForIndex(int index) const
1003 const WayptData& w(d->waypoints[index]);
1004 const std::string& ty(w.wpt->type());
1007 if (d->waypoints[index].skipped) {
1011 if (ty == "vectors") {
1012 // ideally we'd show a stippled line to connect the route?
1016 if (ty == "discontinuity") {
1017 return SGGeodVec(); // no points for a discontinuity of course
1021 return pathForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index);
1025 return pathForHold((Hold*) d->waypoints[index].wpt.get());
1029 const WayptData& prev(d->previousValidWaypoint(index));
1030 prev.turnExitPath(r);
1032 SGGeod from = prev.turnExitPos,
1033 to = w.turnEntryPos;
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);
1041 } // of have previous waypoint
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());
1055 void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
1057 SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
1058 gcTo = SGGeoc::fromGeod(aTo);
1060 double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
1061 if (fabs(lonDelta) < 1e-3) {
1065 lonDelta = SGMiscd::normalizeAngle(lonDelta);
1066 int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
1067 double lonStep = (lonDelta / steps);
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);
1079 SGGeod RoutePath::positionForIndex(int index) const
1081 return d->waypoints[index].pos;
1084 SGGeodVec RoutePath::pathForVia(Via* via, int index) const
1086 // previous waypoint must be valid for a VIA
1087 if ((index == 0) || !d->waypoints[index-1].posValid) {
1091 const WayptData& prev(d->waypoints[index-1]);
1092 WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
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();
1106 SGGeodVec RoutePath::pathForHold(Hold* hold) const
1109 double hdg = hold->inboundRadial();
1110 double turnDelta = 180.0 / turnSteps;
1111 double altFt = 0.0; // FIXME
1112 double gsKts = d->groundSpeedForAltitude(altFt);
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;
1123 if (hold->isLeftHanded()) {
1124 turnDelta = -turnDelta;
1126 SGGeod pos = hold->position();
1129 // turn+leg sides are a mirror
1130 for (int j=0; j < 2; ++j) {
1132 for (int i=0;i<turnSteps; ++i) {
1134 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
1139 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
1141 } // of leg+turn duplication
1146 double RoutePath::computeDistanceForIndex(int index) const
1148 if ((index < 0) || (index >= (int) d->waypoints.size())) {
1149 throw sg_range_exception("waypt index out of range",
1150 "RoutePath::computeDistanceForIndex");
1154 // first waypoint, distance is 0
1158 if (d->waypoints[index].skipped) {
1163 if (d->waypoints[index].wpt->type() == "via") {
1164 return distanceForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index);
1167 const WayptData& prev(d->previousValidWaypoint(index));
1168 double dist = SGGeodesy::distanceM(prev.turnExitPos,
1169 d->waypoints[index].turnEntryPos);
1170 dist += prev.turnDistanceM();
1172 if (!d->waypoints[index].flyOver) {
1173 // add entry distance
1174 dist += d->waypoints[index].turnDistanceM();
1180 double RoutePath::distanceForVia(Via* via, int index) const
1182 // previous waypoint must be valid for a VIA
1183 if ((index == 0) || !d->waypoints[index-1].posValid) {
1187 const WayptData& prev(d->waypoints[index-1]);
1188 WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
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();
1201 double RoutePath::trackForIndex(int index) const
1204 if (d->waypoints[index].skipped)
1205 return trackForIndex(index - 1);
1207 const WayptData& wd(d->waypoints[index]);
1208 if (!wd.legCourseValid)
1211 return wd.legCourseTrue;
1214 double RoutePath::distanceForIndex(int index) const
1216 return d->waypoints[index].pathDistanceM;
1219 double RoutePath::distanceBetweenIndices(int from, int to) const
1221 return d->distanceBetweenIndices(from, to);
1224 SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
1226 int sz = (int) d->waypoints.size();
1228 index = sz - 1; // map negative values to end of the route
1231 if ((index < 0) || (index >= sz)) {
1232 throw sg_range_exception("waypt index out of range",
1233 "RoutePath::positionForDistanceFrom");
1236 // find the actual leg we're within
1237 if (distanceM < 0.0) {
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;
1248 if (distanceM < 0.0) {
1249 // still negative, return route start
1250 return d->waypoints[0].pos;
1255 int nextIndex = index + 1;
1256 while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
1257 distanceM -= d->waypoints[nextIndex].pathDistanceM;
1258 index = nextIndex++;
1262 if ((index + 1) >= sz) {
1263 // past route end, just return final position
1264 return d->waypoints[sz - 1].pos;
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);
1274 if (wpt.turnPathDistanceM > distanceM) {
1275 // on the exit path of current wpt
1276 return wpt.pointAlongExitPath(distanceM);
1278 distanceM -= wpt.turnPathDistanceM;
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);
1287 // linear between turn exit and turn entry points
1288 return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
1291 SGGeod RoutePath::positionAlongVia(Via* via, int previousIndex, double distanceM) const
1293 SG_LOG(SG_NAVAID, SG_ALERT, "RoutePath::positionAlongVia not implemented");