private GCodeCommand ParseLine(MatchCollection matches)
{
int Command;
if (matches[0].Groups[1].Value == "G")
{
float CommandF = float.Parse(matches[0].Groups[2].Value, inv);
Command = (int)CommandF;
if (CommandF == 90.1)
{
ArcDistanceMode = ParseDistanceMode.Absolute;
return(null);
}
if (CommandF == 91.1)
{
ArcDistanceMode = ParseDistanceMode.Incremental;
return(null);
}
if (CommandF != Command) //All other 'G' commands that have a decimal point
{
return(new OtherCode(matches));
}
LastCommand = Command;
}
else
{
if ("XYZIJKR".Contains(matches[0].Groups[1].Value))
{
Command = LastCommand;
}
else
{
return(new OtherCode(matches));
}
}
double?X = null, Y = null, Z = null, I = null, J = null, F = null, R = null;
for (int index = 0; index < matches.Count; index++)
{
double value = double.Parse(matches[index].Groups[2].Value, inv);
if (Units == DistanceUnit.Inches)
{
value *= 25.4f;
}
switch (matches[index].Groups[1].Value)
{
case "X": X = value; break;
case "Y": Y = value; break;
case "Z": Z = value; break;
case "I": I = value; break;
case "J": J = value; break;
case "F": F = value; break;
case "R": R = value; break;
}
}
Vector3 EndPosition = new Vector3(Position);
if (X.HasValue)
{
EndPosition.X = (DistanceMode == ParseDistanceMode.Absolute) ? (double)X : EndPosition.X + (double)X;
}
if (Y.HasValue)
{
EndPosition.Y = (DistanceMode == ParseDistanceMode.Absolute) ? (double)Y : EndPosition.Y + (double)Y;
}
if (Z.HasValue)
{
EndPosition.Z = (DistanceMode == ParseDistanceMode.Absolute) ? (double)Z : EndPosition.Z + (double)Z;
}
switch (Command)
{
case 0:
{
Straight s = new Straight(Position, EndPosition, true);
if (EndPosition != Position)
{
Position = EndPosition;
return(s);
}
return(null);
}
case 1:
{
Straight s = new Straight(Position, EndPosition, false);
s.FeedRate = F;
if (EndPosition != Position)
{
Position = EndPosition;
return(s);
}
return(null);
}
case 2:
case 3:
{
Vector3 center = new Vector3(Position);
if (I.HasValue)
{
center.X = (ArcDistanceMode == ParseDistanceMode.Absolute) ? (double)I : Position.X + (double)I;
}
if (J.HasValue)
{
center.Y = (ArcDistanceMode == ParseDistanceMode.Absolute) ? (double)J : Position.Y + (double)J;
}
if (R.HasValue)
{
Vector3 Parallel = EndPosition - Position;
Vector3 Perpendicular = Parallel.CrossProduct(new Vector3(0, 0, 1));
double PerpLength = Math.Sqrt(((double)R * (double)R) - (Parallel.Magnitude * Parallel.Magnitude / 4));
if (Command == 3 ^ R < 0)
{
PerpLength = -PerpLength;
}
Perpendicular *= PerpLength / Perpendicular.Magnitude;
center = Position + (Parallel / 2) + Perpendicular;
}
Arc a = new Arc(Position, EndPosition, center, Command == 2 ? ArcDirection.CW : ArcDirection.CCW);
a.FeedRate = F;
Position = EndPosition;
return(a);
}
case 20:
Units = DistanceUnit.Inches;
return(null);
case 21:
Units = DistanceUnit.MM;
return(null);
case 90:
DistanceMode = ParseDistanceMode.Absolute;
return(null);
case 91:
DistanceMode = ParseDistanceMode.Incremental;
return(null);
}
return(new OtherCode(matches));
}
public ToolPath ApplyHeightMap(HeightMap map)
{
ToolPath adjusted = new ToolPath();
foreach (GCodeCommand command in this)
{
if (command is OtherCode)
{
adjusted.Add(command);
continue;
}
else
{
Movement m = (Movement)command;
int divisions = (int)Math.Ceiling(m.Length / map.GridSize);
if (m is Straight)
{
Straight s = (Straight)m;
if (s.Rapid)
{
Vector3 newEnd = s.End;
newEnd.Z += map.GetHeightAt(s.End.X, s.End.Y);
adjusted.Add(new Straight(s.Start, newEnd, true));
}
else
{
Vector3 pos = s.Start;
for (int x = 1; x <= divisions; x++)
{
Vector3 end = s.Start.Interpolate(s.End, (double)x / (double)divisions);
end.Z += map.GetHeightAt(end.X, end.Y);
Straight st = new Straight(pos, end, false);
if (x == 1)
{
st.FeedRate = s.FeedRate;
}
adjusted.Add(st);
pos = end;
}
}
}
if (m is Arc)
{
Arc a = (Arc)m;
Vector3 pos = a.Start;
double stretch = a.StartAngle - a.EndAngle;
if (stretch <= 0)
{
stretch += 2 * Math.PI;
}
if (a.Direction == ArcDirection.CCW)
{
stretch = 2 * Math.PI - stretch;
}
if (stretch <= 0)
{
stretch += 2 * Math.PI;
}
for (int x = 1; x <= divisions; x++)
{
Vector3 end = new Vector3(a.Radius, 0, 0);
if (a.Direction != ArcDirection.CCW)
{
end.Roll(a.StartAngle + stretch * (double)x / (double)divisions);
}
else
{
end.Roll(a.StartAngle - stretch * (double)x / (double)divisions);
}
end += a.Center;
end.Z = a.Start.Z + (a.End.Z - a.Start.Z) * (double)x / (double)divisions;
end.Z += map.GetHeightAt(end.X, end.Y);
Arc arc = new Arc(pos, end, a.Center, a.Direction);
if (x == 1)
{
arc.FeedRate = a.FeedRate;
}
adjusted.Add(arc);
pos = end;
}
}
}
}
return(adjusted);
}