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   commonInit();
  58 }
  59
  60 RoutePath::RoutePath(const flightgear::FlightPlan* fp)
  61 {
  62   for (int l=0; l<fp->numLegs(); ++l) {
  63     _waypts.push_back(fp->legAtIndex(l)->waypoint());
  64   }
  65   commonInit();
  66 }
  67
  68 void RoutePath::commonInit()
  69 {
  70   _pathClimbFPM = 1200;
  71   _pathDescentFPM = 800;
  72   _pathIAS = 190;
  73   _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
  74 }
  75
  76 SGGeodVec RoutePath::pathForIndex(int index) const
  77 {
  78   if (index == 0) {
  79     return SGGeodVec(); // no path for first waypoint
  80   }
  81
  82   if (_waypts[index]->type() == "vectors") {
  83     return SGGeodVec(); // empty
  84   }
  85
  86   if (_waypts[index]->type() == "hold") {
  87     return pathForHold((Hold*) _waypts[index].get());
  88   }
  89
  90   SGGeodVec r;
  91   SGGeod pos;
  92   if (!computedPositionForIndex(index-1, pos)) {
  93     return SGGeodVec();
  94   }
  95
  96   r.push_back(pos);
  97   if (!computedPositionForIndex(index, pos)) {
  98     return SGGeodVec();
  99   }
 100
 101   r.push_back(pos);
 102
 103   if (_waypts[index]->type() == "runway") {
 104     // runways get an extra point, at the end. this is particularly
 105     // important so missed approach segments draw correctly
 106     FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
 107     r.push_back(rwy->end());
 108   }
 109
 110   return r;
 111 }
 112
 113 SGGeod RoutePath::positionForIndex(int index) const
 114 {
 115   SGGeod r;
 116   bool ok = computedPositionForIndex(index, r);
 117   if (!ok) {
 118     return SGGeod();
 119   }
 120
 121   return r;
 122 }
 123
 124 SGGeodVec RoutePath::pathForHold(Hold* hold) const
 125 {
 126   int turnSteps = 16;
 127   double hdg = hold->inboundRadial();
 128   double turnDelta = 180.0 / turnSteps;
 129
 130   SGGeodVec r;
 131   double az2;
 132   double stepTime = turnDelta / _pathTurnRate; // in seconds
 133   double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
 134   double legDist = hold->isDistance() ?
 135     hold->timeOrDistance()
 136     : _pathIAS * (hold->timeOrDistance() / 3600.0);
 137   legDist *= SG_NM_TO_METER;
 138
 139   if (hold->isLeftHanded()) {
 140     turnDelta = -turnDelta;
 141   }
 142   SGGeod pos = hold->position();
 143   r.push_back(pos);
 144
 145   // turn+leg sides are a mirror
 146   for (int j=0; j < 2; ++j) {
 147   // turn
 148     for (int i=0;i<turnSteps; ++i) {
 149       hdg += turnDelta;
 150       SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
 151       r.push_back(pos);
 152     }
 153
 154   // leg
 155     SGGeodesy::direct(pos, hdg, legDist, pos, az2);
 156     r.push_back(pos);
 157   } // of leg+turn duplication
 158
 159   return r;
 160 }
 161
 162 /**
 163  * the path context holds the state of of an imaginary aircraft traversing
 164  * the route, and limits the rate at which heading / altitude / position can
 165  * change
 166  */
 167 class RoutePath::PathCtx
 168 {
 169 public:
 170   SGGeod pos;
 171   double heading;
 172 };
 173
 174 bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
 175 {
 176   if ((index < 0) || (index >= (int) _waypts.size())) {
 177     throw sg_range_exception("waypt index out of range",
 178       "RoutePath::computedPositionForIndex");
 179   }
 180
 181   WayptRef w = _waypts[index];
 182   if (!w->flag(WPT_DYNAMIC)) {
 183     r = w->position();
 184     return true;
 185   }
 186
 187   if (w->type() == "radialIntercept") {
 188     // radial intersection along track
 189     SGGeod prev;
 190     if (!computedPositionForIndex(index - 1, prev)) {
 191       return false;
 192     }
 193
 194     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 195     SGGeoc navid = SGGeoc::fromGeod(w->position());
 196     SGGeoc rGc;
 197     double magVar = magVarFor(prev);
 198
 199     RadialIntercept* i = (RadialIntercept*) w.get();
 200     double radial = i->radialDegMagnetic() + magVar;
 201     double track = i->courseDegMagnetic() + magVar;
 202     bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
 203     if (!ok) {
 204       return false;
 205     }
 206
 207     r = SGGeod::fromGeoc(rGc);
 208     return true;
 209   } else if (w->type() == "dmeIntercept") {
 210     // find the point along the DME track, from prev, that is the correct distance
 211     // from the DME
 212     SGGeod prev;
 213     if (!computedPositionForIndex(index - 1, prev)) {
 214       return false;
 215     }
 216
 217     DMEIntercept* di = (DMEIntercept*) w.get();
 218
 219     SGGeoc prevGc = SGGeoc::fromGeod(prev);
 220     SGGeoc navid = SGGeoc::fromGeod(w->position());
 221     double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
 222     SGGeoc rGc;
 223
 224     SGGeoc bPt;
 225     double crs = di->courseDegMagnetic() + magVarFor(prev);
 226     SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
 227
 228     double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
 229     if (dNm < 0.0) {
 230       return false;
 231     }
 232
 233     double az2;
 234     SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
 235     return true;
 236   } else if (w->type() == "hdgToAlt") {
 237     HeadingToAltitude* h = (HeadingToAltitude*) w.get();
 238     double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
 239     double d = distanceForClimb(climb);
 240
 241     SGGeod prevPos;
 242     if (!computedPositionForIndex(index - 1, prevPos)) {
 243       return false;
 244     }
 245
 246     double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
 247
 248     double az2;
 249     SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
 250     return true;
 251   } else if (w->type() == "vectors"){
 252     return false;
 253   } else if (w->type() == "hold") {
 254     r = w->position();
 255     return true;
 256   }
 257
 258   SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
 259   return false;
 260 }
 261
 262 double RoutePath::computeAltitudeForIndex(int index) const
 263 {
 264   if ((index < 0) || (index >= (int) _waypts.size())) {
 265     throw sg_range_exception("waypt index out of range",
 266       "RoutePath::computeAltitudeForIndex");
 267   }
 268
 269   WayptRef w = _waypts[index];
 270   if (w->altitudeRestriction() != RESTRICT_NONE) {
 271     return w->altitudeFt(); // easy!
 272   }
 273
 274   if (w->type() == "runway") {
 275     FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
 276     return rwy->threshold().getElevationFt();
 277   } else if ((w->type() == "hold") || (w->type() == "vectors")) {
 278     // pretend we don't change altitude in holds/vectoring
 279     return computeAltitudeForIndex(index - 1);
 280   }
 281
 282   double prevAlt = computeAltitudeForIndex(index - 1);
 283 // find distance to previous, and hence climb/descent
 284   SGGeod pos, prevPos;
 285
 286   if (!computedPositionForIndex(index, pos) ||
 287       !computedPositionForIndex(index - 1, prevPos))
 288   {
 289     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 290     throw sg_range_exception("unable to compute position for waypoints");
 291   }
 292
 293   double d = SGGeodesy::distanceNm(prevPos, pos);
 294   double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
 295
 296   double deltaFt; // change in altitude in feet
 297   if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
 298     deltaFt = -_pathDescentFPM * tMinutes;
 299   } else {
 300     deltaFt = _pathClimbFPM * tMinutes;
 301   }
 302
 303   return prevAlt + deltaFt;
 304 }
 305
 306 double RoutePath::computeTrackForIndex(int index) const
 307 {
 308   if ((index < 0) || (index >= (int) _waypts.size())) {
 309     throw sg_range_exception("waypt index out of range",
 310       "RoutePath::computeTrackForIndex");
 311   }
 312
 313   WayptRef w = _waypts[index];
 314   if (w->type() == "radialIntercept") {
 315     RadialIntercept* r = (RadialIntercept*) w.get();
 316     return r->courseDegMagnetic();
 317   } else if (w->type() == "dmeIntercept") {
 318     DMEIntercept* d = (DMEIntercept*) w.get();
 319     return d->courseDegMagnetic();
 320   } else if (w->type() == "hdgToAlt") {
 321     HeadingToAltitude* h = (HeadingToAltitude*) w.get();
 322     return h->headingDegMagnetic();
 323   } else if (w->type() == "hold") {
 324     Hold* h = (Hold*) w.get();
 325     return h->inboundRadial();
 326   } else if (w->type() == "vectors") {
 327     SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
 328     throw sg_range_exception("asked for track from vectors waypt");
 329   } else if (w->type() == "runway") {
 330     FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
 331     return rwy->headingDeg();
 332   }
 333
 334 // course based upon previous and current pos
 335   SGGeod pos, prevPos;
 336
 337   if (!computedPositionForIndex(index, pos) ||
 338       !computedPositionForIndex(index - 1, prevPos))
 339   {
 340     SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
 341     throw sg_range_exception("unable to compute position for waypoints");
 342   }
 343
 344   return SGGeodesy::courseDeg(prevPos, pos);
 345 }
 346
 347 double RoutePath::distanceForClimb(double climbFt) const
 348 {
 349   double t = 0.0; // in seconds
 350   if (climbFt > 0.0) {
 351     t = (climbFt / _pathClimbFPM) * 60.0;
 352   } else if (climbFt < 0.0) {
 353     t = (climbFt / _pathDescentFPM) * 60.0;
 354   }
 355
 356   return _pathIAS * (t / 3600.0);
 357 }
 358
 359 double RoutePath::magVarFor(const SGGeod& geod) const
 360 {
 361   double jd = globals->get_time_params()->getJD();
 362   return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
 363 }
 364