2195 lines
78 KiB
C++
2195 lines
78 KiB
C++
/**************************************************************************
|
|
* Copyright (c) Kresimir Tusek (kresimir.tusek@gmail.com) 2018 *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#include "Adaptive.hpp"
|
|
#include <iostream>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <ctime>
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <thread>
|
|
|
|
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)
|
|
/*********************************************
|
|
* 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 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) {
|
|
pt2.X = long( pt1.X + new_length * Dx/l);
|
|
pt2.Y = long(pt1.Y + new_length * dy/l);
|
|
return true;
|
|
}
|
|
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;
|
|
}
|
|
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;i<paths.size();i++) {
|
|
if(paths[i].size()>0) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
inline double averageDV(const vector<double> & 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);
|
|
}
|
|
|
|
// 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<path.size();i++) {
|
|
len+=sqrt(DistanceSqrd(path[i-1],path[i]));
|
|
}
|
|
return len;
|
|
}
|
|
|
|
inline double PointSideOfLine(const IntPoint& p1, const IntPoint& p2,const IntPoint& pt) {
|
|
return double((pt.X - p1.X)*(p2.Y-p1.Y) - (pt.Y - p2.Y)*(p2.X-p1.X));
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
/*********************************************
|
|
* Utils
|
|
***********************************************/
|
|
|
|
|
|
void AverageDirection(const vector<DoublePoint> &unityVectors, DoublePoint& output) {
|
|
int size=unityVectors.size();
|
|
output.X =0;
|
|
output.Y=0;
|
|
// sum vectors
|
|
for(int 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();
|
|
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)<tolerance) {
|
|
outp.pop_back();
|
|
outp.push_back(pt);
|
|
} else {
|
|
outp.push_back(pt);
|
|
}
|
|
} else if(sqrt(DistanceSqrd(outp[outp.size()-1],pt))<tolerance) {
|
|
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;i<paths.size();i++) {
|
|
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<DoublePoint,DoublePoint> & 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<DoublePoint> & result, bool clamp=true)
|
|
{
|
|
// 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; 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) {
|
|
// todo: bounds check for perfomance
|
|
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 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;j<size;j++) {
|
|
// todo: box check for perfomance
|
|
const IntPoint * pp1 = &path->at(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)<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 -= closestPath.size();
|
|
for(long i=clpSegmentIndex1-1;i>=minIndex;i--) {
|
|
long index=i;
|
|
if(index<0) index+= 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<maxIndex;i++) {
|
|
size_t index=i;
|
|
if(index>=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 neccesarily 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<paths.size();pathIndex++) {
|
|
Path & path = paths.at(pathIndex);
|
|
for(size_t i=0;i<path.size();i++) {
|
|
double dist=DistanceSqrd(p1,path.at(i));
|
|
if(dist<minDistSqrd) {
|
|
minDistSqrd=dist;
|
|
closestPathIndex=pathIndex;
|
|
closestPointIndex=i;
|
|
}
|
|
}
|
|
}
|
|
|
|
result.clear();
|
|
// make new path starting with that point
|
|
Path & closestPath = paths.at(closestPathIndex);
|
|
for(size_t i=0;i<closestPath.size();i++) {
|
|
long index=long(closestPointIndex)+i;
|
|
if(index>=long(closestPath.size())) index-=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(!pointExists) {
|
|
all_points_exists=false;
|
|
break;
|
|
}
|
|
}
|
|
if(all_points_exists) {
|
|
duplicate=true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
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()+i);
|
|
anyMatch=true;
|
|
break;
|
|
} else if(DistanceSqrd(n.back(),joined.back())<SAME_POINT_TOL_SQRD_SCALED) {
|
|
ReversePath(n);
|
|
for(auto pt:n) joined.push_back(pt);
|
|
input.erase(input.begin()+i);
|
|
anyMatch=true;
|
|
break;
|
|
} else if(DistanceSqrd(n.front(),joined.front())<SAME_POINT_TOL_SQRD_SCALED) {
|
|
for(auto pt:n) joined.insert(joined.begin(),pt);
|
|
input.erase(input.begin()+i);
|
|
anyMatch=true;
|
|
break;
|
|
} else if(DistanceSqrd(n.back(),joined.front())<SAME_POINT_TOL_SQRD_SCALED) {
|
|
ReversePath(n);
|
|
for(auto pt:n) joined.insert(joined.begin(),pt);
|
|
input.erase(input.begin()+i);
|
|
anyMatch=true;
|
|
break;
|
|
}
|
|
}
|
|
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;
|
|
}
|
|
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_CalcCutArea("CalcCutArea");
|
|
PerfCounter Perf_NextEngagePoint("NextEngagePoint");
|
|
PerfCounter Perf_PointIterations("PointIterations");
|
|
PerfCounter Perf_ExpandCleared("ExpandCleared");
|
|
PerfCounter Perf_DistanceToBoundary("DistanceToBoundary");
|
|
|
|
/*****************************************
|
|
* Linear Interpolation - area vs angle
|
|
* ***************************************/
|
|
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<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);
|
|
}
|
|
}
|
|
}
|
|
|
|
double interpolateAngle(double targetArea) {
|
|
std::size_t size = areas.size();
|
|
if(size<2 || targetArea>areas[size-1]) return MIN_ANGLE; //max engage angle - convinient value to initially measure cut area
|
|
if(targetArea<areas[0]) return MAX_ANGLE; // min engage angle
|
|
|
|
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;
|
|
}
|
|
}
|
|
return MIN_ANGLE;
|
|
}
|
|
|
|
double clampAngle(double angle) {
|
|
if(angle<MIN_ANGLE) return MIN_ANGLE;
|
|
if(angle>MAX_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<double> angles;
|
|
vector<double> 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, const Paths & cleared, 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; // nothin 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 && 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, const Paths &cleared_paths, bool preventConvetional) {
|
|
Perf_CalcCutArea.Start();
|
|
|
|
double dist = DistanceSqrd(c1,c2);
|
|
if(dist<NTOL) return 0;
|
|
|
|
/// new alg
|
|
double rsqrd=toolRadiusScaled*toolRadiusScaled;
|
|
double area =0;
|
|
Paths interPaths;
|
|
IntPoint clp; // to hold closest point
|
|
vector<DoublePoint> 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;i<size;i++) {
|
|
if(DistanceSqrd(path[curPtIndex],c2)>rsqrd) {
|
|
found = true;
|
|
break;
|
|
}
|
|
curPtIndex++; if(curPtIndex>=size) curPtIndex=0;
|
|
}
|
|
if(!found) continue; // try anohter 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];
|
|
|
|
for(size_t i=0;i<size;i++) {
|
|
curPtIndex++; if(curPtIndex>=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;
|
|
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
|
|
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;j<ipc2_size;j++)
|
|
interPathLen+=sqrt(DistanceSqrd(interPath->at(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<minFi) maxFi += 2*M_PI;
|
|
|
|
if(preventConvetional && interPathLen>=RESOLUTION_FACTOR) {
|
|
// detect conventional mode cut - we want only climb mode
|
|
if(!IsPointWithinCutRegion(cleared_paths,c2)) {
|
|
if(PointSideOfLine(fpc2,lpc2,c2)<0) {
|
|
IntPoint midPoint(long(c2.X + toolRadiusScaled*cos(0.5*(maxFi+minFi))),long(c2.Y + toolRadiusScaled*sin(0.5*(maxFi+minFi))));
|
|
if(PointSideOfLine(fpc2,lpc2,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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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)));
|
|
|
|
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);
|
|
}
|
|
}
|
|
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);
|
|
|
|
} else if(interPaths.size()>1)
|
|
{
|
|
// 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 beween 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(cleared_paths,PolyType::ptClip, true);
|
|
Paths cutAreaPoly;
|
|
clip.Execute(ClipType::ctDifference, cutAreaPoly);
|
|
|
|
// calculate resulting area
|
|
area=0;
|
|
for(Path &path : cutAreaPoly) {
|
|
area +=fabs(Area(path));
|
|
}
|
|
}
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
/****************************************
|
|
// Adaptive2d - Execute
|
|
*****************************************/
|
|
std::list<AdaptiveOutput> Adaptive2d::Execute(const DPaths &stockPaths, const DPaths &paths, std::function<bool(TPaths)> progressCallbackFn) {
|
|
//**********************************
|
|
// Initializations
|
|
// **********************************
|
|
|
|
// a 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;
|
|
|
|
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;
|
|
|
|
// 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 referece 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<double,double> 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<double,double> 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();
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
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;i<profilePaths.size();i++) {
|
|
double efOffset= i==0 ? offset : -offset;
|
|
clipof.Clear();
|
|
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();
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//cout<<" Adaptive2d::Execute finish" << endl;
|
|
return results;
|
|
}
|
|
|
|
bool Adaptive2d::FindEntryPoint(TPaths &progressPaths, const Paths & toolBoundPaths,const Paths &boundPaths,
|
|
Paths &cleared /*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 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(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"<<endl;
|
|
found=false;
|
|
}
|
|
}
|
|
|
|
if(!found) { // break simetry and try again
|
|
clip.Clear();
|
|
clip.AddPaths(checkPaths,PolyType::ptSubject, true);
|
|
auto bounds=clip.GetBounds();
|
|
clip.Clear();
|
|
Path rect;
|
|
rect << IntPoint(bounds.left,bounds.bottom);
|
|
rect << IntPoint(bounds.left, (bounds.top+bounds.bottom)/2);
|
|
rect << IntPoint((bounds.left + bounds.right)/2, (bounds.top+bounds.bottom)/2);
|
|
rect << IntPoint((bounds.left + bounds.right)/2, bounds.bottom);
|
|
clip.AddPath(rect,PolyType::ptSubject, true);
|
|
clip.AddPaths(checkPaths,PolyType::ptClip,true);
|
|
clip.Execute(ClipType::ctIntersection,checkPaths);
|
|
}
|
|
if(found) break;
|
|
}
|
|
//DrawCircle(entryPoint,scaleFactor,10);
|
|
if(!found) cerr<<"Start point not found!"<<endl;
|
|
if(found) {
|
|
// visualize/progress for helix
|
|
clipof.Clear();
|
|
Path hp;
|
|
hp << entryPoint;
|
|
clipof.AddPath(hp,JoinType::jtRound,EndType::etOpenRound);
|
|
Paths hps;
|
|
clipof.Execute(hps,helixRampRadiusScaled);
|
|
AddPathsToProgress(progressPaths,hps);
|
|
|
|
|
|
toolPos = IntPoint(entryPoint.X,entryPoint.Y - helixRampRadiusScaled);
|
|
toolDir = DoublePoint(1.0,0.0);
|
|
|
|
}
|
|
return found;
|
|
}
|
|
bool Adaptive2d::FindEntryPointOutside(TPaths &progressPaths, const Paths & toolBoundPaths,const Paths &boundPaths,
|
|
Paths &cleared /*output-initial cleard area by helix*/,
|
|
IntPoint &entryPoint /*output*/,
|
|
IntPoint & toolPos, DoublePoint & toolDir) {
|
|
|
|
UNUSED(progressPaths); // to silence compiler warning
|
|
UNUSED(boundPaths); // to silence compiler warning
|
|
|
|
Clipper clip;
|
|
ClipperOffset clipof;
|
|
// check if boundary shape to cut is outside the stock
|
|
// clip.AddPaths(boundPaths,PolyType::ptSubject, true);
|
|
// clip.AddPaths(stockInputPaths,PolyType::ptClip, true);
|
|
// Paths outsidePaths;
|
|
// clip.Execute(ClipType::ctDifference,outsidePaths);
|
|
for(const auto & pth : toolBoundPaths) {
|
|
for(size_t i=0;i<pth.size();i++) {
|
|
IntPoint checkPoint = pth[i];
|
|
IntPoint lastPoint = i>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(cleared,1000*toolRadiusScaled);
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
//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::IsAllowedToCutTrough(const IntPoint &p1,const IntPoint &p2,const Paths & cleared, const Paths & toolBoundPaths, double areaFactor, bool skipBoundsCheck) {
|
|
double stepSize=2*RESOLUTION_FACTOR;
|
|
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<distance;d+=stepSize) {
|
|
IntPoint toolPos1(long(p1.X + double(p2.X-p1.X)*(d-stepSize)/distance),long(p1.Y + double(p2.Y-p1.Y)*(d-stepSize)/distance));
|
|
IntPoint toolPos2(long(p1.X + double(p2.X-p1.X)*d/distance),long(p1.Y + double(p2.Y-p1.Y)*d/distance));
|
|
double area = CalcCutArea(clip,toolPos1,toolPos2, cleared,false);
|
|
// if we are cutting above optimal -> not clear to cut
|
|
if(area>areaFactor*stepSize*optimalCutAreaPD) return false;
|
|
|
|
//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
|
|
) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void Adaptive2d::AppendToolPath(TPaths &progressPaths,AdaptiveOutput & output,const Path & passToolPath,const Paths & cleared, const Paths & toolBoundPaths) {
|
|
if(passToolPath.size()<1) return;
|
|
|
|
UNUSED(progressPaths); // to silence compiler warning,var is occasionally used in dev. for debugging
|
|
|
|
IntPoint nextPoint(passToolPath[0]);
|
|
|
|
// if there is a previous path - need to resolve linking move to new path
|
|
if(output.AdaptivePaths.size()>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<toolRadiusScaled && IsAllowedToCutTrough(lastPoint,nextPoint,cleared,toolBoundPaths)) {
|
|
//cout << "cleared through" << endl;
|
|
TPath linkPath;
|
|
linkPath.first = MotionType::mtCutting;
|
|
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);
|
|
linkFound=true;
|
|
}
|
|
if(!linkFound) {
|
|
IntPoint lastInterimPoint =lastPoint;
|
|
IntPoint nextInterimPoint =nextPoint;
|
|
// find the closes cleared path within stepover distance, this will be keepToolDownPath if stright line is not possible
|
|
Path keepToolDownLinkPath;
|
|
ClipperOffset clipof;
|
|
clipof.AddPaths(cleared,JoinType::jtRound,EndType::etClosedPolygon);
|
|
Paths clearedOff;
|
|
double offset=stepOverFactor*toolRadiusScaled;
|
|
bool keepDownLinkExists = false;
|
|
bool keepDownLinkTooLong = true;
|
|
for(int i=1;i<10;i+=2) {
|
|
clipof.Execute(clearedOff,-toolRadiusScaled-offset/i);
|
|
// AddPathsToProgress(progressPaths,clearedOff, MotionType::mtLinkClear);
|
|
// CheckReportProgress(progressPaths,true);
|
|
// usleep(100000);
|
|
keepDownLinkExists = FindPathBetweenClosestPoints(clearedOff,lastPoint,nextPoint,toolRadiusScaled + 2*offset/i ,keepToolDownLinkPath);
|
|
if(keepDownLinkExists) {
|
|
lastInterimPoint=keepToolDownLinkPath.front();
|
|
nextInterimPoint=keepToolDownLinkPath.back();
|
|
if(!IsAllowedToCutTrough(lastPoint,lastInterimPoint,cleared,toolBoundPaths)) {
|
|
//cout << "not IsAllowedToCutTrough1" << endl;
|
|
keepDownLinkExists=false;
|
|
}
|
|
if(!IsAllowedToCutTrough(nextPoint,nextInterimPoint,cleared,toolBoundPaths)) {
|
|
//cout << "not IsAllowedToCutTrough2" << endl;
|
|
keepDownLinkExists=false;
|
|
}
|
|
if(!IsClearPath(keepToolDownLinkPath,cleared,stepOverFactor*toolRadiusScaled/2)) {
|
|
//cout << "not IsClearPath keepToolDownLinkPath" << endl;
|
|
keepDownLinkExists=false;
|
|
}
|
|
// is inefficient to link through interim points?
|
|
if(sqrt(DistanceSqrd(lastPoint,lastInterimPoint))+sqrt(DistanceSqrd(nextInterimPoint,nextPoint))
|
|
> 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 && linkDistance<toolRadiusScaled) mt=MotionType::mtCutting;
|
|
|
|
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;
|
|
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);
|
|
}
|
|
|
|
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) {
|
|
//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;
|
|
|
|
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;
|
|
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])<0) 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;
|
|
}
|
|
|
|
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<DoublePoint> gyro; // used to average tool direction
|
|
vector<double> 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;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 = cleared;
|
|
/*******************************
|
|
* 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));
|
|
// 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; //*(0.8*(1-exp(-4*distanceToBoundary/toolRadiusScaled)) + 0.2);
|
|
|
|
// 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)>1e-5) {
|
|
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();
|
|
/************************************************
|
|
* 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(distFromStart!=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 successfull 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+=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<finShiftedPath.size();i++) {
|
|
if(!IsAllowedToCutTrough(finShiftedPath.at(i-1),finShiftedPath.at(i),cleared,toolBoundPaths,2.0,true)) allCutsAllowed=false;
|
|
}
|
|
|
|
Path finCleaned;
|
|
CleanPath(finShiftedPath,finCleaned,FINISHING_CLEAN_PATH_TOLERANCE);
|
|
|
|
|
|
AppendToolPath(progressPaths,output,finCleaned,clearedBeforePass,toolBoundPaths);
|
|
//expand cleared for finishing path
|
|
clipof.Clear();
|
|
clipof.AddPath(finCleaned,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);
|
|
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_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
|
|
|
|
// make sure invalid paths are not used
|
|
if(!allCutsAllowed) {
|
|
cerr << "INVALID CUTS DETECTED! Please try to modify accuracy and/or step-over." << endl;
|
|
}
|
|
|
|
results.push_back(output);
|
|
}
|
|
|
|
} |