1 #include <Navaids/routePath.hxx>
3 #include <simgear/structure/exception.hxx>
4 #include <simgear/magvar/magvar.hxx>
5 #include <simgear/timing/sg_time.hxx>
7 #include <Main/globals.hxx>
8 #include <Airports/runways.hxx>
9 #include <Navaids/waypoint.hxx>
10 #include <Navaids/positioned.hxx>
12 namespace flightgear {
13 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
16 using namespace flightgear;
18 // implement Point(s) known distance from a great circle
20 static double sqr(const double x)
25 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
27 double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
28 double bDist = SGGeodesy::distanceRad(a, d);
30 // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
31 double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5);
33 double p = atan2(sin(bDist)*cos(A), cos(bDist));
35 if (sqr(cos(dist)) > sqr(r)) {
36 SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
40 double dp1 = p + acos(cos(dist)/r);
41 double dp2 = p - acos(cos(dist)/r);
43 double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
44 double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
46 return SGMiscd::min(dp1Nm, dp2Nm);
49 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
53 _pathDescentFPM = 800;
55 _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
58 SGGeodVec RoutePath::pathForIndex(int index) const
61 return SGGeodVec(); // no path for first waypoint
64 if (_waypts[index]->type() == "vectors") {
65 return SGGeodVec(); // empty
68 if (_waypts[index]->type() == "hold") {
69 return pathForHold((Hold*) _waypts[index].get());
74 if (!computedPositionForIndex(index-1, pos)) {
79 if (!computedPositionForIndex(index, pos)) {
85 if (_waypts[index]->type() == "runway") {
86 // runways get an extra point, at the end. this is particularly
87 // important so missed approach segments draw correctly
88 FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
89 r.push_back(rwy->end());
95 SGGeod RoutePath::positionForIndex(int index) const
98 bool ok = computedPositionForIndex(index, r);
106 SGGeodVec RoutePath::pathForHold(Hold* hold) const
109 double hdg = hold->inboundRadial();
110 double turnDelta = 180.0 / turnSteps;
114 double stepTime = turnDelta / _pathTurnRate; // in seconds
115 double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
116 double legDist = hold->isDistance() ?
117 hold->timeOrDistance()
118 : _pathIAS * (hold->timeOrDistance() / 3600.0);
119 legDist *= SG_NM_TO_METER;
121 if (hold->isLeftHanded()) {
122 turnDelta = -turnDelta;
124 SGGeod pos = hold->position();
127 // turn+leg sides are a mirror
128 for (int j=0; j < 2; ++j) {
130 for (int i=0;i<turnSteps; ++i) {
132 SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
137 SGGeodesy::direct(pos, hdg, legDist, pos, az2);
139 } // of leg+turn duplication
145 * the path context holds the state of of an imaginary aircraft traversing
146 * the route, and limits the rate at which heading / altitude / position can
149 class RoutePath::PathCtx
156 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
158 if ((index < 0) || (index >= (int) _waypts.size())) {
159 throw sg_range_exception("waypt index out of range",
160 "RoutePath::computedPositionForIndex");
163 WayptRef w = _waypts[index];
164 if (!w->flag(WPT_DYNAMIC)) {
169 if (w->type() == "radialIntercept") {
170 // radial intersection along track
172 if (!computedPositionForIndex(index - 1, prev)) {
176 SGGeoc prevGc = SGGeoc::fromGeod(prev);
177 SGGeoc navid = SGGeoc::fromGeod(w->position());
179 double magVar = magVarFor(prev);
181 RadialIntercept* i = (RadialIntercept*) w.get();
182 double radial = i->radialDegMagnetic() + magVar;
183 double track = i->courseDegMagnetic() + magVar;
184 bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
189 r = SGGeod::fromGeoc(rGc);
191 } else if (w->type() == "dmeIntercept") {
192 // find the point along the DME track, from prev, that is the correct distance
195 if (!computedPositionForIndex(index - 1, prev)) {
199 DMEIntercept* di = (DMEIntercept*) w.get();
201 SGGeoc prevGc = SGGeoc::fromGeod(prev);
202 SGGeoc navid = SGGeoc::fromGeod(w->position());
203 double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
207 double crs = di->courseDegMagnetic() + magVarFor(prev);
208 SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
210 double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
216 SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
218 } else if (w->type() == "hdgToAlt") {
219 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
220 double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
221 double d = distanceForClimb(climb);
224 if (!computedPositionForIndex(index - 1, prevPos)) {
228 double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
231 SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
233 } else if (w->type() == "vectors"){
235 } else if (w->type() == "hold") {
240 SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
244 double RoutePath::computeAltitudeForIndex(int index) const
246 if ((index < 0) || (index >= (int) _waypts.size())) {
247 throw sg_range_exception("waypt index out of range",
248 "RoutePath::computeAltitudeForIndex");
251 WayptRef w = _waypts[index];
252 if (w->altitudeRestriction() != RESTRICT_NONE) {
253 return w->altitudeFt(); // easy!
256 if (w->type() == "runway") {
257 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
258 return rwy->threshold().getElevationFt();
259 } else if ((w->type() == "hold") || (w->type() == "vectors")) {
260 // pretend we don't change altitude in holds/vectoring
261 return computeAltitudeForIndex(index - 1);
264 double prevAlt = computeAltitudeForIndex(index - 1);
265 // find distance to previous, and hence climb/descent
268 if (!computedPositionForIndex(index, pos) ||
269 !computedPositionForIndex(index - 1, prevPos))
271 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
272 throw sg_range_exception("unable to compute position for waypoints");
275 double d = SGGeodesy::distanceNm(prevPos, pos);
276 double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
278 double deltaFt; // change in altitude in feet
279 if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
280 deltaFt = -_pathDescentFPM * tMinutes;
282 deltaFt = _pathClimbFPM * tMinutes;
285 return prevAlt + deltaFt;
288 double RoutePath::computeTrackForIndex(int index) const
290 if ((index < 0) || (index >= (int) _waypts.size())) {
291 throw sg_range_exception("waypt index out of range",
292 "RoutePath::computeTrackForIndex");
295 WayptRef w = _waypts[index];
296 if (w->type() == "radialIntercept") {
297 RadialIntercept* r = (RadialIntercept*) w.get();
298 return r->courseDegMagnetic();
299 } else if (w->type() == "dmeIntercept") {
300 DMEIntercept* d = (DMEIntercept*) w.get();
301 return d->courseDegMagnetic();
302 } else if (w->type() == "hdgToAlt") {
303 HeadingToAltitude* h = (HeadingToAltitude*) w.get();
304 return h->headingDegMagnetic();
305 } else if (w->type() == "hold") {
306 Hold* h = (Hold*) w.get();
307 return h->inboundRadial();
308 } else if (w->type() == "vectors") {
309 SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
310 throw sg_range_exception("asked for track from vectors waypt");
311 } else if (w->type() == "runway") {
312 FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
313 return rwy->headingDeg();
316 // course based upon previous and current pos
319 if (!computedPositionForIndex(index, pos) ||
320 !computedPositionForIndex(index - 1, prevPos))
322 SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
323 throw sg_range_exception("unable to compute position for waypoints");
326 return SGGeodesy::courseDeg(prevPos, pos);
329 double RoutePath::distanceForClimb(double climbFt) const
331 double t = 0.0; // in seconds
333 t = (climbFt / _pathClimbFPM) * 60.0;
334 } else if (climbFt < 0.0) {
335 t = (climbFt / _pathDescentFPM) * 60.0;
338 return _pathIAS * (t / 3600.0);
341 double RoutePath::magVarFor(const SGGeod& geod) const
343 double jd = globals->get_time_params()->getJD();
344 return sgGetMagVar(geod, jd);