diff --git a/PepLib.Core/Models/Program.cs b/PepLib.Core/Models/Program.cs index ac83971..6e80905 100644 --- a/PepLib.Core/Models/Program.cs +++ b/PepLib.Core/Models/Program.cs @@ -18,7 +18,7 @@ namespace PepLib.Models public ProgrammingMode Mode { - get { return mode; } + get => mode; set { if (value == ProgrammingMode.Absolute) @@ -37,12 +37,9 @@ namespace PepLib.Models var pos = new Vector(0, 0); - for (int i = 0; i < Count; ++i) + foreach (var code in this) { - var code = this[i]; - var motion = code as Motion; - - if (motion != null) + if (code is Motion motion) { var pos2 = motion.EndPoint; motion.Offset(-pos.X, -pos.Y); @@ -60,12 +57,9 @@ namespace PepLib.Models var pos = new Vector(0, 0); - for (int i = 0; i < Count; ++i) + foreach (var code in this) { - var code = this[i]; - var motion = code as Motion; - - if (motion != null) + if (code is Motion motion) { motion.Offset(pos); pos = motion.EndPoint; @@ -75,112 +69,38 @@ namespace PepLib.Models mode = ProgrammingMode.Absolute; } - public virtual void Rotate(double angle) - { - var mode = Mode; + public virtual void Rotate(double angle) => + ApplyTransform(code => (code as IMovable)?.Rotate(angle), angle); + public virtual void Rotate(double angle, Vector origin) => + ApplyTransform(code => (code as IMovable)?.Rotate(angle, origin), angle); + + public void Offset(double x, double y) => + ApplyTransform(code => (code as IMovable)?.Offset(x, y)); + + public void Offset(Vector voffset) => + ApplyTransform(code => (code as IMovable)?.Offset(voffset)); + + private void ApplyTransform(Action transform, double? rotationAngle = null) + { + var previousMode = Mode; SetProgrammingModeAbs(); - for (int i = 0; i < Count; ++i) + foreach (var code in this) { - var code = this[i]; - - if (code.CodeType() == CodeType.SubProgramCall) + if (rotationAngle.HasValue && code is SubProgramCall subpgm) { - var subpgm = (SubProgramCall)code; - - if (subpgm.Loop != null) - subpgm.Loop.Rotate(angle); + subpgm.Loop?.Rotate(rotationAngle.Value); } - if (code is IMovable == false) - continue; - - var code2 = (IMovable)code; - - code2.Rotate(angle); + transform(code); } - if (mode == ProgrammingMode.Incremental) + if (previousMode == ProgrammingMode.Incremental) SetProgrammingModeInc(); - Rotation = MathHelper.NormalizeAngleRad(Rotation + angle); - } - - public virtual void Rotate(double angle, Vector origin) - { - var mode = Mode; - - SetProgrammingModeAbs(); - - for (int i = 0; i < Count; ++i) - { - var code = this[i]; - - if (code.CodeType() == CodeType.SubProgramCall) - { - var subpgm = (SubProgramCall)code; - - if (subpgm.Loop != null) - subpgm.Loop.Rotate(angle); - } - - if (code is IMovable == false) - continue; - - var code2 = (IMovable)code; - - code2.Rotate(angle, origin); - } - - if (mode == ProgrammingMode.Incremental) - SetProgrammingModeInc(); - - Rotation = MathHelper.NormalizeAngleRad(Rotation + angle); - } - - public void Offset(double x, double y) - { - var mode = Mode; - - SetProgrammingModeAbs(); - - for (int i = 0; i < Count; ++i) - { - var code = this[i]; - - if (code is IMovable == false) - continue; - - var code2 = (IMovable)code; - - code2.Offset(x, y); - } - - if (mode == ProgrammingMode.Incremental) - SetProgrammingModeInc(); - } - - public void Offset(Vector voffset) - { - var mode = Mode; - - SetProgrammingModeAbs(); - - for (int i = 0; i < Count; ++i) - { - var code = this[i]; - - if (code is IMovable == false) - continue; - - var code2 = (IMovable)code; - - code2.Offset(voffset); - } - - if (mode == ProgrammingMode.Incremental) - SetProgrammingModeInc(); + if (rotationAngle.HasValue) + Rotation = MathHelper.NormalizeAngleRad(Rotation + rotationAngle.Value); } public Box GetBoundingBox() @@ -191,171 +111,78 @@ namespace PepLib.Models private Box GetBoundingBox(ref Vector pos) { - double minX = 0.0; - double minY = 0.0; - double maxX = 0.0; - double maxY = 0.0; + var bounds = new BoundsTracker(); - for (int i = 0; i < Count; ++i) + foreach (var code in this) { - var code = this[i]; - - switch (code.CodeType()) + switch (code) { - case CodeType.LinearMove: - { - var line = (LinearMove)code; - var pt = Mode == ProgrammingMode.Absolute ? - line.EndPoint : - line.EndPoint + pos; + case LinearMove line: + pos = ProcessLinearMotion(line.EndPoint, pos, bounds); + break; - if (pt.X > maxX) - maxX = pt.X; - else if (pt.X < minX) - minX = pt.X; + case RapidMove rapid: + pos = ProcessLinearMotion(rapid.EndPoint, pos, bounds); + break; - if (pt.Y > maxY) - maxY = pt.Y; - else if (pt.Y < minY) - minY = pt.Y; + case CircularMove arc: + pos = ProcessCircularMotion(arc, pos, bounds); + break; - pos = pt; - - break; - } - - case CodeType.RapidMove: - { - var line = (RapidMove)code; - var pt = Mode == ProgrammingMode.Absolute - ? line.EndPoint - : line.EndPoint + pos; - - if (pt.X > maxX) - maxX = pt.X; - else if (pt.X < minX) - minX = pt.X; - - if (pt.Y > maxY) - maxY = pt.Y; - else if (pt.Y < minY) - minY = pt.Y; - - pos = pt; - - break; - } - - case CodeType.CircularMove: - { - var arc = (CircularMove)code; - var radius = arc.CenterPoint.DistanceTo(arc.EndPoint); - - Vector endpt; - Vector centerpt; - - if (Mode == ProgrammingMode.Incremental) - { - endpt = arc.EndPoint + pos; - centerpt = arc.CenterPoint + pos; - } - else - { - endpt = arc.EndPoint; - centerpt = arc.CenterPoint; - } - - double minX1; - double minY1; - double maxX1; - double maxY1; - - if (pos.X < endpt.X) - { - minX1 = pos.X; - maxX1 = endpt.X; - } - else - { - minX1 = endpt.X; - maxX1 = pos.X; - } - - if (pos.Y < endpt.Y) - { - minY1 = pos.Y; - maxY1 = endpt.Y; - } - else - { - minY1 = endpt.Y; - maxY1 = pos.Y; - } - - var startAngle = pos.AngleFrom(centerpt); - var endAngle = endpt.AngleFrom(centerpt); - - // switch the angle to counter clockwise. - if (arc.Rotation == RotationType.CW) - Generic.Swap(ref startAngle, ref endAngle); - - startAngle = MathHelper.NormalizeAngleRad(startAngle); - endAngle = MathHelper.NormalizeAngleRad(endAngle); - - if (MathHelper.IsAngleBetween(MathHelper.HalfPI, startAngle, endAngle)) - maxY1 = centerpt.Y + radius; - - if (MathHelper.IsAngleBetween(Math.PI, startAngle, endAngle)) - minX1 = centerpt.X - radius; - - const double oneHalfPI = Math.PI * 1.5; - - if (MathHelper.IsAngleBetween(oneHalfPI, startAngle, endAngle)) - minY1 = centerpt.Y - radius; - - if (MathHelper.IsAngleBetween(MathHelper.TwoPI, startAngle, endAngle)) - maxX1 = centerpt.X + radius; - - if (maxX1 > maxX) - maxX = maxX1; - - if (minX1 < minX) - minX = minX1; - - if (maxY1 > maxY) - maxY = maxY1; - - if (minY1 < minY) - minY = minY1; - - pos = endpt; - - break; - } - - case CodeType.SubProgramCall: - { - var subpgm = (SubProgramCall)code; - var box = subpgm.Loop.GetBoundingBox(ref pos); - - if (box.Left < minX) - minX = box.Left; - - if (box.Right > maxX) - maxX = box.Right; - - if (box.Bottom < minY) - minY = box.Bottom; - - if (box.Top > maxY) - maxY = box.Top; - - break; - } + case SubProgramCall subpgm: + var box = subpgm.Loop.GetBoundingBox(ref pos); + bounds.ExpandTo(box); + break; } } - return new Box(minX, minY, maxX - minX, maxY - minY); + return bounds.ToBox(); + } + + private Vector ProcessLinearMotion(Vector endPoint, Vector pos, BoundsTracker bounds) + { + var pt = Mode == ProgrammingMode.Absolute ? endPoint : endPoint + pos; + bounds.Include(pt); + return pt; + } + + private Vector ProcessCircularMotion(CircularMove arc, Vector pos, BoundsTracker bounds) + { + var radius = arc.CenterPoint.DistanceTo(arc.EndPoint); + + var (endpt, centerpt) = Mode == ProgrammingMode.Incremental + ? (arc.EndPoint + pos, arc.CenterPoint + pos) + : (arc.EndPoint, arc.CenterPoint); + + // Start with endpoint bounds + var minX = Math.Min(pos.X, endpt.X); + var maxX = Math.Max(pos.X, endpt.X); + var minY = Math.Min(pos.Y, endpt.Y); + var maxY = Math.Max(pos.Y, endpt.Y); + + // Check if arc crosses cardinal directions + var startAngle = MathHelper.NormalizeAngleRad(pos.AngleFrom(centerpt)); + var endAngle = MathHelper.NormalizeAngleRad(endpt.AngleFrom(centerpt)); + + // Switch angles for clockwise arcs + if (arc.Rotation == RotationType.CW) + Generic.Swap(ref startAngle, ref endAngle); + + // Expand bounds if arc crosses cardinal points + if (MathHelper.IsAngleBetween(MathHelper.HalfPI, startAngle, endAngle)) + maxY = centerpt.Y + radius; + + if (MathHelper.IsAngleBetween(Math.PI, startAngle, endAngle)) + minX = centerpt.X - radius; + + if (MathHelper.IsAngleBetween(Math.PI * 1.5, startAngle, endAngle)) + minY = centerpt.Y - radius; + + if (MathHelper.IsAngleBetween(MathHelper.TwoPI, startAngle, endAngle)) + maxX = centerpt.X + radius; + + bounds.Expand(minX, minY, maxX, maxY); + return endpt; } public static Program Load(Stream stream) @@ -364,5 +191,36 @@ namespace PepLib.Models reader.Read(stream); return reader.Program; } + + private class BoundsTracker + { + private double minX, minY, maxX, maxY; + + public void Include(Vector pt) + { + if (pt.X < minX) minX = pt.X; + if (pt.X > maxX) maxX = pt.X; + if (pt.Y < minY) minY = pt.Y; + if (pt.Y > maxY) maxY = pt.Y; + } + + public void Expand(double left, double bottom, double right, double top) + { + if (left < minX) minX = left; + if (right > maxX) maxX = right; + if (bottom < minY) minY = bottom; + if (top > maxY) maxY = top; + } + + public void ExpandTo(Box box) + { + if (box.Left < minX) minX = box.Left; + if (box.Right > maxX) maxX = box.Right; + if (box.Bottom < minY) minY = box.Bottom; + if (box.Top > maxY) maxY = box.Top; + } + + public Box ToBox() => new(minX, minY, maxX - minX, maxY - minY); + } } }