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();
}
}
}
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);
}
}
}
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;
}
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;
}
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;
}
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));
}
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));
}
}
gcc_const
static SearchPoint Invalid() {
return SearchPoint(GeoPoint::Invalid());
}
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;
}
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;
}
