361 lines
16 KiB
C#
361 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Text.RegularExpressions;
|
|
|
|
namespace ToolPathParser
|
|
{
|
|
public static class ToolPathProcessor
|
|
{
|
|
// Main processing function
|
|
public static List<ToolPosition> ProcessToolPath(
|
|
string inputPath,
|
|
int totalPointsPerLayer = 50,
|
|
double radialDepth = 1.0,
|
|
double zOrigin = 0.0,
|
|
bool verbose = true,
|
|
double zLayerTol = 0.1)
|
|
{
|
|
var lines = File.ReadAllLines(inputPath)
|
|
.Select(l => l.Trim())
|
|
.Where(l => !string.IsNullOrWhiteSpace(l))
|
|
.ToList();
|
|
|
|
var rawPoints = new List<Dictionary<string, object>>();
|
|
double feedrate = 0, spindle = 0;
|
|
for (int i = 0; i < lines.Count; i++)
|
|
{
|
|
string line = lines[i];
|
|
|
|
if (line.StartsWith("FEDRAT", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
var m = Regex.Match(line, @"[-+]?\d*\.?\d+");
|
|
if (m.Success) feedrate = double.Parse(m.Value, CultureInfo.InvariantCulture);
|
|
continue;
|
|
}
|
|
|
|
if (line.StartsWith("SPINDL", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
var m = Regex.Match(line, @"[-+]?\d*\.?\d+");
|
|
if (m.Success) spindle = double.Parse(m.Value, CultureInfo.InvariantCulture);
|
|
continue;
|
|
}
|
|
|
|
if (line.StartsWith("GOTO", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
var g = ParseGotoRobust(line);
|
|
if (g != null)
|
|
{
|
|
g["feedrate"] = feedrate;
|
|
g["spindle"] = spindle;
|
|
rawPoints.Add(g);
|
|
}
|
|
|
|
// check next line for CIRCLE (ARC)
|
|
if (i + 1 < lines.Count && lines[i + 1].StartsWith("CIRCLE", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
var c = ParseCircleRobust(lines[i + 1]);
|
|
if (c != null)
|
|
{
|
|
// find the next GOTO as arc end
|
|
int j = i + 2;
|
|
Dictionary<string, object> endPoint = null;
|
|
while (j < lines.Count)
|
|
{
|
|
if (lines[j].StartsWith("GOTO", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
endPoint = ParseGotoRobust(lines[j]);
|
|
break;
|
|
}
|
|
j++;
|
|
}
|
|
if (endPoint != null)
|
|
{
|
|
rawPoints.Add(new Dictionary<string, object>
|
|
{
|
|
["type"] = "ARC",
|
|
["start"] = g["coord"],
|
|
["end"] = endPoint["coord"],
|
|
["center"] = c["center"],
|
|
["axis"] = c["axis"],
|
|
["radius"] = c["radius"],
|
|
["feedrate"] = feedrate,
|
|
["spindle"] = spindle
|
|
});
|
|
// advance i to j-1 so outer loop continues correctly
|
|
i = j - 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (verbose)
|
|
{
|
|
//Console.WriteLine("=== parsed rawPoints (first 30) ===");
|
|
for (int r = 0; r < Math.Min(30, rawPoints.Count); r++)
|
|
{
|
|
var rp = rawPoints[r];
|
|
if (rp["type"].ToString() == "ARC")
|
|
{
|
|
var s = (double[])rp["start"];
|
|
var e = (double[])rp["end"];
|
|
//Console.WriteLine($"[{r}] ARC start Z={s[2].ToString(CultureInfo.InvariantCulture)} end Z={e[2].ToString(CultureInfo.InvariantCulture)}");
|
|
}
|
|
else
|
|
{
|
|
var c = (double[])rp["coord"];
|
|
//Console.WriteLine($"[{r}] GOTO X={c[0].ToString(CultureInfo.InvariantCulture)} Y={c[1].ToString(CultureInfo.InvariantCulture)} Z={c[2].ToString(CultureInfo.InvariantCulture)} FR={rp["feedrate"]} SP={rp["spindle"]}");
|
|
}
|
|
}
|
|
}
|
|
|
|
// cluster into layers by Z using tolerance (zLayerTol)
|
|
var layers = new List<List<Dictionary<string, object>>>();
|
|
double lastZ = double.NaN;
|
|
List<Dictionary<string, object>> current = null;
|
|
foreach (var pt in rawPoints)
|
|
{
|
|
double z = (pt.ContainsKey("type") && pt["type"].ToString() == "ARC")
|
|
? ((double[])pt["start"])[2]
|
|
: ((double[])pt["coord"])[2];
|
|
|
|
if (double.IsNaN(lastZ) || Math.Abs(z - lastZ) < zLayerTol)
|
|
{
|
|
if (current == null) current = new List<Dictionary<string, object>>();
|
|
current.Add(pt);
|
|
}
|
|
else
|
|
{
|
|
layers.Add(current);
|
|
current = new List<Dictionary<string, object>> { pt };
|
|
}
|
|
lastZ = z;
|
|
}
|
|
if (current != null && current.Count > 0) layers.Add(current);
|
|
|
|
if (verbose)
|
|
{
|
|
//Console.WriteLine("=== detected layers Z (first 20) ===");
|
|
for (int li = 0; li < Math.Min(20, layers.Count); li++)
|
|
{
|
|
double zv = (layers[li].First().ContainsKey("type") && layers[li].First()["type"].ToString() == "ARC")
|
|
? ((double[])layers[li].First()["start"])[2]
|
|
: ((double[])layers[li].First()["coord"])[2];
|
|
//Console.WriteLine($"layer {li} Z ~ {zv.ToString(CultureInfo.InvariantCulture)} pts={layers[li].Count}");
|
|
}
|
|
}
|
|
|
|
// interpolation per layer
|
|
var results = new List<ToolPosition>();
|
|
double prevLayerZ = zOrigin;
|
|
foreach (var layer in layers)
|
|
{
|
|
// build segments: consecutive GOTO -> LINE; include ARC entries as arcs
|
|
var segments = BuildSegments(layer);
|
|
var segLens = segments.Select(seg =>
|
|
{
|
|
if (seg["type"].ToString() == "LINE")
|
|
return LineLength((double[])seg["start"], (double[])seg["end"]);
|
|
else
|
|
return ArcLength((double[])seg["start"], (double[])seg["end"], (double[])seg["center"], ((double[])seg["axis"])[2]);
|
|
}).ToList();
|
|
|
|
double totalLen = segLens.Sum();
|
|
if (totalLen < 1e-9) continue;
|
|
double[] targetS = Enumerable.Range(0, totalPointsPerLayer)
|
|
.Select(k => k * totalLen / (totalPointsPerLayer - 1)).ToArray();
|
|
|
|
double segStart = 0;
|
|
int ptIdx = 0;
|
|
for (int sIdx = 0; sIdx < segments.Count; sIdx++)
|
|
{
|
|
var seg = segments[sIdx];
|
|
double segLen = segLens[sIdx];
|
|
while (ptIdx < totalPointsPerLayer && targetS[ptIdx] <= segStart + segLen)
|
|
{
|
|
double t = (segLen == 0) ? 0.0 : (targetS[ptIdx] - segStart) / segLen;
|
|
double[] pos;
|
|
double curvature = 0;
|
|
double I = 0, J = 0, K = 0, cx = 0, cy = 0, cz = 0;
|
|
int segType;
|
|
if (seg["type"].ToString() == "LINE")
|
|
{
|
|
pos = InterpolateLine((double[])seg["start"], (double[])seg["end"], t);
|
|
segType = 1;
|
|
}
|
|
else
|
|
{
|
|
pos = InterpolateArc((double[])seg["start"], (double[])seg["end"], (double[])seg["center"], (double)seg["radius"], ((double[])seg["axis"])[2], t);
|
|
curvature = (double)seg["radius"];
|
|
var axis = (double[])seg["axis"];
|
|
var c = (double[])seg["center"];
|
|
I = axis[0]; J = axis[1]; K = axis[2];
|
|
cx = c[0]; cy = c[1]; cz = c[2];
|
|
segType = 2;
|
|
}
|
|
|
|
double axialDepth = Math.Abs(pos[2] - prevLayerZ);
|
|
results.Add(new ToolPosition
|
|
{
|
|
X = pos[0],
|
|
Y = pos[1],
|
|
Z = pos[2],
|
|
FeedRate = (double)seg["feedrate"],
|
|
SpindleSpeed = (double)seg["spindle"],
|
|
RadialDepth = radialDepth,
|
|
AxialDepth = axialDepth,
|
|
CurvatureRadius = curvature,
|
|
I = I,
|
|
J = J,
|
|
K = K,
|
|
Cx = cx,
|
|
Cy = cy,
|
|
Cz = cz,
|
|
SegmentType = segType
|
|
});
|
|
|
|
ptIdx++;
|
|
}
|
|
segStart += segLen;
|
|
}
|
|
|
|
// update prevLayerZ from layer: use average Z of this layer's original pts (more robust)
|
|
var zs = new List<double>();
|
|
foreach (var p in layer)
|
|
{
|
|
if (p.ContainsKey("type") && p["type"].ToString() == "ARC")
|
|
{
|
|
zs.Add(((double[])p["start"])[2]);
|
|
zs.Add(((double[])p["end"])[2]);
|
|
}
|
|
else
|
|
{
|
|
zs.Add(((double[])p["coord"])[2]);
|
|
}
|
|
}
|
|
if (zs.Count > 0) prevLayerZ = zs.Average();
|
|
}
|
|
|
|
return results;
|
|
}
|
|
|
|
// robust GOTO parse: accept "GOTO/-99.0092,6.5793,-0.9667" or "GOTO -99.0092,6.5793,-0.9667"
|
|
private static Dictionary<string, object> ParseGotoRobust(string line)
|
|
{
|
|
// find substring after "GOTO"
|
|
int idx = line.IndexOf("GOTO", StringComparison.OrdinalIgnoreCase);
|
|
if (idx < 0) return null;
|
|
string tail = line.Substring(idx + 4).TrimStart(new char[] { '/', ' ', '\t' });
|
|
// tail like "-99.0092,6.5793,-0.9667" possibly with extra commas or fields
|
|
var parts = tail.Split(new[] { ',', ' ' }, StringSplitOptions.RemoveEmptyEntries);
|
|
// find first three numeric tokens
|
|
var nums = new List<double>();
|
|
foreach (var p in parts)
|
|
{
|
|
if (double.TryParse(p, NumberStyles.Float, CultureInfo.InvariantCulture, out double v))
|
|
{
|
|
nums.Add(v);
|
|
if (nums.Count == 3) break;
|
|
}
|
|
}
|
|
if (nums.Count < 3) return null;
|
|
return new Dictionary<string, object> { ["type"] = "GOTO", ["coord"] = nums.ToArray() };
|
|
}
|
|
|
|
// robust CIRCLE parse: accept "CIRCLE/x,y,z,i,j,k,r,..." where first 7 numbers are center(i,j,k)radius...
|
|
private static Dictionary<string, object> ParseCircleRobust(string line)
|
|
{
|
|
int idx = line.IndexOf("CIRCLE", StringComparison.OrdinalIgnoreCase);
|
|
if (idx < 0) return null;
|
|
string tail = line.Substring(idx + 6).TrimStart(new char[] { '/', ' ', '\t' });
|
|
var parts = tail.Split(new[] { ',', ' ' }, StringSplitOptions.RemoveEmptyEntries);
|
|
var nums = new List<double>();
|
|
foreach (var p in parts)
|
|
{
|
|
if (double.TryParse(p, NumberStyles.Float, CultureInfo.InvariantCulture, out double v))
|
|
{
|
|
nums.Add(v);
|
|
}
|
|
}
|
|
if (nums.Count < 7) return null;
|
|
double[] center = nums.Take(3).ToArray();
|
|
double[] axis = nums.Skip(3).Take(3).ToArray();
|
|
double radius = nums[6];
|
|
return new Dictionary<string, object>
|
|
{
|
|
["type"] = "CIRCLE",
|
|
["center"] = center,
|
|
["axis"] = axis,
|
|
["radius"] = radius
|
|
};
|
|
}
|
|
|
|
private static List<Dictionary<string, object>> BuildSegments(List<Dictionary<string, object>> layerPts)
|
|
{
|
|
var segs = new List<Dictionary<string, object>>();
|
|
int i = 0;
|
|
while (i < layerPts.Count)
|
|
{
|
|
var p = layerPts[i];
|
|
if (p.ContainsKey("type") && p["type"].ToString() == "GOTO")
|
|
{
|
|
int j = i + 1;
|
|
while (j < layerPts.Count && layerPts[j].ContainsKey("type") && layerPts[j]["type"].ToString() == "GOTO") j++;
|
|
for (int k = i; k < j - 1; k++)
|
|
{
|
|
segs.Add(new Dictionary<string, object>
|
|
{
|
|
["type"] = "LINE",
|
|
["start"] = (double[])layerPts[k]["coord"],
|
|
["end"] = (double[])layerPts[k + 1]["coord"],
|
|
["feedrate"] = layerPts[k + 1]["feedrate"],
|
|
["spindle"] = layerPts[k + 1]["spindle"]
|
|
});
|
|
}
|
|
i = j;
|
|
}
|
|
else if (p.ContainsKey("type") && p["type"].ToString() == "ARC")
|
|
{
|
|
segs.Add(p); // keep ARC entries as-is
|
|
i++;
|
|
}
|
|
else i++;
|
|
}
|
|
return segs;
|
|
}
|
|
|
|
private static double LineLength(double[] a, double[] b)
|
|
=> Math.Sqrt(Math.Pow(b[0] - a[0], 2) + Math.Pow(b[1] - a[1], 2) + Math.Pow(b[2] - a[2], 2));
|
|
|
|
private static double ArcLength(double[] s, double[] e, double[] c, double axisZ)
|
|
{
|
|
double r = Math.Sqrt(Math.Pow(s[0] - c[0], 2) + Math.Pow(s[1] - c[1], 2));
|
|
double ang1 = Math.Atan2(s[1] - c[1], s[0] - c[0]);
|
|
double ang2 = Math.Atan2(e[1] - c[1], e[0] - c[0]);
|
|
double d = ang2 - ang1;
|
|
// normalize
|
|
if (Math.Abs(d) > Math.PI) d -= Math.Sign(d) * 2 * Math.PI;
|
|
return Math.Abs(d) * r;
|
|
}
|
|
|
|
private static double[] InterpolateLine(double[] a, double[] b, double t)
|
|
=> new double[] { a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t, a[2] + (b[2] - a[2]) * t };
|
|
|
|
private static double[] InterpolateArc(double[] s, double[] e, double[] c, double r, double k, double t)
|
|
{
|
|
double ang1 = Math.Atan2(s[1] - c[1], s[0] - c[0]);
|
|
double ang2 = Math.Atan2(e[1] - c[1], e[0] - c[0]);
|
|
double d = ang2 - ang1;
|
|
if (Math.Abs(d) > Math.PI) d -= Math.Sign(d) * 2 * Math.PI;
|
|
double ang = ang1 + d * t;
|
|
double x = c[0] + r * Math.Cos(ang);
|
|
double y = c[1] + r * Math.Sin(ang);
|
|
double z = s[2] + (e[2] - s[2]) * t;
|
|
return new double[] { x, y, z };
|
|
}
|
|
}
|
|
}
|