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); }