bool Comb::calc(Point startPoint, Point endPoint, std::vector<std::vector<Point>>& combPaths)
{
if (shorterThen(endPoint - startPoint, MM2INT(1.5)))
{
// DEBUG_PRINTLN("too short dist!");
return true;
}
bool addEndpoint = false;
//Check if we are inside the comb boundaries
if (!boundary.inside(startPoint))
{
if (!moveInside(&startPoint)) //If we fail to move the point inside the comb boundary we need to retract.
{
// std::cerr << " fail to move the start point inside the comb boundary we need to retract."<< std::endl;
return false;
}
combPaths.emplace_back();
combPaths.back().push_back(startPoint);
}
if (!boundary.inside(endPoint))
{
if (!moveInside(&endPoint)) //If we fail to move the point inside the comb boundary we need to retract.
{
// std::cerr << " fail to move the end point inside the comb boundary we need to retract."<< std::endl;
return false;
}
addEndpoint = true;
}
//Check if we are crossing any boundaries, and pre-calculate some values.
if (!lineSegmentCollidesWithBoundary(startPoint, endPoint))
{
//We're not crossing any boundaries. So skip the comb generation.
if (!addEndpoint && combPaths.size() == 0) //Only skip if we didn't move the start and end point.
return true;
}
// std::cerr << "calcuklating comb path!" << std::endl;
//Calculate the minimum and maximum positions where we cross the comb boundary
calcMinMax();
std::vector<std::vector<Point>> basicCombPaths;
getBasicCombingPaths(endPoint, basicCombPaths);
bool succeeded = optimizePaths(startPoint, basicCombPaths, combPaths);
if (addEndpoint)
combPaths.back().push_back(endPoint);
// std::cerr << "succeeded = " << succeeded << std::endl;
return succeeded;
}
bool Comb::calc(Point startPoint, Point endPoint, std::vector<Point>& combPoints)
{
if (shorterThen(endPoint - startPoint, MM2INT(1.5)))
return true;
bool addEndpoint = false;
//Check if we are inside the comb boundaries
if (!boundary.inside(startPoint))
{
if (!moveInside(&startPoint)) //If we fail to move the point inside the comb boundary we need to retract.
return false;
combPoints.push_back(startPoint);
}
if (!boundary.inside(endPoint))
{
if (!moveInside(&endPoint)) //If we fail to move the point inside the comb boundary we need to retract.
return false;
addEndpoint = true;
}
//Check if we are crossing any boundaries, and pre-calculate some values.
if (!lineSegmentCollidesWithBoundary(startPoint, endPoint))
{
//We're not crossing any boundaries. So skip the comb generation.
if (!addEndpoint && combPoints.size() == 0) //Only skip if we didn't move the start and end point.
return true;
}
//Calculate the minimum and maximum positions where we cross the comb boundary
calcMinMax();
std::vector<Point> pointList;
getBasicCombingPath(endPoint, pointList);
bool succeeded = optimizePath(startPoint, pointList, combPoints);
if (addEndpoint)
combPoints.push_back(endPoint);
return succeeded;
}
bool Comb::calc(Point startPoint, Point endPoint, vector<Point>& combPoints)
{
if (shorterThen(endPoint - startPoint, MM2INT(1.5)))
return true;
bool addEndpoint = false;
//Check if we are inside the comb boundaries
if (!checkInside(startPoint))
{
if (!moveInside(&startPoint)) //If we fail to move the point inside the comb boundary we need to retract.
return false;
combPoints.push_back(startPoint);
}
if (!checkInside(endPoint))
{
if (!moveInside(&endPoint)) //If we fail to move the point inside the comb boundary we need to retract.
return false;
addEndpoint = true;
}
//Check if we are crossing any bounderies, and pre-calculate some values.
if (!preTest(startPoint, endPoint))
{
//We're not crossing any boundaries. So skip the comb generation.
if (!addEndpoint && combPoints.size() == 0) //Only skip if we didn't move the start and end point.
return true;
}
//Calculate the minimum and maximum positions where we cross the comb boundary
calcMinMax();
int64_t x = sp.X;
vector<Point> pointList;
//Now walk trough the crossings, for every boundary we cross, find the initial cross point and the exit point. Then add all the points in between
// to the pointList and continue with the next boundary we will cross, until there are no more boundaries to cross.
// This gives a path from the start to finish curved around the holes that it encounters.
while(true)
{
unsigned int n = getPolygonAbove(x);
if (n == UINT_MAX) break;
pointList.push_back(matrix.unapply(Point(minX[n] - MM2INT(0.2), sp.Y)));
if ( (minIdx[n] - maxIdx[n] + boundery[n].size()) % boundery[n].size() > (maxIdx[n] - minIdx[n] + boundery[n].size()) % boundery[n].size())
{
for(unsigned int i=minIdx[n]; i != maxIdx[n]; i = (i < boundery[n].size() - 1) ? (i + 1) : (0))
{
pointList.push_back(getBounderyPointWithOffset(n, i));
}
}else{
minIdx[n]--;
if (minIdx[n] == UINT_MAX) minIdx[n] = boundery[n].size() - 1;
maxIdx[n]--;
if (maxIdx[n] == UINT_MAX) maxIdx[n] = boundery[n].size() - 1;
for(unsigned int i=minIdx[n]; i != maxIdx[n]; i = (i > 0) ? (i - 1) : (boundery[n].size() - 1))
{
pointList.push_back(getBounderyPointWithOffset(n, i));
}
}
pointList.push_back(matrix.unapply(Point(maxX[n] + MM2INT(0.2), sp.Y)));
x = maxX[n];
}
pointList.push_back(endPoint);
//Optimize the pointList, skip each point we could already reach by not crossing a boundary. This smooths out the path and makes it skip any unneeded corners.
Point p0 = startPoint;
for(unsigned int n=1; n<pointList.size(); n++)
{
if (collisionTest(p0, pointList[n]))
{
if (collisionTest(p0, pointList[n-1]))
return false;
p0 = pointList[n-1];
combPoints.push_back(p0);
}
}
if (addEndpoint)
combPoints.push_back(endPoint);
return true;
}
bool Comb::calc(Point startPoint, Point endPoint, CombPaths& combPaths, bool _startInside, bool _endInside, int64_t max_comb_distance_ignored, bool via_outside_makes_combing_fail, bool fail_on_unavoidable_obstacles)
{
if (shorterThen(endPoint - startPoint, max_comb_distance_ignored))
{
return true;
}
//Move start and end point inside the comb boundary
unsigned int start_inside_poly = NO_INDEX;
const bool startInside = moveInside(_startInside, startPoint, start_inside_poly);
unsigned int end_inside_poly = NO_INDEX;
const bool endInside = moveInside(_endInside, endPoint, end_inside_poly);
unsigned int start_part_boundary_poly_idx;
unsigned int end_part_boundary_poly_idx;
unsigned int start_part_idx = (start_inside_poly == NO_INDEX)? NO_INDEX : partsView_inside.getPartContaining(start_inside_poly, &start_part_boundary_poly_idx);
unsigned int end_part_idx = (end_inside_poly == NO_INDEX)? NO_INDEX : partsView_inside.getPartContaining(end_inside_poly, &end_part_boundary_poly_idx);
if (startInside && endInside && start_part_idx == end_part_idx)
{ // normal combing within part
PolygonsPart part = partsView_inside.assemblePart(start_part_idx);
combPaths.emplace_back();
return LinePolygonsCrossings::comb(part, *inside_loc_to_line, startPoint, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
}
else
{ // comb inside part to edge (if needed) >> move through air avoiding other parts >> comb inside end part upto the endpoint (if needed)
// INSIDE | in_between | OUTSIDE | in_between | INSIDE
// ^crossing_1_in ^crossing_1_mid ^crossing_1_out ^crossing_2_out ^crossing_2_mid ^crossing_2_in
//
// when startPoint is inside crossing_1_in is of interest
// when it is in between inside and outside it is equal to crossing_1_mid
if (via_outside_makes_combing_fail)
{
return false;
}
Crossing start_crossing(startPoint, startInside, start_part_idx, start_part_boundary_poly_idx, boundary_inside, inside_loc_to_line);
Crossing end_crossing(endPoint, endInside, end_part_idx, end_part_boundary_poly_idx, boundary_inside, inside_loc_to_line);
{ // find crossing over the in-between area between inside and outside
start_crossing.findCrossingInOrMid(partsView_inside, endPoint);
end_crossing.findCrossingInOrMid(partsView_inside, start_crossing.in_or_mid);
}
bool skip_avoid_other_parts_path = false;
if (skip_avoid_other_parts_path && vSize2(start_crossing.in_or_mid - end_crossing.in_or_mid) < offset_from_inside_to_outside * offset_from_inside_to_outside * 4)
{ // parts are next to eachother, i.e. the direct crossing will always be smaller than two crossings via outside
skip_avoid_other_parts_path = true;
}
if (avoid_other_parts && !skip_avoid_other_parts_path)
{ // compute the crossing points when moving through air
// comb through all air, since generally the outside consists of a single part
bool success = start_crossing.findOutside(*boundary_outside, end_crossing.in_or_mid, fail_on_unavoidable_obstacles, *this);
if (!success)
{
return false;
}
success = end_crossing.findOutside(*boundary_outside, start_crossing.out, fail_on_unavoidable_obstacles, *this);
if (!success)
{
return false;
}
}
// generate the actual comb paths
if (startInside)
{
// start to boundary
assert(start_crossing.dest_part.size() > 0 && "The part we start inside when combing should have been computed already!");
combPaths.emplace_back();
bool combing_succeeded = LinePolygonsCrossings::comb(start_crossing.dest_part, *inside_loc_to_line, startPoint, start_crossing.in_or_mid, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
if (!combing_succeeded)
{ // Couldn't comb between start point and computed crossing from the start part! Happens for very thin parts when the offset_to_get_off_boundary moves points to outside the polygon
return false;
}
}
// throught air from boundary to boundary
if (avoid_other_parts && !skip_avoid_other_parts_path)
{
combPaths.emplace_back();
combPaths.throughAir = true;
if ( vSize(start_crossing.in_or_mid - end_crossing.in_or_mid) < vSize(start_crossing.in_or_mid - start_crossing.out) + vSize(end_crossing.in_or_mid - end_crossing.out) )
{ // via outside is moving more over the in-between zone
combPaths.back().push_back(start_crossing.in_or_mid);
combPaths.back().push_back(end_crossing.in_or_mid);
}
else
{
bool combing_succeeded = LinePolygonsCrossings::comb(*boundary_outside, *outside_loc_to_line, start_crossing.out, end_crossing.out, combPaths.back(), offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
if (!combing_succeeded)
{
return false;
}
}
}
else
{ // directly through air (not avoiding other parts)
combPaths.emplace_back();
combPaths.throughAir = true;
combPaths.back().cross_boundary = true; // note: we don't actually know whether this is cross boundary, but it might very well be
combPaths.back().push_back(start_crossing.in_or_mid);
combPaths.back().push_back(end_crossing.in_or_mid);
}
if (skip_avoid_other_parts_path)
{
if (startInside == endInside && start_part_idx == end_part_idx)
{
if (startInside)
{ // both start and end are inside
combPaths.back().cross_boundary = PolygonUtils::polygonCollidesWithLineSegment(startPoint, endPoint, *inside_loc_to_line);
}
else
{ // both start and end are outside
combPaths.back().cross_boundary = PolygonUtils::polygonCollidesWithLineSegment(startPoint, endPoint, *outside_loc_to_line);
}
}
else
{
combPaths.back().cross_boundary = true;
}
}
if (endInside)
{
// boundary to end
assert(end_crossing.dest_part.size() > 0 && "The part we end up inside when combing should have been computed already!");
combPaths.emplace_back();
bool combing_succeeded = LinePolygonsCrossings::comb(end_crossing.dest_part, *inside_loc_to_line, end_crossing.in_or_mid, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
if (!combing_succeeded)
{ // Couldn't comb between end point and computed crossing to the end part! Happens for very thin parts when the offset_to_get_off_boundary moves points to outside the polygon
return false;
}
}
return true;
}
}
bool Comb::calc(Point startPoint, Point endPoint, CombPaths& combPaths)
{
if (shorterThen(endPoint - startPoint, max_comb_distance_ignored))
{
return true;
}
bool startInside = true;
bool endInside = true;
//Move start and end point inside the comb boundary
unsigned int start_inside_poly = boundary_inside.findInside(startPoint, true);
if (start_inside_poly == NO_INDEX)
{
start_inside_poly = moveInside(boundary_inside, startPoint, offset_extra_start_end, max_moveInside_distance2);
if (start_inside_poly == NO_INDEX) //If we fail to move the point inside the comb boundary we need to retract.
{
startInside = false;
}
}
unsigned int end_inside_poly = boundary_inside.findInside(endPoint, true);
if (end_inside_poly == NO_INDEX)
{
end_inside_poly = moveInside(boundary_inside, endPoint, offset_extra_start_end, max_moveInside_distance2);
if (end_inside_poly == NO_INDEX) //If we fail to move the point inside the comb boundary we need to retract.
{
endInside = false;
}
}
unsigned int start_part_boundary_poly_idx;
unsigned int end_part_boundary_poly_idx;
unsigned int start_part_idx = partsView_inside.getPartContaining(start_inside_poly, &start_part_boundary_poly_idx);
unsigned int end_part_idx = partsView_inside.getPartContaining(end_inside_poly, &end_part_boundary_poly_idx);
if (startInside && endInside && start_part_idx == end_part_idx)
{ // normal combing within part
PolygonsPart part = partsView_inside.assemblePart(start_part_idx);
combPaths.emplace_back();
LinePolygonsCrossings::comb(part, startPoint, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
return true;
}
else
{ // comb inside part to edge (if needed) >> move through air avoiding other parts >> comb inside end part upto the endpoint (if needed)
Point middle_from;
Point middle_to;
if (startInside && endInside)
{
ClosestPolygonPoint middle_from_cp = findClosest(endPoint, boundary_inside[start_part_boundary_poly_idx]);
ClosestPolygonPoint middle_to_cp = findClosest(middle_from_cp.location, boundary_inside[end_part_boundary_poly_idx]);
// walkToNearestSmallestConnection(middle_from_cp, middle_to_cp); // TODO: perform this optimization?
middle_from = middle_from_cp.location;
middle_to = middle_to_cp.location;
}
else
{
if (!startInside && !endInside)
{
middle_from = startPoint;
middle_to = endPoint;
}
else if (!startInside && endInside)
{
middle_from = startPoint;
ClosestPolygonPoint middle_to_cp = findClosest(middle_from, boundary_inside[end_part_boundary_poly_idx]);
middle_to = middle_to_cp.location;
}
else if (startInside && !endInside)
{
middle_to = endPoint;
ClosestPolygonPoint middle_from_cp = findClosest(middle_to, boundary_inside[start_part_boundary_poly_idx]);
middle_from = middle_from_cp.location;
}
}
if (startInside)
{
// start to boundary
PolygonsPart part_begin = partsView_inside.assemblePart(start_part_idx); // comb through the starting part only
combPaths.emplace_back();
LinePolygonsCrossings::comb(part_begin, startPoint, middle_from, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
}
// throught air from boundary to boundary
if (avoid_other_parts)
{
Polygons& middle = *getBoundaryOutside(); // comb through all air, since generally the outside consists of a single part
Point from_outside = middle_from;
if (startInside || middle.inside(from_outside, true))
{ // move outside
moveInside(middle, from_outside, -offset_extra_start_end, max_moveOutside_distance2);
}
Point to_outside = middle_to;
if (endInside || middle.inside(to_outside, true))
{ // move outside
moveInside(middle, to_outside, -offset_extra_start_end, max_moveOutside_distance2);
}
combPaths.emplace_back();
combPaths.back().throughAir = true;
if ( vSize(middle_from - middle_to) < vSize(middle_from - from_outside) + vSize(middle_to - to_outside) )
{ // via outside is a detour
combPaths.back().push_back(middle_from);
combPaths.back().push_back(middle_to);
}
else
{
LinePolygonsCrossings::comb(middle, from_outside, to_outside, combPaths.back(), offset_dist_to_get_from_on_the_polygon_to_outside);
}
}
else
{ // directly through air (not avoiding other parts)
combPaths.emplace_back();
combPaths.back().throughAir = true;
combPaths.back().cross_boundary = true; // TODO: calculate whether we cross a boundary!
combPaths.back().push_back(middle_from);
combPaths.back().push_back(middle_to);
}
if (endInside)
{
// boundary to end
PolygonsPart part_end = partsView_inside.assemblePart(end_part_idx); // comb through end part only
combPaths.emplace_back();
LinePolygonsCrossings::comb(part_end, middle_to, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
}
return true;
}
}
unsigned int Comb::moveInside_(Point& from, int distance)
{
return moveInside(boundary_inside, from, distance, max_moveInside_distance2);
}
