double
OrderedTask::ScanDistanceNominal()
{
if (task_points.empty())
return 0;
const auto &start = *task_points.front();
auto d = start.ScanDistanceNominal();
auto radius = GetCylinderRadiusOrMinusOne(start);
if (radius > 0 && radius < d)
d -= radius;
const auto &finish = *task_points.back();
radius = GetCylinderRadiusOrMinusOne(finish);
if (radius > 0 && radius < d)
d -= radius;
return d;
}
fixed
OrderedTask::ScanDistanceNominal()
{
if (task_points.empty())
return fixed(0);
const auto &start = *task_points.front();
fixed d = start.ScanDistanceNominal();
fixed radius = GetCylinderRadiusOrMinusOne(start);
if (positive(radius) && radius < d)
d -= radius;
const auto &finish = *task_points.back();
radius = GetCylinderRadiusOrMinusOne(finish);
if (positive(radius) && radius < d)
d -= radius;
return d;
}
fixed
OrderedTask::ScanDistanceNominal()
{
if (taskpoint_start == nullptr)
return fixed(0);
fixed d = taskpoint_start->ScanDistanceNominal();
if (subtract_start_finish_cylinder_radius) {
fixed radius = GetCylinderRadiusOrMinusOne(*taskpoint_start);
if (positive(radius) && radius < d)
d -= radius;
if (taskpoint_finish != nullptr) {
radius = GetCylinderRadiusOrMinusOne(*taskpoint_finish);
if (positive(radius) && radius < d)
d -= radius;
}
}
return d;
}
/**
* Determine the cylinder radius if this is a CylinderZone. If not,
* return -1.
*/
gcc_pure
static double
GetCylinderRadiusOrMinusOne(const ObservationZoneClient &p)
{
return GetCylinderRadiusOrMinusOne(p.GetObservationZone());
}
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;
}
