std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>> Comb::Crossing::findBestCrossing(const Polygons& outside, const PolygonRef from, const Point estimated_start, const Point estimated_end, Comb& comber)
{
ClosestPolygonPoint* best_in = nullptr;
ClosestPolygonPoint* best_out = nullptr;
int64_t best_detour_score = std::numeric_limits<int64_t>::max();
int64_t best_crossing_dist2;
std::vector<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>> crossing_out_candidates = PolygonUtils::findClose(from, outside, comber.getOutsideLocToLine());
bool seen_close_enough_connection = false;
for (std::pair<ClosestPolygonPoint, ClosestPolygonPoint>& crossing_candidate : crossing_out_candidates)
{
int64_t crossing_dist2 = vSize2(crossing_candidate.first.location - crossing_candidate.second.location);
if (crossing_dist2 > comber.max_crossing_dist2 * 2)
{ // preliminary filtering
continue;
}
int64_t dist_to_start = vSize(crossing_candidate.second.location - estimated_start); // use outside location, so that the crossing direction is taken into account
int64_t dist_to_end = vSize(crossing_candidate.second.location - estimated_end);
int64_t detour_dist = dist_to_start + dist_to_end;
int64_t detour_score = crossing_dist2 + detour_dist * detour_dist / 1000; // prefer a closest connection over a detour
// The detour distance is generally large compared to the crossing distance.
// While the crossing is generally about 1mm across,
// the distance between an arbitrary point and the boundary may well be a couple of centimetres.
// So the crossing_dist2 is about 1.000.000 while the detour_dist_2 is in the order of 400.000.000
// In the end we just want to choose between two points which have the _same_ crossing distance, modulo rounding error.
if ((!seen_close_enough_connection && detour_score < best_detour_score) // keep the best as long as we havent seen one close enough (so that we may walk along the polygon to find a closer connection from it in the code below)
|| (!seen_close_enough_connection && crossing_dist2 <= comber.max_crossing_dist2) // make the one which is close enough the best as soon as we see one close enough
|| (seen_close_enough_connection && crossing_dist2 <= comber.max_crossing_dist2 && detour_score < best_detour_score)) // update to keep the best crossing which is close enough already
{
if (!seen_close_enough_connection && crossing_dist2 <= comber.max_crossing_dist2)
{
seen_close_enough_connection = true;
}
best_in = &crossing_candidate.first;
best_out = &crossing_candidate.second;
best_detour_score = detour_score;
best_crossing_dist2 = crossing_dist2;
}
}
if (best_detour_score == std::numeric_limits<int64_t>::max())
{ // i.e. if best_in == nullptr or if best_out == nullptr
return std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>>();
}
if (best_crossing_dist2 > comber.max_crossing_dist2)
{ // find closer point on line segments, rather than moving between vertices of the polygons only
PolygonUtils::walkToNearestSmallestConnection(*best_in, *best_out);
best_crossing_dist2 = vSize2(best_in->location - best_out->location);
if (best_crossing_dist2 > comber.max_crossing_dist2)
{
return std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>>();
}
}
return std::make_shared<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>>(*best_in, *best_out);
}
bool Comb::Crossing::findOutside(const Polygons& outside, const Point close_to, const bool fail_on_unavoidable_obstacles, Comb& comber)
{
out = in_or_mid;
if (dest_is_inside || outside.inside(in_or_mid, true)) // start in_between
{ // move outside
Point preferred_crossing_1_out = in_or_mid + normal(close_to - in_or_mid, comber.offset_from_inside_to_outside);
std::function<int(Point)> close_to_penalty_function([preferred_crossing_1_out](Point candidate){ return vSize2((candidate - preferred_crossing_1_out) / 2); });
std::optional<ClosestPolygonPoint> crossing_1_out_cpp = PolygonUtils::findClose(in_or_mid, outside, comber.getOutsideLocToLine(), close_to_penalty_function);
if (crossing_1_out_cpp)
{
out = PolygonUtils::moveOutside(*crossing_1_out_cpp, comber.offset_dist_to_get_from_on_the_polygon_to_outside);
}
else
{
PolygonUtils::moveOutside(outside, out, comber.offset_dist_to_get_from_on_the_polygon_to_outside);
}
}
int64_t in_out_dist2_1 = vSize2(out - in_or_mid);
if (dest_is_inside && in_out_dist2_1 > comber.max_crossing_dist2) // moveInside moved too far
{ // if move is too far over in_between
// find crossing closer by
assert(dest_crossing_poly && "destination crossing poly should have been instantiated!");
std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>> best = findBestCrossing(outside, *dest_crossing_poly, dest_point, close_to, comber);
if (best)
{
in_or_mid = PolygonUtils::moveInside(best->first, comber.offset_dist_to_get_from_on_the_polygon_to_outside);
out = PolygonUtils::moveOutside(best->second, comber.offset_dist_to_get_from_on_the_polygon_to_outside);
}
if (fail_on_unavoidable_obstacles && vSize2(out - in_or_mid) > comber.max_crossing_dist2) // moveInside moved still too far
{
return false;
}
}
return true;
}