src/Navaids/routePath.cxx

   1
   2 #ifdef HAVE_CONFIG_H
   3 #  include "config.h"
   4 #endif
   5
   6 #include <Navaids/routePath.hxx>
   7
   8 #include <simgear/structure/exception.hxx>
   9 #include <simgear/magvar/magvar.hxx>
  10 #include <simgear/timing/sg_time.hxx>
  11
  12 #include <Main/globals.hxx>
  13 #include <Airports/runways.hxx>
  14 #include <Navaids/waypoint.hxx>
  15 #include <Navaids/positioned.hxx>
  16
  17 namespace flightgear {
  18   bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
  19 }
  20
  21 using namespace flightgear;
  22
  23 // implement Point(s) known distance from a great circle
  24
  25 static double sqr(const double x)
  26 {
  27   return x * x;
  28 }
  29
  30 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
  31 {
  32   double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
  33   double bDist = SGGeodesy::distanceRad(a, d);
  34
  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);
  37
  38   double p = atan2(sin(bDist)*cos(A), cos(bDist));
  39
  40   if (sqr(cos(dist)) > sqr(r)) {
  41     SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
  42     return -1.0;
  43   }
  44
  45   double dp1 = p + acos(cos(dist)/r);
  46   double dp2 = p - acos(cos(dist)/r);
  47
  48   double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
  49   double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
  50
  51   return SGMiscd::min(dp1Nm, dp2Nm);
  52 }
  53
  54 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
  55   _waypts(wpts)
  56 {
  57   _pathClimbFPM = 1200;
  58   _pathDescentFPM = 800;
  59   _pathIAS = 190;
  60   _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
  61 }
  62
  63 SGGeodVec RoutePath::pathForIndex(int index) const
  64 {
  65   if (index == 0) {
  66     return SGGeodVec(); // no path for first waypoint
  67   }
  68
  69   if (_waypts[index]->type() == "vectors") {
  70     return SGGeodVec(); // empty
  71   }
  72
  73   if (_waypts[index]->type() == "hold") {
  74     return pathForHold((Hold*) _waypts[index].get());
  75   }
  76
  77   SGGeodVec r;
  78   SGGeod pos;
  79   if (!computedPositionForIndex(index-1, pos)) {
  80     return SGGeodVec();
  81   }
  82
  83   r.push_back(pos);
  84   if (!computedPositionForIndex(index, pos)) {
  85     return SGGeodVec();
  86   }
  87
  88   r.push_back(pos);
  89
  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());
  95   }
  96
  97   return r;
  98 }
  99
 100 SGGeod RoutePath::positionForIndex(int index) const
 101 {
 102   SGGeod r;
 103   bool ok = computedPositionForIndex(index, r);
 104   if (!ok) {
 105     return SGGeod();
 106   }
 107
 108   return r;
 109 }
 110
 111 SGGeodVec RoutePath::pathForHold(Hold* hold) const
 112 {
 113   int turnSteps = 16;
 114   double hdg = hold->inboundRadial();
 115   double turnDelta = 180.0 / turnSteps;
 116
 117   SGGeodVec r;
 118   double az2;
 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;
 125
 126   if (hold->isLeftHanded()) {
 127     turnDelta = -turnDelta;
 128   }
 129   SGGeod pos = hold->position();
 130   r.push_back(pos);
 131
 132   // turn+leg sides are a mirror
 133   for (int j=0; j < 2; ++j) {
 134   // turn
 135     for (int i=0;i<turnSteps; ++i) {
 136       hdg += turnDelta;
 137       SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
 138       r.push_back(pos);
 139     }
 140
 141   // leg
 142     SGGeodesy::direct(pos, hdg, legDist, pos, az2);
 143     r.push_back(pos);
 144   } // of leg+turn duplication
 145
 146   return r;
 147 }
 148
 149 /**
 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
 152  * change
 153  */
 154 class RoutePath::PathCtx
 155 {
 156 public:
 157   SGGeod pos;
 158   double heading;
 159 };
 160
 161 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
 162 {
 163   if ((index < 0) || (index >= (int) _waypts.size())) {
 164     throw sg_range_exception("waypt index out of range",
 165       "RoutePath::computedPositionForIndex");
 166   }
 167
 168   WayptRef w = _waypts[index];
 169   if (!w->flag(WPT_DYNAMIC)) {
 170     r = w->position();
 171     return true;
 172   }
 173
 174   if (w->type() == "radialIntercept") {
 175     // radial intersection along track
 176     SGGeod prev;
 177     if (!computedPositionForIndex(index - 1, prev)) {
 178       return false;
 179     }
 180
 181     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 182     SGGeoc navid = SGGeoc::fromGeod(w->position());
 183     SGGeoc rGc;
 184     double magVar = magVarFor(prev);
 185
 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);
 190     if (!ok) {
 191       return false;
 192     }
 193
 194     r = SGGeod::fromGeoc(rGc);
 195     return true;
 196   } else if (w->type() == "dmeIntercept") {
 197     // find the point along the DME track, from prev, that is the correct distance
 198     // from the DME
 199     SGGeod prev;
 200     if (!computedPositionForIndex(index - 1, prev)) {
 201       return false;
 202     }
 203
 204     DMEIntercept* di = (DMEIntercept*) w.get();
 205
 206     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 207     SGGeoc navid = SGGeoc::fromGeod(w->position());
 208     double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
 209     SGGeoc rGc;
 210
 211     SGGeoc bPt;
 212     double crs = di->courseDegMagnetic() + magVarFor(prev);
 213     SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
 214
 215     double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
 216     if (dNm < 0.0) {
 217       return false;
 218     }
 219
 220     double az2;
 221     SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
 222     return true;
 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);
 227
 228     SGGeod prevPos;
 229     if (!computedPositionForIndex(index - 1, prevPos)) {
 230       return false;
 231     }
 232
 233     double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
 234
 235     double az2;
 236     SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
 237     return true;
 238   } else if (w->type() == "vectors"){
 239     return false;
 240   } else if (w->type() == "hold") {
 241     r = w->position();
 242     return true;
 243   }
 244
 245   SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
 246   return false;
 247 }
 248
 249 double RoutePath::computeAltitudeForIndex(int index) const
 250 {
 251   if ((index < 0) || (index >= (int) _waypts.size())) {
 252     throw sg_range_exception("waypt index out of range",
 253       "RoutePath::computeAltitudeForIndex");
 254   }
 255
 256   WayptRef w = _waypts[index];
 257   if (w->altitudeRestriction() != RESTRICT_NONE) {
 258     return w->altitudeFt(); // easy!
 259   }
 260
 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);
 267   }
 268
 269   double prevAlt = computeAltitudeForIndex(index - 1);
 270 // find distance to previous, and hence climb/descent
 271   SGGeod pos, prevPos;
 272
 273   if (!computedPositionForIndex(index, pos) ||
 274       !computedPositionForIndex(index - 1, prevPos))
 275   {
 276     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 277     throw sg_range_exception("unable to compute position for waypoints");
 278   }
 279
 280   double d = SGGeodesy::distanceNm(prevPos, pos);
 281   double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
 282
 283   double deltaFt; // change in altitude in feet
 284   if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
 285     deltaFt = -_pathDescentFPM * tMinutes;
 286   } else {
 287     deltaFt = _pathClimbFPM * tMinutes;
 288   }
 289
 290   return prevAlt + deltaFt;
 291 }
 292
 293 double RoutePath::computeTrackForIndex(int index) const
 294 {
 295   if ((index < 0) || (index >= (int) _waypts.size())) {
 296     throw sg_range_exception("waypt index out of range",
 297       "RoutePath::computeTrackForIndex");
 298   }
 299
 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();
 319   }
 320
 321 // course based upon previous and current pos
 322   SGGeod pos, prevPos;
 323
 324   if (!computedPositionForIndex(index, pos) ||
 325       !computedPositionForIndex(index - 1, prevPos))
 326   {
 327     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 328     throw sg_range_exception("unable to compute position for waypoints");
 329   }
 330
 331   return SGGeodesy::courseDeg(prevPos, pos);
 332 }
 333
 334 double RoutePath::distanceForClimb(double climbFt) const
 335 {
 336   double t = 0.0; // in seconds
 337   if (climbFt > 0.0) {
 338     t = (climbFt / _pathClimbFPM) * 60.0;
 339   } else if (climbFt < 0.0) {
 340     t = (climbFt / _pathDescentFPM) * 60.0;
 341   }
 342
 343   return _pathIAS * (t / 3600.0);
 344 }
 345
 346 double RoutePath::magVarFor(const SGGeod& geod) const
 347 {
 348   double jd = globals->get_time_params()->getJD();
 349   return sgGetMagVar(geod, jd);
 350 }
 351