private static void calcOffsetLine(ref xyPoint[] offset, xyArcPoint P0, xyArcPoint P1, double angle, double radius)
{
offset[0].X = P0.X + Math.Cos(angle) * radius; // 1st point of 1st line
offset[0].Y = P0.Y + Math.Sin(angle) * radius;
offset[1].X = P1.X + Math.Cos(angle) * radius; // 2nd point of 1st line
offset[1].Y = P1.Y + Math.Sin(angle) * radius;
}
private static bool calcIntersectionLineArc(ref xyPoint[] resultOffset, xyPoint[] linePoint, xyArcPoint arc, double radius) // return success
{ // circular equation: r^2 = (x-xm)^2 + (y - ym)^2 = r^2 => y = ym ± √(r2 - (x-xm)2)
// linear equation: y = m*x + n
double x = 0, x1 = 0, x2 = 0, y = 0, y1 = 0, y2 = 0;
double dx = (linePoint[1].X - linePoint[0].X);
double dy = (linePoint[1].Y - linePoint[0].Y);
#if (debuginfo)
log.Add(string.Format(" calcIntersectionLineArc 0x {0:0.00} 0y {1:0.00} 1x {2:0.00} 1y {3:0.00} Arcx {4:0.00} Arcy {5:0.00} ArcCx {6:0.00} ArcCy {7:0.00}", linePoint[0].X, linePoint[0].Y, linePoint[1].X, linePoint[1].Y, arc.X, arc.Y, arc.CX, arc.CY));
log.Add(string.Format(" dx {0:0.00} dy {1:0.00} ", dx, dy));
#endif
if (dx == 0) // vertical line, x is known
{
double a2minusb2 = getA2minusB2(radius, (linePoint[0].X - arc.CX));
if (a2minusb2 >= 0)
{
double tmpRoot = Math.Sqrt(a2minusb2); // getRoot(radius, (linePoint[0].X - arc.CX));
y1 = arc.CY + tmpRoot; // y = ym ± √(r2 - (x-xm)2)
y2 = arc.CY - tmpRoot;
y = y1;
if (Math.Abs(linePoint[1].Y - y2) < Math.Abs(linePoint[1].Y - y1)) // find closer point
{
y = y2;
}
resultOffset[1].X = linePoint[0].X;
resultOffset[1].Y = y;
resultOffset[2] = resultOffset[1];
#if (debuginfo)
log.Add(string.Format(" intersection at x{0:0.000} y{1:0.000}", resultOffset[1].X, resultOffset[1].Y));
#endif
return(true);
}
else
{
resultOffset[1].X = linePoint[0].X;
resultOffset[1].Y = resultOffset[3].Y;// arc.Y-radius;
resultOffset[2] = resultOffset[1];
#if (debuginfo)
log.Add(" no intersection! ");
#endif
return(false);
}
}
else if (dy == 0) // horizontal line, y is known
{
double a2minusb2 = getA2minusB2(radius, (linePoint[0].Y - arc.CY));
if (a2minusb2 >= 0)
{
double tmpRoot = Math.Sqrt(a2minusb2);
x1 = arc.CX + tmpRoot; // getRoot(radius, (linePoint[0].Y - arc.CY)); // y = ym ± √(r2 - (x-xm)2)
x2 = arc.CX - tmpRoot; // getRoot(radius, (linePoint[0].Y - arc.CY));
x = x1;
if (Math.Abs(linePoint[1].X - x2) < Math.Abs(linePoint[1].X - x1))
{
x = x2;
}
resultOffset[1].X = x;
resultOffset[1].Y = linePoint[0].Y;
resultOffset[2] = resultOffset[1];
#if (debuginfo)
log.Add(string.Format(" intersection at x{0:0.000} y{1:0.000}", resultOffset[1].X, resultOffset[1].Y));
#endif
return(true);
}
else
{
resultOffset[1].X = resultOffset[2].X;// arc.X - radius;
resultOffset[1].Y = linePoint[0].Y;
resultOffset[2] = resultOffset[1];
#if (debuginfo)
log.Add(" no intersection! ");
#endif
return(false);
}
}
else
{ // intersection line-arc
// circular equation: r^2 = (x-xm)^2 + (y - ym)^2 = r^2 => y = ym ± √(r2 - (x-xm)2)
// linear equation: y = m*x + n => n = y - m*x
resultOffset[2] = resultOffset[1] = resultOffset[0];
double m = dy / dx; // y=m*x+n
double n = linePoint[1].Y - m * linePoint[1].X; // n=y-m*x
double a = 1 + m * m; // r²=(x-cx)² + (y-cy)²
double b = 2 * (m * n - arc.CX - arc.CY * m); // 0=x²-2*x*cx+cx² + y²-2*y*cy+cy²
double c = arc.CX * arc.CX + arc.CY * arc.CY + n * n - radius * radius - 2 * arc.CY * n;
double a2minusb2 = getA2minusB2((b * b), (4 * a * c));
if (a2minusb2 >= 0)
{
double root = Math.Sqrt((b * b) - (4 * a * c));
x1 = (-b + root) / (2 * a); // ax²+bx+c=0
x2 = (-b - root) / (2 * a); // x=(-b±√b²-4ac)/(2a)
x = x1;
if (Math.Abs(linePoint[1].X - x2) < Math.Abs(linePoint[1].X - x1))
{
x = x2;
}
#if (debuginfo)
log.Add(string.Format(" x1 {0:0.00} x2 {1:0.00} lp1x {2:0.00} x {3:0.00}", x1, x2, linePoint[1].X, x));
#endif
y = m * x + n;
resultOffset[1].X = x; resultOffset[1].Y = y;
resultOffset[2] = resultOffset[1];
return(true);
}
else
{
return(false);
}
}
}
{ // http://www.hinterseher.de/Diplomarbeit/GeometrischeFunktionen.html
// get two lines and calc offsetted points
public static int getPointOffsets(ref xyPoint[] offset, xyArcPoint P0, xyArcPoint P1, xyArcPoint P2, double distance, bool isEnd)
{
xyPoint[] S1off = new xyPoint[2];
xyPoint[] S2off = new xyPoint[2];
double a0 = 0, a1 = 0, a2 = 0, a3 = 0, adelta;
double newRadius1 = distance;
double newRadius2 = distance;
if (P1.mode <= 1) // is a line
{
a1 = getAlphaLine(P0, P1);
a0 = a1;
calcOffsetLine(ref S1off, P0, P1, a1 + Math.PI / 2, distance); // offset by 90°
}
else
{
a0 = getAlphaCenterToPoint(P1, P0); // from center to start
a1 = getAlphaCenterToPoint(P1, P1); // from center to end
double usea0 = a0, usea1 = a1;
a0 -= Math.PI / 2; a1 -= Math.PI / 2; // tangente
if (P1.mode == 3)
{
usea0 += Math.PI; usea1 += Math.PI; // add 180°
a0 += Math.PI; a1 += Math.PI; // tangente reverse
}
S1off[0] = calcOffsetPoint(P0, usea0, distance); // extend radius
S1off[1] = calcOffsetPoint(P1, usea1, distance); // extend radius
newRadius1 = Math.Sqrt((P1.CX - S1off[0].X) * (P1.CX - S1off[0].X) + (P1.CY - S1off[0].Y) * (P1.CY - S1off[0].Y));
}
offset[0] = S1off[0];
offset[1] = S1off[1];
#if (debuginfo)
log.Add(string.Format(" getPointOffsets P0-P1: P1mode: {0} S1offX {1:0.000} S1offX {2:0.000} S1offX {3:0.000} S1offX {4:0.000}", P1.mode, S1off[0].X, S1off[0].Y, S1off[1].X, S1off[1].Y));
#endif
if (P2.mode <= 1) // is a line
{
a2 = getAlphaLine(P1, P2);
a3 = a2;
calcOffsetLine(ref S2off, P1, P2, a2 + Math.PI / 2, distance); // offset by 90°
}
else
{
a2 = getAlphaCenterToPoint(P2, P1); // from center to start
a3 = getAlphaCenterToPoint(P2, P2); // from center to end
double usea2 = a2, usea3 = a3;
a2 -= Math.PI / 2; a3 -= Math.PI / 2; // tangente
if (P2.mode == 3)
{
usea2 += Math.PI; usea3 += Math.PI; // add 180°
a2 += Math.PI; a3 += Math.PI; // tangente reverse
}
S2off[0] = calcOffsetPoint(P1, usea2, distance); // extend radius
S2off[1] = calcOffsetPoint(P2, usea3, distance); // extend radius
newRadius2 = Math.Sqrt((P2.CX - S2off[0].X) * (P2.CX - S2off[0].X) + (P2.CY - S2off[0].Y) * (P2.CY - S2off[0].Y));
}
offset[2] = S2off[0];
offset[3] = S2off[1];
#if (debuginfo)
log.Add(string.Format(" getPointOffsets P1-P2: P2mode: {0} S1offX {1:0.000} S1offX {2:0.000} S1offX {3:0.000} S1offX {4:0.000}", P2.mode, S2off[0].X, S2off[0].Y, S2off[1].X, S2off[1].Y));
#endif
if ((P1.mode == P2.mode) && (P1.X == P2.X) && (P1.Y == P2.Y))
{
a2 = a0; a3 = a1;
}
// compare angle of both lines P0-P1 and P1-P2
adelta = a2 - a1;
double dist = offset[1].DistanceTo(offset[2]);
#if (debuginfo)
log.Add(string.Format(" getPointOffsets Angles: a1 {0:0.000} a2 {1:0.000} delta {2:0.000}", (a1 * 180 / Math.PI), (a2 * 180 / Math.PI), (adelta * 180 / Math.PI)));
#endif
if (adelta >= (Math.PI))
{
adelta -= 2 * Math.PI;
}
if (adelta <= -(Math.PI))
{
adelta += 2 * Math.PI;
}
#if (debuginfo)
log.Add(string.Format(" getPointOffsets adelta corrected {0:0.000}", (adelta * 180 / Math.PI)));
log.Add(string.Format(" getPointOffsets offset [0]x{0:0.000} [0]y{1:0.000} [1]x{2:0.000} [1]y{3:0.000} [2]x{4:0.000} [2]y{5:0.000}", offset[0].X, offset[0].Y, offset[1].X, offset[1].Y, offset[2].X, offset[2].Y));
#endif
if (isEnd || (Math.Abs(adelta) <= 0.2) || (dist < 0.2))
{
return(0); // S1-angle == S2-angle, no correction needed
}
if (Math.Abs(Math.Abs(adelta) - Math.PI) <= 0.2)
{
return(1); // 180°
}
if ((adelta > 0) && (distance < 0))
{
return(1); // connect lines with additional arc
}
if ((adelta < 0) && (distance > 0))
{
return(1); // connect lines with additional arc
}
#if (debuginfo)
log.Add(string.Format(" getPointOffsets Find intersection {0} {1}", P1.mode, P2.mode));
#endif
bool result = false;
// find common intersection
if ((P1.mode <= 1) && (P2.mode <= 1)) // line to line
{ // https://www.java-forum.org/thema/algorithmus-fuer-pruefung-auf-ueberschneidende-linien.117102/
double d = (S1off[1].X - S1off[0].X) * (S2off[0].Y - S2off[1].Y) - (S2off[0].X - S2off[1].X) * (S1off[1].Y - S1off[0].Y);
if (d == 0)
{
offset[2] = offset[1] = offset[3];
}
else
{
double m = ((S2off[0].X - S1off[0].X) * (S2off[0].Y - S2off[1].Y) - (S2off[0].X - S2off[1].X) * (S2off[0].Y - S1off[0].Y)) / d;
double n = ((S1off[1].X - S1off[0].X) * (S2off[0].X - S1off[0].X) - (S2off[0].Y - S1off[0].Y) * (S1off[1].Y - S1off[0].Y)) / d;
offset[1].X = S1off[0].X + m * (S1off[1].X - S1off[0].X);
offset[1].Y = S1off[0].Y + m * (S1off[1].Y - S1off[0].Y);
offset[2] = offset[1];
}
}
else if ((P1.mode <= 1) && (P2.mode >= 2)) // 1st line then arc
{
result = calcIntersectionLineArc(ref offset, S1off, P2, newRadius2);
}
else if ((P1.mode >= 2) && (P2.mode <= 1)) // 1st arc then line
{
xyPoint tmp = S2off[0];
S2off[0] = S2off[1]; S2off[1] = tmp; // switch points, p[1] should be connection to arc
result = calcIntersectionLineArc(ref offset, S2off, P1, newRadius1);
}
else
{ // 1st arc 2nd arc, transfer one arc to line to use available function calcIntersectionLineArc
// http://www2.math.uni-wuppertal.de/~volkert/Das%20Apollonische%20Beruehrproblem,%202007.pdf
double dy = P2.CY - P1.CY;
double dx = P2.CX - P1.CX;
if (dy == 0) // center points of arcs on same y -> chordale = vertical line
{
double a = (newRadius2 * newRadius2 - newRadius1 * newRadius1 - dx * dx) / (-2 * dx);
double px = P1.CX + a; // vertical line
S1off[0].X = S1off[1].X = px;
S1off[0].Y = -P1.Y;
S1off[1].Y = P1.Y;
result = calcIntersectionLineArc(ref offset, S1off, P2, newRadius2);
}
else if (dx == 0) // center points of arcs on same x -> chordale = horizontal line
{
double a = (newRadius2 * newRadius2 - newRadius1 * newRadius1 - dy * dy) / (-2 * dy);
double py = P1.CY + a; // horizontal line
S1off[0].Y = S1off[1].Y = py;
S1off[0].X = -P1.X;
S1off[1].X = P1.X;
result = calcIntersectionLineArc(ref offset, S1off, P2, newRadius2);
}
else
{
double c = Math.Sqrt(dx * dx + dy * dy);
double a = (newRadius2 * newRadius2 - newRadius1 * newRadius1 - c * c) / (-2 * c);
double m = dy / dx;
double angle = getAlphaCenterToCenter(P1, P2);
xyPoint aP = new xyPoint();
aP = calcOffsetPoint(new xyPoint(P1.CX, P1.CY), angle, a);
angle += Math.PI / 2;
S1off[0] = calcOffsetPoint(aP, angle, newRadius1); // create line from point
S1off[1] = calcOffsetPoint(aP, angle, -newRadius1); // create line from point
double d0 = S1off[0].DistanceTo((xyPoint)P1);
double d1 = S1off[1].DistanceTo((xyPoint)P1);
if (d1 > d0) // index 1 should be closer to final pos
{
S1off[1] = calcOffsetPoint(aP, angle, newRadius1); // create line from point
S1off[0] = calcOffsetPoint(aP, angle, -newRadius1); // create line from point
}
result = calcIntersectionLineArc(ref offset, S1off, P2, newRadius2);
}
if ((double.IsNaN(offset[1].X)) || double.IsNaN(offset[1].Y))
{
offset[1].X = 0; offset[1].Y = 0;
offset[2] = offset[1];
}
}
if (result == true) // intersection successful
{
return(-1);
}
else
{
return(-2);
}
}
// calculate and apply offset for given coordinates in gcodeList[prev,act,next] (which should have xy moves)
public static int createOffsetedPath(bool isFirst, bool isEnd, int iInitial, int prev, int act, int next, double radius, ref xyPoint[] offset)
{
xyArcPoint p1 = fillPointData(prev, prev);
xyArcPoint p2 = fillPointData(prev, act);
xyArcPoint p3 = fillPointData(act, next);
bool isArc = (gcodeList[act].motionMode > 1);
bool isFullCircle = ((p1.X == p2.X) && (p1.Y == p2.Y));
int offsetType = crc.getPointOffsets(ref offset, p1, p2, p3, radius, isEnd);
#if (debuginfo)
log.Add(string.Format(" offset typ{0} x{1:0.000} y{2:0.000}", offsetType, offset[1].X, offset[1].Y));
#endif
/* if (offsetType == -2) // intersection not successfull
* {
* gcodeList[act].motionMode = 1; gcodeList[act].x = null; gcodeList[act].y = null; // clear move
* p2 = p3; p3 = fillPointData(next, next+1); // next+1 not save
* offsetType = crc.getPointOffsets(ref offset, p1, p2, p3, radius, isEnd);
#if (debuginfo)
* log.Add(string.Format(" redo offset x{0:0.000} y{1:0.000}", offset[1].X, offset[1].Y));
#endif
* }*/
if (isArc && !isFullCircle) // replace arc by line, if start and end-point are too close
{
double dist = offset[0].DistanceTo(offset[1]);
double a1 = offset[0].AngleTo(offset[1]);
double a2 = ((xyPoint)p1).AngleTo((xyPoint)p2);
if (dist < 0.1)
{
gcodeList[act].motionMode = 1;
gcodeList[act].i = null;
gcodeList[act].j = null;
}
}
if (isFirst) // offset 1st point
{
gcodeList[iInitial].x = offset[0].X; gcodeList[iInitial].y = offset[0].Y; // offset 1st point
}
gcodeList[act].x = offset[1].X; gcodeList[act].y = offset[1].Y; // offset point
#if (debuginfo)
log.Add(string.Format(" createOffsetedPath Offset x{0:0.000} y{1:0.000}", gcodeList[act].x, gcodeList[act].y));
#endif
if (isArc) // offset radius
{
double iNew = p2.CX - (double)gcodeList[prev].x;
double jNew = p2.CY - (double)gcodeList[prev].y;;
gcodeList[act].i = iNew; gcodeList[act].j = jNew; // offset radius
if (!sameSign(gcodeList[act].i, iNew) || !sameSign(gcodeList[act].j, jNew)) // radius now negative
{
}
// if ((gcodeList[act].i == 0) && (gcodeList[act].j == 0)) // radius = 0, command not needed
// { gcodeList[act].motionMode = 1; gcodeList[act].i = null; gcodeList[act].j = null;}
}
if (offsetType >= 1) // insert arc to connect lines
{
int insert = act + 1;
gcodeList.Insert(insert, new gcodeByLine(gcodeList[act])); // insert a copy of actual move
gcodeList[insert].x = offset[2].X; gcodeList[insert].y = offset[2].Y; // set end-pos.
double dist = offset[1].DistanceTo(offset[2]); // if distance great enough use arc
if (dist > 0.1) // make arc, if start and end-point are not too close
{
gcodeList[insert].motionMode = (byte)((radius > 0) ? 2 : 3);
gcodeList[insert].i = gcodeList[insert].actualPos.X - offset[1].X;
gcodeList[insert].j = gcodeList[insert].actualPos.Y - offset[1].Y;
}
else
{
gcodeList[insert].motionMode = 1;
}
#if (debuginfo)
log.Add(string.Format(" createOffsetedPath Insert G{0} x{1:0.000} y{2:0.000}", gcodeList[insert].motionMode, gcodeList[insert].x, gcodeList[insert].y));
#endif
}
return(offsetType);
}
public static string transformGCodeRadiusCorrection(double radius)
{
Logger.Debug("Radius correction r: {0}", radius);
#if (debuginfo)
log.clear();
log.Add("### GCodeVisu radius correction ###");
#endif
if (gcodeList == null)
{
return("");
}
if (lastFigureNumber > 0)
{
pathBackground = (GraphicsPath)pathMarkSelection.Clone();
origWCOLandMark = (xyPoint)grbl.posWCO;
}
else
{
pathBackground = (GraphicsPath)pathPenDown.Clone();
origWCOLandMark = (xyPoint)grbl.posWCO;
}
xyPoint[] offset = new xyPoint[4];
int i, figureStart, figure2nd, prev, act, next;
int counter = 0, isFirst = 0;
bool figureProcessed = false;
bool closeFigure = false;
bool endFigure = false;
figure2nd = figureStart = prev = act = next = 0;
int offType = 0;
for (i = 1; i < gcodeList.Count; i++)
{
if ((i == (gcodeList.Count - 1)) || ((lastFigureNumber > 0) && (gcodeList[i].figureNumber != lastFigureNumber))) // if wrong selection, nothing to do
{
if (figureProcessed) // correct last point
{
figureProcessed = false;
goto ProcessesPath;
}
continue;
}
if (gcodeList[i].ismachineCoordG53) // machine coordinates, do not change
{
continue;
}
if (!xyMove(gcodeList[i])) // no xy moves, nothing to do - except G0 Z
{
continue;
}
while ((gcodeList[i].codeLine == gcodeList[i + 1].codeLine) && (i < gcodeList.Count)) // remove double lines (lff font)
{
gcodeList.RemoveAt(i + 1);
}
while (sameXYPos(gcodeList[i], gcodeList[i + 1]) && (i < (gcodeList.Count - 1))) // remove double coordinates
{
gcodeList.RemoveAt(i + 1);
}
#if (debuginfo)
log.Add("----- " + i.ToString() + " -----");
#endif
if (gcodeList[i].motionMode > 1)
{
double tmpR = Math.Sqrt((double)gcodeList[i].i * (double)gcodeList[i].i + (double)gcodeList[i].j * (double)gcodeList[i].j);
bool remove = false;
double abs_radius = Math.Abs(radius);
if (gcodeList[i].motionMode == 2)
{
if ((radius < 0) && (tmpR < abs_radius))
{
remove = true;
}
}
if (gcodeList[i].motionMode == 3)
{
if ((radius > 0) && (tmpR < abs_radius))
{
remove = true;
}
}
if (remove)
{
gcodeList[i].i = null; gcodeList[i].j = null;
gcodeList[i].motionMode = 1;
#if (debuginfo)
log.Add("Radius too small, do G1 " + gcodeList[act].codeLine + " ##############################");
#endif
}
}
figureProcessed = true; // must stay before jump label
next = i;
endFigure = false;
ProcessesPath:
// gcodeList[i].info += " "+i.ToString()+" "+ figureProcessed.ToString()+" "+lastFigureNumber.ToString();
if (counter == 0)
{
figureStart = prev = act = next;
} // preset indices
if ((gcodeList[prev].motionMode == 0) && (gcodeList[act].motionMode >= 1)) // find start of figure
{
figureStart = prev; figure2nd = act; isFirst = 0;
} //gcodeList[prev].info += " #start "; }
if ((gcodeList[act].motionMode >= 1) && (gcodeList[next].motionMode == 0)) // find end of figure
{
endFigure = true; figureProcessed = false;
} // gcodeList[act].info += " #end "; }
closeFigure = false;
if (act != prev)
{
xyArcPoint p1 = fillPointData(prev, prev);
xyArcPoint p2 = fillPointData(prev, act);
if ((gcodeList[act].actualPos.X == gcodeList[figureStart].actualPos.X) && (gcodeList[act].actualPos.Y == gcodeList[figureStart].actualPos.Y))
{
next = figure2nd; closeFigure = true;
} // gcodeList[act].info += " closefig "; }
#if (debuginfo)
log.Add(gcodeList[act].codeLine);
#endif
offType = createOffsetedPath((isFirst++ == 0), (endFigure && !closeFigure), figureStart, prev, act, next, radius, ref offset);
#if (debuginfo)
log.Add(string.Format(" typ {0} {1} {2} {3} {4} {5} ", offType, endFigure, figureStart, prev, act, next));
#endif
if (closeFigure)// && !endFigure)
{
#if (debuginfo)
log.Add(string.Format(" close Figure {0:0.00} {1:0.00} ", offset[2].X, offset[2].Y));
#endif
gcodeList[figureStart].x = offset[2].X; gcodeList[figureStart].y = offset[2].Y; // close figure
if (gcodeList[figure2nd].motionMode > 1) // act
{
bool isFullCircle = ((p1.X == p2.X) && (p1.Y == p2.Y));
if (!isFullCircle)
{
xyArcPoint p3 = fillPointData(prev, act); //fillPointData(act, next);
gcodeList[figure2nd].i = p3.CX - offset[2].X;
gcodeList[figure2nd].j = p3.CY - offset[2].Y; // offset radius
#if (debuginfo)
log.Add(string.Format(" correct Arc center of f2nd {0} origX {1:0.00} origY {2:0.00} ", figure2nd, p3.CX, p3.CY));
#endif
}
}
}
next = i; // restore next
if (offType >= 1) // arc or line was inserted to connect points
{
act++; next++; counter++; i++;
} // inc. counters
}
prev = act; act = next; counter++;
if (endFigure)
{
prev = act; figureStart = act = i;
} // preset indices
if (closeFigure)
{
isFirst = 0;
}
}
return(createGCodeProg());
}
private static double getAlphaCenterToCenter(xyArcPoint P1, xyArcPoint P2)
{
return(getAlpha(P1.CX, P1.CY, P2.CX, P2.CY));
}
private static double getAlphaLine(xyArcPoint P1, xyArcPoint P2)
{
return(getAlpha(P1.X, P1.Y, P2.X, P2.Y));
}
private static xyPoint calcOffsetPoint(xyArcPoint P, double angle, double radius)
{
return(calcOffsetPoint(new xyPoint(P.X, P.Y), angle, radius));
}
