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) // } }