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)); }