fly/flightgear
1
0
Fork 0
Code Activity

RoutePath fixes.

Browse source 
Should fix:

https://sourceforge.net/p/flightgear/codetickets/1703/
https://sourceforge.net/p/flightgear/codetickets/1939/

Although both of these I had trouble reproducing directly.
This commit is contained in:
James Turner 2017-03-25 15:16:21 +00:00
parent c2da881010
commit 7adb2fa851
1 changed files with 129 additions and 108 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

133
src/Navaids/routePath.cxx
View file

@ -641,9 +641,10 @@ public:
void computeDynamicPosition(int index) void computeDynamicPosition(int index)
{ {
const WayptData& previous(previousValidWaypoint(index)); auto previous(previousValidWaypoint(index));
WayptRef wpt = waypoints[index].wpt; WayptRef wpt = waypoints[index].wpt;
assert(previous.posValid); assert(previous != waypoints.end());
assert(previous->posValid);
const std::string& ty(wpt->type()); const std::string& ty(wpt->type());
if (ty == "hdgToAlt") { if (ty == "hdgToAlt") {
@ -662,17 +663,17 @@ public:
} }
double distanceM = timeToChangeSec * speedMSec; double distanceM = timeToChangeSec * speedMSec;
double hdg = h->headingDegMagnetic() + magVarFor(previous.pos); double hdg = h->headingDegMagnetic() + magVarFor(previous->pos);
waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM); waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, hdg, distanceM);
waypoints[index].posValid = true; waypoints[index].posValid = true;
} else if (ty == "radialIntercept") { } else if (ty == "radialIntercept") {
// start from previous.turnExit // start from previous.turnExit
RadialIntercept* i = (RadialIntercept*) wpt.get(); RadialIntercept* i = (RadialIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position()); SGGeoc navid = SGGeoc::fromGeod(wpt->position());
SGGeoc rGc; SGGeoc rGc;
double magVar = magVarFor(previous.pos); double magVar = magVarFor(previous->pos);
double radial = i->radialDegMagnetic() + magVar; double radial = i->radialDegMagnetic() + magVar;
double track = i->courseDegMagnetic() + magVar; double track = i->courseDegMagnetic() + magVar;
@ -687,7 +688,7 @@ public:
ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc); ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
if (!ok) { if (!ok) {
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:" SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
<< previous.turnExitPos << " / " << track << "/" << wpt->position() << previous->turnExitPos << " / " << track << "/" << wpt->position()
<< "/" << radial); << "/" << radial);
waypoints[index].pos = wpt->position(); // horrible fallback waypoints[index].pos = wpt->position(); // horrible fallback
@ -702,7 +703,7 @@ public:
} else if (ty == "dmeIntercept") { } else if (ty == "dmeIntercept") {
DMEIntercept* di = (DMEIntercept*) wpt.get(); DMEIntercept* di = (DMEIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position()); SGGeoc navid = SGGeoc::fromGeod(wpt->position());
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD; double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc; SGGeoc rGc;
@ -716,12 +717,12 @@ public:
SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed"); SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
waypoints[index].pos = wpt->position(); // horrible fallback waypoints[index].pos = wpt->position(); // horrible fallback
} else { } else {
waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER); waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, crs, dNm * SG_NM_TO_METER);
} }
waypoints[index].posValid = true; waypoints[index].posValid = true;
} else if (ty == "vectors") { } else if (ty == "vectors") {
waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos); waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous->turnExitPos, waypoints[index].pos);
waypoints[index].legCourseValid = true; waypoints[index].legCourseValid = true;
// no turn data // no turn data
} }
@ -914,16 +915,43 @@ public:
} }
} }
const WayptData& previousValidWaypoint(int index) const WayptDataVec::iterator previousValidWaypoint(unsigned int index)
{ {
if (index == 0) {
return waypoints.end();
}
while (waypoints[--index].skipped) {
// waypoint zero should be unskippable, this assert verified that
assert(index > 0); assert(index > 0);
if (waypoints[index-1].skipped) {
return waypoints[index-2];
} }
return waypoints[index-1]; return waypoints.begin() + index;
} }
WayptDataVec::iterator previousValidWaypoint(WayptDataVec::iterator it)
{
return previousValidWaypoint(std::distance(waypoints.begin(), it));
}
WayptDataVec::iterator nextValidWaypoint(int index)
{
return nextValidWaypoint(waypoints.begin() + index);
}
WayptDataVec::iterator nextValidWaypoint(WayptDataVec::iterator it)
{
if (it == waypoints.end()) {
return it;
}
++it;
while ((it != waypoints.end()) && it->skipped) {
++it;
}
return it;
}
}; // of RoutePathPrivate class }; // of RoutePathPrivate class
RoutePath::RoutePath(const flightgear::FlightPlan* fp) : RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
@ -932,7 +960,7 @@ RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
for (int l=0; l<fp->numLegs(); ++l) { for (int l=0; l<fp->numLegs(); ++l) {
Waypt *wpt = fp->legAtIndex(l)->waypoint(); Waypt *wpt = fp->legAtIndex(l)->waypoint();
if (!wpt) { if (!wpt) {
SG_LOG(SG_NAVAID, SG_ALERT, "Waypoint " << l << " of " << fp->numLegs() << "is NULL"); SG_LOG(SG_NAVAID, SG_DEV_ALERT, "Waypoint " << l << " of " << fp->numLegs() << "is NULL");
break; break;
} }
d->waypoints.push_back(WayptData(wpt)); d->waypoints.push_back(WayptData(wpt));
@ -971,21 +999,18 @@ void RoutePath::commonInit()
double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi(); double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();
if (i > 0) { if (i > 0) {
const WayptData& prev(d->previousValidWaypoint(i)); auto prevIt = d->previousValidWaypoint(i);
d->waypoints[i].computeLegCourse(prev, radiusM); assert(prevIt != d->waypoints.end());
d->waypoints[i].computeLegCourse(*prevIt, radiusM);
d->computeDynamicPosition(i); d->computeDynamicPosition(i);
} }
if (i < (d->waypoints.size() - 1)) { auto nextIt = d->nextValidWaypoint(i);
int nextIndex = i + 1; if (nextIt != d->waypoints.end()) {
if (d->waypoints[nextIndex].skipped) { nextIt->computeLegCourse(d->waypoints[i], radiusM);
nextIndex++;
}
WayptData& next(d->waypoints[nextIndex]);
next.computeLegCourse(d->waypoints[i], radiusM);
if (next.legCourseValid) { if (nextIt->legCourseValid) {
d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, next); d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, *nextIt);
} else { } else {
// next waypoint has indeterminate course. Let's create a sharp turn // next waypoint has indeterminate course. Let's create a sharp turn
// this can happen when the following point is ATC vectors, for example. // this can happen when the following point is ATC vectors, for example.
@ -1030,11 +1055,11 @@ SGGeodVec RoutePath::pathForIndex(int index) const
return pathForHold((Hold*) d->waypoints[index].wpt.get()); return pathForHold((Hold*) d->waypoints[index].wpt.get());
} }
if (index > 0) { auto prevIt = d->previousValidWaypoint(index);
const WayptData& prev(d->previousValidWaypoint(index)); if (prevIt != d->waypoints.end()) {
prev.turnExitPath(r); prevIt->turnExitPath(r);
SGGeod from = prev.turnExitPos, SGGeod from = prevIt->turnExitPos,
to = w.turnEntryPos; to = w.turnEntryPos;
// compute rounding offset, we want to round towards the direction of travel // compute rounding offset, we want to round towards the direction of travel
@ -1089,16 +1114,17 @@ SGGeod RoutePath::positionForIndex(int index) const
SGGeodVec RoutePath::pathForVia(Via* via, int index) const SGGeodVec RoutePath::pathForVia(Via* via, int index) const
{ {
// previous waypoint must be valid for a VIA // previous waypoint must be valid for a VIA
if ((index == 0) || !d->waypoints[index-1].posValid) { auto prevIt = d->previousValidWaypoint(index);
if (prevIt == d->waypoints.end()) {
return SGGeodVec(); return SGGeodVec();
} }
const WayptData& prev(d->waypoints[index-1]); WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
SGGeodVec r; SGGeodVec r;
WayptVec::const_iterator it; WayptVec::const_iterator it;
SGGeod legStart = prev.wpt->position(); SGGeod legStart = prevIt->wpt->position();
for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) { for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) {
// interpolate directly into the result vector // interpolate directly into the result vector
interpolateGreatCircle(legStart, (*it)->position(), r); interpolateGreatCircle(legStart, (*it)->position(), r);
@ -1155,28 +1181,25 @@ double RoutePath::computeDistanceForIndex(int index) const
"RoutePath::computeDistanceForIndex"); "RoutePath::computeDistanceForIndex");
} }
if (index == 0) { auto it = d->waypoints.begin() + index;
if ((index == 0) || it->skipped) {
// first waypoint, distance is 0 // first waypoint, distance is 0
return 0.0; return 0.0;
} }
if (d->waypoints[index].skipped) { if (it->wpt->type() == "via") {
return 0.0; return distanceForVia(static_cast<Via*>(it->wpt.get()), index);
} }
auto prevIt = d->previousValidWaypoint(index);
assert(prevIt != d->waypoints.end());
if (d->waypoints[index].wpt->type() == "via") { double dist = SGGeodesy::distanceM(prevIt->turnExitPos, it->turnEntryPos);
return distanceForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index); dist += prevIt->turnDistanceM();
}
const WayptData& prev(d->previousValidWaypoint(index)); if (!it->flyOver) {
double dist = SGGeodesy::distanceM(prev.turnExitPos,
d->waypoints[index].turnEntryPos);
dist += prev.turnDistanceM();
if (!d->waypoints[index].flyOver) {
// add entry distance // add entry distance
dist += d->waypoints[index].turnDistanceM(); dist += it->turnDistanceM();
} }
return dist; return dist;
@ -1184,20 +1207,19 @@ double RoutePath::computeDistanceForIndex(int index) const
double RoutePath::distanceForVia(Via* via, int index) const double RoutePath::distanceForVia(Via* via, int index) const
{ {
// previous waypoint must be valid for a VIA auto prevIt = d->previousValidWaypoint(index);
if ((index == 0) || !d->waypoints[index-1].posValid) { if (prevIt == d->waypoints.end()) {
return 0.0; return 0.0;
} }
const WayptData& prev(d->waypoints[index-1]); WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
double dist = 0.0; double dist = 0.0;
WayptVec::const_iterator it; WayptVec::const_iterator it;
SGGeod legStart = prev.wpt->position(); SGGeod legStart = prevIt->wpt->position();
for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) { for (auto wp : enrouteWaypoints) {
dist += SGGeodesy::distanceM(legStart, (*it)->position()); dist += SGGeodesy::distanceM(legStart, wp->position());
legStart = (*it)->position(); legStart = wp->position();
} }
return dist; return dist;
@ -1205,7 +1227,6 @@ double RoutePath::distanceForVia(Via* via, int index) const
double RoutePath::trackForIndex(int index) const double RoutePath::trackForIndex(int index) const
{ {
if (d->waypoints[index].skipped) if (d->waypoints[index].skipped)
return trackForIndex(index - 1); return trackForIndex(index - 1);