static FlatGeoPoint
SegmentNearestPoint(const SearchPointVector& spv,
const SearchPointVector::const_iterator i1,
const FlatGeoPoint &p3)
{
if (i1+1 == spv.end()) {
return NearestPoint(i1->get_flatLocation(),
spv.begin()->get_flatLocation(),
p3);
} else {
return NearestPoint(i1->get_flatLocation(),
(i1+1)->get_flatLocation(),
p3);
}
}
Point CCurve::NearestPoint(const CCurve& c, double *d)const
{
double best_dist = 0.0;
Point best_point = Point(0, 0);
bool best_point_valid = false;
Point prev_p = Point(0, 0);
bool prev_p_valid = false;
bool first_span = true;
for(std::list<CVertex>::const_iterator It = c.m_vertices.begin(); It != c.m_vertices.end(); It++)
{
const CVertex& vertex = *It;
if(prev_p_valid)
{
double dist;
Point near_point = NearestPoint(Span(prev_p, vertex, first_span), &dist);
first_span = false;
if(!best_point_valid || dist < best_dist)
{
best_dist = dist;
best_point = near_point;
best_point_valid = true;
}
}
prev_p = vertex.m_p;
prev_p_valid = true;
}
if(d)*d = best_dist;
return best_point;
}
Point Span::NearestPoint(const Span& p, double *d)const
{
double best_dist;
Point best_point = this->NearestPointToSpan(p, best_dist);
// try the other way round too
double best_dist2;
Point best_point2 = p.NearestPointToSpan(*this, best_dist2);
if(best_dist2 < best_dist)
{
best_point = NearestPoint(best_point2);
best_dist = best_dist2;
}
if(d)*d = best_dist;
return best_point;
}
bool Line::hit(const Line& ls) const {
auto fn = [](float f){return f;};
Vec3x2 res = NearestPoint(*this, ls, fn, fn);
return res.first.distance(res.second) <= Point::NEAR_THRESHOLD;
}
bool Line::hit(const Vec3& p) const {
Vec3 cp = NearestPoint(*this, p, [](float f){return f;});
return p.distance(cp) <= Point::NEAR_THRESHOLD;
}
Vec3 Line::nearest(const Vec3& p) const {
return NearestPoint(*this, p, [](float f){ return f; });
}
Vec3x2 Line::nearestPoint(const Line& s) const {
auto fn = [](float f) { return f; };
return NearestPoint(*this, s, fn, fn);
}
bool Span::On(const Point& p, double* t)const
{
if(p != NearestPoint(p))return false;
if(t)*t = Parameter(p);
return true;
}
std::vector<City> solve(const std::vector<Point>& points) override {
std::set<City> outTourCities;
for (size_t i = 0; i < points.size(); i++) {
outTourCities.insert(i);
}
std::vector<Edge> edges;
std::vector<City> startingTour = this->startingTour(points);
for (Index i = 0; i < startingTour.size(); i++) {
edges.push_back(Edge(startingTour[i], startingTour[(i+1)%startingTour.size()]));
outTourCities.erase(startingTour[i]);
}
std::vector<Record> outTour;
for (City c : outTourCities) {
City c_in = *NearestPoint(points, startingTour.begin(), startingTour.end(), points[c]);
outTour.push_back(Record(c, c_in, points[c].Distance(points[c_in])));
}
size_t i = startingTour.size();
while (i < points.size()) {
auto it = max_element(outTour.begin(), outTour.end());
Record r = *it;
outTour.erase(it);
typedef std::vector<Edge>::iterator EdgeIt;
EdgeIt es[2];
bool b = 0;
for (auto e_it = edges.begin(); e_it != edges.end(); e_it++) {
if (e_it->hasCity(r.nearestInTourCity)) {
es[b] = e_it;
b = !b;
}
}
assert(b==0);
EdgeIt removingEdge = min(es[0], es[1], [&](const EdgeIt& e_0, const EdgeIt& e_1) {
auto &ps = points;
return -ps[e_0->at(0)].Distance(ps[e_0->at(1)])
+ps[e_0->otherCity(r.nearestInTourCity)].Distance(ps[r.outTourCity]) <
-ps[e_1->at(0)].Distance(ps[e_1->at(1)])
+ps[e_1->otherCity(r.nearestInTourCity)].Distance(ps[r.outTourCity]);
});
City c = removingEdge->otherCity(r.nearestInTourCity);
edges.erase(removingEdge);
edges.push_back(Edge(c, r.outTourCity));
edges.push_back(Edge(r.nearestInTourCity, r.outTourCity));
for (Record& out_r : outTour) {
double d = points[r.outTourCity].Distance(points[out_r.outTourCity]);
if (out_r.distance > d) {
out_r.nearestInTourCity = r.outTourCity;
out_r.distance = d;
}
}
i++;
}
return edgesToTour(edges);
}