fixed
OrderedTask::ScanDistanceMax()
{
if (task_points.empty()) // nothing to do!
return fixed_zero;
assert(active_task_point < task_points.size());
// for max calculations, since one can still travel further in the
// sector, we pretend we are on the previous turnpoint so the
// search samples will contain the full boundary
const unsigned atp = active_task_point;
if (atp) {
active_task_point--;
taskpoint_start->ScanActive(*task_points[active_task_point]);
}
if (dijkstra_max == NULL)
dijkstra_max = new TaskDijkstraMax(*this);
if (dijkstra_max->DistanceMax()) {
for (unsigned i = 0, active = GetActiveIndex(), end = TaskSize();
i != end; ++i) {
const SearchPoint &solution = dijkstra_max->GetSolution(i);
SetPointSearchMax(i, solution);
if (i <= active)
set_tp_search_achieved(i, solution);
}
}
if (atp) {
active_task_point = atp;
taskpoint_start->ScanActive(*task_points[active_task_point]);
}
return taskpoint_start->ScanDistanceMax();
}
inline bool
OrderedTask::RunDijsktraMax()
{
const unsigned task_size = TaskSize();
if (task_size < 2)
return false;
if (dijkstra_max == nullptr)
dijkstra_max = new TaskDijkstraMax();
TaskDijkstraMax &dijkstra = *dijkstra_max;
const unsigned active_index = GetActiveIndex();
dijkstra.SetTaskSize(task_size);
for (unsigned i = 0; i != task_size; ++i) {
const SearchPointVector &boundary = i == active_index
/* since one can still travel further in the current sector, use
the full boundary here */
? task_points[i]->GetBoundaryPoints()
: task_points[i]->GetSearchPoints();
dijkstra_max->SetBoundary(i, boundary);
}
double start_radius(-1), finish_radius(-1);
if (subtract_start_finish_cylinder_radius) {
/* to subtract the start/finish cylinder radius, we use only the
nominal points (i.e. the cylinder's center), and later replace
it with a point on the cylinder boundary */
const auto &start = *task_points.front();
start_radius = GetCylinderRadiusOrMinusOne(start);
if (start_radius > 0)
dijkstra.SetBoundary(0, start.GetNominalPoints());
const auto &finish = *task_points.back();
finish_radius = GetCylinderRadiusOrMinusOne(finish);
if (finish_radius > 0)
dijkstra.SetBoundary(task_size - 1, finish.GetNominalPoints());
}
if (!dijkstra_max->DistanceMax())
return false;
for (unsigned i = 0; i != task_size; ++i) {
SearchPoint solution = dijkstra.GetSolution(i);
if (i == 0 && start_radius > 0) {
/* subtract start cylinder radius by finding the intersection
with the cylinder boundary */
const GeoPoint ¤t = task_points.front()->GetLocation();
const GeoPoint &neighbour = dijkstra.GetSolution(i + 1).GetLocation();
GeoPoint gp = current.IntermediatePoint(neighbour, start_radius);
solution = SearchPoint(gp, task_projection);
}
if (i == task_size - 1 && finish_radius > 0) {
/* subtract finish cylinder radius by finding the intersection
with the cylinder boundary */
const GeoPoint ¤t = task_points.back()->GetLocation();
const GeoPoint &neighbour = dijkstra.GetSolution(i - 1).GetLocation();
GeoPoint gp = current.IntermediatePoint(neighbour, finish_radius);
solution = SearchPoint(gp, task_projection);
}
SetPointSearchMax(i, solution);
if (i <= active_index)
set_tp_search_achieved(i, solution);
}
return true;
}