示例#1
0
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();
}
示例#2
0
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 &current = 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 &current = 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;
}