bool
AbortTask::fill_reachable(const AIRCRAFT_STATE &state,
AlternateVector &approx_waypoints,
const GlidePolar &polar,
const bool only_airfield,
const bool final_glide)
{
if (task_full()) {
return false;
}
bool found = false;
std::priority_queue<Alternate, AlternateVector, AlternateRank> q;
for (AlternateVector::iterator v = approx_waypoints.begin();
v!=approx_waypoints.end(); ) {
if (only_airfield && !v->first.Flags.Airport) {
++v;
continue;
}
UnorderedTaskPoint t(v->first, task_behaviour);
const GlideResult result = TaskSolution::glide_solution_remaining(t, state, polar);
if (is_reachable(result, final_glide)) {
q.push(std::make_pair(v->first, result));
// remove it since it's already in the list now
approx_waypoints.erase(v);
found = true;
} else {
++v;
}
}
while (!q.empty() && !task_full()) {
const Alternate top = q.top();
tps.push_back(std::make_pair(new UnorderedTaskPoint(top.first, task_behaviour),
top.second));
const int i = tps.size()-1;
if (tps[i].first->get_waypoint().id == active_waypoint) {
activeTaskPoint = i;
}
q.pop();
}
return found;
}
bool
AbortTask::FillReachable(const AircraftState &state,
AlternateVector &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<Alternate, AlternateVector, 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(Alternate(v->waypoint, result));
// remove it since it's already in the list now
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 Alternate top = q.top();
task_points.push_back(AlternateTaskPoint(top.waypoint, task_behaviour,
top.solution));
const int i = task_points.size() - 1;
if (task_points[i].GetWaypoint().id == active_waypoint)
active_task_point = i;
q.pop();
}
return found_final_glide;
}