bool
OrderedTask::Commit(const OrderedTask& that)
{
bool modified = false;
SetName(that.GetName());
// change mode to that one
SetFactory(that.factory_mode);
// copy across behaviour
SetOrderedTaskSettings(that.ordered_settings);
// remove if that task is smaller than this one
while (TaskSize() > that.TaskSize()) {
Remove(TaskSize() - 1);
modified = true;
}
// ensure each task point made identical
for (unsigned i = 0; i < that.TaskSize(); ++i) {
if (i >= TaskSize()) {
// that task is larger than this
Append(*that.task_points[i]);
modified = true;
} else if (!task_points[i]->Equals(*that.task_points[i])) {
// that task point is changed
Replace(*that.task_points[i], i);
modified = true;
}
}
// remove if that optional start list is smaller than this one
while (optional_start_points.size() > that.optional_start_points.size()) {
RemoveOptionalStart(optional_start_points.size() - 1);
modified = true;
}
// ensure each task point made identical
for (unsigned i = 0; i < that.optional_start_points.size(); ++i) {
if (i >= optional_start_points.size()) {
// that task is larger than this
AppendOptionalStart(*that.optional_start_points[i]);
modified = true;
} else if (!optional_start_points[i]->Equals(*that.optional_start_points[i])) {
// that task point is changed
ReplaceOptionalStart(*that.optional_start_points[i], i);
modified = true;
}
}
if (modified)
UpdateGeometry();
// @todo also re-scan task sample state,
// potentially resetting task
return modified;
}
unsigned
OrderedTask::GetLastIntermediateAchieved() const
{
if (TaskSize() < 2)
return 0;
for (unsigned i = 1; i < TaskSize() - 1; i++)
if (!task_points[i]->HasEntered())
return i - 1;
return TaskSize() - 2;
}
inline bool
OrderedTask::RunDijsktraMin(const GeoPoint &location)
{
const unsigned task_size = TaskSize();
if (task_size < 2)
return false;
if (dijkstra_min == nullptr)
dijkstra_min = new TaskDijkstraMin();
TaskDijkstraMin &dijkstra = *dijkstra_min;
const unsigned active_index = GetActiveIndex();
dijkstra.SetTaskSize(task_size - active_index);
for (unsigned i = active_index; i != task_size; ++i) {
const SearchPointVector &boundary = task_points[i]->GetSearchPoints();
dijkstra.SetBoundary(i - active_index, boundary);
}
SearchPoint ac(location, task_projection);
if (!dijkstra.DistanceMin(ac))
return false;
for (unsigned i = active_index; i != task_size; ++i)
SetPointSearchMin(i, dijkstra.GetSolution(i - active_index));
return true;
}
FlatBoundingBox
OrderedTask::GetBoundingBox(const GeoBounds &bounds) const
{
if (!TaskSize()) {
// undefined!
return FlatBoundingBox(FlatGeoPoint(0,0),FlatGeoPoint(0,0));
}
FlatGeoPoint ll = task_projection.ProjectInteger(GeoPoint(bounds.west,
bounds.south));
FlatGeoPoint lr = task_projection.ProjectInteger(GeoPoint(bounds.east,
bounds.south));
FlatGeoPoint ul = task_projection.ProjectInteger(GeoPoint(bounds.west,
bounds.north));
FlatGeoPoint ur = task_projection.ProjectInteger(GeoPoint(bounds.east,
bounds.north));
FlatGeoPoint fmin(min(ll.longitude, ul.longitude),
min(ll.latitude, lr.latitude));
FlatGeoPoint fmax(max(lr.longitude, ur.longitude),
max(ul.latitude, ur.latitude));
// note +/- 1 to ensure rounding keeps bb valid
fmin.longitude -= 1; fmin.latitude -= 1;
fmax.longitude += 1; fmax.latitude += 1;
return FlatBoundingBox (fmin, fmax);
}
OrderedTaskPoint*
OrderedTask::get_tp(const unsigned position)
{
if (position >= TaskSize())
return NULL;
return task_points[position];
}
FlatBoundingBox
OrderedTask::get_bounding_box(const GeoPoint& point) const
{
if (!TaskSize()) {
// undefined!
return FlatBoundingBox(FlatGeoPoint(0,0),FlatGeoPoint(0,0));
}
return FlatBoundingBox (task_projection.project(point), 1);
}
void
OrderedTask::RotateOptionalStarts()
{
if (!TaskSize())
return;
if (!optional_start_points.size())
return;
SelectOptionalStart(0);
}
bool
OrderedTask::Relocate(const unsigned position, const Waypoint& waypoint)
{
if (position >= TaskSize())
return false;
OrderedTaskPoint *new_tp = task_points[position]->Clone(task_behaviour,
ordered_behaviour,
&waypoint);
bool success = Replace(*new_tp, position);
delete new_tp;
return success;
}
fixed
OrderedTask::scan_distance_min(const GeoPoint &location, bool full)
{
if (full) {
if (dijkstra_min == NULL)
dijkstra_min = new TaskDijkstraMin(*this);
SearchPoint ac(location, task_projection);
if (dijkstra_min->DistanceMin(ac)) {
for (unsigned i = GetActiveIndex(), end = TaskSize(); i != end; ++i)
set_tp_search_min(i, dijkstra_min->GetSolution(i));
}
m_location_min_last = location;
}
return taskpoint_start->scan_distance_min();
}
inline fixed
OrderedTask::ScanDistanceMin(const GeoPoint &location, bool full)
{
if (full) {
if (dijkstra_min == NULL)
dijkstra_min = new TaskDijkstraMin();
SearchPoint ac(location, task_projection);
if (dijkstra_min->DistanceMin(*this, ac)) {
for (unsigned i = GetActiveIndex(), end = TaskSize(); i != end; ++i)
SetPointSearchMin(i, dijkstra_min->GetSolution(i));
}
last_min_location = location;
}
return taskpoint_start->ScanDistanceMin();
}
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();
}
bool
OrderedTask::IsFull() const
{
return TaskSize() == GetFactory().GetConstraints().max_points;
}
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;
}
