bool
AATPoint::IsCloseToTarget(const AircraftState& state, const fixed threshold) const
{
if (!valid())
return false;
return DoubleLegDistance(state.location)
- DoubleLegDistance(target_location) > -threshold;
}
bool
AATPoint::CheckTargetInside(const AircraftState& state)
{
/* target must be moved if d(p_last,t)+d(t,p_next) <
d(p_last,state)+d(state,p_next) */
if (!IsCloseToTarget(state))
return false;
if (positive(DoubleLegDistance(state.location)
- DoubleLegDistance(GetLocationMax())))
// no improvement available
return false;
target_location = state.location;
return true;
}
void AATDistance::ShiftTargetFromBehind(double longitude, double latitude,
int taskwaypoint) {
// JMWAAT if being externally updated e.g. from task dialog, don't move it
if (TargetDialogOpen) return;
if (taskwaypoint==0) return;
// best is decreasing or first entry in sector, so project
// target in direction of improvement or first entry into sector
double course_bearing;
double course_bearing_orig;
double d_total_orig;
double d_total_this;
d_total_this = DoubleLegDistance(taskwaypoint,
longitude,
latitude);
d_total_orig = DoubleLegDistance(taskwaypoint,
Task[taskwaypoint].AATTargetLon,
Task[taskwaypoint].AATTargetLat);
if (d_total_this>d_total_orig-2.0*AATCloseDistance()) {
// this is better than the previous best! (or very close)
ShiftTargetFromInFront(longitude, latitude, taskwaypoint);
return;
}
// JMWAAT if locked, don't move it
if (Task[taskwaypoint].AATTargetLocked) {
// 20080615 JMW don't do this; locked stays locked
// Task[taskwaypoint].AATTargetLocked = false; // JMWAAT JB
return;
}
/*
// check to see if deviation is big enough to adjust target along track
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
latitude,
longitude,
NULL, &course_bearing);
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
Task[taskwaypoint].AATTargetLat,
Task[taskwaypoint].AATTargetLon,
NULL, &course_bearing_orig);
if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
// don't update it if course deviation is less than 5 degrees,
// otherwise we end up wasting a lot of CPU in recalculating, and also
// the target ends up drifting.
return;
}
course_bearing = AngleLimit360(course_bearing+
Task[taskwaypoint].AATTargetOffsetRadial);
//JMWAAT Task[taskwaypoint].AATTargetOffsetRadial = course_bearing;
*/
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
latitude,
longitude,
NULL, &course_bearing);
course_bearing = AngleLimit360(course_bearing+
Task[taskwaypoint].AATTargetOffsetRadial);
DistanceBearing(latitude,
longitude,
Task[taskwaypoint].AATTargetLat,
Task[taskwaypoint].AATTargetLon,
NULL, &course_bearing_orig);
if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
// don't update it if course deviation is less than 5 degrees,
// otherwise we end up wasting a lot of CPU in recalculating, and also
// the target ends up drifting.
return;
}
double max_distance =
FindInsideAATSectorDistance(latitude,
longitude,
taskwaypoint,
course_bearing,
0);
// total distance of legs from previous through this to next target
double delta = max_distance/2;
// move target in line with previous target along track
// at an offset to improve on max distance
double t_distance_lower = 0;
double t_distance = delta*2;
int steps = 0;
do {
// find target position along projected line but
// make sure it is in sector, and set at a distance
// to preserve total task distance
// we are aiming to make d_total_this = d_total_orig
double t_lat, t_lon;
FindLatitudeLongitude(latitude, longitude,
course_bearing, t_distance,
&t_lat,
&t_lon);
if (InAATTurnSector(t_lon, t_lat, taskwaypoint, 0)) {
d_total_this = DoubleLegDistance(taskwaypoint,
t_lon,
t_lat);
if (d_total_orig - d_total_this>0.0) {
t_distance_lower = t_distance;
// ok, can go further
t_distance += delta;
} else {
t_distance -= delta;
}
} else {
t_distance -= delta;
}
delta /= 2.0;
} while ((delta>5.0) && (steps++<20));
// now scan to edge of sector to find approximate range %
if (t_distance_lower>5.0) {
FindLatitudeLongitude(latitude, longitude,
course_bearing, t_distance_lower,
&Task[taskwaypoint].AATTargetLat,
&Task[taskwaypoint].AATTargetLon);
UpdateTargetAltitude(Task[taskwaypoint]);
Task[taskwaypoint].AATTargetOffsetRadius =
FindInsideAATSectorRange(latitude,
longitude,
taskwaypoint, course_bearing,
t_distance_lower);
TargetModified = true;
CalculateAATIsoLines();
}
// if ((!t_in_sector) && (d_diff_total>1.0)) {
// JMW TODO enhancement: this is too short now so need to lengthen the
// next waypoint if possible
// (re discussion with paul mander)
// }
}
