public static GridDirection GetDirectionFromRotation(Angle pitch, Angle yaw, Angle roll) { Vector3 v = new Vector3(1, 0, 0); v.Pitch(pitch); v.Yaw(yaw); v.Roll(roll); v.X = Math.Round(v.X, 5); v.Y = Math.Round(v.Y, 5); v.Z = Math.Round(v.Z, 5); double val = Math.Sqrt(2 - Math.Sqrt(2)) / 2; if (v.X <= 1 && v.X >= val) v.X = 1; else if (v.X <= val && v.X >= 0) v.X = 0; else if (v.X >= -1 && v.X <= -val) v.X = -1; else if (v.X >= -val && v.X <= 0) v.X = 0; if (v.Y <= 1 && v.Y >= val) v.Y = -1; else if (v.Y <= val && v.Y >= 0) v.Y = 0; else if (v.Y >= -1 && v.Y <= -val) v.Y = 1; else if (v.Y >= -val && v.Y <= 0) v.Y = 0; if (v.Z <= 1 && v.Z >= val) v.Z = 1; else if (v.Z <= val && v.Z >= 0) v.Z = 0; else if (v.Z >= -1 && v.Z <= -val) v.Z = -1; else if (v.Z >= -val && v.Z <= 0) v.Z = 0; return v.AsDirection(); }
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; }
public static GridDirection GetDirectionFromRotation(int pitch, int yaw, int roll) { Vector3 v = new Vector3(1, 0, 0); v.Pitch(new Angle(1, pitch, 0, 0)); v.Yaw(new Angle(1, yaw, 0, 0)); v.Roll(new Angle(1, roll, 0, 0)); v.X = Math.Round(v.X, 5); v.Y = Math.Round(v.Y, 5); v.Z = Math.Round(v.Z, 5); double val = Math.Sqrt(2 - Math.Sqrt(2))/2; if (v.X <= 1 && v.X >= val) v.X = 1; else if (v.X <= val && v.X >= 0) v.X = 0; else if (v.X >= -1 && v.X <= -val) v.X = -1; else if (v.X >= -val && v.X <= 0) v.X = 0; if (v.Y <= 1 && v.Y >= val) v.Y = -1; else if (v.Y <= val && v.Y >= 0) v.Y = 0; else if (v.Y >= -1 && v.Y <= -val) v.Y = 1; else if (v.Y >= -val && v.Y <= 0) v.Y = 0; if (v.Z <= 1 && v.Z >= val) v.Z = 1; else if (v.Z <= val && v.Z >= 0) v.Z = 0; else if (v.Z >= -1 && v.Z <= -val) v.Z = -1; else if (v.Z >= -val && v.Z <= 0) v.Z = 0; return v.AsDirection(); }