diff --git a/src/Mod/Path/libarea/Adaptive.cpp b/src/Mod/Path/libarea/Adaptive.cpp index 513e57549b..8e9208ad6e 100644 --- a/src/Mod/Path/libarea/Adaptive.cpp +++ b/src/Mod/Path/libarea/Adaptive.cpp @@ -28,2164 +28,2997 @@ #include #include -namespace ClipperLib { - void TranslatePath(const Path& input, Path& output, IntPoint delta); +namespace ClipperLib +{ +void TranslatePath(const Path &input, Path &output, IntPoint delta); } -namespace AdaptivePath { - using namespace ClipperLib; - using namespace std; - #define SAME_POINT_TOL_SQRD_SCALED 16.0 - #define UNUSED(expr) (void)(expr) - /********************************************* +namespace AdaptivePath +{ +using namespace ClipperLib; +using namespace std; +#define SAME_POINT_TOL_SQRD_SCALED 16.0 +#define UNUSED(expr) (void)(expr) +#define SLEEPMS(expr) std::this_thread::sleep_for(std::chrono::milliseconds(expr)) +/********************************************* * Utils - inline ***********************************************/ - inline double DistanceSqrd(const IntPoint& pt1, const IntPoint& pt2) - { - double Dx = ((double)pt1.X - pt2.X); - double dy = ((double)pt1.Y - pt2.Y); - return (Dx*Dx + dy*dy); - } +inline double DistanceSqrd(const IntPoint &pt1, const IntPoint &pt2) +{ + double Dx = ((double)pt1.X - pt2.X); + double dy = ((double)pt1.Y - pt2.Y); + return (Dx * Dx + dy * dy); +} - inline bool SetSegmentLength(const IntPoint& pt1, IntPoint& pt2, double new_length) +inline bool SetSegmentLength(const IntPoint &pt1, IntPoint &pt2, double new_length) +{ + double Dx = ((double)pt2.X - pt1.X); + double dy = ((double)pt2.Y - pt1.Y); + double l = sqrt(Dx * Dx + dy * dy); + if (l > 0.0) { - double Dx = ((double)pt2.X - pt1.X); - double dy = ((double)pt2.Y - pt1.Y); - double l=sqrt(Dx*Dx + dy*dy); - if(l>0.0) { - pt2.X = long( pt1.X + new_length * Dx/l); - pt2.Y = long(pt1.Y + new_length * dy/l); + pt2.X = long(pt1.X + new_length * Dx / l); + pt2.Y = long(pt1.Y + new_length * dy / l); return true; - } - return false; } + return false; +} - int getPathNestingLevel(const Path & path, const Paths & paths) { - int nesting = 0; - for(const auto & other : paths) { - if(path.size() >0 && PointInPolygon(path.front(),other)!=0) nesting++; - } - return nesting; +int getPathNestingLevel(const Path &path, const Paths &paths) +{ + int nesting = 0; + for (const auto &other : paths) + { + if (path.size() > 0 && PointInPolygon(path.front(), other) != 0) + nesting++; } - void apendDirectChildPaths(Paths & outPaths, const Path &path, const Paths &paths ) { - int nesting = getPathNestingLevel(path,paths); - for(const auto & other : paths) { - if(path.size()>0 && other.size()>0 && PointInPolygon(other.front(),path)!=0) { - if(getPathNestingLevel(other,paths)==nesting+1) outPaths.push_back(other); - } + return nesting; +} +void apendDirectChildPaths(Paths &outPaths, const Path &path, const Paths &paths) +{ + int nesting = getPathNestingLevel(path, paths); + for (const auto &other : paths) + { + if (path.size() > 0 && other.size() > 0 && PointInPolygon(other.front(), path) != 0) + { + if (getPathNestingLevel(other, paths) == nesting + 1) + outPaths.push_back(other); } } +} - - inline bool HasAnyPath(const Paths &paths) { - for(Paths::size_type i=0;i0) return true; - } - return false; +inline bool HasAnyPath(const Paths &paths) +{ + for (Paths::size_type i = 0; i < paths.size(); i++) + { + if (paths[i].size() > 0) + return true; } + return false; +} - inline double averageDV(const vector & vec) { - double s=0; - std::size_t size = vec.size(); - if(size==0) return 0; - for(std::size_t i=0;i &vec) +{ + double s = 0; + std::size_t size = vec.size(); + if (size == 0) + return 0; + for (std::size_t i = 0; i < size; i++) + s += vec[i]; + return s / double(size); +} - inline DoublePoint rotate(const DoublePoint &in, double rad) { - double c =cos(rad); - double s =sin(rad); - return DoublePoint(c*in.X-s*in.Y,s*in.X + c*in.Y); - } +inline DoublePoint rotate(const DoublePoint &in, double rad) +{ + double c = cos(rad); + double s = sin(rad); + return DoublePoint(c * in.X - s * in.Y, s * in.X + c * in.Y); +} - // calculates path length for open path - inline double PathLength(const Path & path) { - double len=0; - if(path.size()<2) return len; - for(size_t i=1;i NTOL) + { + pt.X /= len; + pt.Y /= len; } +} - inline double Angle3Points(const DoublePoint & p1,const DoublePoint& p2, const DoublePoint& p3) { - double t1= atan2(p1.Y-p2.Y,p1.X-p2.X); - double t2=atan2(p3.Y-p2.Y,p3.X-p2.X); - double a = fabs( t2 - t1 ); - return min(a,2*M_PI-a); - } +inline DoublePoint GetPathDirectionV(const Path &pth, size_t pointIndex) +{ + if (pth.size() < 3) + return DoublePoint(0, 0); + const IntPoint &p1 = pth.at(pointIndex > 0 ? pointIndex - 1 : pth.size() - 1); + const IntPoint &p2 = pth.at(pointIndex); + double segLength = sqrt(DistanceSqrd(p1, p2)); + return DoublePoint(double(p2.X - p1.X) / segLength, double(p2.Y - p1.Y) / segLength); +} - /********************************************* +/********************************************* * Utils ***********************************************/ - - void AverageDirection(const vector &unityVectors, DoublePoint& output) { - std::size_t size=unityVectors.size(); - output.X =0; - output.Y=0; - // sum vectors - for(std::size_t i=0;ilsegLenSqr) parameter=lsegLenSqr; - } - // point on line at parameter - ptParameter=parameter/lsegLenSqr; - closestPoint.X = long(p1.X + ptParameter*D21X); - closestPoint.Y = long(p1.Y + ptParameter*D21Y); - // calculate distance from point on line to pt - double DX=double(pt.X-closestPoint.X); - double DY=double(pt.Y-closestPoint.Y); - return DX*DX+DY*DY; // return distance squared + BoundBox(const IntPoint &p1) + { + minX = p1.X; + maxX = p1.X; + minY = p1.Y; + maxY = p1.Y; } - // joins collinear segments (within the tolerance) - void CleanPath(const Path &inp, Path &outp, double tolerance) { - bool first=true; - outp.clear(); - for(const auto & pt : inp) { - if(first) { - first=false; - outp.push_back(pt); - } else { - if(outp.size()>2) { - IntPoint clp; // to hold closest point - double ptPar; - double distSqrd = DistancePointToLineSegSquared(outp[outp.size()-2],outp[outp.size()-1],pt,clp,ptPar,false); - if(sqrt(distSqrd)= bb2.minX && minY <= bb2.maxY && maxY >= bb2.minY; + } + + // bounds check - contains + inline bool Contains(const BoundBox &bb2) + { + return minX <= bb2.minX && maxX >= bb2.maxX && minY <= bb2.minY && maxY >= bb2.maxY; + } + + ClipperLib::cInt minX; + ClipperLib::cInt maxX; + ClipperLib::cInt minY; + ClipperLib::cInt maxY; +}; + +std::ostream &operator<<(std::ostream &s, const BoundBox &p) +{ + s << "(" << p.minX << "," << p.minY << ") - (" << p.maxX << "," << p.maxY << ")"; + return s; +} + +void AverageDirection(const vector &unityVectors, DoublePoint &output) +{ + std::size_t size = unityVectors.size(); + output.X = 0; + output.Y = 0; + // sum vectors + for (std::size_t i = 0; i < size; i++) + { + DoublePoint v = unityVectors[i]; + output.X += v.X; + output.Y += v.Y; + } + // normalize + double magnitude = sqrt(output.X * output.X + output.Y * output.Y); + output.X /= magnitude; + output.Y /= magnitude; +} + +double DistancePointToLineSegSquared(const IntPoint &p1, const IntPoint &p2, const IntPoint &pt, + IntPoint &closestPoint, double &ptParameter, bool clamp = true) +{ + double D21X = double(p2.X - p1.X); + double D21Y = double(p2.Y - p1.Y); + double DP1X = double(pt.X - p1.X); + double DP1Y = double(pt.Y - p1.Y); + double lsegLenSqr = D21X * D21X + D21Y * D21Y; + if (lsegLenSqr == 0) + { // segment is zero length, return point to point distance + closestPoint = p1; + ptParameter = 0; + return DP1X * DP1X + DP1Y * DP1Y; + } + double parameter = DP1X * D21X + DP1Y * D21Y; + if (clamp) + { + // clamp the parameter + if (parameter < 0) + parameter = 0; + else if (parameter > lsegLenSqr) + parameter = lsegLenSqr; + } + // point on line at parameter + ptParameter = parameter / lsegLenSqr; + closestPoint.X = long(p1.X + ptParameter * D21X); + closestPoint.Y = long(p1.Y + ptParameter * D21Y); + // calculate distance from point on line to pt + double DX = double(pt.X - closestPoint.X); + double DY = double(pt.Y - closestPoint.Y); + return DX * DX + DY * DY; // return distance squared +} + +// joins collinear segments (within the tolerance) +void CleanPath(const Path &inp, Path &outp, double tolerance) +{ + bool first = true; + outp.clear(); + double tolSqrd = tolerance * tolerance; + for (const auto &pt : inp) + { + if (first) + { + first = false; + outp.push_back(pt); + } + else + { + if (outp.size() > 2) + { + IntPoint clp; // to hold closest point + double ptPar; + double distSqrd = DistancePointToLineSegSquared(outp[outp.size() - 2], outp[outp.size() - 1], pt, clp, ptPar, false); + if (distSqrd < tolSqrd) + { + outp.pop_back(); + outp.push_back(pt); + } + else + { outp.push_back(pt); } } - } - } - - double DistancePointToPathsSqrd(const Paths &paths, const IntPoint & pt, IntPoint &closestPointOnPath, - size_t & clpPathIndex, - size_t & clpSegmentIndex, - double & clpParameter) { - double minDistSq=__DBL_MAX__; - IntPoint clp; - // iterate though paths - for(Path::size_type i=0;isize(); - // iterate through segments - for(Path::size_type j=0;jat(j>0 ? j-1 : size-1),path->at(j),pt,clp,ptPar); - if(distSq & intersections ) { - double DX = double(c2.X - c1.X); - double DY = double(c2.Y - c1.Y); - double d = sqrt(DX*DX+DY*DY); - if(d=radius) return false; // do not intersect, or intersect in one point (this case not relevant here) - double a_2 = sqrt(4*radius*radius-d*d)/2.0; - intersections.first = DoublePoint(0.5*(c1.X+c2.X)-DY*a_2/d, 0.5*(c1.Y+c2.Y)+DX*a_2/d); - intersections.second = DoublePoint(0.5*(c1.X+c2.X)+DY*a_2/d, 0.5*(c1.Y+c2.Y)-DX*a_2/d); - return true; - } - - - bool Line2CircleIntersect(const IntPoint &c, double radius,const IntPoint &p1, const IntPoint &p2, vector & result, bool clamp=true) +double DistancePointToPathsSqrd(const Paths &paths, const IntPoint &pt, IntPoint &closestPointOnPath, + size_t &clpPathIndex, + size_t &clpSegmentIndex, + double &clpParameter) +{ + double minDistSq = __DBL_MAX__; + IntPoint clp; + // iterate though paths + for (Path::size_type i = 0; i < paths.size(); i++) { - // if more intersections returned, first is closer to p1 - //to do: box check for performance - double dx=double(p2.X-p1.X); - double dy=double(p2.Y-p1.Y); - double lcx = double(p1.X - c.X); - double lcy = double(p1.Y - c.Y); - double a=dx*dx+dy*dy; - double b=2*dx*lcx+2*dy*lcy; - double C=lcx*lcx+lcy*lcy-radius*radius; - double sq = b*b-4*a*C; - if (sq<0) return false; // no solution - sq=sqrt(sq); - double t1=(-b-sq)/(2*a); - double t2=(-b+sq)/(2*a); - result.clear(); - if(clamp) { - if (t1>=0.0 && t1<=1.0) result.push_back(DoublePoint(p1.X + t1*dx, p1.Y + t1*dy)); - if (t2>=0.0 && t2<=1.0) result.push_back(DoublePoint(p1.X + t2*dx, p1.Y + t2*dy)); - } else { - result.push_back(DoublePoint(p1.X + t2*dx, p1.Y + t2*dy)); - result.push_back(DoublePoint(p1.X + t2*dx, p1.Y + t2*dy)); - } - return result.size()>0; - } - - // calculate center point of polygon - IntPoint Compute2DPolygonCentroid(const Path &vertices) - { - DoublePoint centroid(0,0); - double signedArea = 0.0; - double x0 = 0.0; // Current vertex X - double y0 = 0.0; // Current vertex Y - double x1 = 0.0; // Next vertex X - double y1 = 0.0; // Next vertex Y - double a = 0.0; // Partial signed area - - // For all vertices - size_t i=0; - Path::size_type size = vertices.size(); - for (i=0; i0 && pip!=0) return false; // is inside hole - } - return true; - } - - /* finds intersection of line segment with line segment */ - bool IntersectionPoint(const IntPoint & s1p1, - const IntPoint & s1p2, - const IntPoint & s2p1, - const IntPoint & s2p2, - IntPoint & intersection) { - // TODO: bounds check for performance - double S1DX = double(s1p2.X - s1p1.X); - double S1DY = double(s1p2.Y - s1p1.Y); - double S2DX = double(s2p2.X - s2p1.X); - double S2DY = double(s2p2.Y - s2p1.Y); - double d=S1DY*S2DX - S2DY*S1DX; - if(fabs(d)0 || p2d0 - )) return false ; // intersection not within segment1 - if((d>0) && ( - p1d<0 || p1d>d || p2d<0 || p2d>d - )) return true; // intersection not within segment2 - double t=p1d/d; - intersection=IntPoint(long(s1p1.X + S1DX*t), long(s1p1.Y + S1DY*t)); - return true; - } - - /* finds one/first intersection of line segment with paths */ - bool IntersectionPoint(const Paths & paths,const IntPoint & p1, const IntPoint & p2, IntPoint & intersection) { - for(size_t i=0; i< paths.size(); i++) { - const Path *path = &paths[i]; - size_t size=path->size(); - if(size<2) continue; - for(size_t j=0;jat(j>0?j-1:size-1); - const IntPoint * pp2 = &path->at(j); - double LDY = double(p2.Y - p1.Y); - double LDX = double(p2.X - p1.X); - double PDX = double(pp2->X - pp1->X); - double PDY = double(pp2->Y - pp1->Y); - double d=LDY*PDX - PDY*LDX; - if(fabs(d)X); - double LPDY = double(p1.Y - pp1->Y); - double p1d = PDY*LPDX - PDX*LPDY; - double p2d = LDY*LPDX - LDX*LPDY; - if((d<0) && ( - p1d0 || p2d0 - )) continue; // intersection not within segment - if((d>0) && ( - p1d<0 || p1d>d || p2d<0 || p2d>d - )) continue; // intersection not within segment - double t=p1d/d; - intersection=IntPoint(long(p1.X + LDX*t), long(p1.Y + LDY*t)); - return true; + const Path *path = &paths[i]; + Path::size_type size = path->size(); + // iterate through segments + for (Path::size_type j = 0; j < size; j++) + { + double ptPar; + double distSq = DistancePointToLineSegSquared(path->at(j > 0 ? j - 1 : size - 1), path->at(j), pt, clp, ptPar); + if (distSq < minDistSq) + { + clpPathIndex = i; + clpSegmentIndex = j; + clpParameter = ptPar; + closestPointOnPath = clp; + minDistSq = distSq; } } + } + return minDistSq; +} + +bool Circle2CircleIntersect(const IntPoint &c1, const IntPoint &c2, double radius, pair &intersections) +{ + double DX = double(c2.X - c1.X); + double DY = double(c2.Y - c1.Y); + double d = sqrt(DX * DX + DY * DY); + if (d < NTOL) + return false; // same center + if (d >= radius) + return false; // do not intersect, or intersect in one point (this case not relevant here) + double a_2 = sqrt(4 * radius * radius - d * d) / 2.0; + intersections.first = DoublePoint(0.5 * (c1.X + c2.X) - DY * a_2 / d, 0.5 * (c1.Y + c2.Y) + DX * a_2 / d); + intersections.second = DoublePoint(0.5 * (c1.X + c2.X) + DY * a_2 / d, 0.5 * (c1.Y + c2.Y) - DX * a_2 / d); + return true; +} + +bool Line2CircleIntersect(const IntPoint &c, double radius, const IntPoint &p1, const IntPoint &p2, vector &result, bool clamp = true) +{ + // if more intersections returned, first is closer to p1 + + //box check for performance + if (clamp) + { + BoundBox cBB(c, (ClipperLib::cInt)radius + 1); // circle bound box + BoundBox sBB(p1, p2); + if (!sBB.CollidesWith(cBB)) + return false; + } + + double dx = double(p2.X - p1.X); + double dy = double(p2.Y - p1.Y); + double lcx = double(p1.X - c.X); + double lcy = double(p1.Y - c.Y); + double a = dx * dx + dy * dy; + double b = 2 * dx * lcx + 2 * dy * lcy; + double C = lcx * lcx + lcy * lcy - radius * radius; + double sq = b * b - 4 * a * C; + if (sq < 0) + return false; // no solution + sq = sqrt(sq); + double t1 = (-b - sq) / (2 * a); + double t2 = (-b + sq) / (2 * a); + result.clear(); + if (clamp) + { + if (t1 >= 0.0 && t1 <= 1.0) + result.push_back(DoublePoint(p1.X + t1 * dx, p1.Y + t1 * dy)); + if (t2 >= 0.0 && t2 <= 1.0) + result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy)); + } + else + { + result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy)); + result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy)); + } + return result.size() > 0; +} + +// calculate center point of polygon +IntPoint Compute2DPolygonCentroid(const Path &vertices) +{ + DoublePoint centroid(0, 0); + double signedArea = 0.0; + double x0 = 0.0; // Current vertex X + double y0 = 0.0; // Current vertex Y + double x1 = 0.0; // Next vertex X + double y1 = 0.0; // Next vertex Y + double a = 0.0; // Partial signed area + + // For all vertices + size_t i = 0; + Path::size_type size = vertices.size(); + for (i = 0; i < size; ++i) + { + x0 = double(vertices[i].X); + y0 = double(vertices[i].Y); + x1 = double(vertices[(i + 1) % size].X); + y1 = double(vertices[(i + 1) % size].Y); + a = x0 * y1 - x1 * y0; + signedArea += a; + centroid.X += (x0 + x1) * a; + centroid.Y += (y0 + y1) * a; + } + + signedArea *= 0.5; + centroid.X /= (6.0 * signedArea); + centroid.Y /= (6.0 * signedArea); + return IntPoint(long(centroid.X), long(centroid.Y)); +} + +// point must be within first path (boundary) and must not be within all other paths (holes) +bool IsPointWithinCutRegion(const Paths &toolBoundPaths, const IntPoint &point) +{ + for (size_t i = 0; i < toolBoundPaths.size(); i++) + { + int pip = PointInPolygon(point, toolBoundPaths[i]); + if (i == 0 && pip == 0) + return false; // is outside or on boundary + if (i > 0 && pip != 0) + return false; // is inside hole + } + return true; +} + +/* finds intersection of line segment with line segment */ +bool IntersectionPoint(const IntPoint &s1p1, + const IntPoint &s1p2, + const IntPoint &s2p1, + const IntPoint &s2p2, + IntPoint &intersection) +{ + double S1DX = double(s1p2.X - s1p1.X); + double S1DY = double(s1p2.Y - s1p1.Y); + double S2DX = double(s2p2.X - s2p1.X); + double S2DY = double(s2p2.Y - s2p1.Y); + double d = S1DY * S2DX - S2DY * S1DX; + if (fabs(d) < NTOL) + return false; // lines are parallel + + double LPDX = double(s1p1.X - s2p1.X); + double LPDY = double(s1p1.Y - s2p1.Y); + double p1d = S2DY * LPDX - S2DX * LPDY; + double p2d = S1DY * LPDX - S1DX * LPDY; + if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0)) + return false; // intersection not within segment1 + if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d)) + return false; // intersection not within segment2 + double t = p1d / d; + intersection = IntPoint(long(s1p1.X + S1DX * t), long(s1p1.Y + S1DY * t)); + return true; +} + +/* finds one/first intersection of line segment with paths */ +bool IntersectionPoint(const Paths &paths, const IntPoint &p1, const IntPoint &p2, IntPoint &intersection) +{ + BoundBox segBB(p1, p2); + for (size_t i = 0; i < paths.size(); i++) + { + const Path *path = &paths[i]; + size_t size = path->size(); + if (size < 2) + continue; + BoundBox pathBB(path->front()); + for (size_t j = 0; j < size; j++) + { + + const IntPoint *pp2 = &path->at(j); + + // box check for performance + pathBB.AddPoint(*pp2); + if (!pathBB.CollidesWith(segBB)) + continue; + + const IntPoint *pp1 = &path->at(j > 0 ? j - 1 : size - 1); + double LDY = double(p2.Y - p1.Y); + double LDX = double(p2.X - p1.X); + double PDX = double(pp2->X - pp1->X); + double PDY = double(pp2->Y - pp1->Y); + double d = LDY * PDX - PDY * LDX; + if (fabs(d) < NTOL) + continue; // lines are parallel + + double LPDX = double(p1.X - pp1->X); + double LPDY = double(p1.Y - pp1->Y); + double p1d = PDY * LPDX - PDX * LPDY; + double p2d = LDY * LPDX - LDX * LPDY; + if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0)) + continue; // intersection not within segment + if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d)) + continue; // intersection not within segment + double t = p1d / d; + intersection = IntPoint(long(p1.X + LDX * t), long(p1.Y + LDY * t)); + return true; + } + } + return false; +} + +// finds the section (sub-path) of the one path between points that are closest to p1 and p2, if that distance is lower than distanceLmit +bool FindPathBetweenClosestPoints(const Paths &paths, IntPoint p1, IntPoint p2, double distanceLmit, Path &res) +{ + + size_t clpPathIndex; + IntPoint clp1; + size_t clpSegmentIndex1; + double clpParameter1; + + IntPoint clp2; + size_t clpSegmentIndex2; + double clpParameter2; + + double limitSqrd = distanceLmit * distanceLmit; + + double distSqrd = DistancePointToPathsSqrd(paths, p1, clp1, clpPathIndex, clpSegmentIndex1, clpParameter1); + + if (distSqrd > limitSqrd) + return false; // too far + Path closestPath = paths.at(clpPathIndex); + Paths closestPaths; + closestPaths.push_back(closestPath); // limit to the path where clp is found + + // find second point + distSqrd = DistancePointToPathsSqrd(closestPaths, p2, clp2, clpPathIndex, clpSegmentIndex2, clpParameter2); + + if (distSqrd > limitSqrd) + { + // second point too far, try other way around + distSqrd = DistancePointToPathsSqrd(paths, p2, clp1, clpPathIndex, clpSegmentIndex1, clpParameter1); + if (distSqrd > limitSqrd) + return false; // still too far + closestPath = paths.at(clpPathIndex); + closestPaths.clear(); + closestPaths.push_back(closestPath); + distSqrd = DistancePointToPathsSqrd(closestPaths, p1, clp2, clpPathIndex, clpSegmentIndex2, clpParameter2); + if (distSqrd > limitSqrd) + return false; // still too far + } + + // result in reverse direction + Path rev_result; + rev_result << clp1; + long minIndex = long(clpSegmentIndex2); + if (minIndex >= long(clpSegmentIndex1 - 1)) + minIndex -= long(closestPath.size()); + for (long i = long(clpSegmentIndex1) - 1; i >= minIndex; i--) + { + long index = i; + if (index < 0) + index += long(closestPath.size()); + if (sqrt(DistanceSqrd(rev_result.back(), closestPath.at(index))) > NTOL) + rev_result << closestPath.at(index); + } + if (DistanceSqrd(rev_result.back(), clp2) >= SAME_POINT_TOL_SQRD_SCALED) + rev_result << clp2; + + // result in forward direction + Path fwd_result; + fwd_result << clp1; + size_t maxIndex = clpSegmentIndex2; + if (maxIndex <= clpSegmentIndex1) + maxIndex = closestPath.size() + clpSegmentIndex2; + for (size_t i = clpSegmentIndex1; i < maxIndex; i++) + { + size_t index = i; + if (index >= closestPath.size()) + index -= closestPath.size(); + if (DistanceSqrd(fwd_result.back(), closestPath.at(index)) > SAME_POINT_TOL_SQRD_SCALED) + fwd_result << closestPath.at(index); + } + if (DistanceSqrd(fwd_result.back(), clp2) > SAME_POINT_TOL_SQRD_SCALED) + fwd_result << clp2; + + res = (PathLength(rev_result) < PathLength(fwd_result)) ? rev_result : fwd_result; // take shortest + return res.size() > 1; +} + +// finds interim points not necessarily on the same keeptooldown linking path +bool FindClosestClearPoints(const Paths &paths, IntPoint p1, IntPoint p2, double distanceLmit, IntPoint &interim1, IntPoint &interim2) +{ + size_t clpPathIndex; + size_t clpSegmentIndex1; + double clpParameter1; + size_t clpSegmentIndex2; + double clpParameter2; + double limitSqrd = distanceLmit * distanceLmit; + double distSqrd = DistancePointToPathsSqrd(paths, p1, interim1, clpPathIndex, clpSegmentIndex1, clpParameter1); + if (distSqrd > limitSqrd) + return false; // too far + // find second point + distSqrd = DistancePointToPathsSqrd(paths, p2, interim2, clpPathIndex, clpSegmentIndex2, clpParameter2); + if (distSqrd > limitSqrd) + return false; // too far + return true; +} + +bool PopPathWithClosestPoint(Paths &paths /*closest path is removed from collection */ + , + IntPoint p1, Path &result) +{ + + if (paths.size() == 0) return false; - } - - // finds the section (sub-path) of the one path between points that are closest to p1 and p2, if that distance is lower than distanceLmit - bool FindPathBetweenClosestPoints(const Paths & paths,IntPoint p1,IntPoint p2, double distanceLmit, Path & res) { - - size_t clpPathIndex; - IntPoint clp1; - size_t clpSegmentIndex1; - double clpParameter1; - - IntPoint clp2; - size_t clpSegmentIndex2; - double clpParameter2; - - double limitSqrd = distanceLmit*distanceLmit; - - double distSqrd=DistancePointToPathsSqrd(paths,p1,clp1,clpPathIndex,clpSegmentIndex1,clpParameter1); - - if(distSqrd>limitSqrd) return false; // too far - Path closestPath = paths.at(clpPathIndex); - Paths closestPaths; closestPaths.push_back(closestPath); // limit to the path where clp is found - - // find second point - distSqrd=DistancePointToPathsSqrd(closestPaths,p2,clp2,clpPathIndex,clpSegmentIndex2,clpParameter2); - - if(distSqrd>limitSqrd) { - // second point too far, try other way around - distSqrd=DistancePointToPathsSqrd(paths,p2,clp1,clpPathIndex,clpSegmentIndex1,clpParameter1); - if(distSqrd>limitSqrd) return false; // still too far - closestPath = paths.at(clpPathIndex); - closestPaths.clear(); - closestPaths.push_back(closestPath); - distSqrd=DistancePointToPathsSqrd(closestPaths,p1,clp2,clpPathIndex,clpSegmentIndex2,clpParameter2); - if(distSqrd>limitSqrd) return false; // still too far - } - - // result in reverse direction - Path rev_result; - rev_result << clp1; - long minIndex = long(clpSegmentIndex2); - if(minIndex >= long(clpSegmentIndex1-1)) minIndex -= long(closestPath.size()); - for(long i=long(clpSegmentIndex1)-1;i>=minIndex;i--) { - long index=i; - if(index<0) index+= long(closestPath.size()); - if(sqrt(DistanceSqrd(rev_result.back(),closestPath.at(index)))>NTOL) rev_result << closestPath.at(index); - } - if(sqrt(DistanceSqrd(rev_result.back(),clp2)) >NTOL) rev_result << clp2; - - // result in forward direction - Path fwd_result; - fwd_result << clp1; - size_t maxIndex = clpSegmentIndex2; - if(maxIndex <= clpSegmentIndex1) maxIndex = closestPath.size() + clpSegmentIndex2; - for(size_t i=clpSegmentIndex1;i=closestPath.size()) index-= closestPath.size(); - if(sqrt(DistanceSqrd(fwd_result.back(),closestPath.at(index)))>NTOL) fwd_result << closestPath.at(index); - } - if(sqrt(DistanceSqrd(fwd_result.back(),clp2))>NTOL) fwd_result << clp2; - - res = (PathLength(rev_result) < PathLength(fwd_result)) ? rev_result: fwd_result; // take shortest - return res.size()>1; - } - - - // finds interim points not necessarily on the same keeptooldown linking path - bool FindClosestClearPoints(const Paths & paths,IntPoint p1,IntPoint p2, double distanceLmit, IntPoint &interim1,IntPoint &interim2) { - size_t clpPathIndex; - size_t clpSegmentIndex1; - double clpParameter1; - size_t clpSegmentIndex2; - double clpParameter2; - double limitSqrd = distanceLmit*distanceLmit; - double distSqrd=DistancePointToPathsSqrd(paths,p1,interim1,clpPathIndex,clpSegmentIndex1,clpParameter1); - if(distSqrd>limitSqrd) return false; // too far - // find second point - distSqrd=DistancePointToPathsSqrd(paths,p2,interim2,clpPathIndex,clpSegmentIndex2,clpParameter2); - if(distSqrd>limitSqrd) return false; // still too far - return true; - } - - bool PopPathWithClosestPoint(Paths & paths /*closest path is removed from collection */ - ,IntPoint p1, Path & result) { - - if(paths.size()==0) return false; - - double minDistSqrd=__DBL_MAX__; - size_t closestPathIndex=0; - long closestPointIndex=0; - for(size_t pathIndex=0;pathIndex=long(closestPath.size())) index-=long(closestPath.size()); - result.push_back(closestPath.at(index)); - } - // remove the closest path - paths.erase(paths.begin()+closestPathIndex); - return true; } - void DeduplicatePaths(const Paths & inputs,Paths & outputs) { - outputs.clear(); - for(const auto & new_pth :inputs) { - bool duplicate=false; - // aff all points of new path exist on some of the old paths, path is considered duplicate - for(const auto & old_pth :outputs) { - bool all_points_exists=true; - for(const auto pt1:new_pth) { - bool pointExists=false; - for(const auto pt2:old_pth) { - if(DistanceSqrd(pt1,pt2)= long(closestPath.size())) + index -= long(closestPath.size()); + result.push_back(closestPath.at(index)); + } + // remove the closest path + paths.erase(paths.begin() + closestPathIndex); + return true; +} + +void DeduplicatePaths(const Paths &inputs, Paths &outputs) +{ + outputs.clear(); + for (const auto &new_pth : inputs) + { + bool duplicate = false; + // aff all points of new path exist on some of the old paths, path is considered duplicate + for (const auto &old_pth : outputs) + { + bool all_points_exists = true; + for (const auto pt1 : new_pth) + { + bool pointExists = false; + for (const auto pt2 : old_pth) + { + if (DistanceSqrd(pt1, pt2) < SAME_POINT_TOL_SQRD_SCALED) + { + pointExists = true; + break; } } - if(all_points_exists) { - duplicate=true; + if (!pointExists) + { + all_points_exists = false; break; } } - - if(!duplicate && new_pth.size()>0) { - outputs.push_back(new_pth); + if (all_points_exists) + { + duplicate = true; + break; } } - } - void ConnectPaths(Paths input,Paths & output) { - // remove duplicate paths - output.clear(); - bool newPath = true; - Path joined; - while(input.size()>0) { - if(newPath) { - if(joined.size()>0) output.push_back(joined); - joined.clear(); - for(auto pt:input.front()) { + if (!duplicate && new_pth.size() > 0) + { + outputs.push_back(new_pth); + } + } +} + +void ConnectPaths(Paths input, Paths &output) +{ + // remove duplicate paths + output.clear(); + bool newPath = true; + Path joined; + while (input.size() > 0) + { + if (newPath) + { + if (joined.size() > 0) + output.push_back(joined); + joined.clear(); + for (auto pt : input.front()) + { + joined.push_back(pt); + } + input.erase(input.begin()); + newPath = false; + } + bool anyMatch = false; + for (size_t i = 0; i < input.size(); i++) + { + Path &n = input.at(i); + if (DistanceSqrd(n.front(), joined.back()) < SAME_POINT_TOL_SQRD_SCALED) + { + for (auto pt : n) joined.push_back(pt); - } - input.erase(input.begin()); - newPath=false; + input.erase(input.begin() + i); + anyMatch = true; + break; } - bool anyMatch=false; - for(size_t i=0;i0) output.push_back(joined); + if (!anyMatch) + newPath = true; + } + if (joined.size() > 0) + output.push_back(joined); +} + +// helper class for measuring performance +class PerfCounter +{ + public: + PerfCounter(string p_name) + { + name = p_name; + count = 0; + running = false; + total_ticks = 0; + } + inline void Start() + { +#ifdef DEV_MODE + start_ticks = clock(); + if (running) + { + cerr << "PerfCounter already running:" << name << endl; + } + running = true; +#endif + } + inline void Stop() + { +#ifdef DEV_MODE + if (!running) + { + cerr << "PerfCounter not running:" << name << endl; + } + total_ticks += clock() - start_ticks; + start_ticks = clock(); + count++; + running = false; +#endif + } + void DumpResults() + { + double total_time = double(total_ticks) / CLOCKS_PER_SEC; + cout << "Perf: " << name.c_str() << " total_time: " << total_time << " sec, call_count:" << count << " per_call:" << double(total_time / count) << endl; + start_ticks = clock(); + total_ticks = 0; + count = 0; } + private: + string name; + clock_t start_ticks; + clock_t total_ticks; + size_t count; + bool running = false; +}; - // helper class for measuring performance - class PerfCounter { - public: - PerfCounter(string p_name) { - name = p_name; - count =0; - } - void Start() { - start_ticks=clock(); - } - void Stop() { - total_ticks+=clock()-start_ticks; - count++; - } - void DumpResults() { - double total_time=double(total_ticks)/CLOCKS_PER_SEC; - cout<<"Perf: " << name.c_str() << " total_time: " << total_time << " sec, call_count:" << count << " per_call:" << double(total_time/count) << endl; - } - private: - string name; - clock_t start_ticks; - clock_t total_ticks; - size_t count; +PerfCounter Perf_ProcessPolyNode("ProcessPolyNode"); +PerfCounter Perf_CalcCutAreaCirc("CalcCutArea"); +PerfCounter Perf_CalcCutAreaClip("CalcCutAreaClip"); +PerfCounter Perf_NextEngagePoint("NextEngagePoint"); +PerfCounter Perf_PointIterations("PointIterations"); +PerfCounter Perf_ExpandCleared("ExpandCleared"); +PerfCounter Perf_DistanceToBoundary("DistanceToBoundary"); +PerfCounter Perf_AppendToolPath("AppendToolPath"); +PerfCounter Perf_IsAllowedToCutTrough("IsAllowedToCutTrough"); +PerfCounter Perf_IsClearPath("IsClearPath"); + +/********************************* + * Cleared area handling + ********************************/ +class ClearedArea +{ + public: + ClearedArea(ClipperLib::cInt p_toolRadiusScaled) + { + toolRadiusScaled = p_toolRadiusScaled; }; - PerfCounter Perf_ProcessPolyNode("ProcessPolyNode"); - PerfCounter Perf_CalcCutArea("CalcCutArea"); - PerfCounter Perf_NextEngagePoint("NextEngagePoint"); - PerfCounter Perf_PointIterations("PointIterations"); - PerfCounter Perf_ExpandCleared("ExpandCleared"); - PerfCounter Perf_DistanceToBoundary("DistanceToBoundary"); + void SetClearedPaths(const Paths &paths) + { + clearedPaths = paths; + bboxPathsInvalid = true; + bboxClippedInvalid = true; + } + void ExpandCleared(const Path toClearToolPath) + { + if (toClearToolPath.empty()) + return; + Perf_ExpandCleared.Start(); + clipof.Clear(); + clipof.AddPath(toClearToolPath, JoinType::jtRound, EndType::etOpenRound); + Paths toolCoverPoly; + clipof.Execute(toolCoverPoly, toolRadiusScaled + 1); + clip.Clear(); + clip.AddPaths(clearedPaths, PolyType::ptSubject, true); + clip.AddPaths(toolCoverPoly, PolyType::ptClip, true); + clip.Execute(ClipType::ctUnion, clearedPaths); + CleanPolygons(clearedPaths); + bboxPathsInvalid = true; + bboxClippedInvalid = true; + Perf_ExpandCleared.Stop(); + } - /***************************************** + // gets the path sections inside the ext. tool bounding box + Paths &GetBoundedClearedPaths(const IntPoint &toolPos) + { + BoundBox toolBB(toolPos, toolRadiusScaled); + if (!bboxPathsInvalid && clearedBBPathsInFocus.Contains(toolBB)) + { + return clearedBoundedPaths; + } + ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled; + clearedBBPathsInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta)); + clearedBBPathsInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta)); + + BoundBox bb(toolPos, focusBBFactor2 * toolRadiusScaled); + clearedBoundedPaths.clear(); + for (const auto &pth : clearedPaths) + { + if (pth.size() < 2) + continue; + Path bPath; + size_t size = pth.size(); + for (size_t i = 0; i < size + 1; i++) + { + IntPoint last = (i > 0 ? pth[i - 1] : pth.back()); + IntPoint next = i < size ? pth[i] : pth.front(); + BoundBox ptbox(last, next); + if (ptbox.CollidesWith(bb)) + { + if (bPath.empty() || bPath.back() != last) + bPath.push_back(last); + bPath.push_back(next); + } + else + { + if (!bPath.empty()) + { + clearedBoundedPaths.push_back(bPath); + bPath.clear(); + } + } + } + if (!bPath.empty()) + { + clearedBoundedPaths.push_back(bPath); + bPath.clear(); + } + } + bboxPathsInvalid = false; + return clearedBoundedPaths; + } + + // get cleared area/poly bounded to toolbox + Paths &GetBoundedClearedAreaClipped(const IntPoint &toolPos) + { + BoundBox toolBB(toolPos, toolRadiusScaled); + if (!bboxClippedInvalid && clearedBBClippedInFocus.Contains(toolBB)) + { + return clearedBoundedClipped; + } + ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled; + clearedBBClippedInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta)); + clearedBBClippedInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta)); + + // a little larger area is bounded than checked + ClipperLib::cInt delta2 = focusBBFactor2 * toolRadiusScaled; + Path bbPath; + bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y - delta2)); + bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y - delta2)); + bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y + delta2)); + bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y + delta2)); + clip.Clear(); + clip.AddPath(bbPath, PolyType::ptSubject, true); + clip.AddPaths(clearedPaths, PolyType::ptClip, true); + clip.Execute(ClipType::ctIntersection, clearedBoundedClipped); + bboxClippedInvalid = false; + return clearedBoundedClipped; + } + + // get full cleared area + Paths &GetCleared() + { + return clearedPaths; + } + + private: + Clipper clip; + ClipperOffset clipof; + Paths clearedPaths; + Paths clearedBoundedClipped; + Paths clearedBoundedPaths; + + ClipperLib::cInt toolRadiusScaled; + BoundBox clearedBBClippedInFocus; + BoundBox clearedBBPathsInFocus; + + bool bboxClippedInvalid = false; + bool bboxPathsInvalid = false; + // size of the focus BB + const ClipperLib::cInt focusBBFactor1 = 8; + const ClipperLib::cInt focusBBFactor2 = 9; +}; + +/***************************************** * Linear Interpolation - area vs angle * ***************************************/ - class Interpolation { - public: - const double MIN_ANGLE = -M_PI/4; - const double MAX_ANGLE = M_PI/4; +class Interpolation +{ + public: + const double MIN_ANGLE = -M_PI / 4; + const double MAX_ANGLE = M_PI / 4; - void clear() { - angles.clear(); - areas.clear(); - } - // adds point keeping the incremental order of areas for interpolation to work correctly - void addPoint(double area, double angle) { - std::size_t size = areas.size(); - if(size==0 || area > areas[size-1] + NTOL) { // first point or largest area point - areas.push_back(area); - angles.push_back(angle); - return; - } - - for(std::size_t i=0;i areas[i-1] + NTOL)) { - areas.insert(areas.begin() + i,area); - angles.insert(angles.begin() + i,angle); - } - } - } - - double interpolateAngle(double targetArea) { - std::size_t size = areas.size(); - if(size<2 || targetArea>areas[size-1]) return MIN_ANGLE; //max engage angle - convenient value to initially measure cut area - if(targetAreatargetArea) { - // linear interpolation - double af = (targetArea-areas[i-1])/(areas[i] - areas[i-1]); - double a = angles[i-1] + af*(angles[i] - angles[i-1]); - return a; - } - } - return MIN_ANGLE; - } - - double clampAngle(double angle) { - if(angleMAX_ANGLE) return MAX_ANGLE; - return angle; - } - - double getRandomAngle() { - return MIN_ANGLE + (MAX_ANGLE-MIN_ANGLE)*double(rand())/double(RAND_MAX); - } - size_t getPointCount() { - return areas.size(); - } - - private: - vector angles; - vector areas; - - }; - - /**************************************** - * Engage Point - ***************************************/ - class EngagePoint { - public: - struct EngageState { - size_t currentPathIndex; - size_t currentSegmentIndex; - double segmentPos =0; - double totalDistance=0; - double currentPathLength=0; - int passes=0; - - double metric; // engage point metric - - bool operator < (const EngageState& other) const - { - return (metric < other.metric); - } - }; - EngagePoint(const Paths & p_toolBoundPaths) { - SetPaths(p_toolBoundPaths); - - state.currentPathIndex=0; - state.currentSegmentIndex=0; - state.segmentPos =0; - state.totalDistance=0; - calculateCurrentPathLength(); - } - - void SetPaths(const Paths & paths) { - toolBoundPaths = paths; - state.currentPathIndex=0; - state.currentSegmentIndex=0; - state.segmentPos =0; - state.totalDistance=0; - state.passes =0; - calculateCurrentPathLength(); + void clear() + { + angles.clear(); + areas.clear(); + } + // adds point keeping the incremental order of areas for interpolation to work correctly + void addPoint(double area, double angle) + { + std::size_t size = areas.size(); + if (size == 0 || area > areas[size - 1] + NTOL) + { // first point or largest area point + areas.push_back(area); + angles.push_back(angle); + return; } - EngageState GetState() { - return state; - } - - void SetState(const EngageState &new_state ) { - state=new_state; - } - - void ResetPasses() { - state.passes=0; - } - void moveToClosestPoint(const IntPoint &pt,double step) { - - Path result; - IntPoint current=pt; - // chain paths according to distance in between - Paths toChain = toolBoundPaths; - toolBoundPaths.clear(); - // if(toChain.size()>0) { - // toolBoundPaths.push_back(toChain.front()); - // toChain.erase(toChain.begin()); - // } - while(PopPathWithClosestPoint(toChain,current,result)) { - toolBoundPaths.push_back(result); - if(result.size()>0) current = result.back(); - } - - - double minDistSq = __DBL_MAX__; - size_t minPathIndex = state.currentPathIndex; - size_t minSegmentIndex = state.currentSegmentIndex; - double minSegmentPos = state.segmentPos; - state.totalDistance=0; - for(;;) { - while(moveForward(step)) { - double distSqrd = DistanceSqrd(pt,getCurrentPoint()); - if(distSqrd=maxPases) { - Perf_NextEngagePoint.Stop(); - return false; // nothing more to cut - } - prevArea=0; - } - } - IntPoint cpt = getCurrentPoint(); - double area=parent->CalcCutArea(clip,initialPoint,cpt,cleared); - //cout << "engage scan path: " << currentPathIndex << " distance:" << totalDistance << " area:" << area << " areaPD:" << area/step << " min:" << minCutArea << " max:" << maxCutArea << endl; - if(area>minCutArea && areaprevArea) { - Perf_NextEngagePoint.Stop(); - return true; - } - prevArea=area; + for (std::size_t i = 0; i < size; i++) + { + if (area < areas[i] - NTOL && (i == 0 || area > areas[i - 1] + NTOL)) + { + areas.insert(areas.begin() + i, area); + angles.insert(angles.begin() + i, angle); } } - IntPoint getCurrentPoint() { - const Path * pth = &toolBoundPaths.at(state.currentPathIndex); - const IntPoint * p1=&pth->at(state.currentSegmentIndex>0?state.currentSegmentIndex-1:pth->size()-1); - const IntPoint * p2=&pth->at(state.currentSegmentIndex); - double segLength =sqrt(DistanceSqrd(*p1,*p2)); - return IntPoint(long(p1->X + state.segmentPos*double(p2->X-p1->X)/segLength),long(p1->Y + state.segmentPos*double(p2->Y-p1->Y)/segLength)); - } - - DoublePoint getCurrentDir() { - const Path * pth = &toolBoundPaths.at(state.currentPathIndex); - const IntPoint * p1=&pth->at(state.currentSegmentIndex>0?state.currentSegmentIndex-1:pth->size()-1); - const IntPoint * p2=&pth->at(state.currentSegmentIndex); - double segLength =sqrt(DistanceSqrd(*p1,*p2)); - return DoublePoint(double(p2->X-p1->X)/segLength,double(p2->Y-p1->Y)/segLength); - } - - bool moveForward(double distance) { - const Path * pth = &toolBoundPaths.at(state.currentPathIndex); - if(distancesegmentLength) { - state.currentSegmentIndex++; - if(state.currentSegmentIndex>=pth->size()) { - state.currentSegmentIndex=0; - } - distance=distance-(segmentLength-state.segmentPos); - state.segmentPos =0; - segmentLength =currentSegmentLength(); - } - state.segmentPos+=distance; - return state.totalDistance<=1.2 * state.currentPathLength; - } - - bool nextPath() { - state.currentPathIndex++; - state.currentSegmentIndex=0; - state.segmentPos =0; - state.totalDistance=0; - if(state.currentPathIndex>=toolBoundPaths.size()) { - state.currentPathIndex =0; - calculateCurrentPathLength(); - return false; - } - calculateCurrentPathLength(); - //cout << "nextPath:" << currentPathIndex << endl; - return true; - } - - - - - private: - Paths toolBoundPaths; - EngageState state; - Clipper clip; - void calculateCurrentPathLength() { - const Path * pth = &toolBoundPaths.at(state.currentPathIndex); - size_t size=pth->size(); - state.currentPathLength=0; - for(size_t i=0;iat(i>0?i-1:size-1); - const IntPoint * p2=&pth->at(i); - state.currentPathLength += sqrt(DistanceSqrd(*p1,*p2)); - } - } - - double currentSegmentLength() { - const Path * pth = &toolBoundPaths.at(state.currentPathIndex); - const IntPoint * p1=&pth->at(state.currentSegmentIndex>0?state.currentSegmentIndex-1:pth->size()-1); - const IntPoint * p2=&pth->at(state.currentSegmentIndex); - return sqrt(DistanceSqrd(*p1,*p2)); - } - - }; - - - /**************************************** - // Adaptive2d - constructor - *****************************************/ - Adaptive2d::Adaptive2d() { } - double Adaptive2d::CalcCutArea(Clipper & clip,const IntPoint &c1, const IntPoint &c2, const Paths &cleared_paths, bool preventConvetional) { - Perf_CalcCutArea.Start(); + double interpolateAngle(double targetArea) + { + std::size_t size = areas.size(); + if (size < 2 || targetArea > areas[size - 1]) + return MIN_ANGLE; //max engage angle - convenient value to initially measure cut area + if (targetArea < areas[0]) + return MAX_ANGLE; // min engage angle - double dist = DistanceSqrd(c1,c2); - if(dist inters; // to hold intersection results - - for(const Path &path : cleared_paths) { - size_t size = path.size(); - size_t curPtIndex = 0; - bool found=false; - // step 1: we find the starting point on the cleared path that is outside new tool shape (c2) - for(size_t i=0;irsqrd) { - found = true; - break; - } - curPtIndex++; if(curPtIndex>=size) curPtIndex=0; + for (size_t i = 1; i < size; i++) + { + // find 2 subsequent points where target area is between + if (areas[i - 1] <= targetArea && areas[i] > targetArea) + { + // linear interpolation + double af = (targetArea - areas[i - 1]) / (areas[i] - areas[i - 1]); + double a = angles[i - 1] + af * (angles[i] - angles[i - 1]); + return a; } - if(!found) continue; // try another path + } + return MIN_ANGLE; + } - // step 2: iterate throuh path from starting point and find the part of the path inside the c2 - size_t prevPtIndex = curPtIndex; - Path *interPath=NULL; - bool prev_inside=false; - const IntPoint *p1=&path[prevPtIndex]; + double clampAngle(double angle) + { + if (angle < MIN_ANGLE) + return MIN_ANGLE; + if (angle > MAX_ANGLE) + return MAX_ANGLE; + return angle; + } - for(size_t i=0;i=size) curPtIndex=0; - const IntPoint *p2=&path[curPtIndex]; - if(!prev_inside) { // prev state: outside, find first point inside C2 - // TODO:BBOX check here maybe - double par; - if(DistancePointToLineSegSquared(*p1,*p2,c2,clp,par)<=rsqrd) { // current segment inside, start - prev_inside=true; - interPaths.push_back(Path()); - if(interPaths.size()>1) break; // we will use poly clipping alg. if there are more intersecting paths - interPath=&interPaths.back(); - // current segment inside c2, prev point outside, find intersection: - if(Line2CircleIntersect(c2,toolRadiusScaled,*p1,*p2,inters)) { - interPath->push_back(IntPoint(long(inters[0].X),long(inters[0].Y))); - if(inters.size()>1) { - interPath->push_back(IntPoint(long(inters[1].X),long(inters[1].Y))); - prev_inside=false; - } else { - interPath->push_back(IntPoint(*p2)); - } - } else { // no intersection - must be edge case, add p2 - //prev_inside=false; + double getRandomAngle() + { + return MIN_ANGLE + (MAX_ANGLE - MIN_ANGLE) * double(rand()) / double(RAND_MAX); + } + size_t getPointCount() + { + return areas.size(); + } + + private: + vector angles; + vector areas; +}; + +/**************************************** + * Engage Point + ***************************************/ +class EngagePoint +{ + public: + struct EngageState + { + size_t currentPathIndex; + size_t currentSegmentIndex; + double segmentPos = 0; + double totalDistance = 0; + double currentPathLength = 0; + int passes = 0; + + double metric; // engage point metric + + bool operator<(const EngageState &other) const + { + return (metric < other.metric); + } + }; + EngagePoint(const Paths &p_toolBoundPaths) + { + SetPaths(p_toolBoundPaths); + + state.currentPathIndex = 0; + state.currentSegmentIndex = 0; + state.segmentPos = 0; + state.totalDistance = 0; + calculateCurrentPathLength(); + } + + void SetPaths(const Paths &paths) + { + toolBoundPaths = paths; + state.currentPathIndex = 0; + state.currentSegmentIndex = 0; + state.segmentPos = 0; + state.totalDistance = 0; + state.passes = 0; + calculateCurrentPathLength(); + } + + EngageState GetState() + { + return state; + } + + void SetState(const EngageState &new_state) + { + state = new_state; + } + + void ResetPasses() + { + state.passes = 0; + } + void moveToClosestPoint(const IntPoint &pt, double step) + { + + Path result; + IntPoint current = pt; + // chain paths according to distance in between + Paths toChain = toolBoundPaths; + toolBoundPaths.clear(); + // if(toChain.size()>0) { + // toolBoundPaths.push_back(toChain.front()); + // toChain.erase(toChain.begin()); + // } + while (PopPathWithClosestPoint(toChain, current, result)) + { + toolBoundPaths.push_back(result); + if (result.size() > 0) + current = result.back(); + } + + double minDistSq = __DBL_MAX__; + size_t minPathIndex = state.currentPathIndex; + size_t minSegmentIndex = state.currentSegmentIndex; + double minSegmentPos = state.segmentPos; + state.totalDistance = 0; + for (;;) + { + while (moveForward(step)) + { + double distSqrd = DistanceSqrd(pt, getCurrentPoint()); + if (distSqrd < minDistSq) + { + //cout << sqrt(minDistSq) << endl; + minDistSq = distSqrd; + minPathIndex = state.currentPathIndex; + minSegmentIndex = state.currentSegmentIndex; + minSegmentPos = state.segmentPos; + } + } + if (!nextPath()) + break; + } + state.currentPathIndex = minPathIndex; + state.currentSegmentIndex = minSegmentIndex; + state.segmentPos = minSegmentPos; + calculateCurrentPathLength(); + ResetPasses(); + } + bool nextEngagePoint(Adaptive2d *parent, ClearedArea &clearedArea, double step, double minCutArea, double maxCutArea, int maxPases = 2) + { + //cout << "nextEngagePoint called step: " << step << endl; + Perf_NextEngagePoint.Start(); + double prevArea = 0; // we want to make sure that we catch the point where the area is on raising slope + //IntPoint initialPoint = getCurrentPoint(); + IntPoint initialPoint(-1000000000, -1000000000); + for (;;) + { + if (!moveForward(step)) + { + if (!nextPath()) + { + state.passes++; + if (state.passes >= maxPases) + { + Perf_NextEngagePoint.Stop(); + return false; // nothing more to cut + } + prevArea = 0; + } + } + IntPoint cpt = getCurrentPoint(); + double area = parent->CalcCutArea(clip, initialPoint, cpt, clearedArea); + //cout << "engage scan path: " << currentPathIndex << " distance:" << totalDistance << " area:" << area << " areaPD:" << area/step << " min:" << minCutArea << " max:" << maxCutArea << endl; + if (area > minCutArea && area < maxCutArea && area > prevArea) + { + Perf_NextEngagePoint.Stop(); + return true; + } + prevArea = area; + } + } + IntPoint getCurrentPoint() + { + const Path *pth = &toolBoundPaths.at(state.currentPathIndex); + const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1); + const IntPoint *p2 = &pth->at(state.currentSegmentIndex); + double segLength = sqrt(DistanceSqrd(*p1, *p2)); + return IntPoint(long(p1->X + state.segmentPos * double(p2->X - p1->X) / segLength), long(p1->Y + state.segmentPos * double(p2->Y - p1->Y) / segLength)); + } + + DoublePoint getCurrentDir() + { + const Path *pth = &toolBoundPaths.at(state.currentPathIndex); + const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1); + const IntPoint *p2 = &pth->at(state.currentSegmentIndex); + double segLength = sqrt(DistanceSqrd(*p1, *p2)); + return DoublePoint(double(p2->X - p1->X) / segLength, double(p2->Y - p1->Y) / segLength); + } + + bool moveForward(double distance) + { + const Path *pth = &toolBoundPaths.at(state.currentPathIndex); + if (distance < NTOL) + throw std::invalid_argument("distance must be positive"); + state.totalDistance += distance; + double segmentLength = currentSegmentLength(); + while (state.segmentPos + distance > segmentLength) + { + state.currentSegmentIndex++; + if (state.currentSegmentIndex >= pth->size()) + { + state.currentSegmentIndex = 0; + } + distance = distance - (segmentLength - state.segmentPos); + state.segmentPos = 0; + segmentLength = currentSegmentLength(); + } + state.segmentPos += distance; + return state.totalDistance <= 1.2 * state.currentPathLength; + } + + bool nextPath() + { + state.currentPathIndex++; + state.currentSegmentIndex = 0; + state.segmentPos = 0; + state.totalDistance = 0; + if (state.currentPathIndex >= toolBoundPaths.size()) + { + state.currentPathIndex = 0; + calculateCurrentPathLength(); + return false; + } + calculateCurrentPathLength(); + //cout << "nextPath:" << currentPathIndex << endl; + return true; + } + + private: + Paths toolBoundPaths; + EngageState state; + Clipper clip; + void calculateCurrentPathLength() + { + const Path *pth = &toolBoundPaths.at(state.currentPathIndex); + size_t size = pth->size(); + state.currentPathLength = 0; + for (size_t i = 0; i < size; i++) + { + const IntPoint *p1 = &pth->at(i > 0 ? i - 1 : size - 1); + const IntPoint *p2 = &pth->at(i); + state.currentPathLength += sqrt(DistanceSqrd(*p1, *p2)); + } + } + + double currentSegmentLength() + { + const Path *pth = &toolBoundPaths.at(state.currentPathIndex); + const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1); + const IntPoint *p2 = &pth->at(state.currentSegmentIndex); + return sqrt(DistanceSqrd(*p1, *p2)); + } +}; + +/**************************************** + // Adaptive2d - constructor + *****************************************/ +Adaptive2d::Adaptive2d() +{ +} + +double Adaptive2d::CalcCutArea(Clipper &clip, const IntPoint &c1, const IntPoint &c2, ClearedArea &clearedArea, bool preventConvetional) +{ + + double dist = DistanceSqrd(c1, c2); + if (dist < NTOL) + return 0; + + Perf_CalcCutAreaCirc.Start(); + + /// new alg + double rsqrd = toolRadiusScaled * toolRadiusScaled; + double area = 0; + Paths interPaths; + IntPoint clp; // to hold closest point + vector inters; // to hold intersection results + BoundBox c2BB(c2, toolRadiusScaled); + BoundBox c1BB(c1, toolRadiusScaled); + Paths &clearedBounded = clearedArea.GetBoundedClearedAreaClipped(c2); + for (const Path &path : clearedBounded) + { + size_t size = path.size(); + if (size == 0) + continue; + + //** bound box check + // construct bound box for path + BoundBox pathBB(path.front()); + for (const auto &pt : path) + pathBB.AddPoint(pt); + if (!c2BB.CollidesWith(c2)) + continue; // this path cannot colide with tool + //** + + size_t curPtIndex = 0; + bool found = false; + // step 1: we find the starting point on the cleared path that is outside new tool shape (c2) + for (size_t i = 0; i < size; i++) + { + if (DistanceSqrd(path[curPtIndex], c2) > rsqrd) + { + found = true; + break; + } + curPtIndex++; + if (curPtIndex >= size) + curPtIndex = 0; + } + if (!found) + continue; // try another path + + // step 2: iterate throuh path from starting point and find the part of the path inside the c2 + size_t prevPtIndex = curPtIndex; + Path *interPath = NULL; + bool prev_inside = false; + const IntPoint *p1 = &path[prevPtIndex]; + double par; // to hold parameter output + for (size_t i = 0; i < size; i++) + { + curPtIndex++; + if (curPtIndex >= size) + curPtIndex = 0; + const IntPoint *p2 = &path[curPtIndex]; + BoundBox segBB(*p1, *p2); + if (!prev_inside) + { // prev state: outside, find first point inside C2 + if (segBB.CollidesWith(c2BB) && + DistancePointToLineSegSquared(*p1, *p2, c2, clp, par) <= rsqrd) + { // current segment inside, start + prev_inside = true; + interPaths.push_back(Path()); + if (interPaths.size() > 1) + break; // we will use poly clipping alg. if there are more intersecting paths + interPath = &interPaths.back(); + // current segment inside c2, prev point outside, find intersection: + if (Line2CircleIntersect(c2, toolRadiusScaled, *p1, *p2, inters)) + { + interPath->push_back(IntPoint(long(inters[0].X), long(inters[0].Y))); + if (inters.size() > 1) + { + interPath->push_back(IntPoint(long(inters[1].X), long(inters[1].Y))); + prev_inside = false; + } + else + { interPath->push_back(IntPoint(*p2)); } } - } - else if (interPath!=NULL) { // state: inside - if( (DistanceSqrd(c2,*p2) <= rsqrd)) { // next point still inside, add it and continue, no state change + else + { // no intersection - must be edge case, add p2 + //prev_inside=false; interPath->push_back(IntPoint(*p2)); - } else { // prev point inside, current point outside, find instersection - if(Line2CircleIntersect(c2,toolRadiusScaled,*p1,*p2,inters)) { - if(inters.size()>1) { - interPath->push_back(IntPoint(long(inters[1].X),long(inters[1].Y))); - } else { - interPath->push_back(IntPoint(long(inters[0].X),long(inters[0].Y))); - } - } - prev_inside=false; } } - prevPtIndex = curPtIndex; - p1 = p2; - } - if(interPaths.size()>1) break; // we will use poly clipping alg. if there are more intersecting paths with the tool (rare case) - } - - if(interPaths.size()==1 && interPaths.front().size()>1 ) { - Path *interPath=&interPaths.front(); - // interPath - now contains the part of cleared path inside the C2 - size_t ipc2_size =interPath->size(); - const IntPoint &fpc2=interPath->front(); // first point - const IntPoint &lpc2=interPath->back(); // last point - // path length - double interPathLen=0; - for(size_t j=1;jat(j-1),interPath->at(j))); - - Paths inPaths; - inPaths.reserve(200); - inPaths.push_back(*interPath); - Path pthToSubtract ; - pthToSubtract.push_back(fpc2); - - double fi1 = atan2(fpc2.Y-c2.Y,fpc2.X-c2.X); - double fi2 = atan2(lpc2.Y-c2.Y,lpc2.X-c2.X); - double minFi=fi1; - double maxFi=fi2; - if(maxFi=RESOLUTION_FACTOR) { - // detect conventional mode cut - we want only climb mode - IntPoint midPoint(long(c2.X + toolRadiusScaled*cos(0.5*(maxFi+minFi))),long(c2.Y + toolRadiusScaled*sin(0.5*(maxFi+minFi)))); - if(PointSideOfLine(c1,c2,midPoint)<0) { - area = __DBL_MAX__; - Perf_CalcCutArea.Stop(); - // #ifdef DEV_MODE - // cout << "Break: @(" << double(c2.X)/scaleFactor << "," << double(c2.Y)/scaleFactor << ") conventional mode" << endl; - // #endif - return area; - } + else if (interPath != NULL) + { // state: inside + if ((DistanceSqrd(c2, *p2) <= rsqrd)) + { // next point still inside, add it and continue, no state change + interPath->push_back(IntPoint(*p2)); } - - double scanDistance = 2.5*toolRadiusScaled; - // stepping through path discretized to stepDistance - double stepDistance=min(double(RESOLUTION_FACTOR),interPathLen/24)+1; - //cout << stepDistance << endl; - const IntPoint * prevPt=&interPath->front(); - double distance=0; - for(size_t j=1;jat(j); - double segLen = sqrt(DistanceSqrd(*cpt,*prevPt)); - if(segLensegLen) { - distance+=stepDistance-(pos-segLen); - pos=segLen; // make sure we get exact end point - } else { - distance+=stepDistance; + else + { // prev point inside, current point outside, find instersection + if (Line2CircleIntersect(c2, toolRadiusScaled, *p1, *p2, inters)) + { + if (inters.size() > 1) + { + interPath->push_back(IntPoint(long(inters[1].X), long(inters[1].Y))); } - double dx=double(cpt->X-prevPt->X); - double dy=double(cpt->Y-prevPt->Y); - IntPoint segPoint(long(prevPt->X + dx*pos/segLen),long( prevPt->Y + dy*pos/segLen)); - IntPoint scanPoint(long(c2.X + scanDistance*cos(minFi + distance*(maxFi-minFi)/interPathLen)), - long(c2.Y + scanDistance*sin(minFi + distance*(maxFi-minFi)/interPathLen))); - - IntPoint intersC2(segPoint.X,segPoint.Y); - IntPoint intersC1(segPoint.X,segPoint.Y); - - // there should be intersection with C2 - if(Line2CircleIntersect(c2,toolRadiusScaled,segPoint,scanPoint,inters)) { - if(inters.size()>1) { - intersC2.X = long(inters[1].X); - intersC2.Y = long(inters[1].Y); - } else { - intersC2.X = long(inters[0].X); - intersC2.Y = long(inters[0].Y); - } - } else { - pthToSubtract.push_back(segPoint); - } - - if(Line2CircleIntersect(c1,toolRadiusScaled,segPoint,scanPoint,inters)) { - if(inters.size()>1) { - intersC1.X = long(inters[1].X); - intersC1.Y = long(inters[1].Y); - } else { - intersC1.X = long(inters[0].X); - intersC1.Y = long(inters[0].Y); - } - if(DistanceSqrd(segPoint,intersC2)push_back(IntPoint(long(inters[0].X), long(inters[0].Y))); } } - prevPt = cpt; + prev_inside = false; } + } + prevPtIndex = curPtIndex; + p1 = p2; + } + if (interPaths.size() > 1) + break; // we will use poly clipping alg. if there are more intersecting paths with the tool (rare case) + } + Perf_CalcCutAreaCirc.Stop(); + if (interPaths.size() == 1 && interPaths.front().size() > 1) + { + Perf_CalcCutAreaCirc.Start(); + Path *interPath = &interPaths.front(); + // interPath - now contains the part of cleared path inside the C2 + size_t ipc2_size = interPath->size(); + const IntPoint &fpc2 = interPath->front(); // first point + const IntPoint &lpc2 = interPath->back(); // last point + // path length + double interPathLen = 0; + for (size_t j = 1; j < ipc2_size; j++) + interPathLen += sqrt(DistanceSqrd(interPath->at(j - 1), interPath->at(j))); - pthToSubtract.push_back(lpc2); // add last point - pthToSubtract.push_back(c2); + Paths inPaths; + inPaths.reserve(200); + inPaths.push_back(*interPath); + Path pthToSubtract; + pthToSubtract.push_back(fpc2); - double segArea =Area(pthToSubtract); - double A=(maxFi-minFi)*rsqrd/2; // sector area - area+=A- fabs(segArea); + double fi1 = atan2(fpc2.Y - c2.Y, fpc2.X - c2.X); + double fi2 = atan2(lpc2.Y - c2.Y, lpc2.X - c2.X); + double minFi = fi1; + double maxFi = fi2; + if (maxFi < minFi) + maxFi += 2 * M_PI; - } else if(interPaths.size()>1) + if (preventConvetional && interPathLen >= RESOLUTION_FACTOR) + { + // detect conventional mode cut - we want only climb mode + IntPoint midPoint(long(c2.X + toolRadiusScaled * cos(0.5 * (maxFi + minFi))), long(c2.Y + toolRadiusScaled * sin(0.5 * (maxFi + minFi)))); + if (PointSideOfLine(c1, c2, midPoint) < 0) { - // old way of calculating cut area based on polygon slipping - // used in case when there are multiple intersections of tool with cleared poly (very rare case, but important) - // 1. find differene between old and new tool shape - Path oldTool; - Path newTool; - TranslatePath(toolGeometry,oldTool,c1); - TranslatePath(toolGeometry,newTool,c2); - clip.Clear(); - clip.AddPath(newTool, PolyType::ptSubject, true); - clip.AddPath(oldTool, PolyType::ptClip, true); - Paths toolDiff; - clip.Execute(ClipType::ctDifference,toolDiff); + area = __DBL_MAX__; + Perf_CalcCutAreaCirc.Stop(); + // #ifdef DEV_MODE + // cout << "Break: @(" << double(c2.X)/scaleFactor << "," << double(c2.Y)/scaleFactor << ") conventional mode" << endl; + // #endif + return area; + } + } - // 2. difference to cleared - clip.Clear(); - clip.AddPaths(toolDiff,PolyType::ptSubject, true); - clip.AddPaths(cleared_paths,PolyType::ptClip, true); - Paths cutAreaPoly; - clip.Execute(ClipType::ctDifference, cutAreaPoly); + double scanDistance = 2.5 * toolRadiusScaled; + // stepping through path discretized to stepDistance + double stepDistance = min(double(RESOLUTION_FACTOR), interPathLen / 24) + 1; + //cout << stepDistance << endl; + const IntPoint *prevPt = &interPath->front(); + double distance = 0; + for (size_t j = 1; j < ipc2_size; j++) + { + const IntPoint *cpt = &interPath->at(j); + double segLen = sqrt(DistanceSqrd(*cpt, *prevPt)); + if (segLen < NTOL) + continue; // skip point - segment too short + for (double pos_unclamped = 0.0; pos_unclamped < segLen + stepDistance; pos_unclamped += stepDistance) + { + double pos = pos_unclamped; + if (pos > segLen) + { + distance += stepDistance - (pos - segLen); + pos = segLen; // make sure we get exact end point + } + else + { + distance += stepDistance; + } + double dx = double(cpt->X - prevPt->X); + double dy = double(cpt->Y - prevPt->Y); + IntPoint segPoint(long(prevPt->X + dx * pos / segLen), long(prevPt->Y + dy * pos / segLen)); + IntPoint scanPoint(long(c2.X + scanDistance * cos(minFi + distance * (maxFi - minFi) / interPathLen)), + long(c2.Y + scanDistance * sin(minFi + distance * (maxFi - minFi) / interPathLen))); - // calculate resulting area - area=0; - for(Path &path : cutAreaPoly) { - area +=fabs(Area(path)); + IntPoint intersC2(segPoint.X, segPoint.Y); + IntPoint intersC1(segPoint.X, segPoint.Y); + + // there should be intersection with C2 + if (Line2CircleIntersect(c2, toolRadiusScaled, segPoint, scanPoint, inters)) + { + if (inters.size() > 1) + { + intersC2.X = long(inters[1].X); + intersC2.Y = long(inters[1].Y); + } + else + { + intersC2.X = long(inters[0].X); + intersC2.Y = long(inters[0].Y); + } + } + else + { + pthToSubtract.push_back(segPoint); + } + + if (Line2CircleIntersect(c1, toolRadiusScaled, segPoint, scanPoint, inters)) + { + if (inters.size() > 1) + { + intersC1.X = long(inters[1].X); + intersC1.Y = long(inters[1].Y); + } + else + { + intersC1.X = long(inters[0].X); + intersC1.Y = long(inters[0].Y); + } + if (DistanceSqrd(segPoint, intersC2) < DistanceSqrd(segPoint, intersC1)) + { + pthToSubtract.push_back(intersC2); + } + else + { + pthToSubtract.push_back(intersC1); + } + } + else + { // add the segpoint if no intersection with C1 + pthToSubtract.push_back(segPoint); } } - // cout<< "PolyArea:" << areaSum << " new area:" << area << endl; - Perf_CalcCutArea.Stop(); - return area; - } - - void Adaptive2d::ApplyStockToLeave(Paths &inputPaths) { - ClipperOffset clipof; - if(stockToLeave>NTOL) { - clipof.Clear(); - clipof.AddPaths(inputPaths,JoinType::jtRound,EndType::etClosedPolygon); - if(opType==OperationType::otClearingOutside || opType==OperationType::otProfilingOutside) - clipof.Execute(inputPaths,stockToLeave*scaleFactor); - else - clipof.Execute(inputPaths,-stockToLeave*scaleFactor); - } else { - // fix for clipper glitches - clipof.Clear(); - clipof.AddPaths(inputPaths,JoinType::jtRound,EndType::etClosedPolygon); - clipof.Execute(inputPaths,-1); - clipof.Clear(); - clipof.AddPaths(inputPaths,JoinType::jtRound,EndType::etClosedPolygon); - clipof.Execute(inputPaths,1); + prevPt = cpt; } - } - /**************************************** + pthToSubtract.push_back(lpc2); // add last point + pthToSubtract.push_back(c2); + + double segArea = Area(pthToSubtract); + double A = (maxFi - minFi) * rsqrd / 2; // sector area + area += A - fabs(segArea); + Perf_CalcCutAreaCirc.Stop(); + } + else if (interPaths.size() > 1) + { + Perf_CalcCutAreaClip.Start(); + // old way of calculating cut area based on polygon slipping + // used in case when there are multiple intersections of tool with cleared poly (very rare case, but important) + // 1. find differene between old and new tool shape + Path oldTool; + Path newTool; + TranslatePath(toolGeometry, oldTool, c1); + TranslatePath(toolGeometry, newTool, c2); + clip.Clear(); + clip.AddPath(newTool, PolyType::ptSubject, true); + clip.AddPath(oldTool, PolyType::ptClip, true); + Paths toolDiff; + clip.Execute(ClipType::ctDifference, toolDiff); + + // 2. difference to cleared + clip.Clear(); + clip.AddPaths(toolDiff, PolyType::ptSubject, true); + clip.AddPaths(clearedBounded, PolyType::ptClip, true); + Paths cutAreaPoly; + clip.Execute(ClipType::ctDifference, cutAreaPoly); + + // calculate resulting area + area = 0; + for (Path &path : cutAreaPoly) + { + area += fabs(Area(path)); + } + Perf_CalcCutAreaClip.Stop(); + } + // cout<< "PolyArea:" << areaSum << " new area:" << area << endl; + + return area; +} + +void Adaptive2d::ApplyStockToLeave(Paths &inputPaths) +{ + ClipperOffset clipof; + if (stockToLeave > NTOL) + { + clipof.Clear(); + clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon); + if (opType == OperationType::otClearingOutside || opType == OperationType::otProfilingOutside) + clipof.Execute(inputPaths, stockToLeave * scaleFactor); + else + clipof.Execute(inputPaths, -stockToLeave * scaleFactor); + } + else + { + // fix for clipper glitches + clipof.Clear(); + clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(inputPaths, -1); + clipof.Clear(); + clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(inputPaths, 1); + } +} + +/**************************************** // Adaptive2d - Execute *****************************************/ - std::list Adaptive2d::Execute(const DPaths &stockPaths, const DPaths &paths, std::function progressCallbackFn) { - //********************************** - // Initializations - // ********************************** +std::list Adaptive2d::Execute(const DPaths &stockPaths, const DPaths &paths, std::function progressCallbackFn) +{ + //********************************** + // Initializations + // ********************************** - // a keep the tolerance in workable range - if(tolerance<0.01) tolerance=0.01; - if(tolerance>0.2) tolerance=0.2; + // keep the tolerance in workable range + if (tolerance < 0.01) + tolerance = 0.01; + if (tolerance > 0.2) + tolerance = 0.2; - scaleFactor = RESOLUTION_FACTOR/tolerance; - toolRadiusScaled = long(toolDiameter*scaleFactor/2); - bbox_size =long(toolDiameter*scaleFactor); - progressCallback = &progressCallbackFn; - lastProgressTime=clock(); - stopProcessing=false; + scaleFactor = RESOLUTION_FACTOR / tolerance; + if (stepOverFactor * toolDiameter < 1.0) + scaleFactor *= 1.0 / (stepOverFactor * toolDiameter); + if (scaleFactor > 500) + scaleFactor = 500; - if(helixRampDiameter>toolDiameter) helixRampDiameter = toolDiameter; - if(helixRampDiameter toolDiameter) + helixRampDiameter = toolDiameter; + if (helixRampDiameter < toolDiameter / 8) + helixRampDiameter = toolDiameter / 8; + helixRampRadiusScaled = long(helixRampDiameter * scaleFactor / 2); + finishPassOffsetScaled = long(tolerance * scaleFactor / 2) + 1; - ClipperOffset clipof; - Clipper clip; + ClipperOffset clipof; + Clipper clip; - // generate tool shape + // generate tool shape + clipof.Clear(); + Path p; + p << IntPoint(0, 0); + clipof.AddPath(p, JoinType::jtRound, EndType::etOpenRound); + Paths toolGeometryPaths; + clipof.Execute(toolGeometryPaths, toolRadiusScaled); + toolGeometry = toolGeometryPaths[0]; + //calculate reference area + Path slotCut; + TranslatePath(toolGeometryPaths[0], slotCut, IntPoint(toolRadiusScaled / 2, 0)); + clip.Clear(); + clip.AddPath(toolGeometryPaths[0], PolyType::ptSubject, true); + clip.AddPath(slotCut, PolyType::ptClip, true); + Paths crossing; + clip.Execute(ClipType::ctDifference, crossing); + referenceCutArea = fabs(Area(crossing[0])); + optimalCutAreaPD = 2 * stepOverFactor * referenceCutArea / toolRadiusScaled; +#ifdef DEV_MODE + cout << "optimalCutAreaPD:" << optimalCutAreaPD << " scaleFactor:" << scaleFactor << " toolRadiusScaled:" << toolRadiusScaled << " helixRampRadiusScaled:" << helixRampRadiusScaled << endl; +#endif + + // ********************** + // Convert input paths to clipper + //************************ + Paths converted; + for (size_t i = 0; i < paths.size(); i++) + { + Path cpth; + for (size_t j = 0; j < paths[i].size(); j++) + { + std::pair pt = paths[i][j]; + cpth.push_back(IntPoint(long(pt.first * scaleFactor), long(pt.second * scaleFactor))); + } + converted.push_back(cpth); + } + + DeduplicatePaths(converted, inputPaths); + ConnectPaths(inputPaths, inputPaths); + SimplifyPolygons(inputPaths); + ApplyStockToLeave(inputPaths); + + //************************ + // convert stock paths + // ************************* + stockInputPaths.clear(); + for (size_t i = 0; i < stockPaths.size(); i++) + { + Path cpth; + for (size_t j = 0; j < stockPaths[i].size(); j++) + { + std::pair pt = stockPaths[i][j]; + cpth.push_back(IntPoint(long(pt.first * scaleFactor), long(pt.second * scaleFactor))); + } + stockInputPaths.push_back(cpth); + } + + SimplifyPolygons(stockInputPaths); + + // ******************************* + // Resolve hierarchy and run processing + // ******************************** + + if (opType == OperationType::otClearingInside || opType == OperationType::otClearingOutside) + { + + // prepare stock boundary overshooted paths clipof.Clear(); - Path p; - p << IntPoint(0,0); - clipof.AddPath(p,JoinType::jtRound,EndType::etOpenRound); - Paths toolGeometryPaths; - clipof.Execute(toolGeometryPaths,toolRadiusScaled); - toolGeometry=toolGeometryPaths[0]; - //calculate reference area - Path slotCut; - TranslatePath(toolGeometryPaths[0],slotCut,IntPoint(toolRadiusScaled/2,0)); - clip.Clear(); - clip.AddPath(toolGeometryPaths[0],PolyType::ptSubject,true); - clip.AddPath(slotCut,PolyType::ptClip,true); - Paths crossing; - clip.Execute(ClipType::ctDifference,crossing); - referenceCutArea = fabs(Area(crossing[0])); - optimalCutAreaPD =2 * stepOverFactor * referenceCutArea/toolRadiusScaled; - #ifdef DEV_MODE - cout<< "optimalCutAreaPD:" << optimalCutAreaPD << " scaleFactor:" << scaleFactor << " toolRadiusScaled:" << toolRadiusScaled << " helixRampRadiusScaled:" << helixRampRadiusScaled << endl; - #endif + clipof.AddPaths(stockInputPaths, JoinType::jtSquare, EndType::etClosedPolygon); + double overshootDistance = 4 * toolRadiusScaled + stockToLeave * scaleFactor; + if (forceInsideOut) + overshootDistance = 0; + Paths stockOvershoot; + clipof.Execute(stockOvershoot, overshootDistance); + ReversePaths(stockOvershoot); - // ********************** - // Convert input paths to clipper - //************************ - Paths converted; - for(size_t i=0;i pt = paths[i][j]; - cpth.push_back(IntPoint(long(pt.first*scaleFactor),long(pt.second*scaleFactor))); - } - converted.push_back(cpth); + if (opType == OperationType::otClearingOutside) + { + // add stock paths, with overshooting + for (auto p : stockOvershoot) + inputPaths.push_back(p); + } + else if (opType == OperationType::otClearingInside) + { + // potential TODO: check if there are open paths, and try to close it through overshooted stock boundary } - DeduplicatePaths(converted,inputPaths); - ConnectPaths(inputPaths,inputPaths); - SimplifyPolygons(inputPaths); - ApplyStockToLeave(inputPaths); + clipof.Clear(); + clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon); + Paths paths; + clipof.Execute(paths, -toolRadiusScaled - finishPassOffsetScaled); + for (const auto ¤t : paths) + { + int nesting = getPathNestingLevel(current, paths); + //cout<< " nesting:" << nesting << " limit:" << polyTreeNestingLimit << endl; + if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit)) + { + Paths toolBoundPaths; + toolBoundPaths.push_back(current); + if (polyTreeNestingLimit != nesting) + { + apendDirectChildPaths(toolBoundPaths, current, paths); + } - //************************ - // convert stock paths - // ************************* - stockInputPaths.clear(); - for(size_t i=0;i pt = stockPaths[i][j]; - cpth.push_back(IntPoint(long(pt.first*scaleFactor),long(pt.second*scaleFactor))); - } - stockInputPaths.push_back(cpth); - } - - SimplifyPolygons(stockInputPaths); - - // ******************************* - // Resolve hierarchy and run processing - // ******************************** - - if(opType==OperationType::otClearingInside || opType==OperationType::otClearingOutside) { - - // prepare stock boundary overshooted paths + // calc bounding paths - i.e. area that must be cleared inside + // it's not the same as input paths due to filtering (nesting logic) + Paths boundPaths; clipof.Clear(); - clipof.AddPaths(stockInputPaths,JoinType::jtSquare,EndType::etClosedPolygon); - double overshootDistance =4*toolRadiusScaled + stockToLeave*scaleFactor ; - if(forceInsideOut) overshootDistance=0; - Paths stockOvershoot; - clipof.Execute(stockOvershoot,overshootDistance); - ReversePaths(stockOvershoot); - - if(opType==OperationType::otClearingOutside) { - // add stock paths, with overshooting - for(auto p : stockOvershoot) inputPaths.push_back(p); - } else if(opType==OperationType::otClearingInside) { - // potential TODO: check if there are open paths, and try to close it through overshooted stock boundary - } - - clipof.Clear(); - clipof.AddPaths(inputPaths,JoinType::jtRound,EndType::etClosedPolygon); - Paths paths; - clipof.Execute(paths,-toolRadiusScaled-finishPassOffsetScaled); - for(const auto & current : paths) { - int nesting = getPathNestingLevel(current, paths); - //cout<< " nesting:" << nesting << " limit:" << polyTreeNestingLimit << endl; - if(nesting%2!=0 && (polyTreeNestingLimit==0 || nesting<=polyTreeNestingLimit)) { - Paths toolBoundPaths; - toolBoundPaths.push_back(current); - if(polyTreeNestingLimit != nesting) { - apendDirectChildPaths(toolBoundPaths,current,paths); - } - - // calc bounding paths - i.e. area that must be cleared inside - // it's not the same as input paths due to filtering (nesting logic) - Paths boundPaths; - clipof.Clear(); - clipof.AddPaths(toolBoundPaths,JoinType::jtRound,EndType::etClosedPolygon); - clipof.Execute(boundPaths,toolRadiusScaled+finishPassOffsetScaled); - ProcessPolyNode(boundPaths,toolBoundPaths); - } - } + clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(boundPaths, toolRadiusScaled + finishPassOffsetScaled); + ProcessPolyNode(boundPaths, toolBoundPaths); + } } - - if(opType==OperationType::otProfilingInside || opType==OperationType::otProfilingOutside) { - double offset = opType==OperationType::otProfilingInside ? -2*(helixRampRadiusScaled+toolRadiusScaled)-RESOLUTION_FACTOR : 2*(helixRampRadiusScaled+toolRadiusScaled) + RESOLUTION_FACTOR; - for(const auto & current : inputPaths) { - int nesting = getPathNestingLevel(current,inputPaths); - //cout<< " nesting:" << nesting << " limit:" << polyTreeNestingLimit << " processHoles:" << processHoles << endl; - if(nesting%2!=0 && (polyTreeNestingLimit==0 || nesting<=polyTreeNestingLimit)) { - Paths profilePaths; - profilePaths.push_back(current); - if(polyTreeNestingLimit != nesting) { - apendDirectChildPaths(profilePaths,current,inputPaths); - } - for(size_t i=0;i0) { - clipof.Execute(incOffset,currentDelta); - if(incOffset.size()>0) lastValidOffset=incOffset; - currentDelta-=step; - } - for(size_t i=0;i0) { - entryPoint = Compute2DPolygonCentroid(lastValidOffset[i]); - //DrawPath(lastValidOffset[i],22); - found=true; - break; + if (opType == OperationType::otProfilingInside || opType == OperationType::otProfilingOutside) + { + double offset = opType == OperationType::otProfilingInside ? -2 * (helixRampRadiusScaled + toolRadiusScaled) - RESOLUTION_FACTOR : 2 * (helixRampRadiusScaled + toolRadiusScaled) + RESOLUTION_FACTOR; + for (const auto ¤t : inputPaths) + { + int nesting = getPathNestingLevel(current, inputPaths); + //cout<< " nesting:" << nesting << " limit:" << polyTreeNestingLimit << " processHoles:" << processHoles << endl; + if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit)) + { + Paths profilePaths; + profilePaths.push_back(current); + if (polyTreeNestingLimit != nesting) + { + apendDirectChildPaths(profilePaths, current, inputPaths); } - } - // check if the start point is in any of the holes - // this may happen in case when toolBoundPaths are simetric (boundary + holes) - // we need to break simetry and try again - for(size_t j=0;j0 && pip!=0) ) { - found=false; - break; - } - } - // check if helix fits - if(found) { - // make initial polygon cleard by helix ramp - clipof.Clear(); - Path p1; - p1.push_back(entryPoint); - clipof.AddPath(p1,JoinType::jtRound,EndType::etOpenRound); - clipof.Execute(cleared,helixRampRadiusScaled+toolRadiusScaled); - CleanPolygons(cleared); - // we got first cleared area - check if it is crossing boundary - clip.Clear(); - clip.AddPaths(cleared,PolyType::ptSubject,true); - clip.AddPaths(boundPaths,PolyType::ptClip,true); - Paths crossing; - clip.Execute(ClipType::ctDifference,crossing); - if(crossing.size()>0) { - //cerr<<"Helix does not fit to the cutting area"<0 ? pth[i-1] : pth.back(); - // if point is outside the stock - if(PointInPolygon(checkPoint,stockInputPaths.front())==0) { - + for (size_t i = 0; i < profilePaths.size(); i++) + { + double efOffset = i == 0 ? offset : -offset; clipof.Clear(); - clipof.AddPaths(stockInputPaths,JoinType::jtSquare,EndType::etClosedPolygon); - clipof.Execute(cleared,1000*toolRadiusScaled); - + clipof.AddPath(profilePaths[i], JoinType::jtSquare, EndType::etClosedPolygon); + Paths off1; + clipof.Execute(off1, efOffset); + // make poly between original path and ofset path + Paths boundPaths; clip.Clear(); - clip.AddPaths(cleared,PolyType::ptSubject, true); - clip.AddPaths(stockInputPaths,PolyType::ptClip, true); - clip.Execute(ClipType::ctDifference,cleared); - CleanPolygons(cleared); - SimplifyPolygons(cleared); - //AddPathsToProgress(progressPaths,cleared); - entryPoint=checkPoint; - toolPos = entryPoint; - // find tool dir - double len=sqrt(DistanceSqrd(lastPoint,checkPoint)); - toolDir = DoublePoint((checkPoint.X - lastPoint.X)/len,(checkPoint.Y - lastPoint.Y)/len); - return true; + if (efOffset < 0) + { + clip.AddPath(profilePaths[i], PolyType::ptSubject, true); + clip.AddPaths(off1, PolyType::ptClip, true); + } + else + { + clip.AddPaths(off1, PolyType::ptSubject, true); + clip.AddPath(profilePaths[i], PolyType::ptClip, true); + } + clip.Execute(ClipType::ctDifference, boundPaths, PolyFillType::pftEvenOdd); + + Paths toolBoundPaths; + clipof.Clear(); + clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(toolBoundPaths, -toolRadiusScaled - finishPassOffsetScaled); + ProcessPolyNode(boundPaths, toolBoundPaths); } } } - //AddPathsToProgress(progressPaths,outsidePaths); - return false; } + //cout<<" Adaptive2d::Execute finish" << endl; + return results; +} - /** +bool Adaptive2d::FindEntryPoint(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &boundPaths, + ClearedArea &clearedArea /*output-initial cleard area by helix*/, + IntPoint &entryPoint /*output*/, + IntPoint &toolPos, DoublePoint &toolDir) +{ + Paths incOffset; + Paths lastValidOffset; + Clipper clip; + ClipperOffset clipof; + bool found = false; + Paths clearedPaths; + Paths checkPaths = toolBoundPaths; + for (int iter = 0; iter < 10; iter++) + { + clipof.Clear(); + clipof.AddPaths(checkPaths, JoinType::jtSquare, EndType::etClosedPolygon); + double step = RESOLUTION_FACTOR; + double currentDelta = -1; + clipof.Execute(incOffset, currentDelta); + while (incOffset.size() > 0) + { + clipof.Execute(incOffset, currentDelta); + if (incOffset.size() > 0) + lastValidOffset = incOffset; + currentDelta -= step; + } + for (size_t i = 0; i < lastValidOffset.size(); i++) + { + if (lastValidOffset[i].size() > 0) + { + entryPoint = Compute2DPolygonCentroid(lastValidOffset[i]); + //DrawPath(lastValidOffset[i],22); + found = true; + break; + } + } + // check if the start point is in any of the holes + // this may happen in case when toolBoundPaths are simetric (boundary + holes) + // we need to break simetry and try again + for (size_t j = 0; j < checkPaths.size(); j++) + { + int pip = PointInPolygon(entryPoint, checkPaths[j]); + if ((j == 0 && pip == 0) || (j > 0 && pip != 0)) + { + found = false; + break; + } + } + // check if helix fits + if (found) + { + // make initial polygon cleard by helix ramp + clipof.Clear(); + Path p1; + p1.push_back(entryPoint); + clipof.AddPath(p1, JoinType::jtRound, EndType::etOpenRound); + clipof.Execute(clearedPaths, helixRampRadiusScaled + toolRadiusScaled); + CleanPolygons(clearedPaths); + // we got first cleared area - check if it is crossing boundary + clip.Clear(); + clip.AddPaths(clearedPaths, PolyType::ptSubject, true); + clip.AddPaths(boundPaths, PolyType::ptClip, true); + Paths crossing; + clip.Execute(ClipType::ctDifference, crossing); + if (crossing.size() > 0) + { + //cerr<<"Helix does not fit to the cutting area"< 0 ? pth[i - 1] : pth.back(); + // if point is outside the stock + if (PointInPolygon(checkPoint, stockInputPaths.front()) == 0) + { + + clipof.Clear(); + clipof.AddPaths(stockInputPaths, JoinType::jtSquare, EndType::etClosedPolygon); + clipof.Execute(clearedPaths, 1000 * toolRadiusScaled); + + clip.Clear(); + clip.AddPaths(clearedPaths, PolyType::ptSubject, true); + clip.AddPaths(stockInputPaths, PolyType::ptClip, true); + clip.Execute(ClipType::ctDifference, clearedPaths); + CleanPolygons(clearedPaths); + SimplifyPolygons(clearedPaths); + clearedArea.SetClearedPaths(clearedPaths); + //AddPathsToProgress(progressPaths,cleared); + entryPoint = checkPoint; + toolPos = entryPoint; + // find tool dir + double len = sqrt(DistanceSqrd(lastPoint, checkPoint)); + toolDir = DoublePoint((checkPoint.X - lastPoint.X) / len, (checkPoint.Y - lastPoint.Y) / len); + return true; + } + } + } + //AddPathsToProgress(progressPaths,outsidePaths); + return false; +} + +/** * returns true if path is clear from obstacles */ - bool Adaptive2d::IsClearPath(const Path &tp,const Paths & cleared, double safetyDistanceScaled) { - Clipper clip; - ClipperOffset clipof; - clipof.AddPath(tp,JoinType::jtRound,EndType::etOpenRound); - Paths toolShape; - clipof.Execute(toolShape,toolRadiusScaled+safetyDistanceScaled); - clip.AddPaths(toolShape,PolyType::ptSubject,true); - clip.AddPaths(cleared,PolyType::ptClip,true); - Paths crossing; - clip.Execute(ClipType::ctDifference,crossing); - double collisionArea =0; - for(auto &p : crossing) { - collisionArea += fabs(Area(p)); - } - return collisionArea < optimalCutAreaPD*RESOLUTION_FACTOR/10+1; +bool Adaptive2d::IsClearPath(const Path &tp, ClearedArea &cleared, double safetyClearence) +{ + Perf_IsClearPath.Start(); + Clipper clip; + ClipperOffset clipof; + clipof.AddPath(tp, JoinType::jtRound, EndType::etOpenRound); + Paths toolShape; + clipof.Execute(toolShape, toolRadiusScaled + safetyClearence); + clip.AddPaths(toolShape, PolyType::ptSubject, true); + clip.AddPaths(cleared.GetCleared(), PolyType::ptClip, true); + Paths crossing; + clip.Execute(ClipType::ctDifference, crossing); + double collisionArea = 0; + for (auto &p : crossing) + { + collisionArea += fabs(Area(p)); } + Perf_IsClearPath.Stop(); + //return collisionArea < optimalCutAreaPD*RESOLUTION_FACTOR/10+1; + return collisionArea < 1.0; +} - bool Adaptive2d::IsAllowedToCutTrough(const IntPoint &p1,const IntPoint &p2,const Paths & cleared, const Paths & toolBoundPaths, double areaFactor, bool skipBoundsCheck) { - double stepSize=2*RESOLUTION_FACTOR; +bool Adaptive2d::IsAllowedToCutTrough(const IntPoint &p1, const IntPoint &p2, ClearedArea &cleared, const Paths &toolBoundPaths, double areaFactor, bool skipBoundsCheck) +{ + Perf_IsAllowedToCutTrough.Start(); + + if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p2)) + { + // last point outside boundary - its not clear to cut + Perf_IsAllowedToCutTrough.Stop(); + return false; + } + else if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p1)) + { + // first point outside boundary - its not clear to cut + Perf_IsAllowedToCutTrough.Stop(); + return false; + } + else + { Clipper clip; - size_t clpPathIndex; - size_t clpSegmentIndex; - double clpParameter; - IntPoint clp; - double distance = sqrt(DistanceSqrd(p1,p2)); - if(!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths,p1) // check with some tolerance - && sqrt(DistancePointToPathsSqrd(toolBoundPaths,p1,clp,clpPathIndex,clpSegmentIndex,clpParameter))>RESOLUTION_FACTOR - ) { - // first point outside boundary - its not clear to cut - return false; - } else for(double d=stepSize;d not clear to cut - if(area>areaFactor*stepSize*optimalCutAreaPD) return false; + double distance = sqrt(DistanceSqrd(p1, p2)); + double stepSize = min(0.5 * toolRadiusScaled * stepOverFactor, 8 * RESOLUTION_FACTOR); + if (distance < stepSize / 2) + { // not significant cut + Perf_IsAllowedToCutTrough.Stop(); + return true; + } + if (distance < stepSize) + { // adjust for numeric instability with small distances + areaFactor *= 2; + } - //if tool is outside boundary -> its not clear to cut - if(!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths,toolPos2) - && sqrt(DistancePointToPathsSqrd(toolBoundPaths,toolPos2,clp,clpPathIndex,clpSegmentIndex,clpParameter))>RESOLUTION_FACTOR - ) { + IntPoint toolPos1 = p1; + long steps = long(distance / stepSize) + 1; + stepSize = distance / steps; + for (long i = 1; i <= steps; i++) + { + double p = double(i) / steps; + IntPoint toolPos2(long(p1.X + double(p2.X - p1.X) * p), long(p1.Y + double(p2.Y - p1.Y) * p)); + double area = CalcCutArea(clip, toolPos1, toolPos2, cleared, false); + // if we are cutting above optimal -> not clear to cut + if (area > areaFactor * stepSize * optimalCutAreaPD) + { + // cout << "step:" << i << "/" << steps << "p:" << p << " area:" << area << " stepSize:" <0 && output.AdaptivePaths.back().second.size()>0) { - auto & lastTPath = output.AdaptivePaths.back(); - auto & lastTPoint = lastTPath.second.back(); - IntPoint lastPoint(long(lastTPoint.first*scaleFactor),long(lastTPoint.second*scaleFactor)); - - //first we try to cut through the linking move for short distances - bool linkFound = false; - double linkDistance = sqrt(DistanceSqrd(lastPoint,nextPoint)); - if(linkDistance sqrt(DistanceSqrd(lastPoint,nextPoint))) keepDownLinkExists=false; - } - keepDownLinkTooLong = (PathLength(keepToolDownLinkPath) > linkDistance*keepToolDownDistRatio) && (linkDistance>4*toolRadiusScaled) ; - if(keepDownLinkExists) break; - } - if(keepDownLinkExists) { - // check direct link - Path tp; - tp << lastInterimPoint; - tp << nextInterimPoint; - - bool directLinkInterimLinkClear = IsClearPath(tp,cleared,stepOverFactor*toolRadiusScaled/2); // cleared direct line between interim points? - if(directLinkInterimLinkClear) { // if direct link is ok - // add disengage moves - TPath linkPath1; - linkPath1.first = MotionType::mtCutting; - linkPath1.second.push_back(lastTPoint); - linkPath1.second.push_back(DPoint(double(lastInterimPoint.X)/scaleFactor,double(lastInterimPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath1); - - // add linking move - TPath linkPath2; - linkPath2.first = MotionType::mtLinkClear; - linkPath2.second.push_back(DPoint(double(lastInterimPoint.X)/scaleFactor,double(lastInterimPoint.Y)/scaleFactor)); - linkPath2.second.push_back(DPoint(double(nextInterimPoint.X)/scaleFactor,double(nextInterimPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath2); - - // add engage move - TPath linkPath3; - linkPath3.first = MotionType::mtCutting; - linkPath3.second.push_back(DPoint(double(nextInterimPoint.X)/scaleFactor,double(nextInterimPoint.Y)/scaleFactor)); - linkPath3.second.push_back(DPoint(double(nextPoint.X)/scaleFactor,double(nextPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath3); - linkFound=true; - } - if(!linkFound && keepDownLinkTooLong) { // if keeptooldown link too long make link through interim points with tool raise - // add disengage moves - TPath linkPath1; - linkPath1.first = MotionType::mtCutting; - linkPath1.second.push_back(lastTPoint); - linkPath1.second.push_back(DPoint(double(lastInterimPoint.X)/scaleFactor,double(lastInterimPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath1); - - // add linking move at clear height - TPath linkPath2; - linkPath2.first = MotionType::mtLinkNotClear; - linkPath2.second.push_back(DPoint(double(lastInterimPoint.X)/scaleFactor,double(lastInterimPoint.Y)/scaleFactor)); - linkPath2.second.push_back(DPoint(double(nextInterimPoint.X)/scaleFactor,double(nextInterimPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath2); - - // add engage move - TPath linkPath3; - linkPath3.first = MotionType::mtCutting; - linkPath3.second.push_back(DPoint(double(nextInterimPoint.X)/scaleFactor,double(nextInterimPoint.Y)/scaleFactor)); - linkPath3.second.push_back(DPoint(double(nextPoint.X)/scaleFactor,double(nextPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath3); - linkFound=true; - } - if(!linkFound) { // if direct link over interim points not clear and keepToolDownLinkPath not too long - make it through keep tool down link - // add disengage move - TPath linkPath1; - linkPath1.first = MotionType::mtCutting; - linkPath1.second.push_back(DPoint(double(lastPoint.X)/scaleFactor,double(lastPoint.Y)/scaleFactor)); - linkPath1.second.push_back(DPoint(double(lastInterimPoint.X)/scaleFactor,double(lastInterimPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath1); - - // add linking path - TPath linkPath2; - linkPath2.first = MotionType::mtLinkClear; - for(auto & p : keepToolDownLinkPath) { - linkPath2.second.push_back(DPoint(double(p.X)/scaleFactor,double(p.Y)/scaleFactor)); - } - output.AdaptivePaths.push_back(linkPath2); - - // add engage move - TPath linkPath3; - linkPath3.first = MotionType::mtCutting; - linkPath3.second.push_back(DPoint(double(nextInterimPoint.X)/scaleFactor,double(nextInterimPoint.Y)/scaleFactor)); - linkPath3.second.push_back(DPoint(double(nextPoint.X)/scaleFactor,double(nextPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath3); - linkFound= true; - } - } - } - if(!linkFound) { // keeptool down link not found, try lift through (any) clear interim points - ClipperOffset clipof; - clipof.AddPaths(cleared,JoinType::jtRound,EndType::etClosedPolygon); - Paths clearedOff; - double offset=stepOverFactor*toolRadiusScaled; - - // see if link through interim points is possible, - clipof.Execute(clearedOff,-toolRadiusScaled-offset); - IntPoint interim1; - IntPoint interim2; - if(FindClosestClearPoints(clearedOff,lastPoint,nextPoint,toolRadiusScaled + 2*offset,interim1,interim2)) { - if(IsAllowedToCutTrough(lastPoint,interim1,cleared,toolBoundPaths) - && IsAllowedToCutTrough(nextPoint,interim2,cleared,toolBoundPaths)) { - - // add disengage moves - TPath linkPath1; - linkPath1.first = MotionType::mtCutting; - linkPath1.second.push_back(lastTPoint); - linkPath1.second.push_back(DPoint(double(interim1.X)/scaleFactor,double(interim1.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath1); - - // add linking move - TPath linkPath2; - linkPath2.first = MotionType::mtLinkNotClear; - linkPath2.second.push_back(DPoint(double(interim1.X)/scaleFactor,double(interim1.Y)/scaleFactor)); - linkPath2.second.push_back(DPoint(double(interim2.X)/scaleFactor,double(interim2.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath2); - - // add engage move - TPath linkPath3; - linkPath3.first = MotionType::mtCutting; - linkPath3.second.push_back(DPoint(double(interim2.X)/scaleFactor,double(interim2.Y)/scaleFactor)); - linkPath3.second.push_back(DPoint(double(nextPoint.X)/scaleFactor,double(nextPoint.Y)/scaleFactor)); - output.AdaptivePaths.push_back(linkPath3); - linkFound=true; - } - - } - } - if(!linkFound) { // nothing clear so far - check direct link with no interim points - either this is clear or we need to raise the tool - Path tp; - tp << lastPoint; - tp << nextPoint; - MotionType mt = IsClearPath(tp,cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; - - // make cutting move through small clear links - if(mt==MotionType::mtLinkClear && linkDistance0) output.AdaptivePaths.push_back(cutPath); - } - - void Adaptive2d::CheckReportProgress(TPaths &progressPaths, bool force) { - if(!force && (clock()-lastProgressTimesecond.back(); - DPoint next(lastPoint->first,lastPoint->second); - while(progressPaths.size()>1) progressPaths.pop_back(); - while(progressPaths.front().second.size()>0) progressPaths.front().second.pop_back(); - progressPaths.front().first = MotionType::mtCutting; - progressPaths.front().second.push_back(next); - } - - void Adaptive2d::AddPathsToProgress(TPaths &progressPaths,Paths paths, MotionType mt) { - for(const auto & pth :paths) { - if(pth.size()>0) { - progressPaths.push_back(TPath()); - progressPaths.back().first = mt; - for(const auto pt: pth) - progressPaths.back().second.push_back(DPoint(double(pt.X)/scaleFactor,double(pt.Y)/scaleFactor)); - progressPaths.back().second.push_back(DPoint(double(pth.front().X)/scaleFactor,double(pth.front().Y)/scaleFactor)); + //if tool is outside boundary -> its not clear to cut + if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, toolPos2)) + { + Perf_IsAllowedToCutTrough.Stop(); + return false; } + toolPos1 = toolPos2; } } + Perf_IsAllowedToCutTrough.Stop(); + return true; +} - void Adaptive2d::AddPathToProgress(TPaths &progressPaths,const Path pth, MotionType mt) { - if(pth.size()>0) { - progressPaths.push_back(TPath()); - progressPaths.back().first = mt; - for(const auto pt: pth) - progressPaths.back().second.push_back(DPoint(double(pt.X)/scaleFactor,double(pt.Y)/scaleFactor)); - } - } +bool Adaptive2d::ResolveLinkPath(const IntPoint &startPoint, const IntPoint &endPoint, ClearedArea &clearedArea, Path &output) +{ + vector> queue; + queue.push_back(pair(startPoint, endPoint)); + Path checkPath; + double totalLength = 0; + double directDistance = sqrt(DistanceSqrd(startPoint, endPoint)); + Paths linkPaths; + double scanStep = 2 * RESOLUTION_FACTOR; + if (scanStep > scaleFactor * 0.1) + scanStep = scaleFactor * 0.1; + if (scanStep < scaleFactor * 0.02) + scanStep = scaleFactor * 0.02; + if (scanStep < 2) + scanStep = 2; - void Adaptive2d::ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths) { - //cout << " Adaptive2d::ProcessPolyNode" << endl; - Perf_ProcessPolyNode.Start(); - // node paths are already constrained to tool boundary path for adaptive path before finishing pass - Clipper clip; - ClipperOffset clipof; + long limit = 8 * scaleFactor * keepToolDownDistRatio / scanStep; + if (limit > 400) + limit = 400; + if (limit < 20) + limit = 20; - IntPoint entryPoint; - TPaths progressPaths; - progressPaths.reserve(10000); + double clearence = toolRadiusScaled * stepOverFactor / 2; + double offClearence = 2 * clearence + 1; + if (2 * offClearence > directDistance) + return false; + long cnt = 0; - // AddPathsToProgress(progressPaths,boundPaths, MotionType::mtLinkClear); - // CheckReportProgress(progressPaths, true); - // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // to hold CLP results + IntPoint clp; + size_t pindex; + size_t sindex; + double par; - CleanPolygons(toolBoundPaths); - SimplifyPolygons(toolBoundPaths); + // put a time limit on the resolving the link path + time_t time_out = clock() + CLOCKS_PER_SEC / 2; // 0.5 sec - CleanPolygons(boundPaths); - SimplifyPolygons(boundPaths); - - - AddPathsToProgress(progressPaths,toolBoundPaths, MotionType::mtLinkClear); - - IntPoint toolPos; - DoublePoint toolDir; - Paths cleared; - bool outsideEntry = false; - bool firstEngagePoint=true; - Paths engageBounds = toolBoundPaths; - - - if(!forceInsideOut && FindEntryPointOutside(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos,toolDir)) { - if( Orientation(engageBounds[0]) == false) ReversePath(engageBounds[0]); - //engageBounds.erase(engageBounds.begin()); - // add initial offset of cleared area to engage paths - Paths outsideEngage; - clipof.Clear(); - clipof.AddPaths(stockInputPaths,JoinType::jtRound,EndType::etClosedPolygon); - clipof.Execute(outsideEngage,toolRadiusScaled - stepOverFactor*toolRadiusScaled); - CleanPolygons(outsideEngage); - ReversePaths(outsideEngage); - for(auto p:outsideEngage) engageBounds.push_back(p); - outsideEntry=true; - } else { - if(!FindEntryPoint(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos,toolDir)) return; + while (!queue.empty()) + { + if (stopProcessing) + return false; + if (clock() > time_out) + { + cout << "Unable to resolve tool down linking path (time limit)." << endl; + return false; } - EngagePoint engage(engageBounds); // engage point stepping instance + cnt++; + if (cnt > limit) + { + cout << "Unable to resolve tool down linking path @(" << endPoint.X / scaleFactor << "," << endPoint.Y / scaleFactor << ") (limit " << limit << " segments)." << endl; + return false; + } + pair pointPair = queue.back(); + queue.pop_back(); - - if(outsideEntry) { - engage.moveToClosestPoint(toolPos,2*RESOLUTION_FACTOR); - engage.moveForward(RESOLUTION_FACTOR); - toolPos = engage.getCurrentPoint(); - toolDir = engage.getCurrentDir(); - entryPoint=toolPos; + DoublePoint direction = DirectionV(pointPair.first, pointPair.second); + checkPath.clear(); + if (pointPair.first == startPoint) + { + checkPath.push_back(IntPoint(pointPair.first.X + offClearence * direction.X, pointPair.first.Y + offClearence * direction.Y)); + } + else + { + checkPath.push_back(pointPair.first); + } + if (pointPair.second == endPoint) + { + checkPath.push_back(IntPoint(pointPair.second.X - offClearence * direction.X, pointPair.second.Y - offClearence * direction.Y)); + } + else + { + checkPath.push_back(pointPair.second); } - //cout << "Entry point:" << double(entryPoint.X)/scaleFactor << "," << double(entryPoint.Y)/scaleFactor << endl; - - AdaptiveOutput output; - output.HelixCenterPoint.first = double(entryPoint.X)/scaleFactor; - output.HelixCenterPoint.second =double(entryPoint.Y)/scaleFactor; - - long stepScaled = long(RESOLUTION_FACTOR); - IntPoint engagePoint; - - - IntPoint newToolPos; - DoublePoint newToolDir; - - CheckReportProgress(progressPaths, true); - - IntPoint startPoint = toolPos; - output.StartPoint =DPoint(double(startPoint.X)/scaleFactor,double(startPoint.Y)/scaleFactor); - - - Path passToolPath; // to store pass toolpath - Path toClearPath; // to clear toolpath - IntPoint clp; // to store closest point - vector gyro; // used to average tool direction - vector angleHistory; // use to predict deflection angle - double angle = M_PI; - engagePoint = toolPos; - Interpolation interp; // interpolation instance - - long total_iterations =0; - long total_points =0; - long total_exceeded=0; - long total_output_points=0; - long over_cut_count =0; - long bad_engage_count=0; - unclearLinkingMoveCount=0; - //long engage_no_cut_count=0; - double prevDistFromStart =0; - bool prevDistTrend = false; - - double perf_total_len=0; - #ifdef DEV_MODE - clock_t start_clock=clock(); - #endif - Paths clearedBeforePass; - /******************************* - * LOOP - PASSES - *******************************/ - for(long pass=0;pass0) - progressPaths.push_back(TPath()); + if (IsClearPath(checkPath, clearedArea, clearence)) + { + totalLength += sqrt(DistanceSqrd(pointPair.first, pointPair.second)); + if (totalLength > keepToolDownDistRatio * directDistance) + { + return false; } + Path link; + link.push_back(pointPair.first); + link.push_back(pointPair.second); + linkPaths.push_back(link); + } + else + { + if (sqrt(DistanceSqrd(pointPair.first, pointPair.second)) < RESOLUTION_FACTOR) + { + // segment became too short but still not clear + return false; + } + DoublePoint pDir(-direction.Y, direction.X); + // find mid point + IntPoint midPoint(0.5 * double(pointPair.first.X + pointPair.second.X), 0.5 * double(pointPair.first.Y + pointPair.second.Y)); + for (long i = 1;; i++) + { + if (stopProcessing) + return false; + double offset = i * scanStep; + IntPoint checkPoint1(midPoint.X - offset * pDir.X, midPoint.Y - offset * pDir.Y); + IntPoint checkPoint2(midPoint.X + offset * pDir.X, midPoint.Y + offset * pDir.Y); - angle = M_PI/4; // initial pass angle - bool reachedBoundary = false; - double cumulativeCutArea=0; - // init gyro - gyro.clear(); - for(int i=0;i1e-5) { - stepScaled = long(RESOLUTION_FACTOR/fabs(angle)); - } else { - stepScaled = long(RESOLUTION_FACTOR*4); + if(DistancePointToPathsSqrd(clearedArea.GetCleared(),checkPoint1,clp,pindex,sindex,par)< + DistancePointToPathsSqrd(clearedArea.GetCleared(),checkPoint2,clp,pindex,sindex,par)) { + // exchange points, we want to check first the point which is farther from boundary + IntPoint tmp=checkPoint2; + checkPoint2 = checkPoint1; + checkPoint1 = tmp; } + checkPath.clear(); + checkPath.push_back(checkPoint1); + if (IsClearPath(checkPath, clearedArea, clearence)) + { // check if point clear + queue.push_back(pair(pointPair.first, checkPoint1)); + queue.push_back(pair(checkPoint1, pointPair.second)); + break; + } + else + { // check the other side - - // clamp the step size - for stability - if(stepScaled>min(long(toolRadiusScaled/4), long(RESOLUTION_FACTOR*8))) - stepScaled=min(long(toolRadiusScaled/4), long(RESOLUTION_FACTOR*8)); - if(stepScaled ANGLE_HISTORY_POINTS) - angleHistory.erase(angleHistory.begin()); + checkPath.clear(); + checkPath.push_back(checkPoint2); + if (IsClearPath(checkPath, clearedArea, clearence)) + { + queue.push_back(pair(pointPair.first, checkPoint2)); + queue.push_back(pair(checkPoint2, pointPair.second)); break; } - if(iteration>5 && fabs(error-prev_error)<0.001) break; - if(iteration==MAX_ITERATIONS-1) total_exceeded++; - prev_error = error; } - Perf_PointIterations.Stop(); - /************************************************ - * CHECK AND RECORD NEW TOOL POS - * **********************************************/ - if(!IsPointWithinCutRegion(toolBoundPaths,newToolPos)) { - reachedBoundary=true; - // we reached end of cutting area - IntPoint boundaryPoint; - if(IntersectionPoint(toolBoundPaths,toolPos,newToolPos, boundaryPoint)) { - newToolPos=boundaryPoint; - area = CalcCutArea(clip,toolPos,newToolPos,cleared); - double dist = sqrt(DistanceSqrd(toolPos, newToolPos)); - if(dist>NTOL) - areaPD = area/double(dist); // area per distance - else { - areaPD=0; - area=0; - } - - } else { - newToolPos=toolPos; - area=0; - areaPD=0; - } - } - - if(area>stepScaled*optimalCutAreaPD && areaPD>2*optimalCutAreaPD) { // safety condition - over_cut_count++; - #ifdef DEV_MODE - cout<<"Break: over cut @" << point_index << "(" << double(toolPos.X)/scaleFactor << ","<< double(toolPos.Y)/scaleFactor << ")" - << " iter:" << iteration << " @bound:" << reachedBoundary << endl; - #endif - // ClearScreenFn(); - // DrawCircle(toolPos,toolRadiusScaled,0); - // DrawCircle(newToolPos,toolRadiusScaled,1); - // DrawPaths(cleared,22); - break; - } - - - - // update cleared paths when trend of distance from start point changes sign (starts to get closer, or start to get farther) - double distFromStart = sqrt(DistanceSqrd(toolPos,startPoint)); - bool distanceTrend = distFromStart > prevDistFromStart ? true : false; - - if(distanceTrend!=prevDistTrend || toClearPath.size()>10) { - Perf_ExpandCleared.Start(); - // expand cleared - clipof.Clear(); - clipof.AddPath(toClearPath,JoinType::jtRound,EndType::etOpenRound); - Paths toolCoverPoly; - clipof.Execute(toolCoverPoly,toolRadiusScaled+1); - clip.Clear(); - clip.AddPaths(cleared,PolyType::ptSubject,true); - clip.AddPaths(toolCoverPoly,PolyType::ptClip,true); - clip.Execute(ClipType::ctUnion,cleared); - CleanPolygons(cleared); - toClearPath.clear(); - Perf_ExpandCleared.Stop(); - } - prevDistTrend = distanceTrend; - prevDistFromStart = distFromStart; - - - if(area>0) { // cut is ok - record it - if(toClearPath.size()==0) toClearPath.push_back(toolPos); - toClearPath.push_back(newToolPos); - - cumulativeCutArea+=area; - - // append to toolpaths - if(passToolPath.size()==0) { - // in outside entry first successful cut defines the helix center and start point - if(output.AdaptivePaths.size()==0 && outsideEntry) { - entryPoint=toolPos; - output.HelixCenterPoint.first = double(entryPoint.X)/scaleFactor; - output.HelixCenterPoint.second =double(entryPoint.Y)/scaleFactor; - output.StartPoint =DPoint(double(entryPoint.X)/scaleFactor,double(entryPoint.Y)/scaleFactor); - } - passToolPath.push_back(toolPos); - } - passToolPath.push_back(newToolPos); - perf_total_len+=stepScaled; - toolPos=newToolPos; - - // append to progress info paths - if(progressPaths.size()==0) progressPaths.push_back(TPath()); - progressPaths.back().second.push_back(DPoint(double(newToolPos.X)/scaleFactor,double(newToolPos.Y)/scaleFactor)); - - // apend gyro - gyro.push_back(newToolDir); - gyro.erase(gyro.begin()); - CheckReportProgress(progressPaths); - } else { - #ifdef DEV_MODE - // if(point_index==0) { - // engage_no_cut_count++; - // cout<<"Break:no cut #" << engage_no_cut_count << ", bad engage, pass:" << pass << " over_cut_count:" << over_cut_count << endl; - // } - #endif - //cerr<<"Break: no cut @" << point_index << endl; - break; - } - if(reachedBoundary) - break; - } /* end of points loop*/ - - if(toClearPath.size()>0) { - // expand cleared - Perf_ExpandCleared.Start(); - clipof.Clear(); - clipof.AddPath(toClearPath,JoinType::jtRound,EndType::etOpenRound); - Paths toolCoverPoly; - clipof.Execute(toolCoverPoly,toolRadiusScaled+1); - clip.Clear(); - clip.AddPaths(cleared,PolyType::ptSubject,true); - clip.AddPaths(toolCoverPoly,PolyType::ptClip,true); - clip.Execute(ClipType::ctUnion,cleared); - CleanPolygons(cleared); - toClearPath.clear(); - Perf_ExpandCleared.Stop(); - } - if(cumulativeCutArea>tolerance*MIN_CUT_AREA_FACTOR*stepScaled*stepOverFactor*referenceCutArea) { - Path cleaned; - CleanPath(passToolPath,cleaned,CLEAN_PATH_TOLERANCE); - total_output_points+=long(cleaned.size()); - AppendToolPath(progressPaths, output,cleaned,clearedBeforePass,toolBoundPaths); - CheckReportProgress(progressPaths); - bad_engage_count=0; - //engage.ResetPasses(); - //firstEngagePoint=true; - } else { - bad_engage_count++; - } - /*****NEXT ENGAGE POINT******/ - if(firstEngagePoint) { - engage.moveToClosestPoint(newToolPos,stepScaled+1); - firstEngagePoint=false; - } else { - // check which is better to find next cut from closest point or to continue from current - double moveDistance = ENGAGE_SCAN_DISTANCE_FACTOR * RESOLUTION_FACTOR * 8; - if(!engage.nextEngagePoint(this, cleared,moveDistance, - ENGAGE_AREA_THR_FACTOR*optimalCutAreaPD*RESOLUTION_FACTOR, - 4*referenceCutArea*stepOverFactor)) { - break; - } - } - toolPos = engage.getCurrentPoint(); - toolDir = engage.getCurrentDir(); - } - /**********************************/ - /* FINISHING PASS */ - /**********************************/ - - Paths finishingPaths; - clipof.Clear(); - clipof.AddPaths(boundPaths,JoinType::jtRound,EndType::etClosedPolygon); - clipof.Execute(finishingPaths,-toolRadiusScaled); - IntPoint lastPoint = toolPos; - Path finShiftedPath; - - bool allCutsAllowed=true; - while(!stopProcessing && PopPathWithClosestPoint(finishingPaths,lastPoint,finShiftedPath)) { - if(finShiftedPath.empty()) continue; - // skip finishing passes outside the stock boundary - make no sense to cut where is no material - bool allPointsOutside=true; - IntPoint p1 = finShiftedPath.front(); - for(const auto & pt : finShiftedPath) { - - // midpoint - if(IsPointWithinCutRegion(stockInputPaths,IntPoint((p1.X+pt.X)/2,(p1.Y+pt.Y)/2))) { - allPointsOutside=false; - break; - } - //current point - if(IsPointWithinCutRegion(stockInputPaths,pt)) { - allPointsOutside=false; - break; - } - - p1=pt; - } - if(allPointsOutside) continue; - - // shift the path so that is starts with the closest point to current tool pos - - progressPaths.push_back(TPath()); - // show in progress cb - for(auto & pt:finShiftedPath) { - progressPaths.back().second.push_back(DPoint(double(pt.X)/scaleFactor,double(pt.Y)/scaleFactor)); - } - - if(!finShiftedPath.empty()) finShiftedPath << finShiftedPath.front(); // make sure its closed - - //safety check for finishing paths - check the area of finishing cut - for(size_t i=1;i keepToolDownDistRatio * directDistance) + return false; // can't find keep tool down link } } + } + if (linkPaths.empty()) + return false; + ConnectPaths(linkPaths, linkPaths); + output = linkPaths[0]; + return true; +} - Path returnPath; - returnPath << lastPoint; - returnPath << entryPoint; - output.ReturnMotionType = IsClearPath(returnPath,cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; +bool Adaptive2d::MakeLeadPath(bool leadIn, const IntPoint &startPoint, const DoublePoint &startDir, const IntPoint &beaconPoint, + ClearedArea &clearedArea, const Paths &toolBoundPaths, Path &output) +{ + IntPoint currentPoint = startPoint; + DoublePoint targetDir = DirectionV(currentPoint, beaconPoint); + double distanceToBeacon = sqrt(DistanceSqrd(startPoint, beaconPoint)); + double stepSize = 0.2 * toolRadiusScaled * stepOverFactor + 1; + double maxPathLen = toolRadiusScaled * stepOverFactor; + DoublePoint nextDir = startDir; + IntPoint nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize); + Path checkPath; + double adaptFactor = 0.4; + double alfa = M_PI / 64; + double pathLen = 0; + checkPath.push_back(nextPoint); + for (int i = 0; i < 1000; i++) + { + if (IsAllowedToCutTrough(IntPoint(currentPoint.X + RESOLUTION_FACTOR * nextDir.X, currentPoint.Y + RESOLUTION_FACTOR * nextDir.Y), nextPoint, clearedArea, toolBoundPaths)) + { + if (output.empty()) + output.push_back(currentPoint); + output.push_back(nextPoint); + currentPoint = nextPoint; + pathLen += stepSize; + targetDir = DirectionV(currentPoint, beaconPoint); + nextDir = DoublePoint(nextDir.X + adaptFactor * targetDir.X, nextDir.Y + adaptFactor * targetDir.Y); + NormalizeV(nextDir); + if (pathLen > maxPathLen) + break; + if (pathLen > distanceToBeacon) + break; + } + else + { + nextDir = rotate(nextDir, leadIn ? -alfa : alfa); + } + nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize); + } + if (output.empty()) + output.push_back(startPoint); + return true; +} +void Adaptive2d::AppendToolPath(TPaths &progressPaths, AdaptiveOutput &output, + const Path &passToolPath, ClearedArea &clearedBefore, + ClearedArea &clearedAfter, const Paths &toolBoundPaths) +{ + if (passToolPath.size() < 2) + return; + Perf_AppendToolPath.Start(); + UNUSED(progressPaths); // to silence compiler warning,var is occasionally used in dev. for debugging - // dump performance results - #ifdef DEV_MODE - Perf_ProcessPolyNode.Stop(); - Perf_ProcessPolyNode.DumpResults(); - Perf_PointIterations.DumpResults(); - Perf_CalcCutArea.DumpResults(); - Perf_NextEngagePoint.DumpResults(); - Perf_ExpandCleared.DumpResults(); - Perf_DistanceToBoundary.DumpResults(); - #endif - CheckReportProgress(progressPaths, true); - #ifdef DEV_MODE - double duration=((double)(clock()-start_clock))/CLOCKS_PER_SEC; - cout<<"PolyNode perf:"<< perf_total_len/double(scaleFactor)/duration << " mm/sec" - << " processed_points:" << total_points - << " output_points:" << total_output_points - << " total_iterations:" << total_iterations - << " iter_per_point:" << (double(total_iterations)/((double(total_points)+0.001))) - << " total_exceeded:" << total_exceeded << " (" << 100 * double(total_exceeded)/double(total_points) << "%)" - << " linking moves:" << unclearLinkingMoveCount - << endl; - #endif + IntPoint endPoint(passToolPath[0]); + IntPoint endNextPoint(passToolPath[1]); - // make sure invalid paths are not used - if(!allCutsAllowed) { - cerr << "INVALID CUTS DETECTED! Please try to modify accuracy and/or step-over." << endl; + // if there is a previous path - need to resolve linking move to new path + if (output.AdaptivePaths.size() > 0 && output.AdaptivePaths.back().second.size() > 1) + { + auto &lastTPath = output.AdaptivePaths.back(); + + auto &lastPrevTPoint = lastTPath.second.at(lastTPath.second.size() - 2); + auto &lastTPoint = lastTPath.second.back(); + + IntPoint startPrevPoint(long(lastPrevTPoint.first * scaleFactor), long(lastPrevTPoint.second * scaleFactor)); + IntPoint startPoint(long(lastTPoint.first * scaleFactor), long(lastTPoint.second * scaleFactor)); + + ClipperOffset clipof; + //first we try to cut through the linking move for short distances + bool linkFound = false; + double linkDistance = sqrt(DistanceSqrd(startPoint, endPoint)); + if (linkDistance < NTOL) + linkFound = true; + if (!linkFound && linkDistance < toolRadiusScaled && IsAllowedToCutTrough(IntPoint(startPoint.X + (endPoint.X - startPoint.X) / linkDistance, startPoint.Y + (endPoint.Y - startPoint.Y) / linkDistance), IntPoint(endPoint.X - (endPoint.X - startPoint.X) / linkDistance, endPoint.Y - (endPoint.Y - startPoint.Y) / linkDistance), clearedBefore, toolBoundPaths)) + { + Path toClear; + toClear << startPoint; + toClear << endPoint; + clearedAfter.ExpandCleared(toClear); + //cout << "cleared through" << endl; + TPath linkPath; + linkPath.first = MotionType::mtCutting; + linkPath.second.push_back(DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor)); + linkPath.second.push_back(DPoint(double(endPoint.X) / scaleFactor, double(endPoint.Y) / scaleFactor)); + output.AdaptivePaths.push_back(linkPath); + linkFound = true; } - results.push_back(output); + if (!linkFound) + { + size_t clpPathIndex; + size_t clpSegmentIndex; + double clpParameter; + IntPoint clp; + + double beaconOffset = toolRadiusScaled * stepOverFactor; + if (beaconOffset > linkDistance) + { + beaconOffset = linkDistance; + } + + DistancePointToPathsSqrd(toolBoundPaths, startPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter); + DoublePoint startDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex); + + DistancePointToPathsSqrd(toolBoundPaths, endPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter); + DoublePoint endDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex); + + IntPoint startBeacon(startPoint.X - beaconOffset * (startDir.Y - startDir.X), startPoint.Y + beaconOffset * (startDir.X + startDir.Y)); + IntPoint endBeacon(endPoint.X - beaconOffset * (endDir.X + endDir.Y), endPoint.Y + beaconOffset * (endDir.X - endDir.Y)); + Path leadOutPath; + MakeLeadPath(false, startPoint, startDir, startBeacon, clearedBefore, toolBoundPaths, leadOutPath); + + Path leadInPath; + MakeLeadPath(true, endPoint, DoublePoint(-endDir.X, -endDir.Y), endBeacon, clearedBefore, toolBoundPaths, leadInPath); + ReversePath(leadInPath); + + Path linkPath; + MotionType linkType = MotionType::mtCutting; + + clearedBefore.ExpandCleared(leadInPath); + clearedBefore.ExpandCleared(leadOutPath); + if (ResolveLinkPath(leadOutPath.back(), leadInPath.front(), clearedBefore, linkPath)) + { + linkType = MotionType::mtLinkClear; + } + else + { + linkType = MotionType::mtLinkNotClear; + double dist = sqrt(DistanceSqrd(leadOutPath.back(), leadInPath.front())); + if (dist < 4 * toolRadiusScaled && IsAllowedToCutTrough( + IntPoint(leadOutPath.back().X + (leadInPath.front().X - leadOutPath.back().X) / dist, leadOutPath.back().Y + (leadInPath.front().Y - leadOutPath.back().Y) / dist), + IntPoint(leadInPath.front().X - (leadInPath.front().X - leadOutPath.back().X) / dist, leadInPath.front().Y - (leadInPath.front().Y - leadOutPath.back().Y) / dist), + clearedBefore, toolBoundPaths)) + linkType = MotionType::mtCutting; + // add direct linking move at clear height + + linkPath.clear(); + linkPath.push_back(leadOutPath.back()); + linkPath.push_back(leadInPath.front()); + } + + // add leadout move + TPath linkPath1; + linkPath1.first = MotionType::mtCutting; + for (const auto &pt : leadOutPath) + { + linkPath1.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + } + output.AdaptivePaths.push_back(linkPath1); + + TPath linkPath2; + linkPath2.first = linkType; + for (const auto &pt : linkPath) + { + linkPath2.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + } + output.AdaptivePaths.push_back(linkPath2); + + // add leadin move + TPath linkPath3; + linkPath3.first = MotionType::mtCutting; + for (const auto &pt : leadInPath) + { + linkPath3.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + } + + output.AdaptivePaths.push_back(linkPath3); + + clearedAfter.ExpandCleared(leadInPath); + clearedAfter.ExpandCleared(leadOutPath); + + linkFound = true; + } + if (!linkFound) + { // nothing clear so far - check direct link with no interim points - either this is clear or we need to raise the tool + Path tp; + tp << startPoint; + tp << endPoint; + MotionType mt = IsClearPath(tp, clearedBefore) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; + + // make cutting move through small clear links + if (mt == MotionType::mtLinkClear && linkDistance < toolRadiusScaled) + { + mt = MotionType::mtCutting; + clearedAfter.ExpandCleared(tp); + } + + TPath linkPath; + linkPath.first = mt; + linkPath.second.push_back(DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor)); + linkPath.second.push_back(DPoint(double(endPoint.X) / scaleFactor, double(endPoint.Y) / scaleFactor)); + output.AdaptivePaths.push_back(linkPath); + } + } + TPath cutPath; + cutPath.first = MotionType::mtCutting; + for (const auto &p : passToolPath) + { + DPoint nextT; + nextT.first = double(p.X) / scaleFactor; + nextT.second = double(p.Y) / scaleFactor; + cutPath.second.push_back(nextT); } + // if(close) { + // DPoint nextT; + // nextT.first = double(passToolPath[0].X)/scaleFactor; + // nextT.second = double(passToolPath[0].Y)/scaleFactor; + // cutPath.second.push_back(nextT); + // } + if (cutPath.second.size() > 0) + output.AdaptivePaths.push_back(cutPath); + Perf_AppendToolPath.Stop(); } + +void Adaptive2d::CheckReportProgress(TPaths &progressPaths, bool force) +{ + if (!force && (clock() - lastProgressTime < PROGRESS_TICKS)) + return; // not yet + lastProgressTime = clock(); + if (progressPaths.size() == 0) + return; + if (progressCallback) + if ((*progressCallback)(progressPaths)) + stopProcessing = true; // call python function, if returns true signal stop processing + // clean the paths - keep the last point + if (progressPaths.back().second.size() == 0) + return; + TPath *lastPath = &progressPaths.back(); + DPoint *lastPoint = &lastPath->second.back(); + DPoint next(lastPoint->first, lastPoint->second); + while (progressPaths.size() > 1) + progressPaths.pop_back(); + while (progressPaths.front().second.size() > 0) + progressPaths.front().second.pop_back(); + progressPaths.front().first = MotionType::mtCutting; + progressPaths.front().second.push_back(next); +} + +void Adaptive2d::AddPathsToProgress(TPaths &progressPaths, Paths paths, MotionType mt) +{ + for (const auto &pth : paths) + { + if (pth.size() > 0) + { + progressPaths.push_back(TPath()); + progressPaths.back().first = mt; + for (const auto pt : pth) + progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + progressPaths.back().second.push_back(DPoint(double(pth.front().X) / scaleFactor, double(pth.front().Y) / scaleFactor)); + } + } +} + +void Adaptive2d::AddPathToProgress(TPaths &progressPaths, const Path pth, MotionType mt) +{ + if (pth.size() > 0) + { + progressPaths.push_back(TPath()); + progressPaths.back().first = mt; + for (const auto pt : pth) + progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + } +} + +void Adaptive2d::ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths) +{ + Perf_ProcessPolyNode.Start(); + // node paths are already constrained to tool boundary path for adaptive path before finishing pass + Clipper clip; + ClipperOffset clipof; + + IntPoint entryPoint; + TPaths progressPaths; + progressPaths.reserve(10000); + + // AddPathsToProgress(progressPaths,boundPaths, MotionType::mtLinkClear); + // CheckReportProgress(progressPaths, true); + // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + CleanPolygons(toolBoundPaths); + SimplifyPolygons(toolBoundPaths); + + CleanPolygons(boundPaths); + SimplifyPolygons(boundPaths); + + AddPathsToProgress(progressPaths, toolBoundPaths, MotionType::mtLinkClear); + + IntPoint toolPos; + DoublePoint toolDir; + ClearedArea cleared(toolRadiusScaled); + bool outsideEntry = false; + bool firstEngagePoint = true; + Paths engageBounds = toolBoundPaths; + + if (!forceInsideOut && FindEntryPointOutside(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir)) + { + if (Orientation(engageBounds[0]) == false) + ReversePath(engageBounds[0]); + //engageBounds.erase(engageBounds.begin()); + // add initial offset of cleared area to engage paths + Paths outsideEngage; + clipof.Clear(); + clipof.AddPaths(stockInputPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(outsideEngage, toolRadiusScaled - stepOverFactor * toolRadiusScaled); + CleanPolygons(outsideEngage); + ReversePaths(outsideEngage); + for (auto p : outsideEngage) + engageBounds.push_back(p); + outsideEntry = true; + } + else + { + if (!FindEntryPoint(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir)) + { + Perf_ProcessPolyNode.Stop(); + return; + } + } + + EngagePoint engage(engageBounds); // engage point stepping instance + + if (outsideEntry) + { + engage.moveToClosestPoint(toolPos, 2 * RESOLUTION_FACTOR); + engage.moveForward(RESOLUTION_FACTOR); + toolPos = engage.getCurrentPoint(); + toolDir = engage.getCurrentDir(); + entryPoint = toolPos; + } + + //cout << "Entry point:" << double(entryPoint.X)/scaleFactor << "," << double(entryPoint.Y)/scaleFactor << endl; + + AdaptiveOutput output; + output.HelixCenterPoint.first = double(entryPoint.X) / scaleFactor; + output.HelixCenterPoint.second = double(entryPoint.Y) / scaleFactor; + + long stepScaled = long(RESOLUTION_FACTOR); + IntPoint engagePoint; + + IntPoint newToolPos; + DoublePoint newToolDir; + + CheckReportProgress(progressPaths, true); + + IntPoint startPoint = toolPos; + output.StartPoint = DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor); + + Path passToolPath; // to store pass toolpath + Path toClearPath; // to clear toolpath + IntPoint clp; // to store closest point + vector gyro; // used to average tool direction + vector angleHistory; // use to predict deflection angle + double angle = M_PI; + engagePoint = toolPos; + Interpolation interp; // interpolation instance + + long total_iterations = 0; + long total_points = 0; + long total_exceeded = 0; + long total_output_points = 0; + long over_cut_count = 0; + long bad_engage_count = 0; + unclearLinkingMoveCount = 0; + //long engage_no_cut_count=0; + double prevDistFromStart = 0; + double refinement_factor = 1; + bool prevDistTrend = false; + + double perf_total_len = 0; +#ifdef DEV_MODE + clock_t start_clock = clock(); +#endif + ClearedArea clearedBeforePass(toolRadiusScaled); + clearedBeforePass.SetClearedPaths(cleared.GetCleared()); + + /******************************* + * LOOP - PASSES + *******************************/ + for (long pass = 0; pass < PASSES_LIMIT; pass++) + { + if (stopProcessing) + break; + //cout<<"Pass:"<< pass << endl; + passToolPath.clear(); + toClearPath.clear(); + angleHistory.clear(); + + // append a new path to progress info paths + if (progressPaths.size() == 0) + { + progressPaths.push_back(TPath()); + } + else + { + // append new path if previous not empty + if (progressPaths.back().second.size() > 0) + progressPaths.push_back(TPath()); + } + + angle = M_PI / 4; // initial pass angle + bool reachedBoundary = false; + double cumulativeCutArea = 0; + // init gyro + gyro.clear(); + for (int i = 0; i < DIRECTION_SMOOTHING_BUFLEN; i++) + gyro.push_back(toolDir); + + size_t clpPathIndex; + size_t clpSegmentIndex; + double clpParamter; + clearedBeforePass.SetClearedPaths(cleared.GetCleared()); + /******************************* + * LOOP - POINTS + *******************************/ + for (long point_index = 0; point_index < POINTS_PER_PASS_LIMIT; point_index++) + { + if (stopProcessing) + break; + //cout<<"Pass:"<< pass << " Point:" << point_index << endl; + total_points++; + AverageDirection(gyro, toolDir); + Perf_DistanceToBoundary.Start(); + + //double distanceToBoundary = __DBL_MAX__; + double distanceToBoundary = sqrt(DistancePointToPathsSqrd(toolBoundPaths, toolPos, clp, clpPathIndex, clpSegmentIndex, clpParamter)); + DoublePoint boundaryDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex); + double distBoundaryPointToEngage = sqrt(DistanceSqrd(clp, engagePoint)); + // double range = 2*toolRadiusScaled*stepOverFactor; + // if(IntersectionPoint(toolBoundPaths,toolPos,IntPoint(toolPos.X + range* toolDir.X,toolPos.Y + range* toolDir.Y), clp)) { + // distanceToBoundary=sqrt(DistanceSqrd(toolPos,clp)); + // } + Perf_DistanceToBoundary.Stop(); + double distanceToEngage = sqrt(DistanceSqrd(toolPos, engagePoint)); + //double relDistToBoundary = distanceToBoundary/toolRadiusScaled ; + + double targetAreaPD = optimalCutAreaPD; + + // set the step size + double slowDownDistance = max(double(toolRadiusScaled) / 4, RESOLUTION_FACTOR * 8); + if (distanceToBoundary < slowDownDistance || distanceToEngage < slowDownDistance) + { + stepScaled = long(RESOLUTION_FACTOR); + } + else if (fabs(angle) > NTOL) + { + stepScaled = long(RESOLUTION_FACTOR / fabs(angle)); + } + else + { + stepScaled = long(RESOLUTION_FACTOR * 4); + } + + // clamp the step size - for stability + if (stepScaled > min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8))) + stepScaled = min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8)); + if (stepScaled < RESOLUTION_FACTOR) + stepScaled = long(RESOLUTION_FACTOR); + + //stepScaled=RESOLUTION_FACTOR; + + /************************************ + * ANGLE vs AREA ITERATIONS + *********************************/ + double predictedAngle = averageDV(angleHistory); + double maxError = AREA_ERROR_FACTOR * optimalCutAreaPD; + double area = 0; + double areaPD = 0; + interp.clear(); + /******************************/ + Perf_PointIterations.Start(); + int iteration; + double prev_error = __DBL_MAX__; + for (iteration = 0; iteration < MAX_ITERATIONS; iteration++) + { + total_iterations++; + if (iteration == 0) + angle = predictedAngle; + else if (iteration == 1) + angle = interp.MIN_ANGLE; // max engage + else if (iteration == 3) + angle = interp.MAX_ANGLE; // min engage + else if (interp.getPointCount() < 2) + angle = interp.getRandomAngle(); + else + angle = interp.interpolateAngle(targetAreaPD); + angle = interp.clampAngle(angle); + + newToolDir = rotate(toolDir, angle); + newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled)); + + area = CalcCutArea(clip, toolPos, newToolPos, cleared); + areaPD = area / double(stepScaled); // area per distance + interp.addPoint(areaPD, angle); + double error = areaPD - targetAreaPD; + // cout << " iter:" << iteration << " angle:" << angle << " area:" << areaPD << " target:" << targetAreaPD << " error:" << error << " max:"<< maxError << endl; + if (fabs(error) < maxError) + { + angleHistory.push_back(angle); + if (angleHistory.size() > ANGLE_HISTORY_POINTS) + angleHistory.erase(angleHistory.begin()); + break; + } + if (iteration > 5 && fabs(error - prev_error) < 0.001) + break; + if (iteration == MAX_ITERATIONS - 1) + total_exceeded++; + prev_error = error; + } + Perf_PointIterations.Stop(); + + // approach end boundary tangentially + double relDistToBoundary = distanceToBoundary / (toolRadiusScaled * stepOverFactor); + if (relDistToBoundary <= 1.0 && distBoundaryPointToEngage > toolRadiusScaled * stepOverFactor) + { + double wb = (1 - relDistToBoundary); + //double w=(0.8*(1-exp(-4*distanceToBoundary/toolRadiusScaled)) + 0.2); + newToolDir = DoublePoint(newToolDir.X + wb * boundaryDir.X, newToolDir.Y + wb * boundaryDir.Y); + NormalizeV(newToolDir); + newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled)); + } + + /************************************************ + * CHECK AND RECORD NEW TOOL POS + * **********************************************/ + if (!IsPointWithinCutRegion(toolBoundPaths, newToolPos)) + { + reachedBoundary = true; + // we reached end of cutting area + IntPoint boundaryPoint; + if (IntersectionPoint(toolBoundPaths, toolPos, newToolPos, boundaryPoint)) + { + newToolPos = boundaryPoint; + area = CalcCutArea(clip, toolPos, newToolPos, cleared); + double dist = sqrt(DistanceSqrd(toolPos, newToolPos)); + if (dist > NTOL) + areaPD = area / double(dist); // area per distance + else + { + areaPD = 0; + area = 0; + } + } + else + { + newToolPos = toolPos; + area = 0; + areaPD = 0; + } + } + + if (area > stepScaled * optimalCutAreaPD && areaPD > 2 * optimalCutAreaPD) + { // safety condition + over_cut_count++; + // #ifdef DEV_MODE + // cout<<"Break: over cut @" << point_index << "(" << double(toolPos.X)/scaleFactor << ","<< double(toolPos.Y)/scaleFactor << ")" + // << " iter:" << iteration << " @bound:" << reachedBoundary << endl; + // #endif + // ClearScreenFn(); + // DrawCircle(toolPos,toolRadiusScaled,0); + // DrawCircle(newToolPos,toolRadiusScaled,1); + // DrawPaths(cleared,22); + break; + } + + // update cleared paths when trend of distance from start point changes sign (starts to get closer, or start to get farther) + double distFromStart = sqrt(DistanceSqrd(toolPos, startPoint)); + bool distanceTrend = distFromStart > prevDistFromStart ? true : false; + + if (distanceTrend != prevDistTrend) + { + cleared.ExpandCleared(toClearPath); + toClearPath.clear(); + } + prevDistTrend = distanceTrend; + prevDistFromStart = distFromStart; + + if (area > 0) + { // cut is ok - record it + if (toClearPath.size() == 0) + toClearPath.push_back(toolPos); + toClearPath.push_back(newToolPos); + + cumulativeCutArea += area; + + // append to toolpaths + if (passToolPath.size() == 0) + { + // in outside entry first successful cut defines the helix center and start point + if (output.AdaptivePaths.size() == 0 && outsideEntry) + { + entryPoint = toolPos; + output.HelixCenterPoint.first = double(entryPoint.X) / scaleFactor; + output.HelixCenterPoint.second = double(entryPoint.Y) / scaleFactor; + output.StartPoint = DPoint(double(entryPoint.X) / scaleFactor, double(entryPoint.Y) / scaleFactor); + } + passToolPath.push_back(toolPos); + } + passToolPath.push_back(newToolPos); + perf_total_len += stepScaled; + toolPos = newToolPos; + + // append to progress info paths + if (progressPaths.size() == 0) + progressPaths.push_back(TPath()); + progressPaths.back().second.push_back(DPoint(double(newToolPos.X) / scaleFactor, double(newToolPos.Y) / scaleFactor)); + + // apend gyro + gyro.push_back(newToolDir); + gyro.erase(gyro.begin()); + CheckReportProgress(progressPaths); + } + else + { +#ifdef DEV_MODE + // if(point_index==0) { + // engage_no_cut_count++; + // cout<<"Break:no cut #" << engage_no_cut_count << ", bad engage, pass:" << pass << " over_cut_count:" << over_cut_count << endl; + // } +#endif + //cerr<<"Break: no cut @" << point_index << endl; + break; + } + if (reachedBoundary) + break; + } /* end of points loop*/ + + if (toClearPath.size() > 0) + { + cleared.ExpandCleared(toClearPath); + toClearPath.clear(); + } + if (cumulativeCutArea > tolerance * MIN_CUT_AREA_FACTOR * stepScaled * stepOverFactor * referenceCutArea) + { + Path cleaned; + CleanPath(passToolPath, cleaned, CLEAN_PATH_TOLERANCE); + total_output_points += long(cleaned.size()); + //AddPathsToProgress(progressPaths,clearedBeforePass.GetCleared(),MotionType::mtLinkClear); + + AppendToolPath(progressPaths, output, cleaned, clearedBeforePass, cleared, toolBoundPaths); + CheckReportProgress(progressPaths); + //std::this_thread::sleep_for(std::chrono::seconds(3)); + bad_engage_count = 0; + engage.ResetPasses(); + //firstEngagePoint=true; + } + else + { + bad_engage_count++; + } + /*****NEXT ENGAGE POINT******/ + if (firstEngagePoint) + { + engage.moveToClosestPoint(newToolPos, stepScaled + 1); + firstEngagePoint = false; + } + else + { + //double moveDistance = ENGAGE_SCAN_DISTANCE_FACTOR * RESOLUTION_FACTOR * 32 * stepOverFactor; + double moveDistance = ENGAGE_SCAN_DISTANCE_FACTOR * toolRadiusScaled * stepOverFactor * refinement_factor; + + if (!engage.nextEngagePoint(this, cleared, moveDistance, + ENGAGE_AREA_THR_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR, + 4 * referenceCutArea * stepOverFactor)) + { + break; + } + } + toolPos = engage.getCurrentPoint(); + toolDir = engage.getCurrentDir(); + } + /**********************************/ + /* FINISHING PASS */ + /**********************************/ + + Paths finishingPaths; + clipof.Clear(); + clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(finishingPaths, -toolRadiusScaled); + + clipof.Clear(); + clipof.AddPaths(finishingPaths, JoinType::jtRound, EndType::etClosedPolygon); + clipof.Execute(toolBoundPaths, -1); + + IntPoint lastPoint = toolPos; + Path finShiftedPath; + + bool allCutsAllowed = true; + while (!stopProcessing && PopPathWithClosestPoint(finishingPaths, lastPoint, finShiftedPath)) + { + if (finShiftedPath.empty()) + continue; + // skip finishing passes outside the stock boundary - make no sense to cut where is no material + bool allPointsOutside = true; + IntPoint p1 = finShiftedPath.front(); + for (const auto &pt : finShiftedPath) + { + + // midpoint + if (IsPointWithinCutRegion(stockInputPaths, IntPoint((p1.X + pt.X) / 2, (p1.Y + pt.Y) / 2))) + { + allPointsOutside = false; + break; + } + //current point + if (IsPointWithinCutRegion(stockInputPaths, pt)) + { + allPointsOutside = false; + break; + } + + p1 = pt; + } + if (allPointsOutside) + continue; + + progressPaths.push_back(TPath()); + // show in progress cb + for (auto &pt : finShiftedPath) + { + progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor)); + } + + if (!finShiftedPath.empty()) + finShiftedPath << finShiftedPath.front(); // make sure its closed + + Path finCleaned; + CleanPath(finShiftedPath, finCleaned, FINISHING_CLEAN_PATH_TOLERANCE); + + //safety check for finishing paths - check the area of finishing cut + for (size_t i = 1; i < finCleaned.size(); i++) + { + if (!IsAllowedToCutTrough(finCleaned.at(i - 1), finCleaned.at(i), cleared, toolBoundPaths, 2.0, true)) + { + //cout << "i:" << i << "/" << finCleaned.size() << " (" << finCleaned.at(i-1).X/scaleFactor << "," << finCleaned.at(i-1).Y/scaleFactor << ") - (" << finCleaned.at(i).X/scaleFactor << "," << finCleaned.at(i).Y/scaleFactor << ")" << endl; + allCutsAllowed = false; + } + } + + AppendToolPath(progressPaths, output, finCleaned, cleared, cleared, toolBoundPaths); + + cleared.ExpandCleared(finCleaned); + + if (!finCleaned.empty()) + { + lastPoint.X = finCleaned.back().X; + lastPoint.Y = finCleaned.back().Y; + } + } + + Path returnPath; + returnPath << lastPoint; + returnPath << entryPoint; + output.ReturnMotionType = IsClearPath(returnPath, cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; + +// dump performance results +#ifdef DEV_MODE + Perf_ProcessPolyNode.Stop(); + Perf_ProcessPolyNode.DumpResults(); + Perf_PointIterations.DumpResults(); + Perf_CalcCutAreaCirc.DumpResults(); + Perf_CalcCutAreaClip.DumpResults(); + Perf_NextEngagePoint.DumpResults(); + Perf_ExpandCleared.DumpResults(); + Perf_DistanceToBoundary.DumpResults(); + Perf_AppendToolPath.DumpResults(); + Perf_IsAllowedToCutTrough.DumpResults(); + Perf_IsClearPath.DumpResults(); +#endif + CheckReportProgress(progressPaths, true); +#ifdef DEV_MODE + double duration = ((double)(clock() - start_clock)) / CLOCKS_PER_SEC; + cout << "PolyNode perf:" << perf_total_len / double(scaleFactor) / duration << " mm/sec" + << " processed_points:" << total_points + << " output_points:" << total_output_points + << " total_iterations:" << total_iterations + << " iter_per_point:" << (double(total_iterations) / ((double(total_points) + 0.001))) + << " total_exceeded:" << total_exceeded << " (" << 100 * double(total_exceeded) / double(total_points) << "%)" + << " linking moves:" << unclearLinkingMoveCount + << endl; +#endif + + // make sure invalid paths are not used + if (!allCutsAllowed) + { + cerr << "Warning: some cuts may be above optimal step-over. Please check the output." << endl + << "Hint: try to modify accuracy and/or step-over." << endl; + } + + results.push_back(output); +} + +} // namespace AdaptivePath \ No newline at end of file diff --git a/src/Mod/Path/libarea/Adaptive.hpp b/src/Mod/Path/libarea/Adaptive.hpp index e454140c4d..90e94370c9 100644 --- a/src/Mod/Path/libarea/Adaptive.hpp +++ b/src/Mod/Path/libarea/Adaptive.hpp @@ -19,14 +19,11 @@ * * ***************************************************************************/ - - #include "clipper.hpp" #include #include #include - #ifndef ADAPTIVE_HPP #define ADAPTIVE_HPP @@ -42,118 +39,138 @@ #define M_PI 3.141592653589793238 #endif - //#define DEV_MODE -#define NTOL 1.0e-7 // numeric tolerance +#define NTOL 1.0e-7 // numeric tolerance -namespace AdaptivePath { - using namespace ClipperLib; +namespace AdaptivePath +{ +using namespace ClipperLib; - enum MotionType { mtCutting = 0, mtLinkClear = 1, mtLinkNotClear = 2, mtLinkClearAtPrevPass = 3 }; +enum MotionType +{ + mtCutting = 0, + mtLinkClear = 1, + mtLinkNotClear = 2, + mtLinkClearAtPrevPass = 3 +}; - enum OperationType { otClearingInside = 0, otClearingOutside = 1, otProfilingInside = 2, otProfilingOutside = 3 }; +enum OperationType +{ + otClearingInside = 0, + otClearingOutside = 1, + otProfilingInside = 2, + otProfilingOutside = 3 +}; - typedef std::pair DPoint; - typedef std::vector DPath; - typedef std::vector DPaths; - typedef std::pair TPath; // first parameter is MotionType, must use int due to problem with serialization to JSON in python +typedef std::pair DPoint; +typedef std::vector DPath; +typedef std::vector DPaths; +typedef std::pair TPath; // first parameter is MotionType, must use int due to problem with serialization to JSON in python - // struct TPath { #this does not work correctly with pybind, changed to pair - // DPath Points; - // MotionType MType; - // }; +class ClearedArea; - typedef std::vector TPaths; +// struct TPath { #this does not work correctly with pybind, changed to pair +// DPath Points; +// MotionType MType; +// }; - struct AdaptiveOutput { - DPoint HelixCenterPoint; - DPoint StartPoint; - TPaths AdaptivePaths; - int ReturnMotionType; // MotionType enum, problem with serialization if enum is used - }; +typedef std::vector TPaths; - // used to isolate state -> enable potential adding of multi-threaded processing of separate regions +struct AdaptiveOutput +{ + DPoint HelixCenterPoint; + DPoint StartPoint; + TPaths AdaptivePaths; + int ReturnMotionType; // MotionType enum, problem with serialization if enum is used +}; - class Adaptive2d { - public: - Adaptive2d(); - double toolDiameter=5; - double helixRampDiameter=0; - double stepOverFactor = 0.2; - double tolerance=0.1; - double stockToLeave=0; - bool forceInsideOut = true; - double keepToolDownDistRatio = 3.0; // keep tool down distance ratio - OperationType opType = OperationType::otClearingInside; +// used to isolate state -> enable potential adding of multi-threaded processing of separate regions - std::list Execute(const DPaths &stockPaths, const DPaths &paths, std::function progressCallbackFn); +class Adaptive2d +{ + public: + Adaptive2d(); + double toolDiameter = 5; + double helixRampDiameter = 0; + double stepOverFactor = 0.2; + double tolerance = 0.1; + double stockToLeave = 0; + bool forceInsideOut = true; + double keepToolDownDistRatio = 3.0; // keep tool down distance ratio + OperationType opType = OperationType::otClearingInside; - #ifdef DEV_MODE - /*for debugging*/ - std::function DrawCircleFn; - std::function DrawPathFn; - std::function ClearScreenFn; - #endif + std::list Execute(const DPaths &stockPaths, const DPaths &paths, std::function progressCallbackFn); - private: - std::list results; - Paths inputPaths; - Paths stockInputPaths; - int polyTreeNestingLimit=0; - double scaleFactor=100; - long toolRadiusScaled=10; - long finishPassOffsetScaled=0; - long helixRampRadiusScaled=0; - long bbox_size=0; - double referenceCutArea=0; - double optimalCutAreaPD=0; - //double minCutAreaPD=0; - bool stopProcessing=false; - long unclearLinkingMoveCount = 0; +#ifdef DEV_MODE + /*for debugging*/ + std::function DrawCircleFn; + std::function DrawPathFn; + std::function ClearScreenFn; +#endif - time_t lastProgressTime = 0; - - std::function * progressCallback=NULL; - Path toolGeometry; // tool geometry at coord 0,0, should not be modified + private: + std::list results; + Paths inputPaths; + Paths stockInputPaths; + int polyTreeNestingLimit = 0; + double scaleFactor = 100; + long toolRadiusScaled = 10; + long finishPassOffsetScaled = 0; + long helixRampRadiusScaled = 0; + long bbox_size = 0; + double referenceCutArea = 0; + double optimalCutAreaPD = 0; + //double minCutAreaPD=0; + bool stopProcessing = false; + long unclearLinkingMoveCount = 0; - void ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths); - bool FindEntryPoint(TPaths &progressPaths,const Paths & toolBoundPaths,const Paths &bound, Paths &cleared /*output*/, - IntPoint &entryPoint /*output*/, IntPoint & toolPos, DoublePoint & toolDir); - bool FindEntryPointOutside(TPaths &progressPaths,const Paths & toolBoundPaths,const Paths &bound, Paths &cleared /*output*/, - IntPoint &entryPoint /*output*/, IntPoint & toolPos, DoublePoint & toolDir); - double CalcCutArea(Clipper & clip,const IntPoint &toolPos, const IntPoint &newToolPos, const Paths &cleared_paths, bool preventConvetionalMode=true); - void AppendToolPath(TPaths &progressPaths,AdaptiveOutput & output,const Path & passToolPath,const Paths & cleared,const Paths & toolBoundPaths); - bool IsClearPath(const Path & path,const Paths & cleared, double safetyDistanceScaled=0); - bool IsAllowedToCutTrough(const IntPoint &p1,const IntPoint &p2,const Paths & cleared,const Paths & toolBoundPaths, double areaFactor=1.5, bool skipBoundsCheck=false); + time_t lastProgressTime = 0; - friend class EngagePoint; // for CalcCutArea + std::function *progressCallback = NULL; + Path toolGeometry; // tool geometry at coord 0,0, should not be modified - void CheckReportProgress(TPaths &progressPaths,bool force=false); - void AddPathsToProgress(TPaths &progressPaths,const Paths paths, MotionType mt=MotionType::mtCutting); - void AddPathToProgress(TPaths &progressPaths,const Path pth, MotionType mt=MotionType::mtCutting); - void ApplyStockToLeave(Paths &inputPaths); - private: // constants for fine tuning - const double RESOLUTION_FACTOR = 8.0; - const int MAX_ITERATIONS = 16; - const double AREA_ERROR_FACTOR = 0.05; /* how precise to match the cut area to optimal, reasonable value: 0.05 = 5%*/ - const size_t ANGLE_HISTORY_POINTS=3; // used for angle prediction - const int DIRECTION_SMOOTHING_BUFLEN=3; // gyro points - used for angle smoothing + void ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths); + bool FindEntryPoint(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &bound, ClearedArea &cleared /*output*/, + IntPoint &entryPoint /*output*/, IntPoint &toolPos, DoublePoint &toolDir); + bool FindEntryPointOutside(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &bound, ClearedArea &cleared /*output*/, + IntPoint &entryPoint /*output*/, IntPoint &toolPos, DoublePoint &toolDir); + double CalcCutArea(Clipper &clip, const IntPoint &toolPos, const IntPoint &newToolPos, ClearedArea &clearedArea, bool preventConvetionalMode = true); + void AppendToolPath(TPaths &progressPaths, AdaptiveOutput &output, const Path &passToolPath, ClearedArea &clearedAreaBefore, + ClearedArea &clearedAreaAfter, const Paths &toolBoundPaths); + bool IsClearPath(const Path &path, ClearedArea &clearedArea, double safetyDistanceScaled = 0); + bool IsAllowedToCutTrough(const IntPoint &p1, const IntPoint &p2, ClearedArea &clearedArea, const Paths &toolBoundPaths, double areaFactor = 1.5, bool skipBoundsCheck = false); + bool MakeLeadPath(bool leadIn, const IntPoint &startPoint, const DoublePoint &startDir, const IntPoint &beaconPoint, + ClearedArea &clearedArea, const Paths &toolBoundPaths, Path &output); - const double ENGAGE_AREA_THR_FACTOR=0.2; // influences minimal engage area (factor relation to optimal) - const double ENGAGE_SCAN_DISTANCE_FACTOR=0.1; // influences the engage scan/stepping distance + bool ResolveLinkPath(const IntPoint &startPoint, const IntPoint &endPoint, ClearedArea &clearedArea, Path &output); - const double CLEAN_PATH_TOLERANCE = 0.5; - const double FINISHING_CLEAN_PATH_TOLERANCE = 0.1; + friend class EngagePoint; // for CalcCutArea - // used for filtering out of insignificant cuts: - const double MIN_CUT_AREA_FACTOR = 0.1; // influences filtering of cuts that with cumulative area below threshold, reasonable value is between 0.1 and 1 + void CheckReportProgress(TPaths &progressPaths, bool force = false); + void AddPathsToProgress(TPaths &progressPaths, const Paths paths, MotionType mt = MotionType::mtCutting); + void AddPathToProgress(TPaths &progressPaths, const Path pth, MotionType mt = MotionType::mtCutting); + void ApplyStockToLeave(Paths &inputPaths); - const long PASSES_LIMIT = __LONG_MAX__; // limit used while debugging - const long POINTS_PER_PASS_LIMIT = __LONG_MAX__; // limit used while debugging - const time_t PROGRESS_TICKS = CLOCKS_PER_SEC/20; // progress report interval + private: // constants for fine tuning + const double RESOLUTION_FACTOR = 8.0; + const int MAX_ITERATIONS = 16; + const double AREA_ERROR_FACTOR = 0.05; /* how precise to match the cut area to optimal, reasonable value: 0.05 = 5%*/ + const size_t ANGLE_HISTORY_POINTS = 3; // used for angle prediction + const int DIRECTION_SMOOTHING_BUFLEN = 3; // gyro points - used for angle smoothing + const double ENGAGE_AREA_THR_FACTOR = 0.2; // influences minimal engage area (factor relation to optimal) + const double ENGAGE_SCAN_DISTANCE_FACTOR = 0.5; // influences the engage scan/stepping distance - }; -} + const double CLEAN_PATH_TOLERANCE = 0.5; + const double FINISHING_CLEAN_PATH_TOLERANCE = 0.1; + + // used for filtering out of insignificant cuts: + const double MIN_CUT_AREA_FACTOR = 0.1; // influences filtering of cuts that with cumulative area below threshold, reasonable value is between 0.1 and 1 + + const long PASSES_LIMIT = __LONG_MAX__; // limit used while debugging + const long POINTS_PER_PASS_LIMIT = __LONG_MAX__; // limit used while debugging + const time_t PROGRESS_TICKS = CLOCKS_PER_SEC / 10; // progress report interval +}; +} // namespace AdaptivePath #endif \ No newline at end of file