From 27da44cd8a7edc8d27284e34cda2357cef645288 Mon Sep 17 00:00:00 2001 From: kreso-t Date: Tue, 4 Sep 2018 00:10:43 +0200 Subject: [PATCH] Path: Adaptive - keep tool down feature - alfa --- src/Mod/Path/PathScripts/PathAdaptive.py | 43 +-- src/Mod/Path/libarea/Adaptive.cpp | 345 ++++++++++++++++++----- src/Mod/Path/libarea/Adaptive.hpp | 11 +- 3 files changed, 310 insertions(+), 89 deletions(-) diff --git a/src/Mod/Path/PathScripts/PathAdaptive.py b/src/Mod/Path/PathScripts/PathAdaptive.py index d4a710004c..22cbcf053f 100644 --- a/src/Mod/Path/PathScripts/PathAdaptive.py +++ b/src/Mod/Path/PathScripts/PathAdaptive.py @@ -147,7 +147,7 @@ def GenerateGCode(op,obj,adaptiveResults, helixDiameter): lx=adaptiveResults[0]["HelixCenterPoint"][0] ly=adaptiveResults[0]["HelixCenterPoint"][1] - + lz=passStartDepth step=0 while passStartDepth>obj.FinalDepth.Value and step<1000: step=step+1 @@ -198,38 +198,45 @@ def GenerateGCode(op,obj,adaptiveResults, helixDiameter): op.commandlist.append(Path.Command("G1", { "X":region["StartPoint"][0], "Y": region["StartPoint"][1], "Z": passEndDepth,"F": op.vertFeed})) + lz=passEndDepth + z=obj.ClearanceHeight.Value op.commandlist.append(Path.Command("(adaptive - depth: %f)"%passEndDepth)) #add adaptive paths for pth in region["AdaptivePaths"]: motionType = pth[0] #[0] contains motion type for pt in pth[1]: #[1] contains list of points x=pt[0] - y =pt[1] + y=pt[1] dist=math.sqrt((x-lx)*(x-lx) + (y-ly)*(y-ly)) if motionType == area.AdaptiveMotionType.Cutting: - op.commandlist.append(Path.Command("G1", { "X": x, "Y":y, "Z":passEndDepth, "F": op.horizFeed})) + z=passEndDepth + if z!=lz: + op.commandlist.append(Path.Command("G1", { "Z":z,"F": op.vertFeed})) + op.commandlist.append(Path.Command("G1", { "X": x, "Y":y, "F": op.horizFeed})) elif motionType == area.AdaptiveMotionType.LinkClear: - if dist > minLiftDistance: - if lx!=x or ly!=y: - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":passEndDepth+stepUp})) - op.commandlist.append(Path.Command("G0", { "X": x, "Y":y, "Z":passEndDepth+stepUp})) + z=passEndDepth+stepUp + if z!=lz: + op.commandlist.append(Path.Command("G0", { "Z":z})) + op.commandlist.append(Path.Command("G0", { "X": x, "Y":y})) elif motionType == area.AdaptiveMotionType.LinkNotClear: - if lx!=x or ly!=y: - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":obj.ClearanceHeight.Value})) - op.commandlist.append(Path.Command("G0", { "X": x, "Y":y, "Z":obj.ClearanceHeight.Value})) - elif motionType == area.AdaptiveMotionType.LinkClearAtPrevPass: - if lx!=x or ly!=y: - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":passStartDepth+stepUp})) - op.commandlist.append(Path.Command("G0", { "X": x, "Y":y, "Z":passStartDepth+stepUp})) + z=obj.ClearanceHeight.Value + if z!=lz: + op.commandlist.append(Path.Command("G0", { "Z":z})) + op.commandlist.append(Path.Command("G0", { "X": x, "Y":y})) + # elif motionType == area.AdaptiveMotionType.LinkClearAtPrevPass: + # if lx!=x or ly!=y: + # op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":passStartDepth+stepUp})) + # op.commandlist.append(Path.Command("G0", { "X": x, "Y":y, "Z":passStartDepth+stepUp})) lx=x ly=y + lz=z #return to safe height in this Z pass - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":obj.ClearanceHeight.Value})) + op.commandlist.append(Path.Command("G0", { "Z":obj.ClearanceHeight.Value})) passStartDepth=passEndDepth #return to safe height in this Z pass - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":obj.ClearanceHeight.Value})) + op.commandlist.append(Path.Command("G0", { "Z":obj.ClearanceHeight.Value})) - op.commandlist.append(Path.Command("G0", { "X": lx, "Y":ly, "Z":obj.ClearanceHeight.Value})) + op.commandlist.append(Path.Command("G0", { "Z":obj.ClearanceHeight.Value})) def Execute(op,obj): global sceneGraph @@ -414,7 +421,7 @@ class PathAdaptive(PathOp.ObjectOp): obj.OperationType = "Clearing" obj.Tolerance = 0.1 obj.StepOver = 20 - obj.LiftDistance=1.0 + obj.LiftDistance=0 # obj.ProcessHoles = True obj.ForceInsideOut = True obj.Stopped = False diff --git a/src/Mod/Path/libarea/Adaptive.cpp b/src/Mod/Path/libarea/Adaptive.cpp index 93fe314d5c..d8abe08edb 100644 --- a/src/Mod/Path/libarea/Adaptive.cpp +++ b/src/Mod/Path/libarea/Adaptive.cpp @@ -91,7 +91,8 @@ namespace AdaptivePath { output.Y/=magnitude; } - double DistancePointToLineSegSquared(const IntPoint& p1, const IntPoint& p2,const IntPoint& pt, IntPoint &closestPoint,bool clamp=true) { + 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); @@ -99,6 +100,7 @@ namespace AdaptivePath { 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; @@ -108,8 +110,9 @@ namespace AdaptivePath { else if(parameter>lsegLenSqr) parameter=lsegLenSqr; } // point on line at parameter - closestPoint.X = long(p1.X + parameter*D21X/lsegLenSqr); - closestPoint.Y = long(p1.Y + parameter*D21Y/lsegLenSqr); + 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); @@ -127,7 +130,8 @@ namespace AdaptivePath { } else { if(outp.size()>2) { IntPoint clp; // to hold closest point - double distSqrd = DistancePointToLineSegSquared(outp[outp.size()-2],outp[outp.size()-1],pt,clp,false); + double ptPar; + double distSqrd = DistancePointToLineSegSquared(outp[outp.size()-2],outp[outp.size()-1],pt,clp,ptPar,false); if(sqrt(distSqrd)size(); // iterate through segments for(Path::size_type j=0;jat(j>0 ? j-1 : size-1),path->at(j),pt,clp); + double ptPar; + double distSq=DistancePointToLineSegSquared(path->at(j>0 ? j-1 : size-1),path->at(j),pt,clp,ptPar); if(distSq areas[size-1] + NTOL) { // first point or largest area point @@ -559,12 +570,107 @@ namespace AdaptivePath { return sqrt(DistanceSqrd(*p1,*p2)); } - - - - - }; + + + // 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 + const 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) return false; // too far + + + // result in reverse direction + Path rev_result; + double rev_len=0; + rev_result << clp1; + long minIndex = long(clpSegmentIndex2); + if(minIndex >= long(clpSegmentIndex1-1)) minIndex -= closestPath.size(); + for(long i=clpSegmentIndex1-1;i>=minIndex;i--) { + long index=i; + if(index<0) index+= closestPath.size(); + //if(index>=closestPath.size()) cerr << "index out of range:" << index << " size:" << closestPath.size() << " i:" << i << " max:" << maxIndex << " clpSegmentIndex2:" << clpSegmentIndex2 << endl; + double dist=sqrt(DistanceSqrd(rev_result.back(),closestPath[index])); + if(dist>NTOL) { + rev_result << closestPath[index]; + rev_len+=dist; + } + } + double dist=sqrt(DistanceSqrd(rev_result.back(),clp2)); + if(dist>NTOL) { + rev_result << clp2; + rev_len+=dist; + } + + // result in forward direction + Path fwd_result; + double fwd_len=0; + fwd_result << clp1; + size_t maxIndex = clpSegmentIndex2; + if(maxIndex <= clpSegmentIndex1) maxIndex = closestPath.size() + clpSegmentIndex2-1; + for(size_t i=clpSegmentIndex1;i=closestPath.size()) index-= closestPath.size(); + //if(index>=closestPath.size()) cerr << "index out of range:" << index << " size:" << closestPath.size() << " i:" << i << " max:" << maxIndex << " clpSegmentIndex2:" << clpSegmentIndex2 << endl; + double dist=sqrt(DistanceSqrd(fwd_result.back(),closestPath[index])); + if(dist>NTOL) { + fwd_result << closestPath[index]; + fwd_len+=dist; + } + } + dist=sqrt(DistanceSqrd(rev_result.back(),clp2)); + if(dist>NTOL) { + fwd_result << clp2; + fwd_len+=dist; + } + res = rev_len < fwd_len ? rev_result: fwd_result; // take shortest + return res.size()>1; + } + + void ShiftPathToStartWithClosestPoint(const Path & path,IntPoint p1, Path & result) { + Paths paths; + paths.push_back(path); + IntPoint clp; + size_t clpPathIndex; + size_t clpSegmentIndex1; + double clpParameter1; + // find closest point + double distSqrd=DistancePointToPathsSqrd(paths,p1,clp,clpPathIndex,clpSegmentIndex1,clpParameter1); + result.clear(); + // make new path starting with that point + for(size_t i=0;i=path.size()) index-=path.size(); + result.push_back(path[index]); + } + } + + double PathLength(const Path & path) { + double len=0; + if(path.size()<2) return len; + for(size_t i=1;i1) break; // we will use poly clipping alg. if there are more intersecting paths @@ -1115,12 +1222,9 @@ namespace AdaptivePath { /** * returns true if line from lastPoint to nextPoint is clear from obstacles */ - bool Adaptive2d::CheckCollision(const IntPoint &lastPoint,const IntPoint &nextPoint,const Paths & cleared) { + bool Adaptive2d::IsClearPath(const Path &tp,const Paths & cleared) { Clipper clip; ClipperOffset clipof; - Path tp; - tp <optimalCutAreaPD) { // if we are cutting above optimal -> not clear link - mt=MotionType::mtLinkNotClear; - // cout<<"linking - overcut" << endl; - break; - } + + // try to cut through + bool linkFound = true; + double linkDistance = sqrt(DistanceSqrd(lastPoint,nextPoint)); + if(linkDistance<4*toolRadiusScaled) { + double stepSize=2*RESOLUTION_FACTOR; + Clipper clip; + IntPoint inters; // to hold intersection point + if( + !IsPointWithinCutRegion(toolBoundPaths,lastPoint) + || + !IsPointWithinCutRegion(toolBoundPaths,nextPoint) + || + IntersectionPoint(toolBoundPaths,lastPoint, nextPoint,inters)) { + // if intersect with boundary - its not clear to cut + linkFound=false; + } else for(double d=stepSize;doptimalCutAreaPD) { // if we are cutting above optimal -> not clear link + linkFound=false; + //cout<<"linking - overcut" << endl; + break; } - //if(mt==MotionType::mtCutting) cout<<"cutting link"< 5*linkDistance) && (linkDistance>4*toolRadiusScaled) ; + if(keepDownLinkExists) { + lastInterimPoint=keepToolDownLinkPath.front(); + nextInterimPoint=keepToolDownLinkPath.back(); + Path tp; + tp << lastPoint; + tp << lastInterimPoint; + tp << nextInterimPoint; + tp << nextPoint; + bool directLinkInterimLinkClear = IsClearPath(tp,cleared); + + if(directLinkInterimLinkClear) { // shouldn't apply keep down link + // 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 direct link over interim points not clear + tp.clear(); + //tp << lastPoint; + for(auto & p : keepToolDownLinkPath) tp << p; + //tp << nextPoint; + if(IsClearPath(tp,cleared)) { // clear + //AddPathToProgress(progressPaths,keepToolDownLinkPath); + // 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; + } + } + } + } // first we find the last point + if(!linkFound) { // not clear - check direct link with no interim points - either clear or we neet to raise the tool + //cerr << "keepToolDownLinkPath NOT CLEAR" << endl; + Path tp; + tp << lastPoint; + tp << nextPoint; + MotionType mt = IsClearPath(tp,cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; + TPath linkPath; + linkPath.first = mt; + linkPath.second.push_back(DPoint(double(lastPoint.X)/scaleFactor,double(lastPoint.Y)/scaleFactor)); + linkPath.second.push_back(DPoint(double(nextPoint.X)/scaleFactor,double(nextPoint.Y)/scaleFactor)); + output.AdaptivePaths.push_back(linkPath); + } } TPath cutPath; cutPath.first =MotionType::mtCutting; @@ -1233,6 +1423,15 @@ namespace AdaptivePath { } } + void Adaptive2d::AddPathToProgress(TPaths &progressPaths,const Path pth) { + if(pth.size()>0) { + progressPaths.push_back(TPath()); + 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) { //cout << " Adaptive2d::ProcessPolyNode" << endl; Perf_ProcessPolyNode.Start(); @@ -1342,6 +1541,10 @@ namespace AdaptivePath { // init gyro gyro.clear(); for(int i=0;i0) { - lastPoint.X = pth[pth.size()-1].X; - lastPoint.Y = pth[pth.size()-1].Y; + CleanPath(finShiftedPath,cleaned,FINISHING_CLEAN_PATH_TOLERANCE); + AppendToolPath(progressPaths,output,cleaned,cleared,toolBoundPaths,true); + if(finShiftedPath.size()>0) { + lastPoint.X = finShiftedPath.back().X; + lastPoint.Y = finShiftedPath.back().Y; } } - output.ReturnMotionType = CheckCollision(lastPoint, entryPoint,cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; + Path returnPath; + returnPath << lastPoint; + returnPath << entryPoint; + output.ReturnMotionType = IsClearPath(returnPath,cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear; // dump performance results #ifdef DEV_MODE diff --git a/src/Mod/Path/libarea/Adaptive.hpp b/src/Mod/Path/libarea/Adaptive.hpp index fef1dc33ca..6067fb2034 100644 --- a/src/Mod/Path/libarea/Adaptive.hpp +++ b/src/Mod/Path/libarea/Adaptive.hpp @@ -27,7 +27,7 @@ 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 }; @@ -61,6 +61,7 @@ namespace AdaptivePath { double tolerance=0.1; double stockToLeave=0; bool forceInsideOut = true; + bool keepToolDown = true; OperationType opType = OperationType::otClearingInside; std::list Execute(const DPaths &stockPaths, const DPaths &paths, std::function progressCallbackFn); @@ -98,12 +99,14 @@ namespace AdaptivePath { 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); - void AppendToolPath(AdaptiveOutput & output,const Path & passToolPath,const Paths & cleared,const Paths & toolBoundPaths, bool close=false); - bool CheckCollision(const IntPoint &lastPoint,const IntPoint &nextPoint,const Paths & cleared); + void AppendToolPath(TPaths &progressPaths,AdaptiveOutput & output,const Path & passToolPath,const Paths & cleared,const Paths & toolBoundPaths, bool close=false); + bool IsClearPath(const Path & path,const Paths & cleared); friend class EngagePoint; // for CalcCutArea void CheckReportProgress(TPaths &progressPaths,bool force=false); - void AddPathsToProgress(TPaths &progressPaths,Paths paths); + void AddPathsToProgress(TPaths &progressPaths,const Paths paths); + void AddPathToProgress(TPaths &progressPaths,const Path pth); + private: // constants for fine tuning const bool preventConvetionalMode = true; const double RESOLUTION_FACTOR = 8.0;