예제 #1
0
        private void UpdatePolygon(RobotPose p)
        {
            Matrix3 transMat = Matrix3.Translation(p.x, p.y) * Matrix3.Rotation(p.yaw - Math.PI / 2.0);

            bodyPlygn.Clear();
            bodyPlygn.AddRange(transMat.TransformPoints(origPlygn));
        }
예제 #2
0
		public static Polygon CreateConvexHull(this Polygons polygons)
		{
			Polygon allPoints = new Polygon();
			foreach (Polygon polygon in polygons)
			{
				allPoints.AddRange(polygon);
			}

			return allPoints.CreateConvexHull();
		}
예제 #3
0
        public static Polygon CreateConvexHull(Polygons polygons)
        {
            Polygon allPoints = new Polygon();

            foreach (Polygon polygon in polygons)
            {
                allPoints.AddRange(polygon);
            }

            return(CreateConvexHull(allPoints));
        }
예제 #4
0
        public bool CreatePathInsideBoundary(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, bool optimizePath = true, int layerIndex = -1)
        {
            if (IsSimpleConvex)
            {
                return(true);
            }

            var goodPath = CalculatePath(startPointIn, endPointIn, pathThatIsInside, layerIndex);

            if (goodPath)
            {
                if (optimizePath)
                {
                    OptimizePathPoints(pathThatIsInside);
                }

                if (pathThatIsInside.Count == 0)
                {
                    if ((startPointIn - endPointIn).Length() > InsetAmount * 3)
                    {
                        pathThatIsInside.Add(startPointIn);
                        pathThatIsInside.Add(endPointIn);
                    }
                    else
                    {
                        return(true);
                    }
                }

                CutIntoSmallSegments(pathThatIsInside);

                MovePointsInsideIfPossible(startPointIn, endPointIn, pathThatIsInside);

                var cleanPath = pathThatIsInside.CleanClosedPolygon(InsetAmount / 2);
                pathThatIsInside.Clear();
                pathThatIsInside.AddRange(cleanPath);

                // remove any segment that goes to one point and then back to same point (a -> b -> a)
                RemoveUTurnSegments(startPointIn, endPointIn, pathThatIsInside);
            }

            //Remove0LengthSegments(startPointIn, endPointIn, pathThatIsInside);

            if (saveBadPathToDisk)
            {
                AllPathSegmentsAreInsideOutlines(pathThatIsInside, startPointIn, endPointIn, true, layerIndex);
            }

            CalculatedPath?.Invoke(this, pathThatIsInside, startPointIn, endPointIn);

            return(goodPath);
        }
예제 #5
0
        public Polygon ProjectRectangle(Rectangle rectangle)
        {
            Polygon polygon = new Polygon();

            // get corners counter clockwise starting on the top left and project them to create the polygon:
            polygon.AddRange(new Point[] {
                new Point(rectangle.X, rectangle.Y),                                      // top left
                new Point(rectangle.X, rectangle.Y + rectangle.Height),                   // bottom left
                new Point(rectangle.X + rectangle.Width, rectangle.Y + rectangle.Height), // bottom right
                new Point(rectangle.X + rectangle.Width, rectangle.Y)                     // top right
            }.Select(ProjectPoint));
            return(polygon);
        }
예제 #6
0
        public void UpdatePose(double x, double y, double theta)
        {
            this.x       = (float)x;
            this.y       = (float)y;
            this.heading = (float)theta;

            // Update the polygon that represents the body
            Matrix3 transMat = Matrix3.Translation(x, y) * Matrix3.Rotation(heading - Math.PI / 2.0);

            //transMat.TransformPointsInPlace(origPlygn);
            bodyPlygn.Clear();
            bodyPlygn.AddRange(transMat.TransformPoints(origPlygn));
        }
예제 #7
0
        public void Render(IGraphics g, WorldTransform wt)
        {
            LocalLaneModel laneModel = this.laneModel;

            if (laneModel == null || laneModel.LanePath == null || laneModel.LanePath.Count <= 1)
            {
                return;
            }

            // generate the left and right bounds
            LinePath leftBound  = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0);
            LinePath leftBound2 = null;

            if (laneModel.WidthVariance > 0.01)
            {
                leftBound2 = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0 + Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
            }

            LinePath rightBound  = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0);
            LinePath rightBound2 = null;

            if (laneModel.WidthVariance > 0.01)
            {
                rightBound2 = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0 - Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
            }

            // generate a polygon of the confidence region of the center line
            Polygon centerConfLeft  = null;
            Polygon centerConfRight = null;
            Polygon centerConfFull  = null;

            if (sigma > 0 && laneModel.LaneYVariance != null && laneModel.LaneYVariance.Length == laneModel.LanePath.Count)
            {
                float varianceThreshold = 9f * 9f;
                if (laneModel.LaneYVariance[0] > varianceThreshold)
                {
                    varianceThreshold = 600;
                }
                int numPoints;
                for (numPoints = 0; numPoints < laneModel.LaneYVariance.Length; numPoints++)
                {
                    if (laneModel.LaneYVariance[numPoints] > varianceThreshold)
                    {
                        break;
                    }
                }
                double[] leftDist  = new double[numPoints];
                double[] rightDist = new double[numPoints];

                double prevDist = 0;
                for (int i = 0; i < numPoints; i++)
                {
                    double dist = laneModel.LaneYVariance[i];
                    if (dist > 0.01)
                    {
                        dist         = Math.Sqrt(dist) * sigma;
                        leftDist[i]  = dist;
                        rightDist[i] = -dist;
                        prevDist     = dist;
                    }
                    else
                    {
                        leftDist[i]  = prevDist;
                        rightDist[i] = -prevDist;
                    }
                }

                // get the subset of the lane model we're interested in
                LinePath subset = laneModel.LanePath.SubPath(0, numPoints - 1);

                LinePath left  = subset.ShiftLateral(leftDist);
                LinePath right = subset.ShiftLateral(rightDist);

                Coordinates midTop    = (left[left.Count - 1] + right[right.Count - 1]) / 2.0;
                Coordinates midBottom = (left[0] + right[0]) / 2.0;

                centerConfLeft = new Polygon(numPoints + 2);
                centerConfLeft.Add(midTop);
                centerConfLeft.Add(midBottom);
                centerConfLeft.AddRange(left);

                centerConfRight = new Polygon(numPoints + 2);
                centerConfRight.Add(midTop);
                centerConfRight.Add(midBottom);
                centerConfRight.AddRange(right);

                centerConfFull = new Polygon(numPoints * 2);
                right.Reverse();
                centerConfFull.AddRange(right);
                centerConfFull.AddRange(left);
            }

            // draw the shits
            IPen pen = g.CreatePen();

            pen.Width = 1.0f / wt.Scale;
            pen.Color = color;

            // first draw the confidence polygon
            if (centerConfFull != null)
            {
                g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfLeft));
                g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfRight));
                pen.Color = Color.FromArgb(30, color);
                g.DrawPolygon(pen, Utility.ToPointF(centerConfFull));
            }

            // next draw the center line
            pen.Color = color;
            //g.DrawLines(pen, Utility.ToPointF(laneModel.LanePath));

            //// next draw the left lane confidence bound
            //if (leftBound2 != null) {
            //  pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
            //  g.DrawLines(pen, Utility.ToPointF(leftBound2));
            //}

            //// draw the right lane confidence bound
            //if (rightBound2 != null) {
            //  pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
            //  g.DrawLines(pen, Utility.ToPointF(rightBound2));
            //}

            // draw the left bound
            pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid;
            g.DrawLines(pen, Utility.ToPointF(leftBound));
            // draw the right bound
            g.DrawLines(pen, Utility.ToPointF(rightBound));

            // draw the model confidence
            string labelString = laneModel.Probability.ToString("F3");
            SizeF  stringSize  = g.MeasureString(labelString, labelFont);

            stringSize.Width  /= wt.Scale;
            stringSize.Height /= wt.Scale;
            RectangleF rect         = new RectangleF(Utility.ToPointF(laneModel.LanePath[0]), stringSize);
            float      inflateValue = 4 / wt.Scale;

            rect.X -= inflateValue;
            rect.Y -= inflateValue;
            g.FillRectangle(Color.FromArgb(127, Color.White), rect);
            g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(laneModel.LanePath[0]));

            prevLeftBound  = leftBound;
            prevRightBound = rightBound;
        }
예제 #8
0
        static void Main(string[] args)
        {
            #region 枚举与switch case
            //Color c = (Color)(new Random()).Next(0, 3);
            //switch (c)
            //{
            //    case Color.Red:
            //        Console.WriteLine("The color is red");
            //        break;
            //    case Color.Green:
            //        Console.WriteLine("The color is green");
            //        break;
            //    case Color.Blue:
            //        Console.WriteLine("The color is blue");
            //        break;
            //    default:
            //        Console.WriteLine("The color is unknown.");
            //        break;
            //}
            #endregion

            #region 值类型与引用类型测试
            //Type t = typeof(DateTime);
            //Type t = typeof(string);
            //Type t = typeof(IShape);
            //Type t = typeof(Action);
            //Type t = typeof(int[]);

            //if (t.IsValueType)
            //{
            //    Console.WriteLine($"{t.Name} is ValueType");
            //}
            //else
            //    Console.WriteLine($"{t.Name} is ReferenceType");
            #endregion

            #region 排序算法:冒泡排序与快速排序

            /*
             *          //int[] arr = new int[] { 9, 3, 1, 4, 2, 7, 8, 6, 5, -100, 0, -30, -88 };
             * //List<int> list = new List<int>(arr);
             *
             * Random rd = new Random();
             * int n = 10000000;
             * List<int> list = new List<int>(n);
             * for (int i = 0; i < n; i++)
             * {
             *  list.Add(rd.Next(1, n * 100));
             * }
             *
             * //Console.WriteLine($"排序前的数据:");
             * //Console.WriteLine(string.Join(',', arr));
             *
             * //DateTime dt1 = DateTime.Now;
             * //Sort.BubbleSort(arr);
             * //DateTime dt2 = DateTime.Now;
             *
             * //Console.WriteLine($"排序后的数据:");
             * //Console.WriteLine(string.Join(',', arr));
             *
             * //Console.WriteLine($"排序前的时间:{dt1}");
             * //Console.WriteLine($"排序后的时间:{dt2}");
             *
             * Stopwatch st1 = new Stopwatch(); //计时器用来计算算法要用多久时间
             * st1.Start();
             * //Sort.BubbleSort(list);
             * Sort.QuickSort(list);
             * st1.Stop();
             * Console.WriteLine("运行时间:" + st1.Elapsed);
             * //Console.WriteLine(string.Join(',', list));
             */
            #endregion
            #region SPoint类测试
            //SPoint p1 = new SPoint();
            //Console.WriteLine(p1);

            //SPoint p2 = new SPoint("2", 200, 200, 200);
            //Console.WriteLine(p2);

            //double d = p1.Distance(p2);
            //double d1 = p2.Distance(p1);
            //Console.WriteLine($"p1->p2的距离为{d}");

            //SPoint p3 = new SPoint(300, 300, 300);
            //Console.WriteLine(p3);

            //SPoint p4 = new SPoint(400, 400);
            //p4.Y = -500;
            //Console.WriteLine(p4);

            //SPoint p5 = p3;
            //p5.Y = -500;
            //Console.WriteLine(p5);


            //double d4 = SPoint.Distance(p3, p4);

            //Circle c1 = new Circle(10);
            //Console.WriteLine(c1);
            //c1.R = 20;
            //Console.WriteLine(c1);

            //double rad = ZXY.SurMath.DMStoRad(1.2030);
            ////0.0234165007975906
            //Console.WriteLine($"1 20 30 -> Rad:{rad}");
            #endregion

            #region Event测试代码
            var pub  = new Publisher();
            var sub1 = new Subscriber("sub1", pub);
            var sub2 = new Subscriber("sub2", pub);

            // Call the method that raises the event.
            pub.DoSomething();
            #endregion
            #region 继承与多态测试

            List <Shape> shapes = new List <Shape>();
            shapes.Add(new SPoint());
            shapes.Add(new Circle(10));

            Polyline pl = new Polyline();
            pl.Add(new SPoint(1, 0));
            pl.Add(new SPoint(2, 0));
            pl.Add(new SPoint(3, 0));

            SPoint[] pts = new SPoint[] { new SPoint(4, 0), new SPoint(5, 0), new SPoint(6, 0) };
            pl.AddRange(pts);

            shapes.Add(pl);

            Polyline pl2 = new Polyline();//长度21.494848364936182
            pl2.Add(new SPoint(-1, 0));
            pl2.Add(new SPoint(2, 3));
            pl2.Add(new SPoint(4, 2));

            pts = new SPoint[] { new SPoint(4, 4), new SPoint(6, 8), new SPoint(-2, 5) };
            pl2.AddRange(pts);

            shapes.Add(pl2);


            Polygon pg = new Polygon();
            pg.Add(new SPoint(1, 0));
            pg.Add(new SPoint(2, 0));
            pg.Add(new SPoint(3, 0));

            pts = new SPoint[] { new SPoint(4, 0), new SPoint(5, 0), new SPoint(6, 0) };
            pg.AddRange(pts);

            shapes.Add(pg);

            Polygon pg2 = new Polygon(); //面积28, 长度26.593867878528968
            pg2.AddRange(new SPoint[] {
                new SPoint(-1, 0),
                new SPoint(2, 3),
                new SPoint(4, 2),
                new SPoint(4, 4),
                new SPoint(6, 8),
                new SPoint(-2, 5)
            });

            shapes.Add(pg2);

            //foreach (var item in shapes)
            //{
            //    item.Drawing();
            //}
            DrawAllShapes(shapes);//此处用一个一般化的函数代替上面的循环
            #endregion
        }
예제 #9
0
        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List <Coordinates> polyPoints = new List <Coordinates>();

            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List <Coordinates> updatedPts = new List <Coordinates>();
                        LinePath           interTmp   = ai.InterconnectPath.Clone();
                        Coordinates        pathVec    = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp          = interTmp.ShiftLateral(TahoeParams.VL);
                        rp          = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List <Coordinates> interPoints = new List <Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath  = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il          = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir          = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl        = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr        = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);

                    double      iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);

                    fVec = fVec.Rotate(iRot);
                    double fDeg        = fVec.ToDegrees();
                    double arcTan      = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }
예제 #10
0
        public void WriteQueuedGCode(long layerThickness_um)
        {
            GCodePathConfig lastConfig    = null;
            int             extruderIndex = gcodeExport.GetExtruderIndex();

            for (int pathIndex = 0; pathIndex < paths.Count; pathIndex++)
            {
                var path = paths[pathIndex];
                if (extruderIndex != path.ExtruderIndex)
                {
                    extruderIndex = path.ExtruderIndex;
                    gcodeExport.SwitchExtruder(extruderIndex);
                }
                else if (path.Retract != RetractType.None)
                {
                    double timeOfMove = 0;

                    if (path.Config.LineWidthUM == 0)
                    {
                        var lengthToStart = (gcodeExport.GetPosition() - path.Polygon[0]).Length();
                        var lengthOfMove  = lengthToStart + path.Polygon.PolygonLength();
                        timeOfMove = lengthOfMove / 1000.0 / path.Speed;
                    }

                    gcodeExport.WriteRetraction(timeOfMove, path.Retract == RetractType.Force);
                }
                if (lastConfig != path.Config && path.Config != travelConfig)
                {
                    gcodeExport.WriteComment("TYPE:{0}".FormatWith(path.Config.GCodeComment));
                    lastConfig = path.Config;
                }
                if (path.FanPercent != -1)
                {
                    gcodeExport.WriteFanCommand(path.FanPercent);
                }

                if (path.Polygon.Count == 1 &&
                    path.Config != travelConfig &&
                    (gcodeExport.GetPositionXY() - path.Polygon[0]).ShorterThen(path.Config.LineWidthUM * 2))
                {
                    //Check for lots of small moves and combine them into one large line
                    IntPoint nextPosition = path.Polygon[0];
                    int      i            = pathIndex + 1;
                    while (i < paths.Count && paths[i].Polygon.Count == 1 && (nextPosition - paths[i].Polygon[0]).ShorterThen(path.Config.LineWidthUM * 2))
                    {
                        nextPosition = paths[i].Polygon[0];
                        i++;
                    }
                    if (paths[i - 1].Config == travelConfig)
                    {
                        i--;
                    }

                    if (i > pathIndex + 2)
                    {
                        nextPosition = gcodeExport.GetPosition();
                        for (int x = pathIndex; x < i - 1; x += 2)
                        {
                            long     oldLen   = (nextPosition - paths[x].Polygon[0]).Length();
                            IntPoint newPoint = (paths[x].Polygon[0] + paths[x + 1].Polygon[0]) / 2;
                            long     newLen   = (gcodeExport.GetPosition() - newPoint).Length();
                            if (newLen > 0)
                            {
                                gcodeExport.WriteMove(newPoint, path.Speed, (int)(path.Config.LineWidthUM * oldLen / newLen));
                            }

                            nextPosition = paths[x + 1].Polygon[0];
                        }

                        long lineWidth_um = path.Config.LineWidthUM;
                        if (paths[i - 1].Polygon[0].Width != 0)
                        {
                            lineWidth_um = paths[i - 1].Polygon[0].Width;
                        }

                        gcodeExport.WriteMove(paths[i - 1].Polygon[0], path.Speed, lineWidth_um);
                        pathIndex = i - 1;
                        continue;
                    }
                }

                bool spiralize = path.Config.Spiralize;
                if (spiralize)
                {
                    //Check if we are the last spiralize path in the list, if not, do not spiralize.
                    for (int m = pathIndex + 1; m < paths.Count; m++)
                    {
                        if (paths[m].Config.Spiralize)
                        {
                            spiralize = false;
                        }
                    }
                }

                if (spiralize)                 // if we are still in spiralize mode
                {
                    //If we need to spiralize then raise the head slowly by 1 layer as this path progresses.
                    double   totalLength     = 0;
                    long     z               = gcodeExport.GetPositionZ();
                    IntPoint currentPosition = gcodeExport.GetPositionXY();
                    for (int pointIndex = 0; pointIndex < path.Polygon.Count; pointIndex++)
                    {
                        IntPoint nextPosition = path.Polygon[pointIndex];
                        totalLength    += (currentPosition - nextPosition).LengthMm();
                        currentPosition = nextPosition;
                    }

                    double length = 0.0;
                    currentPosition = gcodeExport.GetPositionXY();
                    for (int i = 0; i < path.Polygon.Count; i++)
                    {
                        IntPoint nextPosition = path.Polygon[i];
                        length         += (currentPosition - nextPosition).LengthMm();
                        currentPosition = nextPosition;
                        IntPoint nextExtrusion = path.Polygon[i];
                        nextExtrusion.Z = (int)(z + layerThickness_um * length / totalLength + .5);
                        gcodeExport.WriteMove(nextExtrusion, path.Speed, path.Config.LineWidthUM);
                    }
                }
                else
                {
                    var loopStart  = gcodeExport.GetPosition();
                    int pointCount = path.Polygon.Count;

                    bool outerPerimeter = path.Config.GCodeComment == "WALL-OUTER";
                    bool innerPerimeter = path.Config.GCodeComment == "WALL-INNER";
                    bool perimeter      = outerPerimeter || innerPerimeter;

                    bool completeLoop = (pointCount > 0 && path.Polygon[pointCount - 1] == loopStart);
                    bool trimmed      = perimeter && completeLoop && perimeterStartEndOverlapRatio < 1;

                    // This is test code to remove double drawn small perimeter lines.
                    if (trimmed)
                    {
                        long targetDistance = (long)(path.Config.LineWidthUM * (1 - perimeterStartEndOverlapRatio));
                        path = TrimGCodePathEnd(path, targetDistance);
                        // update the point count after trimming
                        pointCount = path.Polygon.Count;
                    }

                    for (int i = 0; i < pointCount; i++)
                    {
                        long lineWidth_um = path.Config.LineWidthUM;
                        if (path.Polygon[i].Width != 0)
                        {
                            lineWidth_um = path.Polygon[i].Width;
                        }

                        gcodeExport.WriteMove(path.Polygon[i], path.Speed, lineWidth_um);
                    }

                    if (trimmed)
                    {
                        // go back to the start of the loop
                        gcodeExport.WriteMove(loopStart, path.Speed, 0);

                        var length = path.Polygon.PolygonLength(false);
                        if (outerPerimeter &&
                            config.CoastAtEndDistance_um > 0 &&
                            length > config.CoastAtEndDistance_um)
                        {
                            //gcodeExport.WriteRetraction
                            var wipePoly = new Polygon(new IntPoint[] { loopStart });
                            wipePoly.AddRange(path.Polygon);
                            // then drive down it just a bit more to make sure we have a clean overlap
                            var extraMove = wipePoly.CutToLength(config.CoastAtEndDistance_um);
                            for (int i = 0; i < extraMove.Count; i++)
                            {
                                gcodeExport.WriteMove(extraMove[i], path.Speed, 0);
                            }
                        }
                    }
                }
            }

            gcodeExport.UpdateLayerPrintTime();
        }
예제 #11
0
        public void MakePolygons(OptimizedVolume optomizedMesh, ConfigConstants.REPAIR_OUTLINES outlineRepairTypes)
        {
            for (int startingSegmentIndex = 0; startingSegmentIndex < segmentList.Count; startingSegmentIndex++)
            {
                if (segmentList[startingSegmentIndex].hasBeenAddedToPolygon)
                {
                    continue;
                }

                Polygon poly = new Polygon();
                // We start by adding the start, as we will add ends from now on.
                IntPoint polygonStartPosition = segmentList[startingSegmentIndex].start;
                poly.Add(polygonStartPosition);


                int  segmentIndexBeingAdded = startingSegmentIndex;
                bool canClose;

                bool lastAddWasAStart = true;

                while (true)
                {
                    canClose = false;
                    segmentList[segmentIndexBeingAdded].hasBeenAddedToPolygon = true;
                    IntPoint addedSegmentEndPoint = segmentList[segmentIndexBeingAdded].end;
                    if (!lastAddWasAStart)
                    {
                        // the last added point was an end so add the start as the text end point
                        addedSegmentEndPoint = segmentList[segmentIndexBeingAdded].start;
                    }

                    poly.Add(addedSegmentEndPoint);
                    int           nextSegmentToCheckIndex = -1;
                    OptimizedFace face = optomizedMesh.facesTriangle[segmentList[segmentIndexBeingAdded].faceIndex];
                    for (int connectedFaceIndex = 0; connectedFaceIndex < 3; connectedFaceIndex++)
                    {
                        int testFaceIndex = face.touchingFaces[connectedFaceIndex];
                        if (testFaceIndex > -1)
                        {
                            // If the connected face has an edge that is in the segment list
                            if (faceTo2DSegmentIndex.ContainsKey(testFaceIndex))
                            {
                                int      touchingSegmentIndex = faceTo2DSegmentIndex[testFaceIndex];
                                IntPoint foundSegmentStart    = segmentList[touchingSegmentIndex].start;
                                if (addedSegmentEndPoint == foundSegmentStart)
                                {
                                    // if we have looped back around to where we started
                                    if (addedSegmentEndPoint == polygonStartPosition)
                                    {
                                        canClose = true;
                                    }

                                    // If this segment has already been added
                                    if (segmentList[touchingSegmentIndex].hasBeenAddedToPolygon)
                                    {
                                        continue;
                                    }

                                    nextSegmentToCheckIndex = touchingSegmentIndex;
                                    lastAddWasAStart        = true;
                                }
                                else // let's check if the other side of this segment can hook up (the normal is facing the wrong way)
                                {
                                    IntPoint foundSegmentEnd = segmentList[touchingSegmentIndex].end;
                                    if (addedSegmentEndPoint == foundSegmentEnd)
                                    {
                                        // if we have looped back around to where we started
                                        if (addedSegmentEndPoint == polygonStartPosition)
                                        {
                                            canClose = true;
                                        }

                                        // If this segment has already been added
                                        if (segmentList[touchingSegmentIndex].hasBeenAddedToPolygon)
                                        {
                                            continue;
                                        }

                                        nextSegmentToCheckIndex = touchingSegmentIndex;
                                        lastAddWasAStart        = false;
                                    }
                                }
                            }
                        }
                    }

                    if (nextSegmentToCheckIndex == -1)
                    {
                        break;
                    }

                    segmentIndexBeingAdded = nextSegmentToCheckIndex;
                }

                if (canClose)
                {
                    polygonList.Add(poly);
                }
                else
                {
                    openPolygonList.Add(poly);
                }
            }

            // Link up all the missing ends, closing up the smallest gaps first. This is an inefficient implementation which can run in O(n*n*n) time.
            while (true)
            {
                long bestScore = 10000 * 10000;
                int  bestA     = -1;
                int  bestB     = -1;
                bool reversed  = false;
                for (int polygonAIndex = 0; polygonAIndex < openPolygonList.Count; polygonAIndex++)
                {
                    if (openPolygonList[polygonAIndex].Count < 1)
                    {
                        continue;
                    }

                    for (int polygonBIndex = 0; polygonBIndex < openPolygonList.Count; polygonBIndex++)
                    {
                        if (openPolygonList[polygonBIndex].Count < 1)
                        {
                            continue;
                        }

                        IntPoint diff1        = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][0];
                        long     distSquared1 = (diff1).LengthSquared();
                        if (distSquared1 < bestScore)
                        {
                            bestScore = distSquared1;
                            bestA     = polygonAIndex;
                            bestB     = polygonBIndex;
                            reversed  = false;

                            if (bestScore == 0)
                            {
                                // found a perfect match stop looking
                                break;
                            }
                        }

                        if (polygonAIndex != polygonBIndex)
                        {
                            IntPoint diff2        = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][openPolygonList[polygonBIndex].Count - 1];
                            long     distSquared2 = (diff2).LengthSquared();
                            if (distSquared2 < bestScore)
                            {
                                bestScore = distSquared2;
                                bestA     = polygonAIndex;
                                bestB     = polygonBIndex;
                                reversed  = true;

                                if (bestScore == 0)
                                {
                                    // found a perfect match stop looking
                                    break;
                                }
                            }
                        }
                    }

                    if (bestScore == 0)
                    {
                        // found a perfect match stop looking
                        break;
                    }
                }

                if (bestScore >= 10000 * 10000)
                {
                    break;
                }

                if (bestA == bestB)
                {
                    polygonList.Add(new Polygon(openPolygonList[bestA]));
                    openPolygonList[bestA].Clear();
                }
                else
                {
                    if (reversed)
                    {
                        if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
                        {
                            if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
                            {
                                openPolygonList[bestA].AddRange(openPolygonList[bestB]);
                                openPolygonList[bestB].Clear();
                            }
                            else
                            {
                                openPolygonList[bestB].AddRange(openPolygonList[bestA]);
                                openPolygonList[bestA].Clear();
                            }
                        }
                        else
                        {
                            openPolygonList[bestB].AddRange(openPolygonList[bestA]);
                            openPolygonList[bestB].Clear();
                        }
                    }
                    else
                    {
                        openPolygonList[bestA].AddRange(openPolygonList[bestB]);
                        openPolygonList[bestB].Clear();
                    }
                }
            }


            if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING) == ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING)
            {
                //For extensive stitching find 2 open polygons that are touching 2 closed polygons.
                // Then find the sortest path over this polygon that can be used to connect the open polygons,
                // And generate a path over this shortest bit to link up the 2 open polygons.
                // (If these 2 open polygons are the same polygon, then the final result is a closed polyon)

                while (true)
                {
                    int             bestA      = -1;
                    int             bestB      = -1;
                    GapCloserResult bestResult = new GapCloserResult();
                    bestResult.len          = long.MaxValue;
                    bestResult.polygonIndex = -1;
                    bestResult.pointIndexA  = -1;
                    bestResult.pointIndexB  = -1;

                    for (int i = 0; i < openPolygonList.Count; i++)
                    {
                        if (openPolygonList[i].Count < 1)
                        {
                            continue;
                        }

                        {
                            GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[i][openPolygonList[i].Count - 1]);
                            if (res.len > 0 && res.len < bestResult.len)
                            {
                                bestA      = i;
                                bestB      = i;
                                bestResult = res;
                            }
                        }

                        for (int j = 0; j < openPolygonList.Count; j++)
                        {
                            if (openPolygonList[j].Count < 1 || i == j)
                            {
                                continue;
                            }

                            GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[j][openPolygonList[j].Count - 1]);
                            if (res.len > 0 && res.len < bestResult.len)
                            {
                                bestA      = i;
                                bestB      = j;
                                bestResult = res;
                            }
                        }
                    }

                    if (bestResult.len < long.MaxValue)
                    {
                        if (bestA == bestB)
                        {
                            if (bestResult.pointIndexA == bestResult.pointIndexB)
                            {
                                polygonList.Add(new Polygon(openPolygonList[bestA]));
                                openPolygonList[bestA].Clear();
                            }
                            else if (bestResult.AtoB)
                            {
                                Polygon poly = new Polygon();
                                polygonList.Add(poly);
                                for (int j = bestResult.pointIndexA; j != bestResult.pointIndexB; j = (j + 1) % polygonList[bestResult.polygonIndex].Count)
                                {
                                    poly.Add(polygonList[bestResult.polygonIndex][j]);
                                }

                                poly.AddRange(openPolygonList[bestA]);
                                openPolygonList[bestA].Clear();
                            }
                            else
                            {
                                int n = polygonList.Count;
                                polygonList.Add(new Polygon(openPolygonList[bestA]));
                                for (int j = bestResult.pointIndexB; j != bestResult.pointIndexA; j = (j + 1) % polygonList[bestResult.polygonIndex].Count)
                                {
                                    polygonList[n].Add(polygonList[bestResult.polygonIndex][j]);
                                }
                                openPolygonList[bestA].Clear();
                            }
                        }
                        else
                        {
                            if (bestResult.pointIndexA == bestResult.pointIndexB)
                            {
                                openPolygonList[bestB].AddRange(openPolygonList[bestA]);
                                openPolygonList[bestA].Clear();
                            }
                            else if (bestResult.AtoB)
                            {
                                Polygon poly = new Polygon();
                                for (int n = bestResult.pointIndexA; n != bestResult.pointIndexB; n = (n + 1) % polygonList[bestResult.polygonIndex].Count)
                                {
                                    poly.Add(polygonList[bestResult.polygonIndex][n]);
                                }

                                for (int n = poly.Count - 1; (int)(n) >= 0; n--)
                                {
                                    openPolygonList[bestB].Add(poly[n]);
                                }

                                for (int n = 0; n < openPolygonList[bestA].Count; n++)
                                {
                                    openPolygonList[bestB].Add(openPolygonList[bestA][n]);
                                }

                                openPolygonList[bestA].Clear();
                            }
                            else
                            {
                                for (int n = bestResult.pointIndexB; n != bestResult.pointIndexA; n = (n + 1) % polygonList[bestResult.polygonIndex].Count)
                                {
                                    openPolygonList[bestB].Add(polygonList[bestResult.polygonIndex][n]);
                                }

                                for (int n = openPolygonList[bestA].Count - 1; n >= 0; n--)
                                {
                                    openPolygonList[bestB].Add(openPolygonList[bestA][n]);
                                }

                                openPolygonList[bestA].Clear();
                            }
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }

            if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN) == ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN)
            {
                for (int n = 0; n < openPolygonList.Count; n++)
                {
                    if (openPolygonList[n].Count > 0)
                    {
                        polygonList.Add(new Polygon(openPolygonList[n]));
                    }
                }
            }

            //Remove all the tiny polygons, or polygons that are not closed. As they do not contribute to the actual print.
            int snapDistance = 1000;

            for (int polygonIndex = 0; polygonIndex < polygonList.Count; polygonIndex++)
            {
                int length = 0;

                for (int intPointIndex = 1; intPointIndex < polygonList[polygonIndex].Count; intPointIndex++)
                {
                    length += (polygonList[polygonIndex][intPointIndex] - polygonList[polygonIndex][intPointIndex - 1]).vSize();
                    if (length > snapDistance)
                    {
                        break;
                    }
                }
                if (length < snapDistance)
                {
                    polygonList.RemoveAt(polygonIndex);
                    polygonIndex--;
                }
            }

            //Finally optimize all the polygons. Every point removed saves time in the long run.
            double minimumDistanceToCreateNewPosition = 1.415;

            polygonList = Clipper.CleanPolygons(polygonList, minimumDistanceToCreateNewPosition);
        }
        private void UpdateIntersectionPolygon(ArbiterIntersection aInt)
        {
            // get previous polygon
            Polygon interPoly = new Polygon();

            // add all turn points
            foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
            {
                interPoly.AddRange(ai.TurnPolygon);
            }

            // wrap it to get intersection polygon
            interPoly = Polygon.GrahamScan(interPoly);

            // get outer edges of poly
            List<LinePath> polyEdges = new List<LinePath>();
            for (int i = 0; i < interPoly.Count; i++)
            {
                Coordinates init = interPoly[i];
                Coordinates fin = i == interPoly.Count - 1 ? interPoly[0] : interPoly[i + 1];
                polyEdges.Add(new LinePath(new Coordinates[] { init, fin }));
            }

            // get all edges of all the turns
            List<LinePath> other = new List<LinePath>();
            foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
            {
                for (int i = 0; i < ai.TurnPolygon.Count; i++)
                {
                    Coordinates init = ai.TurnPolygon[i];
                    Coordinates fin = i == ai.TurnPolygon.Count - 1 ? ai.TurnPolygon[0] : ai.TurnPolygon[i + 1];
                    other.Add(new LinePath(new Coordinates[] { init, fin }));
                }
            }

            // test points
            List<Coordinates> testPoints = new List<Coordinates>();

            // path segs of inner turns
            List<LinePath> innerTurnPaths = new List<LinePath>();

            // check all inner points against all turn edges
            foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
            {
                // check for inner point
                if (ai.InnerCoordinates.Count == 3)
                {
                    // inner point of the turn on the inside
                    testPoints.Add(ai.InnerCoordinates[1]);

                    // add to paths
                    innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[0], ai.InnerCoordinates[1] }));
                    innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[1], ai.InnerCoordinates[2] }));
                }
            }

            // list of used segments
            List<LinePath> closed = new List<LinePath>();

            // check all other intersections of turn polygon edges
            foreach (LinePath seg in innerTurnPaths)
            {
                foreach (LinePath otherEdge in other)
                {
                    if (!seg.Equals(otherEdge) && !closed.Contains(otherEdge))
                    {
                        double x1 = seg[0].X;
                        double y1 = seg[0].Y;
                        double x2 = seg[1].X;
                        double y2 = seg[1].Y;
                        double x3 = otherEdge[0].X;
                        double y3 = otherEdge[0].Y;
                        double x4 = otherEdge[1].X;
                        double y4 = otherEdge[1].Y;

                        // get if inside both
                        double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
                        double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));

                        if (0.05 < ua && ua < 0.95 && 0.5 < ub && ub < 0.95)
                        {
                            double x = x1 + ua * (x2 - x1);
                            double y = y1 + ua * (y2 - y1);
                            testPoints.Add(new Coordinates(x, y));
                        }
                    }
                }

                closed.Add(seg);
            }

            // loop through test points
            foreach(Coordinates inner in testPoints)
            {
                // list of replacements
                List<LinePath> toReplace = new List<LinePath>();

                // loop through outer
                foreach (LinePath edge in polyEdges)
                {
                    // flag to replace
                    bool replace = false;

                    // make sure this goes to a valid edge section
                    LinePath.PointOnPath closest = edge.GetClosestPoint(inner);
                    if (!closest.Equals(edge.StartPoint) && !closest.Equals(edge.EndPoint) &&
                        !(closest.Location.DistanceTo(edge.StartPoint.Location)  < 0.5) &&
                        !(closest.Location.DistanceTo(edge.EndPoint.Location)  < 0.5))
                    {
                        // create seg (extend a bit)
                        Coordinates expansion = closest.Location - inner;
                        LinePath seg = new LinePath(new Coordinates[] { inner, closest.Location + expansion.Normalize(1.0) });

                        // set flag
                        replace = true;

                        // loop through other edges
                        foreach (LinePath otherEdge in other)
                        {
                            double x1 = seg[0].X;
                            double y1 = seg[0].Y;
                            double x2 = seg[1].X;
                            double y2 = seg[1].Y;
                            double x3 = otherEdge[0].X;
                            double y3 = otherEdge[0].Y;
                            double x4 = otherEdge[1].X;
                            double y4 = otherEdge[1].Y;

                            // get if inside both
                            double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
                            double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));

                            if (0.01 < ua && ua < 0.99 && 0 < ub && ub < 1)
                            {
                                replace = false;
                            }
                        }
                    }

                    // check if should replace
                    if (replace)
                    {
                        // add analyzed to adjacent
                        toReplace.Add(edge);
                    }
                }

                // loop through replacements
                foreach (LinePath ll in toReplace)
                {
                    LinePath[] tmpArrayPoly = new LinePath[polyEdges.Count];
                    polyEdges.CopyTo(tmpArrayPoly);
                    List<LinePath> tmpPoly = new List<LinePath>(tmpArrayPoly);

                    // get index of edge
                    int index = tmpPoly.IndexOf(ll);

                    // remove
                    tmpPoly.RemoveAt(index);

                    // add correctly to outer
                    tmpPoly.Insert(index, new LinePath(new Coordinates[] { ll[0], inner }));
                    tmpPoly.Insert(index + 1, new LinePath(new Coordinates[] { inner, ll[1] }));

                    // poly
                    Polygon temp = new Polygon();
                    foreach(LinePath lpTemp in tmpPoly)
                        temp.Add(lpTemp[1]);
                    temp.Inflate(0.5);

                    // make sure none of original outside
                    bool ok = true;
                    foreach (LinePath lp in other)
                    {
                        if (!temp.IsInside(lp[1]) && !temp.Contains(lp[1]))
                            ok = false;
                    }

                    // set if created ok
                    if (ok)
                        polyEdges = tmpPoly;
                }
            }

            // create final
            List<Coordinates> finalPoly = new List<Coordinates>();
            foreach (LinePath outerEdge in polyEdges)
                finalPoly.Add(outerEdge[1]);
            interPoly = new Polygon(finalPoly);

            aInt.IntersectionPolygon = interPoly;
            aInt.Center = interPoly.GetCentroid();
        }
예제 #13
0
        public void MakePolygons(ConfigConstants.REPAIR_OUTLINES outlineRepairTypes)
        {
            if (false)             // you can use this output segments for debugging
            {
                using (StreamWriter stream = File.AppendText("segments.txt"))
                {
                    stream.WriteLine(DumpSegmentListToString(SegmentList));
                }
            }

            CreateFastIndexLookup();

            for (int startingSegmentIndex = 0; startingSegmentIndex < SegmentList.Count; startingSegmentIndex++)
            {
                if (SegmentList[startingSegmentIndex].hasBeenAddedToPolygon)
                {
                    continue;
                }

                Polygon poly = new Polygon();
                // We start by adding the start, as we will add ends from now on.
                IntPoint polygonStartPosition = SegmentList[startingSegmentIndex].start;
                poly.Add(polygonStartPosition);

                int  segmentIndexBeingAdded = startingSegmentIndex;
                bool canClose;

                while (true)
                {
                    canClose = false;
                    SegmentList[segmentIndexBeingAdded].hasBeenAddedToPolygon = true;
                    IntPoint addedSegmentEndPoint = SegmentList[segmentIndexBeingAdded].end;

                    poly.Add(addedSegmentEndPoint);
                    segmentIndexBeingAdded = GetTouchingSegmentIndex(addedSegmentEndPoint);
                    if (segmentIndexBeingAdded == -1)
                    {
                        break;
                    }
                    else
                    {
                        IntPoint foundSegmentStart = SegmentList[segmentIndexBeingAdded].start;
                        if (addedSegmentEndPoint == foundSegmentStart)
                        {
                            // if we have looped back around to where we started
                            if (addedSegmentEndPoint == polygonStartPosition)
                            {
                                canClose = true;
                            }
                        }
                    }
                }

                if (canClose)
                {
                    PolygonList.Add(poly);
                }
                else
                {
                    openPolygonList.Add(poly);
                }
            }

            // Link up all the missing ends, closing up the smallest gaps first. This is an inefficient implementation which can run in O(n*n*n) time.
            while (true)
            {
                long bestScore = 10000 * 10000;
                int  bestA     = -1;
                int  bestB     = -1;
                bool reversed  = false;
                for (int polygonAIndex = 0; polygonAIndex < openPolygonList.Count; polygonAIndex++)
                {
                    if (openPolygonList[polygonAIndex].Count < 1)
                    {
                        continue;
                    }

                    for (int polygonBIndex = 0; polygonBIndex < openPolygonList.Count; polygonBIndex++)
                    {
                        if (openPolygonList[polygonBIndex].Count < 1)
                        {
                            continue;
                        }

                        IntPoint diff1        = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][0];
                        long     distSquared1 = (diff1).LengthSquared();
                        if (distSquared1 < bestScore)
                        {
                            bestScore = distSquared1;
                            bestA     = polygonAIndex;
                            bestB     = polygonBIndex;
                            reversed  = false;

                            if (bestScore == 0)
                            {
                                // found a perfect match stop looking
                                break;
                            }
                        }

                        if (polygonAIndex != polygonBIndex)
                        {
                            IntPoint diff2        = openPolygonList[polygonAIndex][openPolygonList[polygonAIndex].Count - 1] - openPolygonList[polygonBIndex][openPolygonList[polygonBIndex].Count - 1];
                            long     distSquared2 = (diff2).LengthSquared();
                            if (distSquared2 < bestScore)
                            {
                                bestScore = distSquared2;
                                bestA     = polygonAIndex;
                                bestB     = polygonBIndex;
                                reversed  = true;

                                if (bestScore == 0)
                                {
                                    // found a perfect match stop looking
                                    break;
                                }
                            }
                        }
                    }

                    if (bestScore == 0)
                    {
                        // found a perfect match stop looking
                        break;
                    }
                }

                if (bestScore >= 10000 * 10000)
                {
                    break;
                }

                if (bestA == bestB)                 // This loop connects to itself, close the polygon.
                {
                    PolygonList.Add(new Polygon(openPolygonList[bestA]));
                    openPolygonList[bestA].Clear();                     // B is cleared as it is A
                }
                else
                {
                    if (reversed)
                    {
                        if (openPolygonList[bestA].PolygonLength() > openPolygonList[bestB].PolygonLength())
                        {
                            openPolygonList[bestA].AddRange(openPolygonList[bestB]);
                            openPolygonList[bestB].Clear();
                        }
                        else
                        {
                            openPolygonList[bestB].AddRange(openPolygonList[bestA]);
                            openPolygonList[bestA].Clear();
                        }
                    }
                    else
                    {
                        openPolygonList[bestA].AddRange(openPolygonList[bestB]);
                        openPolygonList[bestB].Clear();
                    }
                }
            }

            if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING) == ConfigConstants.REPAIR_OUTLINES.EXTENSIVE_STITCHING)
            {
                //For extensive stitching find 2 open polygons that are touching 2 closed polygons.
                // Then find the sortest path over this polygon that can be used to connect the open polygons,
                // And generate a path over this shortest bit to link up the 2 open polygons.
                // (If these 2 open polygons are the same polygon, then the final result is a closed polyon)

                while (true)
                {
                    int             bestA      = -1;
                    int             bestB      = -1;
                    GapCloserResult bestResult = new GapCloserResult();
                    bestResult.len          = long.MaxValue;
                    bestResult.polygonIndex = -1;
                    bestResult.pointIndexA  = -1;
                    bestResult.pointIndexB  = -1;

                    for (int i = 0; i < openPolygonList.Count; i++)
                    {
                        if (openPolygonList[i].Count < 1)
                        {
                            continue;
                        }

                        {
                            GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[i][openPolygonList[i].Count - 1]);
                            if (res.len > 0 && res.len < bestResult.len)
                            {
                                bestA      = i;
                                bestB      = i;
                                bestResult = res;
                            }
                        }

                        for (int j = 0; j < openPolygonList.Count; j++)
                        {
                            if (openPolygonList[j].Count < 1 || i == j)
                            {
                                continue;
                            }

                            GapCloserResult res = FindPolygonGapCloser(openPolygonList[i][0], openPolygonList[j][openPolygonList[j].Count - 1]);
                            if (res.len > 0 && res.len < bestResult.len)
                            {
                                bestA      = i;
                                bestB      = j;
                                bestResult = res;
                            }
                        }
                    }

                    if (bestResult.len < long.MaxValue)
                    {
                        if (bestA == bestB)
                        {
                            if (bestResult.pointIndexA == bestResult.pointIndexB)
                            {
                                PolygonList.Add(new Polygon(openPolygonList[bestA]));
                                openPolygonList[bestA].Clear();
                            }
                            else if (bestResult.AtoB)
                            {
                                Polygon poly = new Polygon();
                                PolygonList.Add(poly);
                                for (int j = bestResult.pointIndexA; j != bestResult.pointIndexB; j = (j + 1) % PolygonList[bestResult.polygonIndex].Count)
                                {
                                    poly.Add(PolygonList[bestResult.polygonIndex][j]);
                                }

                                poly.AddRange(openPolygonList[bestA]);
                                openPolygonList[bestA].Clear();
                            }
                            else
                            {
                                int n = PolygonList.Count;
                                PolygonList.Add(new Polygon(openPolygonList[bestA]));
                                for (int j = bestResult.pointIndexB; j != bestResult.pointIndexA; j = (j + 1) % PolygonList[bestResult.polygonIndex].Count)
                                {
                                    PolygonList[n].Add(PolygonList[bestResult.polygonIndex][j]);
                                }
                                openPolygonList[bestA].Clear();
                            }
                        }
                        else
                        {
                            if (bestResult.pointIndexA == bestResult.pointIndexB)
                            {
                                openPolygonList[bestB].AddRange(openPolygonList[bestA]);
                                openPolygonList[bestA].Clear();
                            }
                            else if (bestResult.AtoB)
                            {
                                Polygon poly = new Polygon();
                                for (int n = bestResult.pointIndexA; n != bestResult.pointIndexB; n = (n + 1) % PolygonList[bestResult.polygonIndex].Count)
                                {
                                    poly.Add(PolygonList[bestResult.polygonIndex][n]);
                                }

                                for (int n = poly.Count - 1; (int)(n) >= 0; n--)
                                {
                                    openPolygonList[bestB].Add(poly[n]);
                                }

                                for (int n = 0; n < openPolygonList[bestA].Count; n++)
                                {
                                    openPolygonList[bestB].Add(openPolygonList[bestA][n]);
                                }

                                openPolygonList[bestA].Clear();
                            }
                            else
                            {
                                for (int n = bestResult.pointIndexB; n != bestResult.pointIndexA; n = (n + 1) % PolygonList[bestResult.polygonIndex].Count)
                                {
                                    openPolygonList[bestB].Add(PolygonList[bestResult.polygonIndex][n]);
                                }

                                for (int n = openPolygonList[bestA].Count - 1; n >= 0; n--)
                                {
                                    openPolygonList[bestB].Add(openPolygonList[bestA][n]);
                                }

                                openPolygonList[bestA].Clear();
                            }
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }

            if ((outlineRepairTypes & ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN) == ConfigConstants.REPAIR_OUTLINES.KEEP_OPEN)
            {
                for (int n = 0; n < openPolygonList.Count; n++)
                {
                    if (openPolygonList[n].Count > 0)
                    {
                        PolygonList.Add(new Polygon(openPolygonList[n]));
                    }
                }
            }

            //Remove all the tiny polygons, or polygons that are not closed. As they do not contribute to the actual print.
            int minimumPerimeter = 1000;

            for (int polygonIndex = 0; polygonIndex < PolygonList.Count; polygonIndex++)
            {
                long perimeterLength = 0;

                for (int intPointIndex = 1; intPointIndex < PolygonList[polygonIndex].Count; intPointIndex++)
                {
                    perimeterLength += (PolygonList[polygonIndex][intPointIndex] - PolygonList[polygonIndex][intPointIndex - 1]).Length();
                    if (perimeterLength > minimumPerimeter)
                    {
                        break;
                    }
                }
                if (perimeterLength < minimumPerimeter)
                {
                    PolygonList.RemoveAt(polygonIndex);
                    polygonIndex--;
                }
            }

            //Finally optimize all the polygons. Every point removed saves time in the long run.
            double minimumDistanceToCreateNewPosition = 10;

            PolygonList = Clipper.CleanPolygons(PolygonList, minimumDistanceToCreateNewPosition);
        }
예제 #14
0
        public static Polygon PartitionPolygon(ArbiterLanePartition alp)
        {
            if (alp.Initial.PreviousPartition != null &&
                alp.Final.NextPartition != null &&
                alp.Length < 30.0 && alp.Length > 4.0)
            {
                // get partition turn direction
                ArbiterTurnDirection pTD = PartitionTurnDirection(alp);

                // check if angles of previous and next are such that not straight through
                if (pTD != ArbiterTurnDirection.Straight)
                {
                    // get partition poly
                    ArbiterInterconnect tmpAi = alp.ToInterconnect;
                    tmpAi.TurnDirection = pTD;
                    GenerateInterconnectPolygon(tmpAi);
                    Polygon pPoly = tmpAi.TurnPolygon;

                    // here is default partition polygon
                    LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
                    LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
                    alprb.Reverse();
                    List <Coordinates> alpdefaultPoly = alplb;
                    alpdefaultPoly.AddRange(alprb);

                    // get full poly
                    pPoly.AddRange(alpdefaultPoly);
                    pPoly = Polygon.GrahamScan(pPoly);

                    return(pPoly);
                }
            }
            else if (alp.Length >= 30)
            {
                Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);

                if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
                {
                    // initial portion
                    Coordinates i1   = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
                    Coordinates i2   = alp.Initial.Position;
                    Coordinates i3   = i2 + alp.Vector().Normalize(15.0);
                    LinePath    il12 = new LinePath(new Coordinates[] { i1, i2 });
                    LinePath    il23 = new LinePath(new Coordinates[] { i2, i3 });
                    LinePath    il13 = new LinePath(new Coordinates[] { i1, i3 });
                    Coordinates iCC  = il13.GetClosestPoint(i2).Location;
                    if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
                    {
                        il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    else
                    {
                        il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
                    iCCP = il13.AdvancePoint(iCCP, -10.0);
                    il13 = il13.SubPath(iCCP, 20.0);
                    Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
                    iBase.Add(il13[1]);
                    Polygon iP = Polygon.GrahamScan(iBase);
                    pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
                }

                if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
                {
                    // initial portion
                    Coordinates i1   = alp.Final.Position - alp.Vector().Normalize(15.0);
                    Coordinates i2   = alp.Final.Position;
                    Coordinates i3   = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
                    LinePath    il12 = new LinePath(new Coordinates[] { i1, i2 });
                    LinePath    il23 = new LinePath(new Coordinates[] { i2, i3 });
                    LinePath    il13 = new LinePath(new Coordinates[] { i1, i3 });
                    Coordinates iCC  = il13.GetClosestPoint(i2).Location;
                    if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
                    {
                        il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    else
                    {
                        il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
                    iCCP = il13.AdvancePoint(iCCP, -10.0);
                    il13 = il13.SubPath(iCCP, 20.0);
                    Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
                    iBase.Add(il13[0]);
                    Polygon iP = Polygon.GrahamScan(iBase);
                    pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
                }

                return(pBase);
            }

            // fall out
            return(null);
        }
예제 #15
0
        public Polygon ProjectRectangle(Rectangle rectangle)
        {
            Polygon polygon = new Polygon();

            // get corners counter clockwise starting on the top left and project them to create the polygon:
            polygon.AddRange(new Point[] {
                new Point(rectangle.X, rectangle.Y), // top left
                new Point(rectangle.X, rectangle.Y + rectangle.Height), // bottom left
                new Point(rectangle.X + rectangle.Width, rectangle.Y + rectangle.Height), // bottom right
                new Point(rectangle.X + rectangle.Width, rectangle.Y) // top right
            }.Select(ProjectPoint));
            return polygon;
        }
예제 #16
0
        // Find the convex hull of a set of (x,y) points.
        // returns a polygon which form the convex hull.
        // Could be:
        //     - empty (if input is empty, or if there is an internal error)
        //     - single point (if all input points are identical)
        //     - 2 points (if >=2 input points, but all collinear)
        //     - 3 or more points forming a convex polygon in CCW order.
        //
        // Returns 0  (or -1 if an internal
        // error occurs, in which case the result will be empty).
        // No case has been found in testing which causes an internal error. It's
        // possible that this could occur if 64-bit multplies and adds overflow.
        public static Polygon CreateConvexHull(Polygon inPolygon)
        {
            Polygon points = new Polygon(inPolygon);
            int     count  = points.Count;

            if (count <= 2)
            {
                if (count == 2 &&
                    points[0].X == points[1].X &&
                    points[1].Y == points[1].Y)
                {
                    // remove one point if two are the same
                    points.RemoveAt(1);
                }
                return(points);
            }

            // step 1: sort the values in order from min y to max y.
            // (and increasing X where Y are equal)
            points.Sort(new IntPointSorterYX());

            long minY = points[0].Y;
            long maxY = points[count - 1].Y;

            // (2) make a pass, find the min and max x.
            long minX = points[0].X;
            long maxX = minX;

            for (int i = 1; i < count; i++)
            {
                minX = Math.Min(minX, points[i].X);
                maxX = Math.Max(maxX, points[i].X);
            }

            long upperLeftX;
            long upperRightX;

            long bottomLeftY;
            long bottomRightY;

            long bottomLeftX;
            long bottomRightX;

            int leftCount;
            int rightCount;
            int indexOfLowestPoint;

            // OK next step is to find out if there's more than one identical
            // 'y' value at the end of the list. Don't forget to
            // consider the case where all of these are identical...
            {
                int sameYFromEndCount = 1;
                int lastValidIndex    = count - 1;
                upperLeftX = upperRightX = points[lastValidIndex].X;
                while (sameYFromEndCount < count && points[lastValidIndex - sameYFromEndCount].Y == maxY)
                {
                    sameYFromEndCount++;
                }

                // if more than one, they will be sorted by increasing X.
                // Delete any in the middle.
                //
                if (sameYFromEndCount >= 2)
                {
                    int deleteCount = sameYFromEndCount - 2;                            // always delete this many...
                    upperRightX = points[count - 1].X;
                    upperLeftX  = points[count - sameYFromEndCount].X;
                    if (upperLeftX == upperRightX)
                    {
                        deleteCount++;                                                          // delete one more if all the same...
                    }
                    if (deleteCount > 0)
                    {
                        points[count - 1 - deleteCount] = points[count - 1];
                        count -= deleteCount;
                    }
                }
            }

            // We may now have only 1 or 2 points.
            if (count <= 2)
            {
                points = new Polygon();
                return(points);
            }

            // same thing at the bottoom
            {
                int sameYFromStartCount = 1;
                bottomLeftX = bottomRightX = points[0].X;
                while (sameYFromStartCount < count && points[sameYFromStartCount].Y == minY)
                {
                    sameYFromStartCount++;
                }

                // if more than one, They will be sorted left to right. Delete any in the middle.
                indexOfLowestPoint = 0;
                if (sameYFromStartCount >= 2)
                {
                    int deleteCount = sameYFromStartCount - 2;                          // always delete this many...
                    bottomLeftX  = points[0].X;
                    bottomRightX = points[sameYFromStartCount - 1].X;
                    if (bottomLeftX == bottomRightX)
                    {
                        deleteCount++;                         // delete one more if all the same...
                    }
                    else
                    {
                        indexOfLowestPoint = 1;                                         // otherwise we start with 1,
                    }

                    if (deleteCount > 0)
                    {
                        for (int i = 0; i < count - (deleteCount + 1); i++)
                        {
                            points[1 + i] = points[deleteCount + 1 + i];
                            count        -= deleteCount;
                        }
                    }
                }

                // OK, we now have the 'UL/UR' points and the 'LL/LR' points.
                // Make a left-side list and a right-side list, each
                // of capacity 'num'.
                // note that 'pleft' is in reverse order...
                // pright grows up from 0, and pleft down from 'num+1',
                // in the same array - they can't overlap.
                Polygon temp_array = new Polygon(points);
                temp_array.Add(new IntPoint());
                temp_array.Add(new IntPoint());
                int pleftIndex  = count + 1;
                int prightIndex = 0;

                // set up left and right
                temp_array[pleftIndex] = points[0];
                leftCount = 1;
                temp_array[prightIndex] = points[indexOfLowestPoint];
                rightCount = 1;

                bottomLeftY = bottomRightY = minY;

                for (int ipos = indexOfLowestPoint + 1; ipos < count; ipos++)
                {
                    IntPoint pointToCheck = temp_array[ipos];                                   // get new point.

                    // left side test:
                    // is the new point is strictly to the left of a line from (bottomLeftX, BottomLeftY) to ( upperLeftX, maxy )?
                    if (convex3(upperLeftX, maxY, pointToCheck.X, pointToCheck.Y, bottomLeftX, bottomLeftY))
                    {
                        // if so, append to the left side list, but first peel off any existing
                        // points there which would be  on or inside the new line.
                        while (leftCount > 1 && !convex3(pointToCheck, temp_array[pleftIndex - (leftCount - 1)], temp_array[pleftIndex - (leftCount - 2)]))
                        {
                            --leftCount;
                        }
                        temp_array[pleftIndex - leftCount] = pointToCheck;
                        leftCount++;
                        bottomLeftX = pointToCheck.X;
                        bottomLeftY = pointToCheck.Y;
                    }
                    else if (convex3(bottomRightX, bottomRightY, pointToCheck.X, pointToCheck.Y, upperRightX, maxY))                     // right side test is the new point strictly to the right of a line from (URx, maxy) to ( DRx, DRy )?
                    {
                        // if so, append to the right side list, but first peel off any existing
                        // points there which would be  on or inside the new line.
                        //
                        while (rightCount > 1 && !convex3(temp_array[prightIndex + rightCount - 2], temp_array[prightIndex + rightCount - 1], pointToCheck))
                        {
                            --rightCount;
                        }
                        temp_array[prightIndex + rightCount] = pointToCheck;
                        rightCount++;
                        bottomRightX = pointToCheck.X;
                        bottomRightY = pointToCheck.Y;
                    }
                    // if neither of the above are true we throw out the point.
                }

                // now add the 'maxy' points to the lists (it will have failed the insert test)
                temp_array[pleftIndex - leftCount] = new IntPoint(upperLeftX, maxY);
                ++leftCount;
                if (upperRightX > upperLeftX)
                {
                    temp_array[prightIndex + rightCount] = new IntPoint(upperRightX, maxY);
                    ++rightCount;
                }
                // now copy the lists to the output area.
                //
                // if both lists have the same lower point, delete one now.
                // (pleft could be empty as a result!)
                if (indexOfLowestPoint == 0)
                {
                    --pleftIndex;
                    --leftCount;
                }

                // this condition should be true now...
                if (!(leftCount + rightCount <= count))
                {
                    // failure... return the original concave list
                    return(inPolygon);
                }

                // now just pack the pright and pleft lists into the output.
                count = leftCount + rightCount;

                for (int i = 0; i < rightCount; i++)
                {
                    points[i] = temp_array[i + prightIndex];
                }

                if (leftCount > 0)
                {
                    for (int i = 0; i < leftCount; i++)
                    {
                        points[i + rightCount] = temp_array[i + pleftIndex - (leftCount - 1)];
                    }
                }

                if (points.Count > count)
                {
                    Polygon newList = new Polygon();
                    newList.AddRange(points.GetRange(0, count));
                    points = newList;
                }

                return(points);
            }
        }
        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List<Coordinates> polyPoints = new List<Coordinates>();
            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List<Coordinates> updatedPts = new List<Coordinates>();
                        LinePath interTmp = ai.InterconnectPath.Clone();
                        Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp = interTmp.ShiftLateral(TahoeParams.VL);
                        rp = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List<Coordinates> interPoints = new List<Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();
                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }