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)); }
public static Polygon CreateConvexHull(this Polygons polygons) { Polygon allPoints = new Polygon(); foreach (Polygon polygon in polygons) { allPoints.AddRange(polygon); } return allPoints.CreateConvexHull(); }
public static Polygon CreateConvexHull(Polygons polygons) { Polygon allPoints = new Polygon(); foreach (Polygon polygon in polygons) { allPoints.AddRange(polygon); } return(CreateConvexHull(allPoints)); }
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); }
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); }
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)); }
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; }
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 }
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(); } }
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(); }
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(); }
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); }
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); }
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; }
// 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(); } }