src/Navaids/routePath.cxx

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