bool
AbortTask::FillReachable(const AircraftState &state,
AlternateList &approx_waypoints,
const GlidePolar &polar, bool only_airfield,
bool final_glide, bool safety)
{
if (IsTaskFull() || approx_waypoints.empty())
return false;
const AGeoPoint p_start(state.location, state.altitude);
bool found_final_glide = false;
reservable_priority_queue<AlternatePoint, AlternateList, AbortRank> q;
q.reserve(32);
for (auto v = approx_waypoints.begin(); v != approx_waypoints.end();) {
if (only_airfield && !v->waypoint.IsAirport()) {
++v;
continue;
}
UnorderedTaskPoint t(v->waypoint, task_behaviour);
GlideResult result =
TaskSolution::GlideSolutionRemaining(t, state,
task_behaviour.glide, polar);
if (IsReachable(result, final_glide)) {
bool intersects = false;
const bool is_reachable_final = IsReachable(result, true);
if (intersection_test && final_glide && is_reachable_final)
intersects = intersection_test->Intersects(
AGeoPoint(v->waypoint.location, result.min_arrival_altitude));
if (!intersects) {
q.push(AlternatePoint(v->waypoint, result));
// remove it since it's already in the list now
v = approx_waypoints.erase(v);
if (is_reachable_final)
found_final_glide = true;
continue; // skip incrementing v since we just erased it
}
}
++v;
}
while (!q.empty() && !IsTaskFull()) {
const AlternatePoint top = q.top();
task_points.emplace_back(top.waypoint, task_behaviour, top.solution);
const int i = task_points.size() - 1;
if (task_points[i].point.GetWaypoint().id == active_waypoint)
active_task_point = i;
q.pop();
}
return found_final_glide;
}
