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/positioned.hxx>
17 namespace flightgear {
18 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
21 using namespace flightgear;
23 // implement Point(s) known distance from a great circle
25 static double sqr(const double x)
30 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
32 double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
33 double bDist = SGGeodesy::distanceRad(a, d);
35 // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
36 double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5);
38 double p = atan2(sin(bDist)*cos(A), cos(bDist));
40 if (sqr(cos(dist)) > sqr(r)) {
41 SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
45 double dp1 = p + acos(cos(dist)/r);
46 double dp2 = p - acos(cos(dist)/r);
48 double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
49 double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
51 return SGMiscd::min(dp1Nm, dp2Nm);
54 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
58 _pathDescentFPM = 800;
60 _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
63 SGGeodVec RoutePath::pathForIndex(int index) const
66 return SGGeodVec(); // no path for first waypoint
69 if (_waypts[index]->type() == "vectors") {
70 return SGGeodVec(); // empty
73 if (_waypts[index]->type() == "hold") {
74 return pathForHold((Hold*) _waypts[index].get());
79 if (!computedPositionForIndex(index-1, pos)) {
84 if (!computedPositionForIndex(index, pos)) {
90 if (_waypts[index]->type() == "runway") {
91 // runways get an extra point, at the end. this is particularly
92 // important so missed approach segments draw correctly
93 FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
94 r.push_back(rwy->end());
100 SGGeod RoutePath::positionForIndex(int index) const
103 bool ok = computedPositionForIndex(index, r);
111 SGGeodVec RoutePath::pathForHold(Hold* hold) const
114 double hdg = hold->inboundRadial();
115 double turnDelta = 180.0 / turnSteps;
119 double stepTime = turnDelta / _pathTurnRate; // in seconds
120 double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
121 double legDist = hold->isDistance() ?
122 hold->timeOrDistance()
123 : _pathIAS * (hold->timeOrDistance() / 3600.0);
124 legDist *= SG_NM_TO_METER;
126 if (hold->isLeftHanded()) {
127 turnDelta = -turnDelta;
129 SGGeod pos = hold->position();
132 // turn+leg sides are a mirror
133 for (int j=0; j < 2; ++j) {
135 for (int i=0;i<turnSteps; ++i) {
137 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
142 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
144 } // of leg+turn duplication
150 * the path context holds the state of of an imaginary aircraft traversing
151 * the route, and limits the rate at which heading / altitude / position can
154 class RoutePath::PathCtx
161 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
163 if ((index < 0) || (index >= (int) _waypts.size())) {
164 throw sg_range_exception("waypt index out of range",
165 "RoutePath::computedPositionForIndex");
168 WayptRef w = _waypts[index];
169 if (!w->flag(WPT_DYNAMIC)) {
174 if (w->type() == "radialIntercept") {
175 // radial intersection along track
177 if (!computedPositionForIndex(index - 1, prev)) {
181 SGGeoc prevGc = SGGeoc::fromGeod(prev);
182 SGGeoc navid = SGGeoc::fromGeod(w->position());
184 double magVar = magVarFor(prev);
186 RadialIntercept* i = (RadialIntercept*) w.get();
187 double radial = i->radialDegMagnetic() + magVar;
188 double track = i->courseDegMagnetic() + magVar;
189 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
194 r = SGGeod::fromGeoc(rGc);
196 } else if (w->type() == "dmeIntercept") {
197 // find the point along the DME track, from prev, that is the correct distance
200 if (!computedPositionForIndex(index - 1, prev)) {
204 DMEIntercept* di = (DMEIntercept*) w.get();
206 SGGeoc prevGc = SGGeoc::fromGeod(prev);
207 SGGeoc navid = SGGeoc::fromGeod(w->position());
208 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
212 double crs = di->courseDegMagnetic() + magVarFor(prev);
213 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
215 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
221 SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
223 } else if (w->type() == "hdgToAlt") {
224 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
225 double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
226 double d = distanceForClimb(climb);
229 if (!computedPositionForIndex(index - 1, prevPos)) {
233 double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
236 SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
238 } else if (w->type() == "vectors"){
240 } else if (w->type() == "hold") {
245 SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
249 double RoutePath::computeAltitudeForIndex(int index) const
251 if ((index < 0) || (index >= (int) _waypts.size())) {
252 throw sg_range_exception("waypt index out of range",
253 "RoutePath::computeAltitudeForIndex");
256 WayptRef w = _waypts[index];
257 if (w->altitudeRestriction() != RESTRICT_NONE) {
258 return w->altitudeFt(); // easy!
261 if (w->type() == "runway") {
262 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
263 return rwy->threshold().getElevationFt();
264 } else if ((w->type() == "hold") || (w->type() == "vectors")) {
265 // pretend we don't change altitude in holds/vectoring
266 return computeAltitudeForIndex(index - 1);
269 double prevAlt = computeAltitudeForIndex(index - 1);
270 // find distance to previous, and hence climb/descent
273 if (!computedPositionForIndex(index, pos) ||
274 !computedPositionForIndex(index - 1, prevPos))
276 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
277 throw sg_range_exception("unable to compute position for waypoints");
280 double d = SGGeodesy::distanceNm(prevPos, pos);
281 double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
283 double deltaFt; // change in altitude in feet
284 if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
285 deltaFt = -_pathDescentFPM * tMinutes;
287 deltaFt = _pathClimbFPM * tMinutes;
290 return prevAlt + deltaFt;
293 double RoutePath::computeTrackForIndex(int index) const
295 if ((index < 0) || (index >= (int) _waypts.size())) {
296 throw sg_range_exception("waypt index out of range",
297 "RoutePath::computeTrackForIndex");
300 WayptRef w = _waypts[index];
301 if (w->type() == "radialIntercept") {
302 RadialIntercept* r = (RadialIntercept*) w.get();
303 return r->courseDegMagnetic();
304 } else if (w->type() == "dmeIntercept") {
305 DMEIntercept* d = (DMEIntercept*) w.get();
306 return d->courseDegMagnetic();
307 } else if (w->type() == "hdgToAlt") {
308 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
309 return h->headingDegMagnetic();
310 } else if (w->type() == "hold") {
311 Hold* h = (Hold*) w.get();
312 return h->inboundRadial();
313 } else if (w->type() == "vectors") {
314 SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
315 throw sg_range_exception("asked for track from vectors waypt");
316 } else if (w->type() == "runway") {
317 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
318 return rwy->headingDeg();
321 // course based upon previous and current pos
324 if (!computedPositionForIndex(index, pos) ||
325 !computedPositionForIndex(index - 1, prevPos))
327 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
328 throw sg_range_exception("unable to compute position for waypoints");
331 return SGGeodesy::courseDeg(prevPos, pos);
334 double RoutePath::distanceForClimb(double climbFt) const
336 double t = 0.0; // in seconds
338 t = (climbFt / _pathClimbFPM) * 60.0;
339 } else if (climbFt < 0.0) {
340 t = (climbFt / _pathDescentFPM) * 60.0;
343 return _pathIAS * (t / 3600.0);
346 double RoutePath::magVarFor(const SGGeod& geod) const
348 double jd = globals->get_time_params()->getJD();
349 return sgGetMagVar(geod, jd);