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