bool Comb::optimizePath(Point startPoint, std::vector<Point>& pointList, std::vector<Point>& combPoints)
{
Point current_point = startPoint;
for(unsigned int point_idx = 1; point_idx<pointList.size(); point_idx++)
{
if (lineSegmentCollidesWithBoundary(current_point, pointList[point_idx]))
{
if (lineSegmentCollidesWithBoundary(current_point, pointList[point_idx - 1]))
{
return false;
}
current_point = pointList[point_idx - 1];
combPoints.push_back(current_point);
}
}
return true;
}
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;
}
void LinePolygonsCrossings::getCombingPath(CombPath& combPath)
{
if (shorterThen(endPoint - startPoint, Comb::max_comb_distance_ignored) || !lineSegmentCollidesWithBoundary())
{
//We're not crossing any boundaries. So skip the comb generation.
combPath.push_back(startPoint);
combPath.push_back(endPoint);
return;
}
calcScanlineCrossings();
CombPath basicPath;
getBasicCombingPath(basicPath);
optimizePath(basicPath, combPath);
}
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 LinePolygonsCrossings::getCombingPath(CombPath& combPath, int64_t max_comb_distance_ignored, bool fail_on_unavoidable_obstacles)
{
if (shorterThen(endPoint - startPoint, max_comb_distance_ignored) || !lineSegmentCollidesWithBoundary())
{
//We're not crossing any boundaries. So skip the comb generation.
combPath.push_back(startPoint);
combPath.push_back(endPoint);
return true;
}
bool success = calcScanlineCrossings(fail_on_unavoidable_obstacles);
if (!success)
{
return false;
}
CombPath basicPath;
getBasicCombingPath(basicPath);
optimizePath(basicPath, combPath);
// combPath = basicPath; // uncomment to disable comb path optimization
return true;
}
