3142 lines
91 KiB
C++
3142 lines
91 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>
|
|
|
|
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 4.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;
|
|
}
|
|
|
|
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(p2.Y - p1.Y, p2.X - p1.X);
|
|
double t2 = atan2(p3.Y - p2.Y, p3.X - p2.X);
|
|
double a = fabs(t2 - t1);
|
|
return min(a, 2 * M_PI - a);
|
|
}
|
|
|
|
inline DoublePoint DirectionV(const IntPoint &pt1, const IntPoint &pt2)
|
|
{
|
|
double DX = double(pt2.X - pt1.X);
|
|
double DY = double(pt2.Y - pt1.Y);
|
|
double l = sqrt(DX * DX + DY * DY);
|
|
if (l < NTOL)
|
|
return DoublePoint(0, 0);
|
|
return DoublePoint(DX / l, DY / l);
|
|
}
|
|
|
|
inline void NormalizeV(DoublePoint &pt)
|
|
{
|
|
double len = sqrt(pt.X * pt.X + pt.Y * pt.Y);
|
|
if (len > NTOL)
|
|
{
|
|
pt.X /= len;
|
|
pt.Y /= len;
|
|
}
|
|
}
|
|
|
|
inline DoublePoint GetPathDirectionV(const Path &pth, size_t pointIndex)
|
|
{
|
|
if (pth.size() < 2)
|
|
return DoublePoint(0, 0);
|
|
const IntPoint &p1 = pth.at(pointIndex > 0 ? pointIndex - 1 : pth.size() - 1);
|
|
const IntPoint &p2 = pth.at(pointIndex);
|
|
return DirectionV(p1, p2);
|
|
}
|
|
|
|
//*****************************************
|
|
// Utils
|
|
//*****************************************
|
|
|
|
class BoundBox
|
|
{
|
|
public:
|
|
BoundBox()
|
|
{
|
|
minX = 0;
|
|
maxX = 0;
|
|
minY = 0;
|
|
maxY = 0;
|
|
}
|
|
|
|
// generic: first point
|
|
BoundBox(const IntPoint &p1)
|
|
{
|
|
minX = p1.X;
|
|
maxX = p1.X;
|
|
minY = p1.Y;
|
|
maxY = p1.Y;
|
|
}
|
|
|
|
void SetFirstPoint(const IntPoint &p1)
|
|
{
|
|
minX = p1.X;
|
|
maxX = p1.X;
|
|
minY = p1.Y;
|
|
maxY = p1.Y;
|
|
}
|
|
|
|
// generic: subsequent points
|
|
void AddPoint(const IntPoint &pt)
|
|
{
|
|
minX = min(pt.X, minX);
|
|
maxX = max(pt.X, maxX);
|
|
minY = min(pt.Y, minY);
|
|
maxY = max(pt.Y, maxY);
|
|
}
|
|
|
|
// line segment: two points
|
|
BoundBox(const IntPoint &p1, const IntPoint &p2)
|
|
{
|
|
if (p1.X < p2.X)
|
|
{
|
|
minX = p1.X;
|
|
maxX = p2.X;
|
|
}
|
|
else
|
|
{
|
|
minX = p2.X;
|
|
maxX = p1.X;
|
|
}
|
|
if (p1.Y < p2.Y)
|
|
{
|
|
minY = p1.Y;
|
|
maxY = p2.Y;
|
|
}
|
|
else
|
|
{
|
|
minY = p2.Y;
|
|
maxY = p1.Y;
|
|
}
|
|
}
|
|
|
|
// for circle: center and radius
|
|
BoundBox(const IntPoint ¢er, long radius)
|
|
{
|
|
minX = center.X - radius;
|
|
maxX = center.X + radius;
|
|
minY = center.Y - radius;
|
|
maxY = center.Y + radius;
|
|
}
|
|
|
|
// bounds check - intersection
|
|
inline bool CollidesWith(const BoundBox &bb2)
|
|
{
|
|
return minX <= bb2.maxX && maxX >= bb2.minX && minY <= bb2.maxY && maxY >= bb2.minY;
|
|
}
|
|
|
|
// bounds check - contains
|
|
inline bool Contains(const BoundBox &bb2)
|
|
{
|
|
return minX <= bb2.minX && maxX >= bb2.maxX && minY <= bb2.minY && maxY >= bb2.maxY;
|
|
}
|
|
|
|
ClipperLib::cInt minX;
|
|
ClipperLib::cInt maxX;
|
|
ClipperLib::cInt minY;
|
|
ClipperLib::cInt maxY;
|
|
};
|
|
|
|
std::ostream &operator<<(std::ostream &s, const BoundBox &p)
|
|
{
|
|
s << "(" << p.minX << "," << p.minY << ") - (" << p.maxX << "," << p.maxY << ")";
|
|
return s;
|
|
}
|
|
|
|
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 appendDirectChildPaths(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);
|
|
}
|
|
}
|
|
}
|
|
|
|
void AverageDirection(const vector<DoublePoint> &unityVectors, DoublePoint &output)
|
|
{
|
|
std::size_t size = unityVectors.size();
|
|
output.X = 0;
|
|
output.Y = 0;
|
|
// sum vectors
|
|
for (std::size_t i = 0; i < size; i++)
|
|
{
|
|
DoublePoint v = unityVectors[i];
|
|
output.X += v.X;
|
|
output.Y += v.Y;
|
|
}
|
|
// normalize
|
|
double magnitude = sqrt(output.X * output.X + output.Y * output.Y);
|
|
output.X /= magnitude;
|
|
output.Y /= magnitude;
|
|
}
|
|
|
|
double DistancePointToLineSegSquared(const IntPoint &p1, const IntPoint &p2, const IntPoint &pt,
|
|
IntPoint &closestPoint, double &ptParameter, bool clamp = true)
|
|
{
|
|
double D21X = double(p2.X - p1.X);
|
|
double D21Y = double(p2.Y - p1.Y);
|
|
double DP1X = double(pt.X - p1.X);
|
|
double DP1Y = double(pt.Y - p1.Y);
|
|
double lsegLenSqr = D21X * D21X + D21Y * D21Y;
|
|
if (lsegLenSqr == 0)
|
|
{ // segment is zero length, return point to point distance
|
|
closestPoint = p1;
|
|
ptParameter = 0;
|
|
return DP1X * DP1X + DP1Y * DP1Y;
|
|
}
|
|
double parameter = DP1X * D21X + DP1Y * D21Y;
|
|
if (clamp)
|
|
{
|
|
// clamp the parameter
|
|
if (parameter < 0)
|
|
parameter = 0;
|
|
else if (parameter > lsegLenSqr)
|
|
parameter = lsegLenSqr;
|
|
}
|
|
// point on line at parameter
|
|
ptParameter = parameter / lsegLenSqr;
|
|
closestPoint.X = long(p1.X + ptParameter * D21X);
|
|
closestPoint.Y = long(p1.Y + ptParameter * D21Y);
|
|
// calculate distance from point on line to pt
|
|
double DX = double(pt.X - closestPoint.X);
|
|
double DY = double(pt.Y - closestPoint.Y);
|
|
return DX * DX + DY * DY; // return distance squared
|
|
}
|
|
|
|
void ScaleUpPaths(Paths &paths,long scaleFactor) {
|
|
for(auto &pth:paths) {
|
|
for(auto &pt:pth) {
|
|
pt.X*=scaleFactor;
|
|
pt.Y*=scaleFactor;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ScaleDownPaths(Paths &paths,long scaleFactor) {
|
|
for(auto &pth:paths) {
|
|
for(auto &pt:pth) {
|
|
pt.X/=scaleFactor;
|
|
pt.Y/=scaleFactor;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
// joins collinear segments (within the tolerance)
|
|
void CleanPath(const Path &inp, Path &outpt, double tolerance)
|
|
{
|
|
if (inp.size() < 3)
|
|
{
|
|
outpt = inp;
|
|
return;
|
|
}
|
|
outpt.clear();
|
|
Path tmp;
|
|
CleanPolygon(inp, tmp, tolerance);
|
|
long size=long(tmp.size());
|
|
|
|
// CleanPolygon will have empty result if all points are collinear,
|
|
// need to add first and last point to the output
|
|
if(size<=2) {
|
|
outpt.push_back(inp.front());
|
|
outpt.push_back(inp.back());
|
|
return;
|
|
}
|
|
|
|
// restore starting point
|
|
double clpPar = 0;
|
|
size_t clpSegmentIndex=0;
|
|
size_t clpPathIndex=0;
|
|
Paths tmpPaths;
|
|
tmpPaths.push_back(tmp);
|
|
IntPoint clp;
|
|
// find point on cleaned poly that is closest to original starting point
|
|
DistancePointToPathsSqrd(tmpPaths,inp.front(),clp,clpPathIndex,clpSegmentIndex,clpPar);
|
|
|
|
|
|
// if closes point is not one of the polygon points, add it as separate first point
|
|
if(DistanceSqrd(clp,tmp.at(clpSegmentIndex)) > 0 &&
|
|
DistanceSqrd(clp,tmp.at(clpSegmentIndex>0 ? clpSegmentIndex-1 : size-1)) > 0) outpt.push_back(clp);
|
|
|
|
// add remaining points starting from closest
|
|
long index;
|
|
for (long i = 0; i < size; i++)
|
|
{
|
|
index = static_cast<long>(clpSegmentIndex + i);
|
|
if (index >= size) index -= size;
|
|
outpt.push_back(tmp.at(index));
|
|
}
|
|
|
|
|
|
if(DistanceSqrd(outpt.front(),inp.front()) > SAME_POINT_TOL_SQRD_SCALED)
|
|
outpt.insert(outpt.begin(), inp.front());
|
|
|
|
if(DistanceSqrd(outpt.back(),inp.back()) > SAME_POINT_TOL_SQRD_SCALED)
|
|
outpt.push_back( inp.back());
|
|
|
|
}
|
|
|
|
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
|
|
|
|
//box check for performance
|
|
if (clamp)
|
|
{
|
|
BoundBox cBB(c, (ClipperLib::cInt)radius + 1); // circle bound box
|
|
BoundBox sBB(p1, p2);
|
|
if (!sBB.CollidesWith(cBB))
|
|
return false;
|
|
}
|
|
|
|
double dx = double(p2.X - p1.X);
|
|
double dy = double(p2.Y - p1.Y);
|
|
double lcx = double(p1.X - c.X);
|
|
double lcy = double(p1.Y - c.Y);
|
|
double a = dx * dx + dy * dy;
|
|
double b = 2 * dx * lcx + 2 * dy * lcy;
|
|
double C = lcx * lcx + lcy * lcy - radius * radius;
|
|
double sq = b * b - 4 * a * C;
|
|
if (sq < 0)
|
|
return false; // no solution
|
|
sq = sqrt(sq);
|
|
double t1 = (-b - sq) / (2 * a);
|
|
double t2 = (-b + sq) / (2 * a);
|
|
result.clear();
|
|
if (clamp)
|
|
{
|
|
if (t1 >= 0.0 && t1 <= 1.0)
|
|
result.push_back(DoublePoint(p1.X + t1 * dx, p1.Y + t1 * dy));
|
|
if (t2 >= 0.0 && t2 <= 1.0)
|
|
result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
|
|
}
|
|
else
|
|
{
|
|
result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
|
|
result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
|
|
}
|
|
return result.size() > 0;
|
|
}
|
|
|
|
// calculate center point of polygon
|
|
IntPoint Compute2DPolygonCentroid(const Path &vertices)
|
|
{
|
|
DoublePoint centroid(0, 0);
|
|
double signedArea = 0.0;
|
|
double x0 = 0.0; // Current vertex X
|
|
double y0 = 0.0; // Current vertex Y
|
|
double x1 = 0.0; // Next vertex X
|
|
double y1 = 0.0; // Next vertex Y
|
|
double a = 0.0; // Partial signed area
|
|
|
|
// For all vertices
|
|
size_t i = 0;
|
|
Path::size_type size = vertices.size();
|
|
for (i = 0; i < size; ++i)
|
|
{
|
|
x0 = double(vertices[i].X);
|
|
y0 = double(vertices[i].Y);
|
|
x1 = double(vertices[(i + 1) % size].X);
|
|
y1 = double(vertices[(i + 1) % size].Y);
|
|
a = x0 * y1 - x1 * y0;
|
|
signedArea += a;
|
|
centroid.X += (x0 + x1) * a;
|
|
centroid.Y += (y0 + y1) * a;
|
|
}
|
|
|
|
signedArea *= 0.5;
|
|
centroid.X /= (6.0 * signedArea);
|
|
centroid.Y /= (6.0 * signedArea);
|
|
return IntPoint(long(centroid.X), long(centroid.Y));
|
|
}
|
|
|
|
// point must be within first path (boundary) and must not be within all other paths (holes)
|
|
bool IsPointWithinCutRegion(const Paths &toolBoundPaths, const IntPoint &point)
|
|
{
|
|
for (size_t i = 0; i < toolBoundPaths.size(); i++)
|
|
{
|
|
int pip = PointInPolygon(point, toolBoundPaths[i]);
|
|
if (i == 0 && pip == 0)
|
|
return false; // is outside or on boundary
|
|
if (i > 0 && pip != 0)
|
|
return false; // is inside hole
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/* finds intersection of line segment with line segment */
|
|
bool IntersectionPoint(const IntPoint &s1p1,
|
|
const IntPoint &s1p2,
|
|
const IntPoint &s2p1,
|
|
const IntPoint &s2p2,
|
|
IntPoint &intersection)
|
|
{
|
|
double S1DX = double(s1p2.X - s1p1.X);
|
|
double S1DY = double(s1p2.Y - s1p1.Y);
|
|
double S2DX = double(s2p2.X - s2p1.X);
|
|
double S2DY = double(s2p2.Y - s2p1.Y);
|
|
double d = S1DY * S2DX - S2DY * S1DX;
|
|
if (fabs(d) < NTOL)
|
|
return false; // lines are parallel
|
|
|
|
double LPDX = double(s1p1.X - s2p1.X);
|
|
double LPDY = double(s1p1.Y - s2p1.Y);
|
|
double p1d = S2DY * LPDX - S2DX * LPDY;
|
|
double p2d = S1DY * LPDX - S1DX * LPDY;
|
|
if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0))
|
|
return false; // intersection not within segment1
|
|
if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d))
|
|
return false; // intersection not within segment2
|
|
double t = p1d / d;
|
|
intersection = IntPoint(long(s1p1.X + S1DX * t), long(s1p1.Y + S1DY * t));
|
|
return true;
|
|
}
|
|
|
|
/* finds one/first intersection of line segment with paths */
|
|
bool IntersectionPoint(const Paths &paths, const IntPoint &p1, const IntPoint &p2, IntPoint &intersection)
|
|
{
|
|
BoundBox segBB(p1, p2);
|
|
for (size_t i = 0; i < paths.size(); i++)
|
|
{
|
|
const Path *path = &paths[i];
|
|
size_t size = path->size();
|
|
if (size < 2)
|
|
continue;
|
|
BoundBox pathBB(path->front());
|
|
for (size_t j = 0; j < size; j++)
|
|
{
|
|
|
|
const IntPoint *pp2 = &path->at(j);
|
|
|
|
// box check for performance
|
|
pathBB.AddPoint(*pp2);
|
|
if (!pathBB.CollidesWith(segBB))
|
|
continue;
|
|
|
|
const IntPoint *pp1 = &path->at(j > 0 ? j - 1 : size - 1);
|
|
double LDY = double(p2.Y - p1.Y);
|
|
double LDX = double(p2.X - p1.X);
|
|
double PDX = double(pp2->X - pp1->X);
|
|
double PDY = double(pp2->Y - pp1->Y);
|
|
double d = LDY * PDX - PDY * LDX;
|
|
if (fabs(d) < NTOL)
|
|
continue; // lines are parallel
|
|
|
|
double LPDX = double(p1.X - pp1->X);
|
|
double LPDY = double(p1.Y - pp1->Y);
|
|
double p1d = PDY * LPDX - PDX * LPDY;
|
|
double p2d = LDY * LPDX - LDX * LPDY;
|
|
if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0))
|
|
continue; // intersection not within segment
|
|
if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d))
|
|
continue; // intersection not within segment
|
|
double t = p1d / d;
|
|
intersection = IntPoint(long(p1.X + LDX * t), long(p1.Y + LDY * t));
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void SmoothPaths(Paths &paths, double stepSize, long pointCount, long iterations)
|
|
{
|
|
Paths output;
|
|
output.resize(paths.size());
|
|
const long scale=1000;
|
|
const double stepScaled = stepSize*scale;
|
|
|
|
ScaleUpPaths(paths,scale);
|
|
vector<pair<size_t /*path index*/, IntPoint>> points;
|
|
for (size_t i = 0; i < paths.size(); i++)
|
|
{
|
|
for (const auto &pt : paths[i])
|
|
{
|
|
if (points.empty())
|
|
{
|
|
points.push_back(pair<size_t /*path index*/, IntPoint>(i, pt));
|
|
continue;
|
|
}
|
|
const auto back=points.back();
|
|
const IntPoint & lastPt = back.second;
|
|
|
|
|
|
const double l = sqrt(DistanceSqrd(lastPt, pt));
|
|
|
|
if (l < 0.5*stepScaled )
|
|
{
|
|
if(points.size()>1) points.pop_back();
|
|
points.push_back(pair<size_t /*path index*/, IntPoint>(i, pt));
|
|
continue;
|
|
}
|
|
size_t lastPathIndex = back.first;
|
|
const long steps = max(long(l / stepScaled),1L);
|
|
const long left=pointCount*iterations*2;
|
|
const long right =steps-pointCount*iterations*2;
|
|
for (long idx = 0; idx <= steps; idx++)
|
|
{
|
|
if(idx>left && idx<right) {
|
|
idx=right;
|
|
continue;
|
|
}
|
|
const double p = double(idx) / steps;
|
|
const IntPoint ptx(long(lastPt.X + double(pt.X - lastPt.X) * p),
|
|
long(lastPt.Y + double(pt.Y - lastPt.Y) * p));
|
|
|
|
if(idx==0 && DistanceSqrd(back.second,ptx)<scale && points.size()>1) points.pop_back();
|
|
|
|
if (p < 0.5)
|
|
points.push_back(pair<size_t /*path index*/, IntPoint>(lastPathIndex, ptx));
|
|
else
|
|
points.push_back(pair<size_t /*path index*/, IntPoint>(i, ptx));
|
|
}
|
|
}
|
|
}
|
|
if (points.empty())
|
|
return;
|
|
const long size=long(points.size());
|
|
for(long iter=0;iter<iterations; iter++) {
|
|
for (long i = 1; i < size-1; i++)
|
|
{
|
|
IntPoint &cp = points[i].second;
|
|
IntPoint avgPoint(cp);
|
|
long cnt = 1;
|
|
|
|
long ptsToAverage = pointCount;
|
|
if (i <= ptsToAverage)
|
|
ptsToAverage = max(i-1,0L);
|
|
else if(i + ptsToAverage >= size-1)
|
|
ptsToAverage = size-1-i;
|
|
for (long j = i-ptsToAverage; j <= i+ptsToAverage; j++)
|
|
{
|
|
if (j == i) continue;
|
|
long index=j;
|
|
if(index<0) index=0;
|
|
if(index>=size) index=size-1;
|
|
IntPoint &p = points[index].second;
|
|
avgPoint.X += p.X ;
|
|
avgPoint.Y += p.Y ;
|
|
cnt++;
|
|
}
|
|
cp.X=avgPoint.X/cnt;
|
|
cp.Y=avgPoint.Y/cnt;
|
|
}
|
|
}
|
|
|
|
for (const auto &pr:points)
|
|
{
|
|
output[pr.first].push_back(pr.second);
|
|
}
|
|
for (size_t i = 0; i < paths.size(); i++)
|
|
{
|
|
CleanPath(output[i], paths[i], 1.4*scale);
|
|
}
|
|
ScaleDownPaths(paths,scale);
|
|
}
|
|
|
|
bool PopPathWithClosestPoint(Paths &paths /*closest path is removed from collection and shifted to start with closest point */
|
|
,
|
|
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 = long(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 = closestPointIndex + long(i);
|
|
if (index >= long(closestPath.size()))
|
|
index -= long(closestPath.size());
|
|
result.push_back(closestPath.at(index));
|
|
}
|
|
// remove the closest path
|
|
paths.erase(paths.begin() + closestPathIndex);
|
|
return true;
|
|
}
|
|
|
|
void DeduplicatePaths(const Paths &inputs, Paths &outputs)
|
|
{
|
|
outputs.clear();
|
|
for (const auto &new_pth : inputs)
|
|
{
|
|
bool duplicate = false;
|
|
// if 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)
|
|
{
|
|
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;
|
|
running = false;
|
|
start_ticks = 0;
|
|
total_ticks = 0;
|
|
}
|
|
inline void Start()
|
|
{
|
|
#ifdef DEV_MODE
|
|
start_ticks = clock();
|
|
if (running)
|
|
{
|
|
cerr << "PerfCounter already running:" << name << endl;
|
|
}
|
|
running = true;
|
|
#endif
|
|
}
|
|
inline void Stop()
|
|
{
|
|
#ifdef DEV_MODE
|
|
if (!running)
|
|
{
|
|
cerr << "PerfCounter not running:" << name << endl;
|
|
}
|
|
total_ticks += clock() - start_ticks;
|
|
start_ticks = clock();
|
|
count++;
|
|
running = false;
|
|
#endif
|
|
}
|
|
void DumpResults()
|
|
{
|
|
double total_time = double(total_ticks) / CLOCKS_PER_SEC;
|
|
cout << "Perf: " << name.c_str() << " total_time: " << total_time << " sec, call_count:" << count << " per_call:" << double(total_time / count) << endl;
|
|
start_ticks = clock();
|
|
total_ticks = 0;
|
|
count = 0;
|
|
}
|
|
|
|
private:
|
|
string name;
|
|
clock_t start_ticks;
|
|
clock_t total_ticks;
|
|
size_t count;
|
|
bool running = false;
|
|
};
|
|
|
|
PerfCounter Perf_ProcessPolyNode("ProcessPolyNode");
|
|
PerfCounter Perf_CalcCutAreaCirc("CalcCutArea");
|
|
PerfCounter Perf_CalcCutAreaClip("CalcCutAreaClip");
|
|
PerfCounter Perf_NextEngagePoint("NextEngagePoint");
|
|
PerfCounter Perf_PointIterations("PointIterations");
|
|
PerfCounter Perf_ExpandCleared("ExpandCleared");
|
|
PerfCounter Perf_DistanceToBoundary("DistanceToBoundary");
|
|
PerfCounter Perf_AppendToolPath("AppendToolPath");
|
|
PerfCounter Perf_IsAllowedToCutTrough("IsAllowedToCutTrough");
|
|
PerfCounter Perf_IsClearPath("IsClearPath");
|
|
|
|
//***********************************
|
|
// Cleared area bounding support
|
|
//***********************************
|
|
class ClearedArea
|
|
{
|
|
public:
|
|
ClearedArea(ClipperLib::cInt p_toolRadiusScaled)
|
|
{
|
|
toolRadiusScaled = p_toolRadiusScaled;
|
|
};
|
|
|
|
void SetClearedPaths(const Paths &paths)
|
|
{
|
|
clearedPaths = paths;
|
|
bboxPathsInvalid = true;
|
|
bboxClippedInvalid = true;
|
|
}
|
|
void ExpandCleared(const Path toClearToolPath)
|
|
{
|
|
if (toClearToolPath.empty())
|
|
return;
|
|
Perf_ExpandCleared.Start();
|
|
clipof.Clear();
|
|
clipof.AddPath(toClearToolPath, JoinType::jtRound, EndType::etOpenRound);
|
|
Paths toolCoverPoly;
|
|
clipof.Execute(toolCoverPoly, toolRadiusScaled + 1);
|
|
clip.Clear();
|
|
clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
|
|
clip.AddPaths(toolCoverPoly, PolyType::ptClip, true);
|
|
clip.Execute(ClipType::ctUnion, clearedPaths);
|
|
CleanPolygons(clearedPaths);
|
|
bboxPathsInvalid = true;
|
|
bboxClippedInvalid = true;
|
|
Perf_ExpandCleared.Stop();
|
|
}
|
|
|
|
// gets the path sections inside the ext. tool bounding box
|
|
Paths &GetBoundedClearedPaths(const IntPoint &toolPos)
|
|
{
|
|
BoundBox toolBB(toolPos, toolRadiusScaled);
|
|
if (!bboxPathsInvalid && clearedBBPathsInFocus.Contains(toolBB))
|
|
{
|
|
return clearedBoundedPaths;
|
|
}
|
|
ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled;
|
|
clearedBBPathsInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta));
|
|
clearedBBPathsInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta));
|
|
|
|
BoundBox bb(toolPos, focusBBFactor2 * toolRadiusScaled);
|
|
clearedBoundedPaths.clear();
|
|
for (const auto &pth : clearedPaths)
|
|
{
|
|
if (pth.size() < 2)
|
|
continue;
|
|
Path bPath;
|
|
size_t size = pth.size();
|
|
for (size_t i = 0; i < size + 1; i++)
|
|
{
|
|
IntPoint last = (i > 0 ? pth[i - 1] : pth.back());
|
|
IntPoint next = i < size ? pth[i] : pth.front();
|
|
BoundBox ptbox(last, next);
|
|
if (ptbox.CollidesWith(bb))
|
|
{
|
|
if (bPath.empty() || bPath.back() != last)
|
|
bPath.push_back(last);
|
|
bPath.push_back(next);
|
|
}
|
|
else
|
|
{
|
|
if (!bPath.empty())
|
|
{
|
|
clearedBoundedPaths.push_back(bPath);
|
|
bPath.clear();
|
|
}
|
|
}
|
|
}
|
|
if (!bPath.empty())
|
|
{
|
|
clearedBoundedPaths.push_back(bPath);
|
|
bPath.clear();
|
|
}
|
|
}
|
|
bboxPathsInvalid = false;
|
|
return clearedBoundedPaths;
|
|
}
|
|
|
|
// get cleared area/poly bounded to toolbox
|
|
Paths &GetBoundedClearedAreaClipped(const IntPoint &toolPos)
|
|
{
|
|
BoundBox toolBB(toolPos, toolRadiusScaled);
|
|
if (!bboxClippedInvalid && clearedBBClippedInFocus.Contains(toolBB))
|
|
{
|
|
return clearedBoundedClipped;
|
|
}
|
|
ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled;
|
|
clearedBBClippedInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta));
|
|
clearedBBClippedInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta));
|
|
|
|
// a little larger area is bounded than checked
|
|
ClipperLib::cInt delta2 = focusBBFactor2 * toolRadiusScaled;
|
|
Path bbPath;
|
|
bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y - delta2));
|
|
bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y - delta2));
|
|
bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y + delta2));
|
|
bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y + delta2));
|
|
clip.Clear();
|
|
clip.AddPath(bbPath, PolyType::ptSubject, true);
|
|
clip.AddPaths(clearedPaths, PolyType::ptClip, true);
|
|
clip.Execute(ClipType::ctIntersection, clearedBoundedClipped);
|
|
bboxClippedInvalid = false;
|
|
return clearedBoundedClipped;
|
|
}
|
|
|
|
// get full cleared area
|
|
Paths &GetCleared()
|
|
{
|
|
return clearedPaths;
|
|
}
|
|
|
|
private:
|
|
Clipper clip;
|
|
ClipperOffset clipof;
|
|
Paths clearedPaths;
|
|
Paths clearedBoundedClipped;
|
|
Paths clearedBoundedPaths;
|
|
|
|
ClipperLib::cInt toolRadiusScaled;
|
|
BoundBox clearedBBClippedInFocus;
|
|
BoundBox clearedBBPathsInFocus;
|
|
|
|
bool bboxClippedInvalid = false;
|
|
bool bboxPathsInvalid = false;
|
|
// size of the focus BB
|
|
const ClipperLib::cInt focusBBFactor1 = 8;
|
|
const ClipperLib::cInt focusBBFactor2 = 9;
|
|
};
|
|
|
|
//***************************************
|
|
// Linear Interpolation - area vs angle
|
|
//***************************************
|
|
class Interpolation
|
|
{
|
|
public:
|
|
const double MIN_ANGLE = -M_PI / 4;
|
|
const double MAX_ANGLE = M_PI / 4;
|
|
|
|
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 - convenient 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 = 0;
|
|
size_t currentSegmentIndex = 0;
|
|
double segmentPos = 0;
|
|
double totalDistance = 0;
|
|
double currentPathLength = 0;
|
|
int passes = 0;
|
|
|
|
double metric = 0; // 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)
|
|
{
|
|
minDistSq = distSqrd;
|
|
minPathIndex = state.currentPathIndex;
|
|
minSegmentIndex = state.currentSegmentIndex;
|
|
minSegmentPos = state.segmentPos;
|
|
}
|
|
}
|
|
if (!nextPath())
|
|
break;
|
|
}
|
|
state.currentPathIndex = minPathIndex;
|
|
state.currentSegmentIndex = minSegmentIndex;
|
|
state.segmentPos = minSegmentPos;
|
|
calculateCurrentPathLength();
|
|
ResetPasses();
|
|
}
|
|
bool nextEngagePoint(Adaptive2d *parent, ClearedArea &clearedArea, double step, double minCutArea, double maxCutArea, int maxPases = 2)
|
|
{
|
|
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(-1000000000, -1000000000);
|
|
for (;;)
|
|
{
|
|
if (!moveForward(step))
|
|
{
|
|
if (!nextPath())
|
|
{
|
|
state.passes++;
|
|
if (state.passes >= maxPases)
|
|
{
|
|
Perf_NextEngagePoint.Stop();
|
|
return false; // nothing more to cut
|
|
}
|
|
prevArea = 0;
|
|
}
|
|
}
|
|
IntPoint cpt = getCurrentPoint();
|
|
double area = parent->CalcCutArea(clip, initialPoint, cpt, clearedArea);
|
|
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();
|
|
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 main class - implementation
|
|
//***************************************
|
|
|
|
Adaptive2d::Adaptive2d()
|
|
{
|
|
}
|
|
|
|
double Adaptive2d::CalcCutArea(Clipper &clip, const IntPoint &c1, const IntPoint &c2, ClearedArea &clearedArea, bool preventConventional)
|
|
{
|
|
|
|
double dist = DistanceSqrd(c1, c2);
|
|
if (dist < NTOL)
|
|
return 0;
|
|
|
|
Perf_CalcCutAreaCirc.Start();
|
|
|
|
/// new alg
|
|
double rsqrd = toolRadiusScaled * toolRadiusScaled;
|
|
double area = 0;
|
|
Paths interPaths;
|
|
IntPoint clp; // to hold closest point
|
|
vector<DoublePoint> inters; // to hold intersection results
|
|
BoundBox c2BB(c2, toolRadiusScaled);
|
|
BoundBox c1BB(c1, toolRadiusScaled);
|
|
Paths &clearedBounded = clearedArea.GetBoundedClearedAreaClipped(c2);
|
|
for (const Path &path : clearedBounded)
|
|
{
|
|
size_t size = path.size();
|
|
if (size == 0)
|
|
continue;
|
|
|
|
//** bound box check
|
|
// construct bound box for path
|
|
BoundBox pathBB(path.front());
|
|
for (const auto &pt : path)
|
|
pathBB.AddPoint(pt);
|
|
if (!c2BB.CollidesWith(c2))
|
|
continue; // this path cannot colide with tool
|
|
//** end of BB check
|
|
|
|
size_t curPtIndex = 0;
|
|
bool found = false;
|
|
// step 1: we find the starting point on the cleared path that is outside new tool shape (c2)
|
|
for (size_t i = 0; i < size; i++)
|
|
{
|
|
if (DistanceSqrd(path[curPtIndex], c2) > rsqrd)
|
|
{
|
|
found = true;
|
|
break;
|
|
}
|
|
curPtIndex++;
|
|
if (curPtIndex >= size)
|
|
curPtIndex = 0;
|
|
}
|
|
if (!found)
|
|
continue; // try another path
|
|
|
|
// step 2: iterate through path from starting point and find the part of the path inside the c2
|
|
size_t prevPtIndex = curPtIndex;
|
|
Path *interPath = NULL;
|
|
bool prev_inside = false;
|
|
const IntPoint *p1 = &path[prevPtIndex];
|
|
double par; // to hold parameter output
|
|
for (size_t i = 0; i < size; i++)
|
|
{
|
|
curPtIndex++;
|
|
if (curPtIndex >= size)
|
|
curPtIndex = 0;
|
|
const IntPoint *p2 = &path[curPtIndex];
|
|
BoundBox segBB(*p1, *p2);
|
|
if (!prev_inside)
|
|
{ // prev state: outside, find first point inside C2
|
|
if (segBB.CollidesWith(c2BB) &&
|
|
DistancePointToLineSegSquared(*p1, *p2, c2, clp, par) <= rsqrd)
|
|
{ // current segment inside, start
|
|
prev_inside = true;
|
|
interPaths.push_back(Path());
|
|
if (interPaths.size() > 1)
|
|
break; // we will use poly clipping alg. if there are more intersecting paths
|
|
interPath = &interPaths.back();
|
|
// current segment inside c2, prev point outside, find intersection:
|
|
if (Line2CircleIntersect(c2, toolRadiusScaled, *p1, *p2, inters))
|
|
{
|
|
interPath->push_back(IntPoint(long(inters[0].X), long(inters[0].Y)));
|
|
if (inters.size() > 1)
|
|
{
|
|
interPath->push_back(IntPoint(long(inters[1].X), long(inters[1].Y)));
|
|
prev_inside = false;
|
|
}
|
|
else
|
|
{
|
|
interPath->push_back(IntPoint(*p2));
|
|
}
|
|
}
|
|
else
|
|
{ // no intersection - must be edge case, add p2
|
|
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)
|
|
}
|
|
Perf_CalcCutAreaCirc.Stop();
|
|
if (interPaths.size() == 1 && interPaths.front().size() > 1)
|
|
{
|
|
Perf_CalcCutAreaCirc.Start();
|
|
Path *interPath = &interPaths.front();
|
|
// interPath - now contains the part of cleared path inside the C2
|
|
size_t ipc2_size = interPath->size();
|
|
const IntPoint &fpc2 = interPath->front(); // first point
|
|
const IntPoint &lpc2 = interPath->back(); // last point
|
|
// path length
|
|
double interPathLen = 0;
|
|
for (size_t j = 1; j < ipc2_size; j++)
|
|
interPathLen += sqrt(DistanceSqrd(interPath->at(j - 1), interPath->at(j)));
|
|
|
|
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 (preventConventional && interPathLen >= RESOLUTION_FACTOR)
|
|
{
|
|
// detect conventional mode cut - we want only climb mode
|
|
IntPoint midPoint(long(c2.X + toolRadiusScaled * cos(0.5 * (maxFi + minFi))), long(c2.Y + toolRadiusScaled * sin(0.5 * (maxFi + minFi))));
|
|
if (PointSideOfLine(c1, c2, midPoint) < 0)
|
|
{
|
|
area = __DBL_MAX__;
|
|
Perf_CalcCutAreaCirc.Stop();
|
|
// #ifdef DEV_MODE
|
|
// cout << "Break: @(" << double(c2.X)/scaleFactor << "," << double(c2.Y)/scaleFactor << ") conventional mode" << endl;
|
|
// #endif
|
|
return area;
|
|
}
|
|
}
|
|
|
|
double scanDistance = 2.5 * toolRadiusScaled;
|
|
// stepping through path discretized to stepDistance
|
|
double stepDistance = min(double(RESOLUTION_FACTOR), interPathLen / 24) + 1;
|
|
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);
|
|
Perf_CalcCutAreaCirc.Stop();
|
|
}
|
|
else if (interPaths.size() > 1)
|
|
{
|
|
Perf_CalcCutAreaClip.Start();
|
|
// old way of calculating cut area based on polygon clipping
|
|
// used in case when there are multiple intersections of tool with cleared poly (very rare case, but important)
|
|
// 1. find difference between old and new tool shape
|
|
Path oldTool;
|
|
Path newTool;
|
|
TranslatePath(toolGeometry, oldTool, c1);
|
|
TranslatePath(toolGeometry, newTool, c2);
|
|
clip.Clear();
|
|
clip.AddPath(newTool, PolyType::ptSubject, true);
|
|
clip.AddPath(oldTool, PolyType::ptClip, true);
|
|
Paths toolDiff;
|
|
clip.Execute(ClipType::ctDifference, toolDiff);
|
|
|
|
// 2. difference to cleared
|
|
clip.Clear();
|
|
clip.AddPaths(toolDiff, PolyType::ptSubject, true);
|
|
clip.AddPaths(clearedBounded, PolyType::ptClip, true);
|
|
Paths cutAreaPoly;
|
|
clip.Execute(ClipType::ctDifference, cutAreaPoly);
|
|
|
|
// calculate resulting area
|
|
area = 0;
|
|
for (Path &path : cutAreaPoly)
|
|
{
|
|
area += fabs(Area(path));
|
|
}
|
|
Perf_CalcCutAreaClip.Stop();
|
|
}
|
|
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
|
|
//**********************************
|
|
|
|
// keep the tolerance in workable range
|
|
if (tolerance < 0.01)
|
|
tolerance = 0.01;
|
|
if (tolerance > 0.2)
|
|
tolerance = 0.2;
|
|
|
|
scaleFactor = RESOLUTION_FACTOR / tolerance;
|
|
if (stepOverFactor * toolDiameter < 1.0)
|
|
scaleFactor *= 1.0 / (stepOverFactor * toolDiameter);
|
|
if (scaleFactor > 1000)
|
|
scaleFactor = 1000;
|
|
|
|
//scaleFactor = round(scaleFactor);
|
|
|
|
current_region=0;
|
|
cout << "Tool Diameter: " << toolDiameter << endl;
|
|
cout << "Accuracy: " << round(10000.0/scaleFactor)/10 << " um" << endl;
|
|
cout << flush;
|
|
|
|
toolRadiusScaled = long(toolDiameter * scaleFactor / 2);
|
|
stepOverScaled = toolRadiusScaled * stepOverFactor;
|
|
progressCallback = &progressCallbackFn;
|
|
lastProgressTime = clock();
|
|
stopProcessing = false;
|
|
|
|
if(helixRampDiameter<NTOL)
|
|
helixRampDiameter=0.75*toolDiameter;
|
|
if (helixRampDiameter > toolDiameter)
|
|
helixRampDiameter = toolDiameter;
|
|
if (helixRampDiameter < toolDiameter / 8)
|
|
helixRampDiameter = toolDiameter / 8;
|
|
|
|
helixRampRadiusScaled = long(helixRampDiameter * scaleFactor / 2);
|
|
finishPassOffsetScaled = long(stepOverScaled / 10);
|
|
|
|
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 reference area
|
|
Path slotCut;
|
|
TranslatePath(toolGeometryPaths[0], slotCut, IntPoint(toolRadiusScaled / 2, 0));
|
|
clip.Clear();
|
|
clip.AddPath(toolGeometryPaths[0], PolyType::ptSubject, true);
|
|
clip.AddPath(slotCut, PolyType::ptClip, true);
|
|
Paths crossing;
|
|
clip.Execute(ClipType::ctDifference, crossing);
|
|
referenceCutArea = fabs(Area(crossing[0]));
|
|
optimalCutAreaPD = 2 * stepOverFactor * referenceCutArea / toolRadiusScaled;
|
|
#ifdef DEV_MODE
|
|
cout << "optimalCutAreaPD:" << optimalCutAreaPD << " scaleFactor:" << scaleFactor << " toolRadiusScaled:" << toolRadiusScaled << " helixRampRadiusScaled:" << helixRampRadiusScaled << endl;
|
|
#endif
|
|
//******************************
|
|
// Convert input paths to clipper
|
|
//******************************
|
|
Paths converted;
|
|
for (size_t i = 0; i < paths.size(); i++)
|
|
{
|
|
Path cpth;
|
|
for (size_t j = 0; j < paths[i].size(); j++)
|
|
{
|
|
std::pair<double, double> pt = paths[i][j];
|
|
cpth.push_back(IntPoint(long(pt.first * scaleFactor), long(pt.second * scaleFactor)));
|
|
}
|
|
Path cpth2;
|
|
CleanPath(cpth,cpth2,FINISHING_CLEAN_PATH_TOLERANCE);
|
|
converted.push_back(cpth2);
|
|
}
|
|
|
|
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);
|
|
//CleanPolygons(stockInputPaths,0.707);
|
|
|
|
//***************************************
|
|
// Resolve hierarchy and run processing
|
|
//***************************************
|
|
double cornerRoundingOffset = 0.15 * toolRadiusScaled / 2;
|
|
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 - cornerRoundingOffset);
|
|
for (const auto ¤t : paths)
|
|
{
|
|
int nesting = getPathNestingLevel(current, paths);
|
|
if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit))
|
|
{
|
|
Paths toolBoundPaths;
|
|
toolBoundPaths.push_back(current);
|
|
if (polyTreeNestingLimit != nesting)
|
|
{
|
|
appendDirectChildPaths(toolBoundPaths, current, paths);
|
|
}
|
|
|
|
// offset back outwards - corner rounding
|
|
clipof.Clear();
|
|
clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(toolBoundPaths, cornerRoundingOffset);
|
|
|
|
// restore original bound paths
|
|
// bounding paths - i.e. area that must be cleared inside
|
|
// it's not the same as input paths due to filtering (nesting logic) and corner rounding
|
|
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 ¤t : inputPaths)
|
|
{
|
|
int nesting = getPathNestingLevel(current, inputPaths);
|
|
if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit))
|
|
{
|
|
Paths profilePaths;
|
|
profilePaths.push_back(current);
|
|
if (polyTreeNestingLimit != nesting)
|
|
{
|
|
appendDirectChildPaths(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 offset 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);
|
|
|
|
/** tool bounds */
|
|
Paths toolBoundPaths;
|
|
clipof.Clear();
|
|
clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(toolBoundPaths, -toolRadiusScaled - finishPassOffsetScaled - cornerRoundingOffset);
|
|
|
|
/** offset back outwards - corner rounding */
|
|
clipof.Clear();
|
|
clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(toolBoundPaths, cornerRoundingOffset);
|
|
|
|
// restore original bound paths
|
|
// bounding paths - i.e. area that must be cleared inside
|
|
// it's not the same as above due to corner rounding
|
|
clipof.Clear();
|
|
clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(boundPaths, toolRadiusScaled + finishPassOffsetScaled);
|
|
|
|
ProcessPolyNode(boundPaths, toolBoundPaths);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return results;
|
|
}
|
|
|
|
bool Adaptive2d::FindEntryPoint(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &boundPaths,
|
|
ClearedArea &clearedArea /*output-initial cleared area by helix*/,
|
|
IntPoint &entryPoint /*output*/,
|
|
IntPoint &toolPos, DoublePoint &toolDir)
|
|
{
|
|
Paths incOffset;
|
|
Paths lastValidOffset;
|
|
Clipper clip;
|
|
ClipperOffset clipof;
|
|
bool found = false;
|
|
Paths clearedPaths;
|
|
Paths checkPaths = toolBoundPaths;
|
|
for (int iter = 0; iter < 10; iter++)
|
|
{
|
|
clipof.Clear();
|
|
clipof.AddPaths(checkPaths, JoinType::jtSquare, EndType::etClosedPolygon);
|
|
double step = RESOLUTION_FACTOR;
|
|
double currentDelta = -1;
|
|
clipof.Execute(incOffset, currentDelta);
|
|
while (incOffset.size() > 0)
|
|
{
|
|
clipof.Execute(incOffset, currentDelta);
|
|
if (incOffset.size() > 0)
|
|
lastValidOffset = incOffset;
|
|
currentDelta -= step;
|
|
}
|
|
for (size_t i = 0; i < lastValidOffset.size(); i++)
|
|
{
|
|
if (lastValidOffset[i].size() > 0)
|
|
{
|
|
entryPoint = Compute2DPolygonCentroid(lastValidOffset[i]);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
// check if the start point is in any of the holes
|
|
// this may happen in case when toolBoundPaths are symmetric (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 cleared by helix ramp
|
|
clipof.Clear();
|
|
Path p1;
|
|
p1.push_back(entryPoint);
|
|
clipof.AddPath(p1, JoinType::jtRound, EndType::etOpenRound);
|
|
clipof.Execute(clearedPaths, helixRampRadiusScaled + toolRadiusScaled);
|
|
CleanPolygons(clearedPaths);
|
|
// we got first cleared area - check if it is crossing boundary
|
|
clip.Clear();
|
|
clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
|
|
clip.AddPaths(boundPaths, PolyType::ptClip, true);
|
|
Paths crossing;
|
|
clip.Execute(ClipType::ctDifference, crossing);
|
|
if (crossing.size() > 0)
|
|
{
|
|
// helix does not fit to the cutting area
|
|
found = false;
|
|
}
|
|
else
|
|
{
|
|
clearedArea.SetClearedPaths(clearedPaths);
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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,
|
|
ClearedArea &clearedArea /*output-initial cleared 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;
|
|
Paths clearedPaths;
|
|
// check if boundary shape to cut is outside the stock
|
|
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(clearedPaths, 1000 * toolRadiusScaled);
|
|
|
|
clip.Clear();
|
|
clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
|
|
clip.AddPaths(stockInputPaths, PolyType::ptClip, true);
|
|
clip.Execute(ClipType::ctDifference, clearedPaths);
|
|
CleanPolygons(clearedPaths);
|
|
SimplifyPolygons(clearedPaths);
|
|
clearedArea.SetClearedPaths(clearedPaths);
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//************************************************************
|
|
// IsClearPath - returns true if path is clear from obstacles
|
|
//***********************************************************
|
|
bool Adaptive2d::IsClearPath(const Path &tp, ClearedArea &cleared, double safetyClearance)
|
|
{
|
|
Perf_IsClearPath.Start();
|
|
Clipper clip;
|
|
ClipperOffset clipof;
|
|
clipof.AddPath(tp, JoinType::jtRound, EndType::etOpenRound);
|
|
Paths toolShape;
|
|
clipof.Execute(toolShape, toolRadiusScaled + safetyClearance);
|
|
clip.AddPaths(toolShape, PolyType::ptSubject, true);
|
|
clip.AddPaths(cleared.GetCleared(), PolyType::ptClip, true);
|
|
Paths crossing;
|
|
clip.Execute(ClipType::ctDifference, crossing);
|
|
double collisionArea = 0;
|
|
for (auto &p : crossing)
|
|
{
|
|
collisionArea += fabs(Area(p));
|
|
}
|
|
Perf_IsClearPath.Stop();
|
|
return collisionArea < 1.0;
|
|
}
|
|
|
|
bool Adaptive2d::IsAllowedToCutTrough(const IntPoint &p1, const IntPoint &p2, ClearedArea &cleared, const Paths &toolBoundPaths, double areaFactor, bool skipBoundsCheck)
|
|
{
|
|
Perf_IsAllowedToCutTrough.Start();
|
|
|
|
if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p2))
|
|
{
|
|
// last point outside boundary - its not clear to cut
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return false;
|
|
}
|
|
else if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p1))
|
|
{
|
|
// first point outside boundary - its not clear to cut
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
Clipper clip;
|
|
double distance = sqrt(DistanceSqrd(p1, p2));
|
|
double stepSize = min(0.5 * stepOverScaled, 8 * RESOLUTION_FACTOR);
|
|
if (distance < stepSize / 2)
|
|
{ // not significant cut
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return true;
|
|
}
|
|
if (distance < stepSize)
|
|
{ // adjust for numeric instability with small distances
|
|
areaFactor *= 2;
|
|
}
|
|
|
|
IntPoint toolPos1 = p1;
|
|
long steps = long(distance / stepSize) + 1;
|
|
stepSize = distance / steps;
|
|
for (long i = 1; i <= steps; i++)
|
|
{
|
|
double p = double(i) / steps;
|
|
IntPoint toolPos2(long(p1.X + double(p2.X - p1.X) * p), long(p1.Y + double(p2.Y - p1.Y) * p));
|
|
double area = CalcCutArea(clip, toolPos1, toolPos2, cleared, false);
|
|
// if we are cutting above optimal -> not clear to cut
|
|
if (area > areaFactor * stepSize * optimalCutAreaPD)
|
|
{
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return false;
|
|
}
|
|
//if tool is outside boundary -> its not clear to cut
|
|
if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, toolPos2))
|
|
{
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return false;
|
|
}
|
|
toolPos1 = toolPos2;
|
|
}
|
|
}
|
|
Perf_IsAllowedToCutTrough.Stop();
|
|
return true;
|
|
}
|
|
|
|
bool Adaptive2d::ResolveLinkPath(const IntPoint &startPoint, const IntPoint &endPoint, ClearedArea &clearedArea, Path &output)
|
|
{
|
|
vector<pair<IntPoint, IntPoint>> queue;
|
|
queue.push_back(pair<IntPoint, IntPoint>(startPoint, endPoint));
|
|
Path checkPath;
|
|
double totalLength = 0;
|
|
double directDistance = sqrt(DistanceSqrd(startPoint, endPoint));
|
|
Paths linkPaths;
|
|
|
|
double scanStep = 2 * RESOLUTION_FACTOR;
|
|
if (scanStep > scaleFactor * 0.1)
|
|
scanStep = scaleFactor * 0.1;
|
|
if (scanStep < scaleFactor * 0.01)
|
|
scanStep = scaleFactor * 0.01;
|
|
long limit = 10000;
|
|
|
|
double clearance = stepOverScaled;
|
|
double offClearance = 2 * stepOverScaled;
|
|
if (offClearance > directDistance / 2)
|
|
{
|
|
offClearance = directDistance / 2;
|
|
clearance = 0;
|
|
}
|
|
|
|
long cnt = 0;
|
|
|
|
// to hold CLP results
|
|
IntPoint clp;
|
|
size_t pindex;
|
|
size_t sindex;
|
|
double par;
|
|
|
|
// put a time limit on the resolving the link path
|
|
clock_t time_limit = (clock_t)(max(keepToolDownDistRatio, 3.0) * CLOCKS_PER_SEC / 6);
|
|
|
|
clock_t time_out = clock() + time_limit;
|
|
|
|
while (!queue.empty())
|
|
{
|
|
if (stopProcessing)
|
|
return false;
|
|
if (clock() > time_out)
|
|
{
|
|
cout << "Unable to resolve tool down linking path (limit reached)." << endl;
|
|
return false;
|
|
}
|
|
|
|
cnt++;
|
|
if (cnt > limit)
|
|
{
|
|
cout << "Unable to resolve tool down linking path @(" << endPoint.X / scaleFactor << "," << endPoint.Y / scaleFactor << ") (" << limit << " points limit reached)." << endl;
|
|
return false;
|
|
}
|
|
pair<IntPoint, IntPoint> pointPair = queue.back();
|
|
queue.pop_back();
|
|
|
|
// check for self intersections - if found discard the link path
|
|
for (size_t i = 0; i < linkPaths.size(); i++)
|
|
{
|
|
if (linkPaths[i].front() != pointPair.first && linkPaths[i].back() != pointPair.first && linkPaths[i].front() != pointPair.second && linkPaths[i].back() != pointPair.second && IntersectionPoint(linkPaths[i].front(), linkPaths[i].back(), pointPair.first, pointPair.second, clp))
|
|
{
|
|
cout << "Unable to resolve tool down linking path (self-intersects)." << endl;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
DoublePoint direction = DirectionV(pointPair.first, pointPair.second);
|
|
checkPath.clear();
|
|
if (pointPair.first == startPoint)
|
|
{
|
|
checkPath.push_back(IntPoint(pointPair.first.X + offClearance * direction.X, pointPair.first.Y + offClearance * direction.Y));
|
|
}
|
|
else
|
|
{
|
|
checkPath.push_back(pointPair.first);
|
|
}
|
|
if (pointPair.second == endPoint)
|
|
{
|
|
checkPath.push_back(IntPoint(pointPair.second.X - offClearance * direction.X, pointPair.second.Y - offClearance * direction.Y));
|
|
}
|
|
else
|
|
{
|
|
checkPath.push_back(pointPair.second);
|
|
}
|
|
|
|
if (IsClearPath(checkPath, clearedArea, clearance))
|
|
{
|
|
totalLength += sqrt(DistanceSqrd(pointPair.first, pointPair.second));
|
|
if (totalLength > keepToolDownDistRatio * directDistance)
|
|
{
|
|
return false;
|
|
}
|
|
Path link;
|
|
link.push_back(pointPair.first);
|
|
link.push_back(pointPair.second);
|
|
linkPaths.push_back(link);
|
|
}
|
|
else
|
|
{
|
|
if (sqrt(DistanceSqrd(pointPair.first, pointPair.second)) < 4)
|
|
{
|
|
//segment became too short but still not clear
|
|
return false;
|
|
}
|
|
DoublePoint pDir(-direction.Y, direction.X);
|
|
// find mid point
|
|
IntPoint midPoint(0.5 * double(pointPair.first.X + pointPair.second.X), 0.5 * double(pointPair.first.Y + pointPair.second.Y));
|
|
for (long i = 1;; i++)
|
|
{
|
|
if (stopProcessing)
|
|
return false;
|
|
double offset = i * scanStep;
|
|
IntPoint checkPoint1(midPoint.X + offset * pDir.X, midPoint.Y + offset * pDir.Y);
|
|
IntPoint checkPoint2(midPoint.X - offset * pDir.X, midPoint.Y - offset * pDir.Y);
|
|
|
|
if (DistancePointToPathsSqrd(clearedArea.GetCleared(), checkPoint1, clp, pindex, sindex, par) <
|
|
DistancePointToPathsSqrd(clearedArea.GetCleared(), checkPoint2, clp, pindex, sindex, par))
|
|
{
|
|
// exchange points
|
|
IntPoint tmp = checkPoint2;
|
|
checkPoint2 = checkPoint1;
|
|
checkPoint1 = tmp;
|
|
}
|
|
|
|
checkPath.clear();
|
|
checkPath.push_back(checkPoint1);
|
|
if (IsClearPath(checkPath, clearedArea, clearance + 1))
|
|
{ // check if point clear
|
|
queue.push_back(pair<IntPoint, IntPoint>(pointPair.first, checkPoint1));
|
|
queue.push_back(pair<IntPoint, IntPoint>(checkPoint1, pointPair.second));
|
|
break;
|
|
}
|
|
else
|
|
{ // check the other side
|
|
|
|
checkPath.clear();
|
|
checkPath.push_back(checkPoint2);
|
|
if (IsClearPath(checkPath, clearedArea, clearance + 1))
|
|
{
|
|
queue.push_back(pair<IntPoint, IntPoint>(pointPair.first, checkPoint2));
|
|
queue.push_back(pair<IntPoint, IntPoint>(checkPoint2, pointPair.second));
|
|
break;
|
|
}
|
|
}
|
|
if (offset > keepToolDownDistRatio * directDistance)
|
|
return false; // can't find keep tool down link
|
|
}
|
|
}
|
|
}
|
|
if (linkPaths.empty())
|
|
return false;
|
|
ConnectPaths(linkPaths, linkPaths);
|
|
output = linkPaths[0];
|
|
return true;
|
|
}
|
|
|
|
bool Adaptive2d::MakeLeadPath(bool leadIn, const IntPoint &startPoint, const DoublePoint &startDir, const IntPoint &beaconPoint,
|
|
ClearedArea &clearedArea, const Paths &toolBoundPaths, Path &output)
|
|
{
|
|
IntPoint currentPoint = startPoint;
|
|
DoublePoint targetDir = DirectionV(currentPoint, beaconPoint);
|
|
double distanceToBeacon = sqrt(DistanceSqrd(startPoint, beaconPoint));
|
|
double stepSize = 0.2 * stepOverScaled + 1;
|
|
double maxPathLen = stepOverScaled;
|
|
DoublePoint nextDir = startDir;
|
|
IntPoint nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize);
|
|
Path checkPath;
|
|
double adaptFactor = 0.4;
|
|
double alfa = M_PI / 64;
|
|
double pathLen = 0;
|
|
checkPath.push_back(nextPoint);
|
|
for (int i = 0; i < 10000; i++)
|
|
{
|
|
if (IsAllowedToCutTrough(IntPoint(currentPoint.X + RESOLUTION_FACTOR * nextDir.X, currentPoint.Y + RESOLUTION_FACTOR * nextDir.Y), nextPoint, clearedArea, toolBoundPaths))
|
|
{
|
|
if (output.empty())
|
|
output.push_back(currentPoint);
|
|
output.push_back(nextPoint);
|
|
currentPoint = nextPoint;
|
|
pathLen += stepSize;
|
|
targetDir = DirectionV(currentPoint, beaconPoint);
|
|
nextDir = DoublePoint(nextDir.X + adaptFactor * targetDir.X, nextDir.Y + adaptFactor * targetDir.Y);
|
|
NormalizeV(nextDir);
|
|
if (pathLen > maxPathLen)
|
|
break;
|
|
if (pathLen > distanceToBeacon / 2)
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
nextDir = rotate(nextDir, leadIn ? -alfa : alfa);
|
|
}
|
|
nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize);
|
|
}
|
|
if (output.empty())
|
|
output.push_back(startPoint);
|
|
return true;
|
|
}
|
|
void Adaptive2d::AppendToolPath(TPaths &progressPaths, AdaptiveOutput &output,
|
|
const Path &passToolPath, ClearedArea &clearedBefore,
|
|
ClearedArea &clearedAfter, const Paths &toolBoundPaths)
|
|
{
|
|
if (passToolPath.size() < 2)
|
|
return;
|
|
Perf_AppendToolPath.Start();
|
|
UNUSED(progressPaths); // to silence compiler warning,var is occasionally used in dev. for debugging
|
|
|
|
IntPoint endPoint(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() > 1)
|
|
{
|
|
auto &lastTPath = output.AdaptivePaths.back();
|
|
|
|
auto &lastPrevTPoint = lastTPath.second.at(lastTPath.second.size() - 2);
|
|
auto &lastTPoint = lastTPath.second.back();
|
|
|
|
IntPoint startPrevPoint(long(lastPrevTPoint.first * scaleFactor), long(lastPrevTPoint.second * scaleFactor));
|
|
IntPoint startPoint(long(lastTPoint.first * scaleFactor), long(lastTPoint.second * scaleFactor));
|
|
|
|
ClipperOffset clipof;
|
|
//first we try to cut through the linking move for short distances
|
|
bool linkFound = false;
|
|
double linkDistance = sqrt(DistanceSqrd(startPoint, endPoint));
|
|
if (linkDistance < NTOL)
|
|
linkFound = true;
|
|
|
|
if (!linkFound)
|
|
{
|
|
size_t clpPathIndex;
|
|
size_t clpSegmentIndex;
|
|
double clpParameter;
|
|
IntPoint clp;
|
|
|
|
double beaconOffset = stepOverScaled;
|
|
if (beaconOffset > linkDistance)
|
|
{
|
|
beaconOffset = linkDistance;
|
|
}
|
|
|
|
double pathLen = PathLength(passToolPath);
|
|
if (beaconOffset > pathLen / 2)
|
|
{
|
|
beaconOffset = pathLen / 2;
|
|
}
|
|
if (beaconOffset > linkDistance / 2)
|
|
{
|
|
beaconOffset = linkDistance / 2;
|
|
}
|
|
|
|
DistancePointToPathsSqrd(toolBoundPaths, startPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter);
|
|
DoublePoint startDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);
|
|
|
|
DistancePointToPathsSqrd(toolBoundPaths, endPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter);
|
|
DoublePoint endDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);
|
|
|
|
IntPoint startBeacon(startPoint.X - beaconOffset * (startDir.Y - startDir.X), startPoint.Y + beaconOffset * (startDir.X + startDir.Y));
|
|
IntPoint endBeacon(endPoint.X - beaconOffset * (endDir.X + endDir.Y), endPoint.Y + beaconOffset * (endDir.X - endDir.Y));
|
|
Path leadOutPath;
|
|
MakeLeadPath(false, startPoint, startDir, startBeacon, clearedBefore, toolBoundPaths, leadOutPath);
|
|
|
|
Path leadInPath;
|
|
MakeLeadPath(true, endPoint, DoublePoint(-endDir.X, -endDir.Y), endBeacon, clearedBefore, toolBoundPaths, leadInPath);
|
|
ReversePath(leadInPath);
|
|
|
|
Path linkPath;
|
|
MotionType linkType = MotionType::mtCutting;
|
|
|
|
// this is not needed:
|
|
//clearedBefore.ExpandCleared(leadInPath);
|
|
//clearedBefore.ExpandCleared(leadOutPath);
|
|
|
|
if (ResolveLinkPath(leadOutPath.back(), leadInPath.front(), clearedBefore, linkPath))
|
|
{
|
|
linkType = MotionType::mtLinkClear;
|
|
double remainingLeadInExtension = stepOverScaled / 2;
|
|
while (linkPath.size() >= 2 && remainingLeadInExtension > NTOL)
|
|
{
|
|
IntPoint p1 = linkPath.at(linkPath.size() - 2);
|
|
IntPoint p2 = linkPath.at(linkPath.size() - 1);
|
|
double l = sqrt(DistanceSqrd(p1, p2));
|
|
if (l >= remainingLeadInExtension)
|
|
{
|
|
IntPoint splitPoint(p1.X + (p2.X - p1.X) * (l - remainingLeadInExtension) / l, p1.Y + (p2.Y - p1.Y) * (l - remainingLeadInExtension) / l);
|
|
linkPath.pop_back();
|
|
linkPath.push_back(splitPoint);
|
|
leadInPath.insert(leadInPath.begin(), splitPoint);
|
|
remainingLeadInExtension = 0;
|
|
Path checkPath;
|
|
checkPath.push_back(p2);
|
|
checkPath.push_back(splitPoint);
|
|
if (!IsClearPath(checkPath, clearedBefore, 0))
|
|
{
|
|
remainingLeadInExtension = stepOverScaled / 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
linkPath.pop_back();
|
|
leadInPath.insert(leadInPath.begin(), p1);
|
|
remainingLeadInExtension -= l;
|
|
if (remainingLeadInExtension < NTOL)
|
|
{
|
|
Path checkPath;
|
|
checkPath.push_back(p2);
|
|
checkPath.push_back(p1);
|
|
if (!IsClearPath(checkPath, clearedBefore, 0))
|
|
{
|
|
remainingLeadInExtension = stepOverScaled / 2;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
linkType = MotionType::mtLinkNotClear;
|
|
double dist = sqrt(DistanceSqrd(leadOutPath.back(), leadInPath.front()));
|
|
if (dist < 2 * stepOverScaled && IsAllowedToCutTrough(
|
|
IntPoint(leadOutPath.back().X + (leadInPath.front().X - leadOutPath.back().X) / dist, leadOutPath.back().Y + (leadInPath.front().Y - leadOutPath.back().Y) / dist),
|
|
IntPoint(leadInPath.front().X - (leadInPath.front().X - leadOutPath.back().X) / dist, leadInPath.front().Y - (leadInPath.front().Y - leadOutPath.back().Y) / dist),
|
|
clearedBefore, toolBoundPaths))
|
|
linkType = MotionType::mtCutting;
|
|
// add direct linking move at clear height
|
|
|
|
linkPath.clear();
|
|
linkPath.push_back(leadOutPath.back());
|
|
linkPath.push_back(leadInPath.front());
|
|
}
|
|
|
|
/* paths smoothing*/
|
|
Paths linkPaths;
|
|
linkPaths.push_back(leadOutPath);
|
|
linkPaths.push_back(linkPath);
|
|
linkPaths.push_back(leadInPath);
|
|
|
|
if (linkType == MotionType::mtLinkClear)
|
|
{
|
|
SmoothPaths(linkPaths, 0.1*stepOverScaled,1,4);
|
|
}
|
|
|
|
leadOutPath = linkPaths[0];
|
|
linkPath = linkPaths[1];
|
|
leadInPath = linkPaths[2];
|
|
|
|
// add lead-out move
|
|
TPath linkPath1;
|
|
linkPath1.first = MotionType::mtCutting;
|
|
for (const auto &pt : leadOutPath)
|
|
{
|
|
linkPath1.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
}
|
|
output.AdaptivePaths.push_back(linkPath1);
|
|
|
|
// add linking path
|
|
TPath linkPath2;
|
|
linkPath2.first = linkType;
|
|
for (const auto &pt : linkPath)
|
|
{
|
|
linkPath2.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
}
|
|
output.AdaptivePaths.push_back(linkPath2);
|
|
|
|
// add lead-in move
|
|
TPath linkPath3;
|
|
linkPath3.first = MotionType::mtCutting;
|
|
for (const auto &pt : leadInPath)
|
|
{
|
|
linkPath3.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
}
|
|
|
|
output.AdaptivePaths.push_back(linkPath3);
|
|
|
|
clearedAfter.ExpandCleared(leadInPath);
|
|
clearedAfter.ExpandCleared(leadOutPath);
|
|
|
|
linkFound = true;
|
|
}
|
|
if (!linkFound)
|
|
{ // nothing clear so far - check direct link with no interim points - either this is clear or we need to raise the tool
|
|
Path tp;
|
|
tp << startPoint;
|
|
tp << endPoint;
|
|
MotionType mt = IsClearPath(tp, clearedBefore) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear;
|
|
|
|
// make cutting move through small clear links
|
|
if (mt == MotionType::mtLinkClear && linkDistance < toolRadiusScaled)
|
|
{
|
|
mt = MotionType::mtCutting;
|
|
clearedAfter.ExpandCleared(tp);
|
|
}
|
|
|
|
TPath linkPath;
|
|
linkPath.first = mt;
|
|
linkPath.second.push_back(DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor));
|
|
linkPath.second.push_back(DPoint(double(endPoint.X) / scaleFactor, double(endPoint.Y) / scaleFactor));
|
|
output.AdaptivePaths.push_back(linkPath);
|
|
}
|
|
}
|
|
TPath cutPath;
|
|
cutPath.first = MotionType::mtCutting;
|
|
for (const auto &p : passToolPath)
|
|
{
|
|
DPoint nextT;
|
|
nextT.first = double(p.X) / scaleFactor;
|
|
nextT.second = double(p.Y) / scaleFactor;
|
|
cutPath.second.push_back(nextT);
|
|
}
|
|
|
|
if (cutPath.second.size() > 0)
|
|
output.AdaptivePaths.push_back(cutPath);
|
|
Perf_AppendToolPath.Stop();
|
|
}
|
|
|
|
void Adaptive2d::CheckReportProgress(TPaths &progressPaths, bool force)
|
|
{
|
|
if (!force && (clock() - lastProgressTime < PROGRESS_TICKS))
|
|
return; // not yet
|
|
lastProgressTime = clock();
|
|
if (progressPaths.size() == 0)
|
|
return;
|
|
if (progressCallback)
|
|
if ((*progressCallback)(progressPaths))
|
|
stopProcessing = true; // call python function, if returns true signal stop processing
|
|
// clean the paths - keep the last point
|
|
if (progressPaths.back().second.size() == 0)
|
|
return;
|
|
TPath *lastPath = &progressPaths.back();
|
|
DPoint *lastPoint = &lastPath->second.back();
|
|
DPoint next(lastPoint->first, lastPoint->second);
|
|
while (progressPaths.size() > 1)
|
|
progressPaths.pop_back();
|
|
while (progressPaths.front().second.size() > 0)
|
|
progressPaths.front().second.pop_back();
|
|
progressPaths.front().first = MotionType::mtCutting;
|
|
progressPaths.front().second.push_back(next);
|
|
}
|
|
|
|
void Adaptive2d::AddPathsToProgress(TPaths &progressPaths, Paths paths, MotionType mt)
|
|
{
|
|
for (const auto &pth : paths)
|
|
{
|
|
if (pth.size() > 0)
|
|
{
|
|
progressPaths.push_back(TPath());
|
|
progressPaths.back().first = mt;
|
|
for (const auto pt : pth)
|
|
progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
progressPaths.back().second.push_back(DPoint(double(pth.front().X) / scaleFactor, double(pth.front().Y) / scaleFactor));
|
|
}
|
|
}
|
|
}
|
|
|
|
void Adaptive2d::AddPathToProgress(TPaths &progressPaths, const Path pth, MotionType mt)
|
|
{
|
|
if (pth.size() > 0)
|
|
{
|
|
progressPaths.push_back(TPath());
|
|
progressPaths.back().first = mt;
|
|
for (const auto pt : pth)
|
|
progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
}
|
|
}
|
|
|
|
void Adaptive2d::ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths)
|
|
{
|
|
Perf_ProcessPolyNode.Start();
|
|
current_region++;
|
|
cout << "** Processing region: " << current_region << endl;
|
|
|
|
// 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);
|
|
|
|
CleanPolygons(toolBoundPaths);
|
|
SimplifyPolygons(toolBoundPaths);
|
|
|
|
CleanPolygons(boundPaths);
|
|
SimplifyPolygons(boundPaths);
|
|
|
|
AddPathsToProgress(progressPaths, toolBoundPaths, MotionType::mtLinkClear);
|
|
|
|
IntPoint toolPos;
|
|
DoublePoint toolDir;
|
|
ClearedArea cleared(toolRadiusScaled);
|
|
bool outsideEntry = false;
|
|
bool firstEngagePoint = true;
|
|
Paths engageBounds = toolBoundPaths;
|
|
|
|
if (!forceInsideOut && FindEntryPointOutside(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir))
|
|
{
|
|
if (Orientation(engageBounds[0]) == false)
|
|
ReversePath(engageBounds[0]);
|
|
// add initial offset of cleared area to engage paths
|
|
Paths outsideEngage;
|
|
clipof.Clear();
|
|
clipof.AddPaths(stockInputPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(outsideEngage, toolRadiusScaled - stepOverFactor * toolRadiusScaled);
|
|
CleanPolygons(outsideEngage);
|
|
ReversePaths(outsideEngage);
|
|
for (auto p : outsideEngage)
|
|
engageBounds.push_back(p);
|
|
outsideEntry = true;
|
|
}
|
|
else
|
|
{
|
|
if (!FindEntryPoint(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir))
|
|
{
|
|
Perf_ProcessPolyNode.Stop();
|
|
return;
|
|
}
|
|
}
|
|
|
|
EngagePoint engage(engageBounds); // engage point stepping instance
|
|
|
|
if (outsideEntry)
|
|
{
|
|
engage.moveToClosestPoint(toolPos, 2 * RESOLUTION_FACTOR);
|
|
engage.moveForward(RESOLUTION_FACTOR);
|
|
toolPos = engage.getCurrentPoint();
|
|
toolDir = engage.getCurrentDir();
|
|
entryPoint = toolPos;
|
|
}
|
|
|
|
//cout << "Entry point:" << double(entryPoint.X)/scaleFactor << "," << double(entryPoint.Y)/scaleFactor << endl;
|
|
|
|
AdaptiveOutput output;
|
|
output.HelixCenterPoint.first = double(entryPoint.X) / scaleFactor;
|
|
output.HelixCenterPoint.second = double(entryPoint.Y) / scaleFactor;
|
|
|
|
long stepScaled = long(RESOLUTION_FACTOR);
|
|
IntPoint engagePoint;
|
|
|
|
IntPoint newToolPos;
|
|
DoublePoint newToolDir;
|
|
|
|
CheckReportProgress(progressPaths, true);
|
|
|
|
IntPoint startPoint = toolPos;
|
|
output.StartPoint = DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor);
|
|
|
|
Path passToolPath; // to store pass toolpath
|
|
Path toClearPath;
|
|
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;
|
|
double prevDistFromStart = 0;
|
|
double refinement_factor = 1;
|
|
bool prevDistTrend = false;
|
|
|
|
double perf_total_len = 0;
|
|
#ifdef DEV_MODE
|
|
clock_t start_clock = clock();
|
|
#endif
|
|
ClearedArea clearedBeforePass(toolRadiusScaled);
|
|
clearedBeforePass.SetClearedPaths(cleared.GetCleared());
|
|
|
|
//*******************************
|
|
// LOOP - PASSES
|
|
//*******************************
|
|
for (long pass = 0; pass < PASSES_LIMIT; pass++)
|
|
{
|
|
if (stopProcessing)
|
|
break;
|
|
|
|
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 recalcArea = 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;
|
|
double passLength = 0;
|
|
double noCutDistance=0;
|
|
clearedBeforePass.SetClearedPaths(cleared.GetCleared());
|
|
//*******************************
|
|
// LOOP - POINTS
|
|
//*******************************
|
|
for (long point_index = 0; point_index < POINTS_PER_PASS_LIMIT; point_index++)
|
|
{
|
|
if (stopProcessing)
|
|
break;
|
|
|
|
total_points++;
|
|
AverageDirection(gyro, toolDir);
|
|
Perf_DistanceToBoundary.Start();
|
|
|
|
double distanceToBoundary = sqrt(DistancePointToPathsSqrd(toolBoundPaths, toolPos, clp, clpPathIndex, clpSegmentIndex, clpParamter));
|
|
DoublePoint boundaryDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);
|
|
double distBoundaryPointToEngage = sqrt(DistanceSqrd(clp, engagePoint));
|
|
|
|
Perf_DistanceToBoundary.Stop();
|
|
double distanceToEngage = sqrt(DistanceSqrd(toolPos, engagePoint));
|
|
|
|
double targetAreaPD = optimalCutAreaPD;
|
|
|
|
// set the step size
|
|
double slowDownDistance = max(double(toolRadiusScaled) / 4, RESOLUTION_FACTOR * 8);
|
|
if (distanceToBoundary < slowDownDistance || distanceToEngage < slowDownDistance)
|
|
{
|
|
stepScaled = long(RESOLUTION_FACTOR);
|
|
}
|
|
else if (fabs(angle) > NTOL)
|
|
{
|
|
stepScaled = long(RESOLUTION_FACTOR / fabs(angle));
|
|
}
|
|
else
|
|
{
|
|
stepScaled = long(RESOLUTION_FACTOR * 4);
|
|
}
|
|
|
|
// clamp the step size - for stability
|
|
if (stepScaled > min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8)))
|
|
stepScaled = min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8));
|
|
if (stepScaled < RESOLUTION_FACTOR)
|
|
stepScaled = long(RESOLUTION_FACTOR);
|
|
|
|
//*****************************
|
|
// 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();
|
|
|
|
recalcArea = false;
|
|
// approach end boundary tangentially
|
|
double relDistToBoundary = 4 * distanceToBoundary / stepOverScaled;
|
|
if (relDistToBoundary <= 1.0 && passLength > 2 * stepOverFactor && distanceToEngage > 2 * stepOverScaled && distBoundaryPointToEngage > 2 * stepOverScaled)
|
|
{
|
|
double wb = 1 - relDistToBoundary;
|
|
newToolDir = DoublePoint(newToolDir.X + wb * boundaryDir.X, newToolDir.Y + wb * boundaryDir.Y);
|
|
NormalizeV(newToolDir);
|
|
newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled));
|
|
recalcArea = true;
|
|
}
|
|
|
|
//**********************************************
|
|
// CHECK AND RECORD NEW TOOL POS
|
|
//**********************************************
|
|
long rotateStep = 0;
|
|
while (!IsPointWithinCutRegion(toolBoundPaths, newToolPos) && rotateStep < 180)
|
|
{
|
|
rotateStep++;
|
|
// if new tool pos. outside boundary rotate until back in
|
|
recalcArea = true;
|
|
newToolDir = rotate(newToolDir, M_PI / 90);
|
|
newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled));
|
|
}
|
|
if (rotateStep >= 180)
|
|
{
|
|
#ifdef DEV_MODE
|
|
cerr << "Warning: unexpected number of rotate iterations." << endl;
|
|
#endif
|
|
break;
|
|
}
|
|
|
|
if (recalcArea)
|
|
{
|
|
area = CalcCutArea(clip, toolPos, newToolPos, cleared);
|
|
}
|
|
|
|
// safety condition
|
|
if (area > stepScaled * optimalCutAreaPD && areaPD > 2 * optimalCutAreaPD)
|
|
{
|
|
over_cut_count++;
|
|
break;
|
|
}
|
|
|
|
// update cleared paths when trend of distance from start point changes sign (starts to get closer, or start to get farther)
|
|
double distFromStart = sqrt(DistanceSqrd(toolPos, startPoint));
|
|
bool distanceTrend = distFromStart > prevDistFromStart ? true : false;
|
|
|
|
if (distanceTrend != prevDistTrend)
|
|
{
|
|
cleared.ExpandCleared(toClearPath);
|
|
toClearPath.clear();
|
|
}
|
|
prevDistTrend = distanceTrend;
|
|
prevDistFromStart = distFromStart;
|
|
|
|
if (area > 0.5 * MIN_CUT_AREA_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR)
|
|
{ // cut is ok - record it
|
|
noCutDistance=0;
|
|
if (toClearPath.size() == 0)
|
|
toClearPath.push_back(toolPos);
|
|
toClearPath.push_back(newToolPos);
|
|
|
|
cumulativeCutArea += area;
|
|
|
|
// append to toolpaths
|
|
if (passToolPath.size() == 0)
|
|
{
|
|
// in outside entry first successful cut defines the "helix center" and start point
|
|
// in this case helix diameter is 0 (straight line downwards)
|
|
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;
|
|
passLength += 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));
|
|
|
|
// append 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
|
|
//cout<<"Break: no cut @" << point_index << endl;
|
|
if(noCutDistance>stepOverScaled) break;
|
|
noCutDistance+=stepScaled;
|
|
}
|
|
} /* end of points loop*/
|
|
|
|
if (toClearPath.size() > 0)
|
|
{
|
|
cleared.ExpandCleared(toClearPath);
|
|
toClearPath.clear();
|
|
}
|
|
if (cumulativeCutArea > MIN_CUT_AREA_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR)
|
|
{
|
|
Path cleaned;
|
|
CleanPath(passToolPath, cleaned, CLEAN_PATH_TOLERANCE);
|
|
total_output_points += long(cleaned.size());
|
|
AppendToolPath(progressPaths, output, cleaned, clearedBeforePass, cleared, toolBoundPaths);
|
|
CheckReportProgress(progressPaths);
|
|
bad_engage_count = 0;
|
|
engage.ResetPasses();
|
|
}
|
|
else
|
|
{
|
|
bad_engage_count++;
|
|
}
|
|
|
|
if (bad_engage_count > 10000)
|
|
{
|
|
cerr << "Break (next valid engage point not found)." << endl;
|
|
break;
|
|
}
|
|
|
|
/*****NEXT ENGAGE POINT******/
|
|
if (firstEngagePoint)
|
|
{
|
|
engage.moveToClosestPoint(newToolPos, stepScaled + 1);
|
|
firstEngagePoint = false;
|
|
}
|
|
else
|
|
{
|
|
double moveDistance = ENGAGE_SCAN_DISTANCE_FACTOR * stepOverScaled * refinement_factor;
|
|
|
|
if (!engage.nextEngagePoint(this, cleared, moveDistance,
|
|
ENGAGE_AREA_THR_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR,
|
|
4 * referenceCutArea * stepOverFactor))
|
|
{
|
|
// check if there are any uncleared area left
|
|
Paths remaining;
|
|
for (const auto &p : cleared.GetCleared())
|
|
{
|
|
if (!p.empty() && IsPointWithinCutRegion(toolBoundPaths, p.front()) && DistancePointToPathsSqrd(boundPaths, p.front(), clp, clpPathIndex, clpSegmentIndex, clpParamter) > 4 * toolRadiusScaled * toolRadiusScaled)
|
|
{
|
|
remaining.push_back(p);
|
|
}
|
|
};
|
|
if (remaining.empty())
|
|
{
|
|
cout << "All cleared." << endl;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
cout << "Clearing " << remaining.size() << " remaining internal path(s)." << endl;
|
|
}
|
|
|
|
// try to find new engage point along the remaining
|
|
clipof.Clear();
|
|
clipof.AddPaths(remaining, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(remaining, toolRadiusScaled - 0.5 * stepOverScaled);
|
|
|
|
ReversePaths(remaining);
|
|
engage.SetPaths(remaining);
|
|
engage.moveToClosestPoint(newToolPos, stepScaled + 1);
|
|
if (!engage.nextEngagePoint(this, cleared, moveDistance,
|
|
ENGAGE_AREA_THR_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR,
|
|
4 * referenceCutArea * stepOverFactor))
|
|
break;
|
|
}
|
|
}
|
|
toolPos = engage.getCurrentPoint();
|
|
toolDir = engage.getCurrentDir();
|
|
}
|
|
|
|
//**********************************
|
|
//* FINISHING PASS *
|
|
//**********************************
|
|
|
|
Paths finishingPaths;
|
|
clipof.Clear();
|
|
clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(finishingPaths, -toolRadiusScaled);
|
|
|
|
clipof.Clear();
|
|
clipof.AddPaths(finishingPaths, JoinType::jtRound, EndType::etClosedPolygon);
|
|
clipof.Execute(toolBoundPaths, -1);
|
|
|
|
IntPoint lastPoint = toolPos;
|
|
Path finShiftedPath;
|
|
|
|
bool allCutsAllowed = true;
|
|
while (!stopProcessing && PopPathWithClosestPoint(finishingPaths, lastPoint, finShiftedPath))
|
|
{
|
|
if (finShiftedPath.empty())
|
|
continue;
|
|
// skip finishing passes outside the stock boundary - no sense to cut where is no material
|
|
bool allPointsOutside = true;
|
|
IntPoint p1 = finShiftedPath.front();
|
|
for (const auto &pt : finShiftedPath)
|
|
{
|
|
|
|
// midpoint
|
|
if (IsPointWithinCutRegion(stockInputPaths, IntPoint((p1.X + pt.X) / 2, (p1.Y + pt.Y) / 2)))
|
|
{
|
|
allPointsOutside = false;
|
|
break;
|
|
}
|
|
//current point
|
|
if (IsPointWithinCutRegion(stockInputPaths, pt))
|
|
{
|
|
allPointsOutside = false;
|
|
break;
|
|
}
|
|
|
|
p1 = pt;
|
|
}
|
|
if (allPointsOutside)
|
|
continue;
|
|
|
|
progressPaths.push_back(TPath());
|
|
// show in progress cb
|
|
for (auto &pt : finShiftedPath)
|
|
{
|
|
progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
|
|
}
|
|
|
|
if (!finShiftedPath.empty())
|
|
finShiftedPath << finShiftedPath.front(); // make sure its closed
|
|
|
|
Path finCleaned;
|
|
CleanPath(finShiftedPath, finCleaned, FINISHING_CLEAN_PATH_TOLERANCE);
|
|
|
|
// sanity check for finishing paths - check the area of finishing cut
|
|
for (size_t i = 1; i < finCleaned.size(); i++)
|
|
{
|
|
if (!IsAllowedToCutTrough(finCleaned.at(i - 1), finCleaned.at(i), cleared, toolBoundPaths, 2.0, true))
|
|
{
|
|
allCutsAllowed = false;
|
|
}
|
|
}
|
|
|
|
// make sure it's closed
|
|
finCleaned.push_back(finCleaned.front());
|
|
AppendToolPath(progressPaths, output, finCleaned, cleared, cleared, toolBoundPaths);
|
|
|
|
cleared.ExpandCleared(finCleaned);
|
|
|
|
if (!finCleaned.empty())
|
|
{
|
|
lastPoint.X = finCleaned.back().X;
|
|
lastPoint.Y = finCleaned.back().Y;
|
|
}
|
|
}
|
|
|
|
Path returnPath;
|
|
returnPath << lastPoint;
|
|
returnPath << entryPoint;
|
|
output.ReturnMotionType = IsClearPath(returnPath, cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear;
|
|
|
|
// dump performance results
|
|
#ifdef DEV_MODE
|
|
Perf_ProcessPolyNode.Stop();
|
|
Perf_ProcessPolyNode.DumpResults();
|
|
Perf_PointIterations.DumpResults();
|
|
Perf_CalcCutAreaCirc.DumpResults();
|
|
Perf_CalcCutAreaClip.DumpResults();
|
|
Perf_NextEngagePoint.DumpResults();
|
|
Perf_ExpandCleared.DumpResults();
|
|
Perf_DistanceToBoundary.DumpResults();
|
|
Perf_AppendToolPath.DumpResults();
|
|
Perf_IsAllowedToCutTrough.DumpResults();
|
|
Perf_IsClearPath.DumpResults();
|
|
#endif
|
|
CheckReportProgress(progressPaths, true);
|
|
#ifdef DEV_MODE
|
|
double duration = ((double)(clock() - start_clock)) / CLOCKS_PER_SEC;
|
|
cout << "PolyNode perf:" << perf_total_len / double(scaleFactor) / duration << " mm/sec"
|
|
<< " processed_points:" << total_points
|
|
<< " output_points:" << total_output_points
|
|
<< " total_iterations:" << total_iterations
|
|
<< " iter_per_point:" << (double(total_iterations) / ((double(total_points) + 0.001)))
|
|
<< " total_exceeded:" << total_exceeded << " (" << 100 * double(total_exceeded) / double(total_points) << "%)"
|
|
<< endl;
|
|
#endif
|
|
|
|
// warn about invalid paths being detected
|
|
if (!allCutsAllowed)
|
|
{
|
|
cerr << "Warning: some cuts may be above optimal step-over. Please double check the results." << endl
|
|
<< "Hint: try to modify accuracy and/or step-over." << endl;
|
|
}
|
|
|
|
results.push_back(output);
|
|
}
|
|
|
|
} // namespace AdaptivePath
|