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 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
33 double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
34 double bDist = SGGeodesy::distanceRad(a, d);
36 // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
37 double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5);
39 double p = atan2(sin(bDist)*cos(A), cos(bDist));
41 if (sqr(cos(dist)) > sqr(r)) {
42 SG_LOG(SG_NAVAID, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
46 double dp1 = p + acos(cos(dist)/r);
47 double dp2 = p - acos(cos(dist)/r);
49 double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
50 double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
52 return SGMiscd::min(dp1Nm, dp2Nm);
55 // http://williams.best.vwh.net/avform.htm#Int
56 double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
59 Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:
61 lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2)
62 -sin(lat2)*cos(lat1)*sin(lon-lon1))/(cos(lat1)*cos(lat2)*sin(lon1-lon2)))
64 double lonDiff = a.getLongitudeRad() - b.getLongitudeRad();
65 double cosLat1 = cos(a.getLatitudeRad()),
66 cosLat2 = cos(b.getLatitudeRad());
67 double x = sin(a.getLatitudeRad()) * cosLat2 * sin(lon - b.getLongitudeRad());
68 double y = sin(b.getLatitudeRad()) * cosLat1 * sin(lon - a.getLongitudeRad());
69 double denom = cosLat1 * cosLat2 * sin(lonDiff);
70 double lat = atan((x - y) / denom);
74 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
80 RoutePath::RoutePath(const flightgear::FlightPlan* fp)
82 for (int l=0; l<fp->numLegs(); ++l) {
83 _waypts.push_back(fp->legAtIndex(l)->waypoint());
88 void RoutePath::commonInit()
91 _pathDescentFPM = 800;
93 _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
96 SGGeodVec RoutePath::pathForIndex(int index) const
99 return SGGeodVec(); // no path for first waypoint
102 if (_waypts[index]->type() == "vectors") {
103 return SGGeodVec(); // empty
106 if (_waypts[index]->type() == "hold") {
107 return pathForHold((Hold*) _waypts[index].get());
112 if (!computedPositionForIndex(index-1, from)) {
117 if (!computedPositionForIndex(index, to)) {
121 // compute rounding offset, we want to round towards the direction of travel
122 // which depends on the east/west sign of the longitude change
123 double lonDelta = to.getLongitudeDeg() - from.getLongitudeDeg();
124 if (fabs(lonDelta) > 0.5) {
125 interpolateGreatCircle(from, to, r);
130 if (_waypts[index]->type() == "runway") {
131 // runways get an extra point, at the end. this is particularly
132 // important so missed approach segments draw correctly
133 FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
134 r.push_back(rwy->end());
140 void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
142 SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
143 gcTo = SGGeoc::fromGeod(aTo);
145 double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
146 if (fabs(lonDelta) < 1e-3) {
150 lonDelta = SGMiscd::normalizeAngle(lonDelta);
151 int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
152 double lonStep = (lonDelta / steps);
154 double lon = gcFrom.getLongitudeRad() + lonStep;
155 for (int s=0; s < (steps - 1); ++s) {
156 lon = SGMiscd::normalizeAngle(lon);
157 double lat = latitudeForGCLongitude(gcFrom, gcTo, lon);
158 r.push_back(SGGeod::fromGeoc(SGGeoc::fromRadM(lon, lat, SGGeodesy::EQURAD)));
159 //SG_LOG(SG_GENERAL, SG_INFO, "lon:" << lon * SG_RADIANS_TO_DEGREES << " gives lat " << lat * SG_RADIANS_TO_DEGREES);
164 SGGeod RoutePath::positionForIndex(int index) const
167 bool ok = computedPositionForIndex(index, r);
175 SGGeodVec RoutePath::pathForHold(Hold* hold) const
178 double hdg = hold->inboundRadial();
179 double turnDelta = 180.0 / turnSteps;
183 double stepTime = turnDelta / _pathTurnRate; // in seconds
184 double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
185 double legDist = hold->isDistance() ?
186 hold->timeOrDistance()
187 : _pathIAS * (hold->timeOrDistance() / 3600.0);
188 legDist *= SG_NM_TO_METER;
190 if (hold->isLeftHanded()) {
191 turnDelta = -turnDelta;
193 SGGeod pos = hold->position();
196 // turn+leg sides are a mirror
197 for (int j=0; j < 2; ++j) {
199 for (int i=0;i<turnSteps; ++i) {
201 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
206 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
208 } // of leg+turn duplication
214 * the path context holds the state of of an imaginary aircraft traversing
215 * the route, and limits the rate at which heading / altitude / position can
218 class RoutePath::PathCtx
225 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
227 if ((index < 0) || (index >= (int) _waypts.size())) {
228 throw sg_range_exception("waypt index out of range",
229 "RoutePath::computedPositionForIndex");
232 WayptRef w = _waypts[index];
233 if (!w->flag(WPT_DYNAMIC)) {
238 if (w->type() == "radialIntercept") {
239 // radial intersection along track
241 if (!computedPositionForIndex(index - 1, prev)) {
245 SGGeoc prevGc = SGGeoc::fromGeod(prev);
246 SGGeoc navid = SGGeoc::fromGeod(w->position());
248 double magVar = magVarFor(prev);
250 RadialIntercept* i = (RadialIntercept*) w.get();
251 double radial = i->radialDegMagnetic() + magVar;
252 double track = i->courseDegMagnetic() + magVar;
253 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
258 r = SGGeod::fromGeoc(rGc);
260 } else if (w->type() == "dmeIntercept") {
261 // find the point along the DME track, from prev, that is the correct distance
264 if (!computedPositionForIndex(index - 1, prev)) {
268 DMEIntercept* di = (DMEIntercept*) w.get();
270 SGGeoc prevGc = SGGeoc::fromGeod(prev);
271 SGGeoc navid = SGGeoc::fromGeod(w->position());
272 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
276 double crs = di->courseDegMagnetic() + magVarFor(prev);
277 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
279 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
285 SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
287 } else if (w->type() == "hdgToAlt") {
288 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
289 double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
290 double d = distanceForClimb(climb);
293 if (!computedPositionForIndex(index - 1, prevPos)) {
297 double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
300 SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
302 } else if (w->type() == "vectors"){
304 } else if (w->type() == "hold") {
309 SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
313 double RoutePath::computeAltitudeForIndex(int index) const
315 if ((index < 0) || (index >= (int) _waypts.size())) {
316 throw sg_range_exception("waypt index out of range",
317 "RoutePath::computeAltitudeForIndex");
320 WayptRef w = _waypts[index];
321 if (w->altitudeRestriction() != RESTRICT_NONE) {
322 return w->altitudeFt(); // easy!
325 if (w->type() == "runway") {
326 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
327 return rwy->threshold().getElevationFt();
328 } else if ((w->type() == "hold") || (w->type() == "vectors")) {
329 // pretend we don't change altitude in holds/vectoring
330 return computeAltitudeForIndex(index - 1);
333 double prevAlt = computeAltitudeForIndex(index - 1);
334 // find distance to previous, and hence climb/descent
337 if (!computedPositionForIndex(index, pos) ||
338 !computedPositionForIndex(index - 1, prevPos))
340 SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints");
341 throw sg_range_exception("unable to compute position for waypoints");
344 double d = SGGeodesy::distanceNm(prevPos, pos);
345 double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
347 double deltaFt; // change in altitude in feet
348 if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
349 deltaFt = -_pathDescentFPM * tMinutes;
351 deltaFt = _pathClimbFPM * tMinutes;
354 return prevAlt + deltaFt;
357 double RoutePath::computeTrackForIndex(int index) const
359 if ((index < 0) || (index >= (int) _waypts.size())) {
360 throw sg_range_exception("waypt index out of range",
361 "RoutePath::computeTrackForIndex");
364 WayptRef w = _waypts[index];
365 if (w->type() == "radialIntercept") {
366 RadialIntercept* r = (RadialIntercept*) w.get();
367 return r->courseDegMagnetic();
368 } else if (w->type() == "dmeIntercept") {
369 DMEIntercept* d = (DMEIntercept*) w.get();
370 return d->courseDegMagnetic();
371 } else if (w->type() == "hdgToAlt") {
372 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
373 return h->headingDegMagnetic();
374 } else if (w->type() == "hold") {
375 Hold* h = (Hold*) w.get();
376 return h->inboundRadial();
377 } else if (w->type() == "vectors") {
378 SG_LOG(SG_NAVAID, SG_WARN, "asked for track from VECTORS");
379 throw sg_range_exception("asked for track from vectors waypt");
380 } else if (w->type() == "runway") {
381 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
382 return rwy->headingDeg();
385 // course based upon previous and current pos
388 if (!computedPositionForIndex(index, pos) ||
389 !computedPositionForIndex(index - 1, prevPos))
391 SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints");
392 throw sg_range_exception("unable to compute position for waypoints");
395 return SGGeodesy::courseDeg(prevPos, pos);
398 double RoutePath::distanceForClimb(double climbFt) const
400 double t = 0.0; // in seconds
402 t = (climbFt / _pathClimbFPM) * 60.0;
403 } else if (climbFt < 0.0) {
404 t = (climbFt / _pathDescentFPM) * 60.0;
407 return _pathIAS * (t / 3600.0);
410 double RoutePath::magVarFor(const SGGeod& geod) const
412 double jd = globals->get_time_params()->getJD();
413 return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;