private void WriteErrorForTesting(IntPoint startPoint, IntPoint endPoint, long edgeLength) { var bounds = OutlinePolygons.GetBounds(); long length = (startPoint - endPoint).Length(); string startEndString = $"start:({startPoint.X}, {startPoint.Y}), end:({endPoint.X}, {endPoint.Y})"; string outlineString = OutlinePolygons.WriteToString(); // just some code to set a break point on string fullPath = Path.GetFullPath("DebugPathFinder.txt"); if (fullPath.Contains("MatterControl")) { using (StreamWriter sw = File.AppendText(fullPath)) { if (lastOutlineString != outlineString) { sw.WriteLine(""); sw.WriteLine($"polyPath = \"{outlineString}\";"); lastOutlineString = outlineString; } sw.WriteLine($"// Length of this segment (start->end) {length}. Length of bad edge {edgeLength}"); sw.WriteLine($"// startOverride = new MSIntPoint({startPoint.X}, {startPoint.Y}); endOverride = new MSIntPoint({endPoint.X}, {endPoint.Y});"); sw.WriteLine($"TestSinglePathIsInside(polyPath, new IntPoint({startPoint.X}, {startPoint.Y}), new IntPoint({endPoint.X}, {endPoint.Y}));"); } } }
private bool ValidPoint(IntPoint position) { Tuple <int, int, IntPoint> movedPosition; long movedDist = 0; BoundaryPolygons.MovePointInsideBoundary(position, out movedPosition, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees); if (movedPosition != null) { movedDist = (position - movedPosition.Item3).Length(); } if (OutlinePolygons.TouchingEdge(position, OutlineEdgeQuadTrees) || OutlinePolygons.PointIsInside(position, OutlineEdgeQuadTrees, OutlinePointQuadTrees) || movedDist <= 1) { return(true); } return(false); }
public PathFinder(Polygons outlinePolygons, long avoidInset, IntRect?stayInsideBounds = null) { if (outlinePolygons.Count == 0) { return; } OutlinePolygons = FixWinding(outlinePolygons); OutlinePolygons = Clipper.CleanPolygons(OutlinePolygons, avoidInset / 60); InsetAmount = avoidInset; if (stayInsideBounds != null) { var boundary = stayInsideBounds.Value; OutlinePolygons.Add(new Polygon() { new IntPoint(boundary.minX, boundary.minY), new IntPoint(boundary.maxX, boundary.minY), new IntPoint(boundary.maxX, boundary.maxY), new IntPoint(boundary.minX, boundary.maxY), }); OutlinePolygons = FixWinding(OutlinePolygons); } BoundaryPolygons = OutlinePolygons.Offset(stayInsideBounds == null ? -avoidInset : -2 * avoidInset); BoundaryPolygons = FixWinding(BoundaryPolygons); OutlineEdgeQuadTrees = OutlinePolygons.GetEdgeQuadTrees(); OutlinePointQuadTrees = OutlinePolygons.GetPointQuadTrees(); BoundaryEdgeQuadTrees = BoundaryPolygons.GetEdgeQuadTrees(); BoundaryPointQuadTrees = BoundaryPolygons.GetPointQuadTrees(); foreach (var polygon in BoundaryPolygons) { Waypoints.AddPolygon(polygon); } // hook up path segments between the separate islands if (simpleHookup) // do a simple hookup { for (int indexA = 0; indexA < BoundaryPolygons.Count; indexA++) { var polyA = BoundaryPolygons[indexA]; if (polyA.GetWindingDirection() > 0) { Func <int, Polygon, bool> ConsiderPolygon = (polyIndex, poly) => { return(polyIndex != indexA && poly.GetWindingDirection() > 0); }; // find the closest two points between A and any other polygon IntPoint bestAPos = polyA.Center(); Func <int, IntPoint, bool> ConsiderPoint = (polyIndex, edgeEnd) => { if (OutlinePolygons.PointIsInside((bestAPos + edgeEnd) / 2, OutlineEdgeQuadTrees, OutlinePointQuadTrees)) { return(true); } return(false); }; var bestBPoly = BoundaryPolygons.FindClosestPoint(bestAPos, ConsiderPolygon, ConsiderPoint); if (bestBPoly.polyIndex == -1) { // find one that intersects bestBPoly = BoundaryPolygons.FindClosestPoint(bestAPos, ConsiderPolygon); } if (bestBPoly.polyIndex != -1) { bestAPos = polyA.FindClosestPoint(bestBPoly.Item3).Item2; var bestBResult = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos, ConsiderPoint); IntPoint bestBPos = new IntPoint(); if (bestBResult.index != -1) { bestBPos = bestBResult.Item2; } else { // find one that intersects bestBPos = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos).Item2; } bestAPos = polyA.FindClosestPoint(bestBPos).Item2; bestBPos = BoundaryPolygons[bestBPoly.Item1].FindClosestPoint(bestAPos).Item2; // hook the polygons up along this connection IntPointNode nodeA = Waypoints.FindNode(bestAPos); IntPointNode nodeB = Waypoints.FindNode(bestBPos); Waypoints.AddPathLink(nodeA, nodeB); } } } } else // hook up using thin lines code { // this is done with merge close edges and finding candidates // then joining the ends of the merged segments with the closest points Polygons thinLines; if (OutlinePolygons.FindThinLines(avoidInset * 2, 0, out thinLines)) { ThinLinePolygons = thinLines; for (int thinIndex = 0; thinIndex < thinLines.Count; thinIndex++) { var thinPolygon = thinLines[thinIndex]; if (thinPolygon.Count > 1) { Waypoints.AddPolygon(thinPolygon, false); } } Polygons allPolygons = new Polygons(thinLines); allPolygons.AddRange(BoundaryPolygons); for (int thinIndex = 0; thinIndex < thinLines.Count; thinIndex++) { var thinPolygon = thinLines[thinIndex]; if (thinPolygon.Count > 1) { // now hook up the start and end of this polygon to the existing way points var closestStart = allPolygons.FindClosestPoint(thinPolygon[0], (polyIndex, poly) => { return(polyIndex == thinIndex); }); var closestEnd = allPolygons.FindClosestPoint(thinPolygon[thinPolygon.Count - 1], (polyIndex, poly) => { return(polyIndex == thinIndex); }); // last point if (OutlinePolygons.PointIsInside((closestStart.Item3 + closestEnd.Item3) / 2, OutlineEdgeQuadTrees)) { IntPointNode nodeA = Waypoints.FindNode(closestStart.Item3); IntPointNode nodeB = Waypoints.FindNode(closestEnd.Item3); if (nodeA != null && nodeB != null) { Waypoints.AddPathLink(nodeA, nodeB); } } } } } } removePointList = new WayPointsToRemove(Waypoints); }