public static IEnumerable <GCodeCommand> ApplyHeightMap(this IEnumerable <GCodeCommand> commands, HeightMap map)
{
foreach (GCodeCommand command in commands)
{
if (command is OtherCode)
{
yield return(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);
yield return(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, (float)x / (float)divisions);
end.Z += map.GetHeightAt(end.X, end.Y);
Straight st = new Straight(pos, end, false);
if (x == 1)
{
st.FeedRate = s.FeedRate;
}
yield return(st);
pos = end;
}
}
}
if (m is Arc)
{
Arc a = (Arc)m;
Vector3 pos = a.Start;
float stretch = a.StartAngle - a.EndAngle;
if (stretch <= 0)
{
stretch += 2 * (float)Math.PI;
}
if (a.Direction == ArcDirection.CCW)
{
stretch = 2 * (float)Math.PI - stretch;
}
if (stretch <= 0)
{
stretch += 2 * (float)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 * (float)x / (float)divisions);
}
else
{
end.Roll(a.StartAngle - stretch * (float)x / (float)divisions);
}
end += a.Center;
end.Z = a.Start.Z + (a.End.Z - a.Start.Z) * (float)x / (float)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;
}
yield return(arc);
pos = end;
}
}
}
}
yield break;
}
public static IEnumerable<GCodeCommand> ApplyHeightMap(this IEnumerable<GCodeCommand> commands, HeightMap map)
{
foreach (GCodeCommand command in commands)
{
if (command is OtherCode)
{
yield return 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);
yield return 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, (float)x / (float)divisions);
end.Z += map.GetHeightAt(end.X, end.Y);
Straight st = new Straight(pos, end, false);
if (x == 1)
st.FeedRate = s.FeedRate;
yield return st;
pos = end;
}
}
}
if (m is Arc)
{
Arc a = (Arc)m;
Vector3 pos = a.Start;
float stretch = a.StartAngle - a.EndAngle;
if (stretch <= 0)
stretch += 2 * (float)Math.PI;
if (a.Direction == ArcDirection.CCW)
{
stretch = 2 * (float)Math.PI - stretch;
}
if (stretch <= 0)
stretch += 2 * (float)Math.PI;
for (int x = 1; x <= divisions; x++)
{
Vector3 end = new Vector3(a.Radius, 0, 0);
if (a.Direction != ArcDirection.CW)
end.Roll(a.StartAngle + stretch * (float)x / (float)divisions);
else
end.Roll(a.StartAngle - stretch * (float)x / (float)divisions);
end += a.Center;
end.Z = a.Start.Z + (a.End.Z - a.Start.Z) * (float)x / (float)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;
yield return arc;
pos = end;
}
}
}
}
yield break;
}
void RedrawPreview()
{
Stopwatch stopW = Stopwatch.StartNew();
if (pictureBox1.Image != null)
{
pictureBox1.Image.Dispose();
pictureBox1.Image = null;
}
Bounds dimensions = Commands.Dimensions();
dimensions.MinX -= 5;
dimensions.MaxX += 5;
dimensions.MinY -= 5;
dimensions.MaxY += 5;
float pxPerMM = Math.Min(
(float)pictureBox1.Width / dimensions.SizeX,
(float)pictureBox1.Height / dimensions.SizeY);
Bitmap b = new Bitmap((int)(dimensions.SizeX * pxPerMM), (int)(dimensions.SizeY * pxPerMM));
Graphics gfx = Graphics.FromImage(b);
gfx.Clear(Color.Snow);
float pensize = Math.Min(pictureBox1.Width, pictureBox1.Height) / 360f;
if (pensize < 1)
{
pensize = 1;
}
Pen straightPen = new Pen(Color.Black, pensize);
Pen rapidPen = new Pen(Color.Gray, pensize / 2f);
Pen arcPen = new Pen(Color.DarkBlue, pensize);
Func <float, float> getX = (x) => { return((x - dimensions.MinX) * pxPerMM); };
Func <float, float> getY = (y) => { return(b.Height - (y - dimensions.MinY) * pxPerMM); };
Func <float, float> deg = (rad) => { return(rad * 180 / (float)Math.PI); };
foreach (GCodeCommand command in Commands)
{
Arc arc = command as Arc;
if (arc != null)
{
//System.Drawing interprets angles in exactly the opposite of the "industry standard" System.Math way ... thanks for that MS
//countless hours of trial and error rearranging these few lines... don't you dare touch them
float start = 360 - deg(arc.StartAngle);
float end = 360 - deg(arc.EndAngle);
float sweepAngle = start - end;
if (sweepAngle < 0)
{
sweepAngle += 360;
}
if (arc.Direction == ArcDirection.CW)
{
sweepAngle = 360 - sweepAngle;
}
else
{
start = end;
}
gfx.DrawArc(arcPen,
getX(arc.Center.X - arc.Radius),
getY(arc.Center.Y + arc.Radius),
arc.Radius * 2 * pxPerMM,
arc.Radius * 2 * pxPerMM,
start,
sweepAngle
);
}
Straight str = command as Straight;
if (str != null)
{
gfx.DrawLine(str.Rapid ? rapidPen : straightPen,
getX(str.Start.X),
getY(str.Start.Y),
getX(str.End.X),
getY(str.End.Y)
);
}
}
gfx.Flush();
gfx.Dispose();
straightPen.Dispose();
rapidPen.Dispose();
arcPen.Dispose();
pictureBox1.Image = b;
stopW.Stop();
Console.WriteLine("Updating GCode Preview: {0} ms", stopW.Elapsed.TotalMilliseconds);
}
private GCodeCommand ParseLine(MatchCollection matches)
{
if (matches[0].Groups[1].Value != "G")
{
return(new OtherCode(matches));
}
if (matches[0].Groups[2].Value.Contains('.'))
{
return(new OtherCode(matches));
}
int Command = int.Parse(matches[0].Groups[2].Value, inv);
float?X = null, Y = null, Z = null, I = null, J = null, F = null, R = null;
for (int index = 1; index < matches.Count; index++)
{
float value = float.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) ? (float)X : EndPosition.X + (float)X;
}
if (Y.HasValue)
{
EndPosition.Y = (DistanceMode == ParseDistanceMode.Absolute) ? (float)Y : EndPosition.Y + (float)Y;
}
if (Z.HasValue)
{
EndPosition.Z = (DistanceMode == ParseDistanceMode.Absolute) ? (float)Z : EndPosition.Z + (float)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) ? (float)I : Position.X + (float)I;
}
if (J.HasValue)
{
center.Y = (ArcDistanceMode == ParseDistanceMode.Absolute) ? (float)J : Position.Y + (float)J;
}
if (R.HasValue)
{
Vector3 Parallel = EndPosition - Position;
Vector3 Perpendicular = Parallel.CrossProduct(new Vector3(0, 0, 1));
float PerpLength = (float)Math.Sqrt(((float)R * (float)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));
}
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);
}
}
float? X = null, Y = null, Z = null, I = null, J = null, F = null, R = null;
for (int index = 0; index < matches.Count; index++)
{
float value = float.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) ? (float)X : EndPosition.X + (float)X;
}
if (Y.HasValue)
{
EndPosition.Y = (DistanceMode == ParseDistanceMode.Absolute) ? (float)Y : EndPosition.Y + (float)Y;
}
if (Z.HasValue)
{
EndPosition.Z = (DistanceMode == ParseDistanceMode.Absolute) ? (float)Z : EndPosition.Z + (float)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) ? (float)I : Position.X + (float)I;
}
if (J.HasValue)
{
center.Y = (ArcDistanceMode == ParseDistanceMode.Absolute) ? (float)J : Position.Y + (float)J;
}
if (R.HasValue)
{
Vector3 Parallel = EndPosition - Position;
Vector3 Perpendicular = Parallel.CrossProduct(new Vector3(0, 0, 1));
float PerpLength = (float)Math.Sqrt(((float)R * (float)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);
}
