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; }