void
project(SearchPointVector& spv, const TaskProjection& tp)
{
for (SearchPointVector::iterator i = spv.begin(); i!= spv.end(); ++i) {
i->project(tp);
}
}
bool
GrahamScan::PruneInterior()
{
SearchPointVector res;
/* the result is usually one more than the input vector - is that a
bug? */
res.reserve(size + 1);
if (size < 3) {
std::copy(raw_points.begin(), raw_points.end(), std::back_inserter(res));
return false;
// nothing to do
}
PartitionPoints();
BuildHull();
for (unsigned i = 0; i + 1 < lower_hull.size(); i++)
res.push_back(*lower_hull[i]);
for (int i = upper_hull.size() - 1; i >= 0; i--)
res.push_back(*upper_hull[i]);
if (res.size() == size)
return false;
raw_vector.swap(res);
return true;
}
fixed
OLCSISAT::calc_score() const
{
// build convex hull from solution
SearchPointVector spv;
for (unsigned i = 0; i < num_stages; ++i)
spv.push_back(solution[i]);
prune_interior(spv);
// 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].distance(spv[i + 1].get_location());
// closing leg (end to start)
G += spv[spv.size() - 1].distance(spv[0].get_location());
}
// R distance (start to end)
const fixed R = solution[0].distance(solution[num_stages - 1].get_location());
// V zigzag-free distance
const fixed V = G - R;
// S = total distance
const fixed S = calc_distance();
return apply_handicap((V + fixed(3) * S) * fixed(0.00025));
}
void
MapCanvas::Project(const Projection &projection,
const SearchPointVector &points, BulkPixelPoint *screen)
{
for (auto it = points.begin(); it != points.end(); ++it)
*screen++ = projection.GeoToScreen(it->GetLocation());
}
void
MapCanvas::project(const Projection &projection,
const SearchPointVector &points, RasterPoint *screen)
{
for (auto it = points.begin(); it != points.end(); ++it)
*screen++ = projection.GeoToScreen(it->get_location());
}
void write_spv (const SearchPointVector& spv)
{
for (auto v = spv.begin(); v != spv.end(); ++v) {
write_point(*v, v->get_flatLocation(), "spv");
}
printf("spv\n");
fflush(stdout);
}
AirspaceRoute::ClearingPair
AirspaceRoute::GetPairs(const SearchPointVector &spv,
const RoutePoint &start, const RoutePoint &dest) const
{
SearchPointVector::const_iterator i_closest = spv.NearestIndexConvex(start);
SearchPointVector::const_iterator i_furthest = spv.NearestIndexConvex(dest);
return FindClearingPair(spv, i_closest, i_furthest, start);
}
bool
is_convex(const SearchPointVector& spv)
{
bool changed=false;
GrahamScan gs(spv);
size_t size_before = spv.size();
SearchPointVector res;
res = gs.prune_interior(&changed);
return res.size() != size_before;
}
static FlatGeoPoint
nearest_point_convex(const SearchPointVector& spv, const FlatGeoPoint &p3)
{
unsigned distance_min = 0-1;
SearchPointVector::const_iterator i_best = spv.end();
// find nearest point in vector
for (SearchPointVector::const_iterator i = spv.begin();
i!= spv.end(); ++i) {
unsigned d_this = p3.distance_sq_to(i->get_flatLocation());
if (d_this<distance_min) {
distance_min = d_this;
i_best = i;
}
}
FlatGeoPoint pc = i_best->get_flatLocation();
// find nearest point on this segment
FlatGeoPoint pa = segment_nearest_point(spv,i_best,p3);
if (!(pa == pc)) {
unsigned d_seg = pa.distance_sq_to(p3);
if (d_seg < distance_min) {
distance_min = d_seg;
pc = pa;
}
}
// find nearest point on previous segment
SearchPointVector::const_iterator i_prev;
if (i_best == spv.begin()) {
i_prev = spv.end()-1;
} else {
i_prev = i_best-1;
}
FlatGeoPoint pb = segment_nearest_point(spv,i_prev,p3);
if (!(pb == pc)) {
unsigned d_seg = pb.distance_sq_to(p3);
if (d_seg < distance_min) {
distance_min = d_seg;
pc = pb;
}
}
return pc;
}
static FlatGeoPoint
segment_nearest_point(const SearchPointVector& spv,
const SearchPointVector::const_iterator i1,
const FlatGeoPoint &p3)
{
if (i1+1 == spv.end()) {
return nearest_point(i1->get_flatLocation(),
spv.begin()->get_flatLocation(),
p3);
} else {
return nearest_point(i1->get_flatLocation(),
(i1+1)->get_flatLocation(),
p3);
}
}
void
MapDrawHelper::draw_search_point_vector(const SearchPointVector& points)
{
size_t size = points.size();
if (size < 3)
return;
/* copy all SearchPointVector elements to geo_points */
geo_points.GrowDiscard(size * 3);
for (unsigned i = 0; i < size; ++i)
geo_points[i] = points[i].get_location();
/* clip them */
size = clip.ClipPolygon(geo_points.begin(), geo_points.begin(), size);
if (size < 3)
/* it's completely outside the screen */
return;
/* draw it all */
RasterPoint screen[size];
for (unsigned i = 0; i < size; ++i)
screen[i] = m_proj.GeoToScreen(geo_points[i]);
if (!MapCanvas::visible(m_canvas, screen, size))
return;
m_buffer.polygon(&screen[0], size);
if (m_use_stencil)
m_stencil.polygon(&screen[0], size);
}
gcc_pure
static FlatGeoPoint
SegmentNearestPoint(const SearchPointVector& spv,
const SearchPointVector::const_iterator i1,
const FlatGeoPoint &p3)
{
if (i1+1 == spv.end()) {
return NearestPoint(i1->GetFlatLocation(),
spv.begin()->GetFlatLocation(),
p3);
} else {
return NearestPoint(i1->GetFlatLocation(),
(i1 + 1)->GetFlatLocation(),
p3);
}
}
void
StencilMapCanvas::DrawSearchPointVector(const SearchPointVector &points)
{
size_t size = points.size();
if (size < 3)
return;
/* copy all SearchPointVector elements to geo_points */
geo_points.GrowDiscard(size * 3);
for (unsigned i = 0; i < size; ++i)
geo_points[i] = points[i].GetLocation();
/* clip them */
size = clip.ClipPolygon(geo_points.begin(), geo_points.begin(), size);
if (size < 3)
/* it's completely outside the screen */
return;
/* draw it all */
RasterPoint screen[size];
for (unsigned i = 0; i < size; ++i)
screen[i] = proj.GeoToScreen(geo_points[i]);
buffer.DrawPolygon(&screen[0], size);
if (use_stencil)
stencil.DrawPolygon(&screen[0], size);
}
AirspaceRoute::ClearingPair
AirspaceRoute::GetBackupPairs(const SearchPointVector& spv,
const RoutePoint &_start,
const RoutePoint &intc) const
{
SearchPointVector::const_iterator start = spv.NearestIndexConvex(intc);
ClearingPair p(intc, intc);
SearchPointVector::const_iterator i_left = spv.NextCircular(start);
p.first = AFlatGeoPoint(i_left->GetFlatLocation(), _start.altitude); // @todo alt!
SearchPointVector::const_iterator i_right = spv.PreviousCircular(start);
p.second = AFlatGeoPoint(i_right->GetFlatLocation(), _start.altitude); // @todo alt!
return p;
}
bool
MapCanvas::prepare_polygon(const SearchPointVector &points)
{
unsigned num_points = points.size();
if (num_points < 3)
return false;
/* copy all SearchPointVector elements to geo_points */
geo_points.GrowDiscard(num_points * 3);
for (unsigned i = 0; i < num_points; ++i)
geo_points[i] = points[i].get_location();
/* clip them */
num_raster_points = clip.ClipPolygon(geo_points.begin(),
geo_points.begin(), num_points);
if (num_raster_points < 3)
/* it's completely outside the screen */
return false;
/* project all GeoPoints to screen coordinates */
raster_points.GrowDiscard(num_raster_points);
for (unsigned i = 0; i < num_raster_points; ++i)
raster_points[i] = projection.GeoToScreen(geo_points[i]);
return visible(raster_points.begin(), num_raster_points);
}
static FlatGeoPoint
NearestPointNonConvex(const SearchPointVector& spv, const FlatGeoPoint &p3)
{
unsigned distance_min = 0-1;
FlatGeoPoint point_best;
for (SearchPointVector::const_iterator i = spv.begin();
i!= spv.end(); ++i) {
FlatGeoPoint pa = SegmentNearestPoint(spv,i,p3);
unsigned d_this = p3.distance_sq_to(pa);
if (d_this<distance_min) {
distance_min = d_this;
point_best = pa;
}
}
return point_best;
}
static FlatGeoPoint
nearest_point_nonconvex(const SearchPointVector& spv, const FlatGeoPoint &p3)
{
unsigned distance_min = 0-1;
SearchPointVector::const_iterator i_best = spv.end();
for (SearchPointVector::const_iterator i = spv.begin();
i!= spv.end(); ++i) {
FlatGeoPoint pa = segment_nearest_point(spv,i,p3);
unsigned d_this = p3.distance_sq_to(pa);
if (d_this<distance_min) {
distance_min = d_this;
i_best = i;
}
}
return i_best->get_flatLocation();
}
FlatGeoPoint nearest_point(const SearchPointVector& spv,
const FlatGeoPoint &p3,
const bool is_convex)
{
// special case
if (spv.empty()) {
return p3; // really should be error
} else if (spv.size()==1) {
return spv[0].get_flatLocation();
}
if (is_convex) {
/** \todo Strictly speaking it isn't correct to use this function
*/
return nearest_point_convex(spv,p3);
} else {
return nearest_point_nonconvex(spv,p3);
}
}
AirspaceRoute::ClearingPair
AirspaceRoute::FindClearingPair(const SearchPointVector& spv,
const SearchPointVector::const_iterator start,
const SearchPointVector::const_iterator end,
const RoutePoint &dest) const
{
bool backwards = false;
ClearingPair p(dest, dest);
bool check_others = false;
SearchPointVector::const_iterator i= start;
int j=0;
while ((i != end)&&(j<2)) {
AFlatGeoPoint pborder(i->get_flatLocation(), dest.altitude); // @todo alt!
const FlatRay ray(pborder, dest);
if (spv.IntersectsWith(ray)) {
j++;
if (j==1) {
i = start;
backwards = true;
continue;
}
} else {
AGeoPoint gborder(task_projection.unproject(pborder), dest.altitude); // @todo alt!
if (!check_others || !InsideOthers(gborder)) {
if (j==0) {
p.first = pborder;
} else if (j==1) {
p.second = pborder;
}
}
}
if (backwards)
spv.PreviousCircular(i);
else
spv.NextCircular(i);
}
return p;
}
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;
}
bool
GrahamScan::prune_interior()
{
SearchPointVector res;
/* the result is usually one more than the input vector - is that a
bug? */
res.reserve(size + 1);
if (size<3) {
std::copy(raw_points.begin(), raw_points.end(),
std::back_inserter(res));
return false;
// nothing to do
}
partition_points();
build_hull();
for ( unsigned i = 0 ; i+1 < lower_hull.size() ; i++ ) {
res.push_back(*lower_hull[i]);
}
for ( int i = upper_hull.size()-1; i>=0 ; i-- ) {
res.push_back(*upper_hull[i]);
}
// if (!has_changed) {
// *changed = !std::equal(res.begin(), res.end(), raw_vector.begin() );
// }
if (res.size() != size) {
raw_vector.swap(res);
return true;
} else {
return false;
}
}
void
MapDrawHelper::draw_search_point_vector(Canvas& the_canvas,
const SearchPointVector& points)
{
const size_t size = points.size();
if (size<3) {
return;
}
MapCanvas map_canvas(the_canvas, m_proj);
POINT screen[size];
map_canvas.project(points, screen);
if (!map_canvas.visible(screen, size))
return;
the_canvas.polygon(&screen[0], size);
if (m_use_stencil) {
m_stencil.polygon(&screen[0], size);
}
}
GrahamScan::GrahamScan(SearchPointVector& sps, const fixed sign_tolerance):
raw_points(sps.begin(), sps.end()), raw_vector(sps), size(sps.size()),
tolerance(sign_tolerance)
{
}
const GeoPoint &GetLocation() const {
return nominal_points.front().GetLocation();
}
/**
* Test if the task point has recorded presence of the aircraft
* in this sector
*
* @return True if sample present
*/
gcc_pure
bool HasSampled() const {
return !sampled_points.empty();
}
