예제 #1
0
AirspacePolygon::AirspacePolygon(const std::vector<GeoPoint> &pts,
                                 const bool prune)
  :AbstractAirspace(Shape::POLYGON)
{
  if (pts.size() < 2) {
    m_is_convex = true;
  } else {
    m_border.reserve(pts.size() + 1);

    for (auto v = pts.begin(); v != pts.end(); ++v)
      m_border.push_back(SearchPoint(*v));

    // ensure airspace is closed
    GeoPoint p_start = pts[0];
    GeoPoint p_end = *(pts.end() - 1);
    if (p_start != p_end)
      m_border.push_back(SearchPoint(p_start));


    if (prune) {
      // only for testing
      m_border.PruneInterior();
      m_is_convex = true;
    } else {
      m_is_convex = m_border.IsConvex();
    }
  }
}
예제 #2
0
AirspacePolygon::AirspacePolygon(const std::vector<GeoPoint>& pts,
  const bool prune)
  :AbstractAirspace(POLYGON)
{
  if (pts.size()<2) {
    m_is_convex = true;
  } else {
    TaskProjection task_projection; // note dummy blank projection
    for (std::vector<GeoPoint>::const_iterator v = pts.begin();
         v != pts.end(); ++v) {
      m_border.push_back(SearchPoint(*v, task_projection));
    }
    // ensure airspace is closed
    GeoPoint p_start = pts[0];
    GeoPoint p_end = *(pts.end()-1);
    if (p_start != p_end) {
      m_border.push_back(SearchPoint(p_start, task_projection));
    }
    
    if (prune) {
      // only for testing
      prune_interior(m_border);
      m_is_convex = true;
    } else {
      m_is_convex = is_convex(m_border);
    }
  }
}
예제 #3
0
const SearchPointVector&
AbstractAirspace::GetClearance(const FlatProjection &projection) const
{
  #define RADIUS 5

  if (!m_clearance.empty())
    return m_clearance;

  m_clearance = m_border;
  if (is_convex != TriState::FALSE)
    is_convex = m_clearance.PruneInterior() ? TriState::FALSE : TriState::TRUE;

  FlatBoundingBox bb = m_clearance.CalculateBoundingbox();
  FlatGeoPoint center = bb.GetCenter();

  for (SearchPoint &i : m_clearance) {
    FlatGeoPoint p = i.GetFlatLocation();
    FlatRay r(center, p);
    int mag = r.Magnitude();
    int mag_new = mag + RADIUS;
    p = r.Parametric((double)mag_new / mag);
    i = SearchPoint(projection.Unproject(p), p);
  }

  return m_clearance;
}
const SearchPointVector&
AbstractAirspace::get_clearance() const
{
  #define RADIUS 5

  if (!m_clearance.empty())
    return m_clearance;

  assert(m_task_projection != NULL);

  m_clearance = m_border;
  if (!m_is_convex) {
    prune_interior(m_clearance);
  }

  FlatBoundingBox bb = ::compute_boundingbox(m_clearance);
  FlatGeoPoint center = bb.get_center();

  for (SearchPointVector::iterator i= m_clearance.begin();
       i != m_clearance.end(); ++i) {
    FlatGeoPoint p = i->get_flatLocation();
    FlatRay r(center, p);
    int mag = hypot(r.vector.Longitude, r.vector.Latitude);
    int mag_new = mag+RADIUS;
    p = r.parametric((fixed)mag_new/mag);
    *i = SearchPoint(m_task_projection->unproject(p));
    i->project(*m_task_projection);
  }
  return m_clearance;
}
예제 #5
0
const SearchPointVector&
AbstractAirspace::GetClearance(const TaskProjection &projection) const
{
  #define RADIUS 5

  if (!m_clearance.empty())
    return m_clearance;

  m_clearance = m_border;
  if (!m_is_convex)
    m_clearance.PruneInterior();

  FlatBoundingBox bb = m_clearance.CalculateBoundingbox();
  FlatGeoPoint center = bb.GetCenter();

  for (auto i= m_clearance.begin(); i != m_clearance.end(); ++i) {
    FlatGeoPoint p = i->get_flatLocation();
    FlatRay r(center, p);
    int mag = r.Magnitude();
    int mag_new = mag + RADIUS;
    p = r.Parametric((fixed)mag_new / mag);
    *i = SearchPoint(projection.unproject(p), p);
  }

  return m_clearance;
}
예제 #6
0
ContestResult
OLCSISAT::CalculateResult(const ContestTraceVector &solution) const
{
  // build convex hull from solution
  SearchPointVector spv;
  for (unsigned i = 0; i < num_stages; ++i)
    spv.push_back(SearchPoint(solution[i].location));

  spv.PruneInterior();

  // now add leg distances making up the convex hull
  fixed G = fixed_zero;

  if (spv.size() > 1) {
    for (unsigned i = 0; i + 1 < spv.size(); ++i)
      G += spv[i].DistanceTo(spv[i + 1].GetLocation());

    // closing leg (end to start)
    G += spv[spv.size() - 1].DistanceTo(spv[0].GetLocation());
  }

  // R distance (start to end)
  const fixed R = solution[0].DistanceTo(solution[num_stages - 1].GetLocation());

  // V zigzag-free distance
  const fixed V = G - R;

  // S = total distance
  ContestResult result = ContestDijkstra::CalculateResult(solution);
  result.score = ApplyHandicap((V + 3 * result.distance) / 4000);
  return result;
}
예제 #7
0
void
StartPoint::find_best_start(const AircraftState &state,
                            const OrderedTaskPoint &next,
                            const TaskProjection &projection)
{
  /* check which boundary point results in the smallest distance to
     fly */

  const OZBoundary boundary = next.GetBoundary();
  assert(!boundary.empty());

  const auto end = boundary.end();
  auto i = boundary.begin();
  assert(i != end);

  const GeoPoint &next_location = next.GetLocationRemaining();

  GeoPoint best_location = *i;
  fixed best_distance = ::DoubleDistance(state.location, *i, next_location);

  for (++i; i != end; ++i) {
    fixed distance = ::DoubleDistance(state.location, *i, next_location);
    if (distance < best_distance) {
      best_location = *i;
      best_distance = distance;
    }
  }

  SetSearchMin(SearchPoint(best_location, projection));
}
예제 #8
0
AirspaceCircle::AirspaceCircle(const GeoPoint &loc, const fixed _radius)
  :AbstractAirspace(Shape::CIRCLE), m_center(loc), m_radius(_radius)
{
  m_is_convex = true;

  // @todo: find better enclosing radius as fn of NUM_SEGMENTS
  #define NUM_SEGMENTS 12
  m_border.reserve(NUM_SEGMENTS);
  for (unsigned i = 0; i <= 12; ++i) {
    const Angle angle = Angle::Degrees(fixed(i * 360 / NUM_SEGMENTS));
    const GeoPoint p = GeoVector(m_radius * fixed(1.1), angle).EndPoint(m_center);
    m_border.push_back(SearchPoint(p));
  }
}
예제 #9
0
 gcc_const
 static SearchPoint Invalid() {
   return SearchPoint(GeoPoint::Invalid());
 }
예제 #10
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;
}
예제 #11
0
bool
RoutePlanner::solve(const AGeoPoint& origin,
                    const AGeoPoint& destination,
                    const RoutePlannerConfig& config,
                    const short h_ceiling)
{
  on_solve(origin, destination);
  rpolars_route.set_config(config, std::max(destination.altitude, origin.altitude),
                           h_ceiling);
  rpolars_reach.set_config(config, std::max(destination.altitude, origin.altitude),
                           h_ceiling);

  m_reach_polar_mode = config.reach_polar_mode;

  {
    const AFlatGeoPoint s_origin(task_projection.project(origin), origin.altitude);
    const AFlatGeoPoint s_destination(task_projection.project(destination), destination.altitude);

    if (!(s_origin == origin_last) || !(s_destination == destination_last))
      dirty = true;

    if (is_trivial())
      return false;

    dirty = false;
    origin_last = s_origin;
    destination_last = s_destination;

    h_min = std::min(s_origin.altitude, s_destination.altitude);
    h_max = rpolars_route.cruise_altitude;
  }

  solution_route.clear();
  solution_route.push_back(origin);
  solution_route.push_back(destination);

  if (!rpolars_route.terrain_enabled() && !rpolars_route.airspace_enabled())
    return false; // trivial

  m_search_hull.clear();
  m_search_hull.push_back(SearchPoint(origin_last, task_projection));

  RoutePoint start = origin_last;
  m_astar_goal = destination_last;

  RouteLink e_test(start, m_astar_goal, task_projection);
  if (e_test.is_short())
    return false;
  if (!rpolars_route.achievable(e_test))
    return false;

  count_dij=0;
  count_airspace=0;
  count_terrain=0;
  count_supressed=0;

  bool retval = false;
  m_planner.restart(start);

  unsigned best_d = UINT_MAX;

  if (verbose) {
    printf("# goal node (%d,%d,%d)\n",
           m_astar_goal.Longitude, m_astar_goal.Latitude, m_astar_goal.altitude);
    printf("# start node (%d,%d,%d)\n",
           start.Longitude, start.Latitude, start.altitude);
  }

  while (!m_planner.empty()) {
    const RoutePoint node = m_planner.pop();

    if (verbose>1) {
      printf("# processing node (%d,%d,%d)  %d,%d  q size %d\n",
             node.Longitude, node.Latitude, node.altitude,
             m_planner.get_node_value(node).g,
             m_planner.get_node_value(node).h,
             m_planner.queue_size());
    }

    h_min = std::min(h_min, node.altitude);
    h_max = std::max(h_max, node.altitude);

    bool is_final = (node == m_astar_goal);
    if (is_final) {
      if (!retval)
        best_d = UINT_MAX;
      retval = true;
    }

    if (is_final) // @todo: allow fallback if failed
    { // copy improving solutions
      Route this_solution;
      unsigned d = find_solution(node, this_solution);
      if (d< best_d) {
        best_d = d;
        solution_route = this_solution;
      }
    }

    if (retval)
      break; // want top solution only

    // shoot for final
    RouteLink e(node, m_astar_goal, task_projection);
    if (set_unique(e))
      add_edges(e);

    while (!m_links.empty()) {
      add_edges(m_links.front());
      m_links.pop();
    }

  }
  count_unique = m_unique.size();

  if (retval && verbose) {
    printf("# solved with %d intermediate points\n", (int)(solution_route.size()-2));
  }

  if (retval) {
    // correct solution for rounding
    assert(solution_route.size()>=2);
    for (unsigned i=0; i< solution_route.size(); ++i) {
      FlatGeoPoint p(task_projection.project(solution_route[i]));
      if (p== origin_last) {
        solution_route[i] = AGeoPoint(origin, solution_route[i].altitude);
      } else if (p== destination_last) {
        solution_route[i] = AGeoPoint(destination, solution_route[i].altitude);
      }
    }

  } else {
    solution_route.clear();
    solution_route.push_back(origin);
    solution_route.push_back(destination);
  }

  m_planner.clear();
  m_unique.clear();
//  m_search_hull.clear();
  return retval;
}