static void Parse(string line, int lineNumber)
{
MatchCollection matches = GCodeSplitter.Matches(line);
List<Word> Words = new List<Word>(matches.Count);
foreach (Match match in matches)
{
Words.Add(new Word() { Command = match.Groups[1].Value[0], Parameter = double.Parse(match.Groups[2].Value, Constants.DecimalParseFormat) });
}
for (int i = 0; i < Words.Count; i++)
{
if (!ValidWords.Contains(Words[i].Command))
{
Words.RemoveAt(i);
continue;
}
if (Words[i].Command != 'F')
continue;
State.Feed = Words[i].Parameter;
Words.RemoveAt(i);
continue;
}
while (Words.Count > 0)
{
if (Words.First().Command == 'M')
{
int param = (int)Words.First().Parameter;
if (param != Words.First().Parameter || param < 0)
throw new ParseException("MCode can only have integer parameters", lineNumber);
Commands.Add(new MCode() { Code = param });
Words.RemoveAt(0);
continue;
}
if (Words.First().Command == 'G' && !MotionCommands.Contains(Words.First().Parameter))
{
#region UnitPlaneDistanceMode
double param = Words.First().Parameter;
if (param == 90)
{
State.DistanceMode = ParseDistanceMode.Absolute;
Words.RemoveAt(0);
continue;
}
if (param == 91)
{
State.DistanceMode = ParseDistanceMode.Incremental;
Words.RemoveAt(0);
continue;
}
if (param == 90.1)
{
State.ArcDistanceMode = ParseDistanceMode.Absolute;
Words.RemoveAt(0);
continue;
}
if (param == 91.1)
{
State.ArcDistanceMode = ParseDistanceMode.Incremental;
Words.RemoveAt(0);
continue;
}
if (param == 21)
{
State.Unit = ParseUnit.Metric;
Words.RemoveAt(0);
continue;
}
if (param == 20)
{
State.Unit = ParseUnit.Imperial;
Words.RemoveAt(0);
continue;
}
if (param == 17)
{
State.Plane = ArcPlane.XY;
Words.RemoveAt(0);
continue;
}
if (param == 18)
{
State.Plane = ArcPlane.ZX;
Words.RemoveAt(0);
continue;
}
if (param == 19)
{
State.Plane = ArcPlane.YZ;
Words.RemoveAt(0);
continue;
}
Words.RemoveAt(0); //unsupported G-Command
continue;
#endregion
}
break;
}
if (Words.Count == 0)
return;
int MotionMode = State.LastMotionMode;
if (Words.First().Command == 'G')
{
MotionMode = (int)Words.First().Parameter;
State.LastMotionMode = MotionMode;
Words.RemoveAt(0);
}
if (MotionMode < 0)
throw new ParseException("No Motion Mode active", lineNumber);
double UnitMultiplier = (State.Unit == ParseUnit.Metric) ? 1 : 25.4;
Vector3 EndPos = State.Position;
#region FindEndPos
{
int Incremental = (State.DistanceMode == ParseDistanceMode.Incremental) ? 1 : 0;
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'X')
continue;
EndPos.X = Words[i].Parameter * UnitMultiplier + Incremental * EndPos.X;
Words.RemoveAt(i);
break;
}
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'Y')
continue;
EndPos.Y = Words[i].Parameter * UnitMultiplier + Incremental * EndPos.Y;
Words.RemoveAt(i);
break;
}
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'Z')
continue;
EndPos.Z = Words[i].Parameter * UnitMultiplier + Incremental * EndPos.Z;
Words.RemoveAt(i);
break;
}
}
#endregion
if (MotionMode <= 1)
{
if (Words.Count > 0)
throw new ParseException("Motion Command must be last in line (unused Words in Block)", lineNumber);
Line motion = new Line();
motion.Start = State.Position;
motion.End = EndPos;
motion.Feed = State.Feed;
motion.Rapid = MotionMode == 0;
Commands.Add(motion);
State.Position = EndPos;
return;
}
double U, V;
bool IJKused = false;
switch (State.Plane)
{
default:
U = State.Position.X;
V = State.Position.Y;
break;
case ArcPlane.YZ:
U = State.Position.Y;
V = State.Position.Z;
break;
case ArcPlane.ZX:
U = State.Position.Z;
V = State.Position.X;
break;
}
#region FindIJK
{
int ArcIncremental = (State.ArcDistanceMode == ParseDistanceMode.Incremental) ? 1 : 0;
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'I')
continue;
switch(State.Plane)
{
case ArcPlane.XY:
U = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.X;
break;
case ArcPlane.YZ:
throw new ParseException("Current Plane is YZ, I word is invalid", lineNumber);
case ArcPlane.ZX:
V = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.X;
break;
}
IJKused = true;
Words.RemoveAt(i);
break;
}
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'J')
continue;
switch (State.Plane)
{
case ArcPlane.XY:
V = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.Y;
break;
case ArcPlane.YZ:
U = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.Y;
break;
case ArcPlane.ZX:
throw new ParseException("Current Plane is ZX, J word is invalid", lineNumber);
}
IJKused = true;
Words.RemoveAt(i);
break;
}
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'K')
continue;
switch (State.Plane)
{
case ArcPlane.XY:
throw new ParseException("Current Plane is XY, K word is invalid", lineNumber);
case ArcPlane.YZ:
V = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.Z;
break;
case ArcPlane.ZX:
U = Words[i].Parameter * UnitMultiplier + ArcIncremental * State.Position.Z;
break;
}
IJKused = true;
Words.RemoveAt(i);
break;
}
}
#endregion
#region ResolveRadius
for (int i = 0; i < Words.Count; i++)
{
if (Words[i].Command != 'R')
continue;
if (IJKused)
throw new ParseException("Both IJK and R notation used", lineNumber);
if (State.Position == EndPos)
throw new ParseException("arcs in R-notation must have non-coincident start and end points", lineNumber);
double Radius = Words[i].Parameter * UnitMultiplier;
if (Radius == 0)
throw new ParseException("Radius can't be zero", lineNumber);
double A, B;
switch (State.Plane)
{
default:
A = EndPos.X;
B = EndPos.Y;
break;
case ArcPlane.YZ:
A = EndPos.Y;
B = EndPos.Z;
break;
case ArcPlane.ZX:
A = EndPos.Z;
B = EndPos.X;
break;
}
A -= U; //(AB) = vector from start to end of arc along the axes of the current plane
B -= V;
double C = -B; //(UV) = vector perpendicular to (AB)
double D = A;
{ //normalize perpendicular vector
double perpLength = Math.Sqrt(C * C + D * D);
C /= perpLength;
D /= perpLength;
}
double PerpSquare = (Radius * Radius) - ((A * A + B * B) / 4);
if (PerpSquare < 0)
throw new ParseException("arc radius too small to reach both ends", lineNumber);
double PerpLength = Math.Sqrt(PerpSquare);
if (MotionMode == 3 ^ Radius < 0)
PerpLength = -PerpLength;
U += (A / 2) + C * (PerpLength);
V += (B / 2) + (D * PerpLength);
Words.RemoveAt(i);
break;
}
#endregion
Arc arc = new Arc();
arc.Start = State.Position;
arc.End = EndPos;
arc.Feed = State.Feed;
arc.Direction = (MotionMode == 2) ? ArcDirection.CW : ArcDirection.CCW;
arc.U = U;
arc.V = V;
arc.Plane = State.Plane;
Commands.Add(arc);
State.Position = EndPos;
return;
}
public override IEnumerable<Motion> Split(double length)
{
int divisions = (int)Math.Ceiling(Length / length);
if (divisions < 1)
divisions = 1;
Vector3 lastEnd = Start;
for(int i = 1; i <= divisions; i++)
{
Vector3 end = Interpolate(((double)i) / divisions);
Arc immediate = new Arc();
immediate.Start = lastEnd;
immediate.End = end;
immediate.Feed = Feed;
immediate.Direction = Direction;
immediate.Plane = Plane;
immediate.U = U;
immediate.V = V;
yield return immediate;
lastEnd = end;
}
}