private void joinShapes(List <Point2Dmm[]> shapes, int shape1Index, Point2Dmm shape1Point, int shape2Index, Point2Dmm shape2Point)
{
var shape1 = shapes[shape1Index];
var shape2 = shapes[shape2Index];
for (var i = 0; i < shape1.Length; ++i)
{
var point = shape1[i];
if (point != shape1Point)
{
continue;
}
var reorderedShape2 = reorderClosedShapeTo(shape2, shape2Point);
var newShape = shape1.Take(i + 1).Concat(reorderedShape2).Concat(new[] { reorderedShape2.First() }).Concat(shape1.Skip(i)).ToArray();
var targetShape1 = shapes[shape1Index];
var targetShape2 = shapes[shape2Index];
for (var j = 0; j < shapes.Count; ++j)
{
if (shapes[j] == targetShape1 || shapes[j] == targetShape2)
{
shapes[j] = newShape;
}
}
return;
}
throw new InvalidOperationException("Join was not successful");
}
private IEnumerable <Point2Dmm> joinParts(IEnumerable <Point2Dmm[]> parts)
{
var closedParts = new List <Point2Dmm[]>();
foreach (var part in parts)
{
//for joining we need closed parts
var closedPart = part.Concat(new[] { part[0] }).ToArray();
closedParts.Add(closedPart);
}
//fill table of join length between shapes
var joinLengthSqr = new double[closedParts.Count, closedParts.Count];
var joinStart = new Point2Dmm[closedParts.Count, closedParts.Count];
var joinEnd = new Point2Dmm[closedParts.Count, closedParts.Count];
for (var shape1Index = 0; shape1Index < closedParts.Count - 1; ++shape1Index)
{
joinLengthSqr[shape1Index, shape1Index] = double.PositiveInfinity;
var shape1 = closedParts[shape1Index];
for (var shape2Index = shape1Index + 1; shape2Index < closedParts.Count; ++shape2Index)
{
var shape2 = closedParts[shape2Index];
joinLengthSqr[shape2Index, shape1Index] = double.PositiveInfinity;
joinLengthSqr[shape1Index, shape2Index] = double.PositiveInfinity;
for (var shape1PointIndex = 0; shape1PointIndex < shape1.Length; ++shape1PointIndex)
{
for (var shape2PointIndex = 0; shape2PointIndex < shape2.Length; ++shape2PointIndex)
{
var shape1Point = shape1[shape1PointIndex];
var shape2Point = shape2[shape2PointIndex];
var diffX = shape1Point.C1 - shape2Point.C1;
var diffY = shape1Point.C2 - shape2Point.C2;
var lengthSqr = 1.0 * diffX * diffX + diffY * diffY;
if (lengthSqr < joinLengthSqr[shape1Index, shape2Index])
{
joinLengthSqr[shape1Index, shape2Index] = lengthSqr;
joinStart[shape1Index, shape2Index] = shape1Point;
joinEnd[shape1Index, shape2Index] = shape2Point;
}
}
}
}
}
//join shapes by the shortest joins
for (var i = 0; i < closedParts.Count - 1; ++i)
{
//n shapes will have n-1 joins
var bestShape1Index = 0;
var bestShape2Index = 0;
for (var shape1Index = 0; shape1Index < closedParts.Count - 1; ++shape1Index)
{
for (var shape2Index = shape1Index + 1; shape2Index < closedParts.Count; ++shape2Index)
{
var shape1 = closedParts[shape1Index];
var shape2 = closedParts[shape2Index];
if (shape1 == shape2)
{
//shapes are already joined
continue;
}
if (joinLengthSqr[shape1Index, shape2Index] < joinLengthSqr[bestShape1Index, bestShape2Index])
{
bestShape1Index = shape1Index;
bestShape2Index = shape2Index;
}
}
}
var shape1Point = joinStart[bestShape1Index, bestShape2Index];
var shape2Point = joinEnd[bestShape1Index, bestShape2Index];
joinShapes(closedParts, bestShape1Index, shape1Point, bestShape2Index, shape2Point);
joinLengthSqr[bestShape1Index, bestShape2Index] = double.NaN;
}
//all slots should contain same shape
return(closedParts[0]);
}
// if the two points p1 and p2 are both on the same side of the line a,b, return true
private static bool PointsOnSameSide(Point2Dmm p1, Point2Dmm p2, Point2Dmm a, Point2Dmm b)
{
// these are probably the most interesting three lines of code in the algorithm (probably because I don't fully understand them)
// the concept is nicely described at http://www.blackpawn.com/texts/pointinpoly/default.html
double cp1 = CrossProduct(VSub(b, a), VSub(p1, a));
double cp2 = CrossProduct(VSub(b, a), VSub(p2, a));
return((cp1 * cp2) >= 0); // they have the same sign if on the same side of the line
}
// subtract the vector (point) b from the vector (point) a
private static Point2Dmm VSub(Point2Dmm a, Point2Dmm b)
{
return(new Point2Dmm(a.C1 - b.C1, a.C2 - b.C2));
}
private static bool IsSamePoint(Point2Dmm pt1, Point2Dmm pt2)
{
return(pt1.C1 == pt2.C1 && pt1.C2 == pt2.C2);
}
// find the cross product of two x,y vectors, which is always a single value, z, representing the three dimensional vector (0,0,z)
private static double CrossProduct(Point2Dmm p1, Point2Dmm p2)
{
return((p1.C1 * p2.C2) - (p1.C2 * p2.C1));
}
void _statusTimer_Tick(object sender, EventArgs e)
{
var currentU = Cnc.EstimationU;
var currentV = Cnc.EstimationV;
var currentX = Cnc.EstimationX;
var currentY = Cnc.EstimationY;
var positionU = Configuration.MilimetersPerStep * (currentU - _positionOffsetU);
var positionV = Configuration.MilimetersPerStep * (currentV - _positionOffsetV);
var positionX = Configuration.MilimetersPerStep * (currentX - _positionOffsetX);
var positionY = Configuration.MilimetersPerStep * (currentY - _positionOffsetY);
PositionU.Text = positionU.ToString("0.000");
PositionV.Text = positionV.ToString("0.000");
PositionX.Text = positionX.ToString("0.000");
PositionY.Text = positionY.ToString("0.000");
positionU = Configuration.MilimetersPerStep * currentU;
positionV = Configuration.MilimetersPerStep * currentV;
positionX = Configuration.MilimetersPerStep * currentX;
positionY = Configuration.MilimetersPerStep * currentY;
var uv = new Point2Dmm(positionU, positionV);
var xy = new Point2Dmm(positionX, positionY);
Workspace.HeadUV.Position = uv;
Workspace.HeadXY.Position = xy;
if (_isPlanRunning)
{
var remainingTicks = Cnc.RemainingPlanTickEstimation;
var remainingSeconds = (int)(remainingTicks / Configuration.TimerFrequency);
if (_lastRemainingSeconds > remainingSeconds || Math.Abs(_lastRemainingSeconds - remainingSeconds) > 2)
{
_lastRemainingSeconds = remainingSeconds;
var remainingTime = new TimeSpan(0, 0, 0, _lastRemainingSeconds);
var elapsedTime = new TimeSpan(0, 0, 0, (int)(DateTime.Now - _planStart).TotalSeconds);
if (remainingTime.TotalDays < 2)
{
ShowMessage(elapsedTime.ToString() + "/" + remainingTime.ToString());
}
}
}
if (Cnc.CurrentState.IsHomeCalibrated)
{
Calibration.Foreground = Brushes.Black;
}
else
{
var blinkFrequency = 1000;
if (DateTime.Now.Millisecond % blinkFrequency > blinkFrequency / 2)
{
Calibration.Foreground = Brushes.Red;
}
else
{
Calibration.Foreground = Brushes.Green;
}
}
}
private static double diffProduct(Point2Dmm p1, Point2Dmm p2)
{
return((p2.C1 - p1.C1) * (p2.C2 + p1.C2));
}
private static int[] getTriangleIndexes(bool needClockwise, Point2Dmm p0, int i0, Point2Dmm p1, int i1, Point2Dmm p2, int i2)
{
var isClockwise = diffProduct(p0, p1) + diffProduct(p1, p2) + diffProduct(p2, p0) >= 0;
var isClockwise2 = diffProduct(p2, p1) + diffProduct(p1, p0) + diffProduct(p0, p2) >= 0;
if (isClockwise == isClockwise2)
{
//throw new NotSupportedException();
return(new int[0]);
}
if (!isClockwise)
{
needClockwise = !needClockwise;
}
var triangleIndexes = needClockwise ? new[] { i0, i1, i2 } : new[] { i2, i1, i0 };
return(triangleIndexes);
}
