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/FlightPlan.hxx>
  16 #include <Navaids/positioned.hxx>
  17
  18 namespace flightgear {
  19   bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
  20 }
  21
  22 using namespace flightgear;
  23
  24 // implement Point(s) known distance from a great circle
  25
  26 static double sqr(const double x)
  27 {
  28   return x * x;
  29 }
  30
  31 double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
  32 {
  33   double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
  34   double bDist = SGGeodesy::distanceRad(a, d);
  35
  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);
  38
  39   double p = atan2(sin(bDist)*cos(A), cos(bDist));
  40
  41   if (sqr(cos(dist)) > sqr(r)) {
  42     SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
  43     return -1.0;
  44   }
  45
  46   double dp1 = p + acos(cos(dist)/r);
  47   double dp2 = p - acos(cos(dist)/r);
  48
  49   double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
  50   double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
  51
  52   return SGMiscd::min(dp1Nm, dp2Nm);
  53 }
  54
  55 RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
  56   _waypts(wpts)
  57 {
  58   commonInit();
  59 }
  60
  61 RoutePath::RoutePath(const flightgear::FlightPlan* fp)
  62 {
  63   for (int l=0; l<fp->numLegs(); ++l) {
  64     _waypts.push_back(fp->legAtIndex(l)->waypoint());
  65   }
  66   commonInit();
  67 }
  68
  69 void RoutePath::commonInit()
  70 {
  71   _pathClimbFPM = 1200;
  72   _pathDescentFPM = 800;
  73   _pathIAS = 190;
  74   _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
  75 }
  76
  77 SGGeodVec RoutePath::pathForIndex(int index) const
  78 {
  79   if (index == 0) {
  80     return SGGeodVec(); // no path for first waypoint
  81   }
  82
  83   if (_waypts[index]->type() == "vectors") {
  84     return SGGeodVec(); // empty
  85   }
  86
  87   if (_waypts[index]->type() == "hold") {
  88     return pathForHold((Hold*) _waypts[index].get());
  89   }
  90
  91   SGGeodVec r;
  92   SGGeod pos;
  93   if (!computedPositionForIndex(index-1, pos)) {
  94     return SGGeodVec();
  95   }
  96
  97   r.push_back(pos);
  98   if (!computedPositionForIndex(index, pos)) {
  99     return SGGeodVec();
 100   }
 101
 102   r.push_back(pos);
 103
 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());
 109   }
 110
 111   return r;
 112 }
 113
 114 SGGeod RoutePath::positionForIndex(int index) const
 115 {
 116   SGGeod r;
 117   bool ok = computedPositionForIndex(index, r);
 118   if (!ok) {
 119     return SGGeod();
 120   }
 121
 122   return r;
 123 }
 124
 125 SGGeodVec RoutePath::pathForHold(Hold* hold) const
 126 {
 127   int turnSteps = 16;
 128   double hdg = hold->inboundRadial();
 129   double turnDelta = 180.0 / turnSteps;
 130
 131   SGGeodVec r;
 132   double az2;
 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;
 139
 140   if (hold->isLeftHanded()) {
 141     turnDelta = -turnDelta;
 142   }
 143   SGGeod pos = hold->position();
 144   r.push_back(pos);
 145
 146   // turn+leg sides are a mirror
 147   for (int j=0; j < 2; ++j) {
 148   // turn
 149     for (int i=0;i<turnSteps; ++i) {
 150       hdg += turnDelta;
 151       SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
 152       r.push_back(pos);
 153     }
 154
 155   // leg
 156     SGGeodesy::direct(pos, hdg, legDist, pos, az2);
 157     r.push_back(pos);
 158   } // of leg+turn duplication
 159
 160   return r;
 161 }
 162
 163 /**
 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
 166  * change
 167  */
 168 class RoutePath::PathCtx
 169 {
 170 public:
 171   SGGeod pos;
 172   double heading;
 173 };
 174
 175 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
 176 {
 177   if ((index < 0) || (index >= (int) _waypts.size())) {
 178     throw sg_range_exception("waypt index out of range",
 179       "RoutePath::computedPositionForIndex");
 180   }
 181
 182   WayptRef w = _waypts[index];
 183   if (!w->flag(WPT_DYNAMIC)) {
 184     r = w->position();
 185     return true;
 186   }
 187
 188   if (w->type() == "radialIntercept") {
 189     // radial intersection along track
 190     SGGeod prev;
 191     if (!computedPositionForIndex(index - 1, prev)) {
 192       return false;
 193     }
 194
 195     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 196     SGGeoc navid = SGGeoc::fromGeod(w->position());
 197     SGGeoc rGc;
 198     double magVar = magVarFor(prev);
 199
 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);
 204     if (!ok) {
 205       return false;
 206     }
 207
 208     r = SGGeod::fromGeoc(rGc);
 209     return true;
 210   } else if (w->type() == "dmeIntercept") {
 211     // find the point along the DME track, from prev, that is the correct distance
 212     // from the DME
 213     SGGeod prev;
 214     if (!computedPositionForIndex(index - 1, prev)) {
 215       return false;
 216     }
 217
 218     DMEIntercept* di = (DMEIntercept*) w.get();
 219
 220     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 221     SGGeoc navid = SGGeoc::fromGeod(w->position());
 222     double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
 223     SGGeoc rGc;
 224
 225     SGGeoc bPt;
 226     double crs = di->courseDegMagnetic() + magVarFor(prev);
 227     SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
 228
 229     double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
 230     if (dNm < 0.0) {
 231       return false;
 232     }
 233
 234     double az2;
 235     SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
 236     return true;
 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);
 241
 242     SGGeod prevPos;
 243     if (!computedPositionForIndex(index - 1, prevPos)) {
 244       return false;
 245     }
 246
 247     double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
 248
 249     double az2;
 250     SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
 251     return true;
 252   } else if (w->type() == "vectors"){
 253     return false;
 254   } else if (w->type() == "hold") {
 255     r = w->position();
 256     return true;
 257   }
 258
 259   SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
 260   return false;
 261 }
 262
 263 double RoutePath::computeAltitudeForIndex(int index) const
 264 {
 265   if ((index < 0) || (index >= (int) _waypts.size())) {
 266     throw sg_range_exception("waypt index out of range",
 267       "RoutePath::computeAltitudeForIndex");
 268   }
 269
 270   WayptRef w = _waypts[index];
 271   if (w->altitudeRestriction() != RESTRICT_NONE) {
 272     return w->altitudeFt(); // easy!
 273   }
 274
 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);
 281   }
 282
 283   double prevAlt = computeAltitudeForIndex(index - 1);
 284 // find distance to previous, and hence climb/descent
 285   SGGeod pos, prevPos;
 286
 287   if (!computedPositionForIndex(index, pos) ||
 288       !computedPositionForIndex(index - 1, prevPos))
 289   {
 290     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 291     throw sg_range_exception("unable to compute position for waypoints");
 292   }
 293
 294   double d = SGGeodesy::distanceNm(prevPos, pos);
 295   double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
 296
 297   double deltaFt; // change in altitude in feet
 298   if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
 299     deltaFt = -_pathDescentFPM * tMinutes;
 300   } else {
 301     deltaFt = _pathClimbFPM * tMinutes;
 302   }
 303
 304   return prevAlt + deltaFt;
 305 }
 306
 307 double RoutePath::computeTrackForIndex(int index) const
 308 {
 309   if ((index < 0) || (index >= (int) _waypts.size())) {
 310     throw sg_range_exception("waypt index out of range",
 311       "RoutePath::computeTrackForIndex");
 312   }
 313
 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();
 333   }
 334
 335 // course based upon previous and current pos
 336   SGGeod pos, prevPos;
 337
 338   if (!computedPositionForIndex(index, pos) ||
 339       !computedPositionForIndex(index - 1, prevPos))
 340   {
 341     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 342     throw sg_range_exception("unable to compute position for waypoints");
 343   }
 344
 345   return SGGeodesy::courseDeg(prevPos, pos);
 346 }
 347
 348 double RoutePath::distanceForClimb(double climbFt) const
 349 {
 350   double t = 0.0; // in seconds
 351   if (climbFt > 0.0) {
 352     t = (climbFt / _pathClimbFPM) * 60.0;
 353   } else if (climbFt < 0.0) {
 354     t = (climbFt / _pathDescentFPM) * 60.0;
 355   }
 356
 357   return _pathIAS * (t / 3600.0);
 358 }
 359
 360 double RoutePath::magVarFor(const SGGeod& geod) const
 361 {
 362   double jd = globals->get_time_params()->getJD();
 363   return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
 364 }
 365