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_GENERAL, 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 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
61 RoutePath::RoutePath(const flightgear::FlightPlan* fp)
63 for (int l=0; l<fp->numLegs(); ++l) {
64 _waypts.push_back(fp->legAtIndex(l)->waypoint());
69 void RoutePath::commonInit()
72 _pathDescentFPM = 800;
74 _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
77 SGGeodVec RoutePath::pathForIndex(int index) const
80 return SGGeodVec(); // no path for first waypoint
83 if (_waypts[index]->type() == "vectors") {
84 return SGGeodVec(); // empty
87 if (_waypts[index]->type() == "hold") {
88 return pathForHold((Hold*) _waypts[index].get());
93 if (!computedPositionForIndex(index-1, pos)) {
98 if (!computedPositionForIndex(index, pos)) {
104 if (_waypts[index]->type() == "runway") {
105 // runways get an extra point, at the end. this is particularly
106 // important so missed approach segments draw correctly
107 FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
108 r.push_back(rwy->end());
114 SGGeod RoutePath::positionForIndex(int index) const
117 bool ok = computedPositionForIndex(index, r);
125 SGGeodVec RoutePath::pathForHold(Hold* hold) const
128 double hdg = hold->inboundRadial();
129 double turnDelta = 180.0 / turnSteps;
133 double stepTime = turnDelta / _pathTurnRate; // in seconds
134 double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
135 double legDist = hold->isDistance() ?
136 hold->timeOrDistance()
137 : _pathIAS * (hold->timeOrDistance() / 3600.0);
138 legDist *= SG_NM_TO_METER;
140 if (hold->isLeftHanded()) {
141 turnDelta = -turnDelta;
143 SGGeod pos = hold->position();
146 // turn+leg sides are a mirror
147 for (int j=0; j < 2; ++j) {
149 for (int i=0;i<turnSteps; ++i) {
151 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
156 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
158 } // of leg+turn duplication
164 * the path context holds the state of of an imaginary aircraft traversing
165 * the route, and limits the rate at which heading / altitude / position can
168 class RoutePath::PathCtx
175 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
177 if ((index < 0) || (index >= (int) _waypts.size())) {
178 throw sg_range_exception("waypt index out of range",
179 "RoutePath::computedPositionForIndex");
182 WayptRef w = _waypts[index];
183 if (!w->flag(WPT_DYNAMIC)) {
188 if (w->type() == "radialIntercept") {
189 // radial intersection along track
191 if (!computedPositionForIndex(index - 1, prev)) {
195 SGGeoc prevGc = SGGeoc::fromGeod(prev);
196 SGGeoc navid = SGGeoc::fromGeod(w->position());
198 double magVar = magVarFor(prev);
200 RadialIntercept* i = (RadialIntercept*) w.get();
201 double radial = i->radialDegMagnetic() + magVar;
202 double track = i->courseDegMagnetic() + magVar;
203 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
208 r = SGGeod::fromGeoc(rGc);
210 } else if (w->type() == "dmeIntercept") {
211 // find the point along the DME track, from prev, that is the correct distance
214 if (!computedPositionForIndex(index - 1, prev)) {
218 DMEIntercept* di = (DMEIntercept*) w.get();
220 SGGeoc prevGc = SGGeoc::fromGeod(prev);
221 SGGeoc navid = SGGeoc::fromGeod(w->position());
222 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
226 double crs = di->courseDegMagnetic() + magVarFor(prev);
227 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
229 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
235 SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
237 } else if (w->type() == "hdgToAlt") {
238 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
239 double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
240 double d = distanceForClimb(climb);
243 if (!computedPositionForIndex(index - 1, prevPos)) {
247 double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
250 SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
252 } else if (w->type() == "vectors"){
254 } else if (w->type() == "hold") {
259 SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
263 double RoutePath::computeAltitudeForIndex(int index) const
265 if ((index < 0) || (index >= (int) _waypts.size())) {
266 throw sg_range_exception("waypt index out of range",
267 "RoutePath::computeAltitudeForIndex");
270 WayptRef w = _waypts[index];
271 if (w->altitudeRestriction() != RESTRICT_NONE) {
272 return w->altitudeFt(); // easy!
275 if (w->type() == "runway") {
276 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
277 return rwy->threshold().getElevationFt();
278 } else if ((w->type() == "hold") || (w->type() == "vectors")) {
279 // pretend we don't change altitude in holds/vectoring
280 return computeAltitudeForIndex(index - 1);
283 double prevAlt = computeAltitudeForIndex(index - 1);
284 // find distance to previous, and hence climb/descent
287 if (!computedPositionForIndex(index, pos) ||
288 !computedPositionForIndex(index - 1, prevPos))
290 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
291 throw sg_range_exception("unable to compute position for waypoints");
294 double d = SGGeodesy::distanceNm(prevPos, pos);
295 double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
297 double deltaFt; // change in altitude in feet
298 if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
299 deltaFt = -_pathDescentFPM * tMinutes;
301 deltaFt = _pathClimbFPM * tMinutes;
304 return prevAlt + deltaFt;
307 double RoutePath::computeTrackForIndex(int index) const
309 if ((index < 0) || (index >= (int) _waypts.size())) {
310 throw sg_range_exception("waypt index out of range",
311 "RoutePath::computeTrackForIndex");
314 WayptRef w = _waypts[index];
315 if (w->type() == "radialIntercept") {
316 RadialIntercept* r = (RadialIntercept*) w.get();
317 return r->courseDegMagnetic();
318 } else if (w->type() == "dmeIntercept") {
319 DMEIntercept* d = (DMEIntercept*) w.get();
320 return d->courseDegMagnetic();
321 } else if (w->type() == "hdgToAlt") {
322 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
323 return h->headingDegMagnetic();
324 } else if (w->type() == "hold") {
325 Hold* h = (Hold*) w.get();
326 return h->inboundRadial();
327 } else if (w->type() == "vectors") {
328 SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
329 throw sg_range_exception("asked for track from vectors waypt");
330 } else if (w->type() == "runway") {
331 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
332 return rwy->headingDeg();
335 // course based upon previous and current pos
338 if (!computedPositionForIndex(index, pos) ||
339 !computedPositionForIndex(index - 1, prevPos))
341 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
342 throw sg_range_exception("unable to compute position for waypoints");
345 return SGGeodesy::courseDeg(prevPos, pos);
348 double RoutePath::distanceForClimb(double climbFt) const
350 double t = 0.0; // in seconds
352 t = (climbFt / _pathClimbFPM) * 60.0;
353 } else if (climbFt < 0.0) {
354 t = (climbFt / _pathDescentFPM) * 60.0;
357 return _pathIAS * (t / 3600.0);
360 double RoutePath::magVarFor(const SGGeod& geod) const
362 double jd = globals->get_time_params()->getJD();
363 return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;