← Back to branch summary

~ubuntu-branches/ubuntu/precise/flightgear/precise

« back to all changes in this revision

Viewing changes to src/Navaids/routePath.cxx

  • Committer: Package Import Robot
  • Author(s): Ove Kaaven
  • Date: 2011-09-03 22:16:12 UTC
  • mfrom: (3.1.9 sid)
  • Revision ID: package-import@ubuntu.com-20110903221612-2cjy0z7ztj5nkln5
Tags: 2.4.0-1
http://bugs.debian.org/638588
* New upstream release. Closes: #638588.
* Build-Depend on OpenSceneGraph 3.0, and the Subversion library.
* Recommend fgfs-scenery-base.
* Enable parallel builds (shorter compile times on multicore CPUs).
* Removed hack that tried to build without optimizations if
  building with optimizations fails.

Show diffs side-by-side

added added

removed removed

Lines of Context:
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