Пример #1
0
        private IntPointNode GetWayPointInside(IntPoint position, out IntPointNode waypointAtPosition)
        {
            Tuple <int, int, IntPoint> foundPolyPointPosition;

            waypointAtPosition = null;
            BoundaryPolygons.MovePointInsideBoundary(position, out foundPolyPointPosition, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees);
            if (foundPolyPointPosition == null)
            {
                // The point is already inside
                var existingNode = Waypoints.FindNode(position, findNodeDist);
                if (existingNode == null)
                {
                    waypointAtPosition = AddTempWayPoint(removePointList, position);
                    return(waypointAtPosition);
                }
                waypointAtPosition = existingNode;
                return(waypointAtPosition);
            }
            else             // The point had to be moved inside the polygon
            {
                if (position == foundPolyPointPosition.Item3)
                {
                    var existingNode = Waypoints.FindNode(position, findNodeDist);
                    if (existingNode != null)
                    {
                        waypointAtPosition = existingNode;
                        return(waypointAtPosition);
                    }
                    else
                    {
                        // get the way point that we need to insert
                        waypointAtPosition = AddTempWayPoint(removePointList, position);
                        HookUpToEdge(waypointAtPosition, foundPolyPointPosition.Item1, foundPolyPointPosition.Item2);
                        return(waypointAtPosition);
                    }
                }
                else                 // the point was outside and hook it up to the nearest edge
                {
                    // fand the start node if we can
                    IntPointNode startNode = Waypoints.FindNode(foundPolyPointPosition.Item3, findNodeDist);

                    // After that create a temp way point at the current position
                    waypointAtPosition = AddTempWayPoint(removePointList, position);
                    if (startNode != null)
                    {
                        Waypoints.AddPathLink(startNode, waypointAtPosition);
                    }
                    else
                    {
                        // get the way point that we need to insert
                        startNode = AddTempWayPoint(removePointList, foundPolyPointPosition.Item3);
                        HookUpToEdge(startNode, foundPolyPointPosition.Item1, foundPolyPointPosition.Item2);
                        Waypoints.AddPathLink(startNode, waypointAtPosition);
                    }
                    return(startNode);
                }
            }
        }
Пример #2
0
        private void HookUpToEdge(IntPointNode crossingNode, int polyIndex, int pointIndex)
        {
            int count = BoundaryPolygons[polyIndex].Count;

            pointIndex = (pointIndex + count) % count;
            IntPointNode prevPolyPointNode = Waypoints.FindNode(BoundaryPolygons[polyIndex][pointIndex]);

            Waypoints.AddPathLink(crossingNode, prevPolyPointNode);
            IntPointNode nextPolyPointNode = Waypoints.FindNode(BoundaryPolygons[polyIndex][(pointIndex + 1) % count]);

            Waypoints.AddPathLink(crossingNode, nextPolyPointNode);
        }
Пример #3
0
        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);
        }
Пример #4
0
        public bool CreatePathInsideBoundary(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, bool optomizePath = true)
        {
            double z = startPointIn.Z;

            startPointIn.Z = 0;
            endPointIn.Z   = 0;
            if (BoundaryPolygons == null || BoundaryPolygons.Count == 0)
            {
                return(false);
            }

            // neither needed to be moved
            if (BoundaryPolygons.FindIntersection(startPointIn, endPointIn, BoundaryEdgeQuadTrees) == Intersection.None &&
                BoundaryPolygons.PointIsInside((startPointIn + endPointIn) / 2, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees))
            {
                return(true);
            }

            removePointList.Dispose();

            pathThatIsInside.Clear();

            //Check if we are inside the boundaries
            IntPointNode startPlanNode = null;
            var          lastAddedNode = GetWayPointInside(startPointIn, out startPlanNode);

            IntPointNode endPlanNode   = null;
            var          lastToAddNode = GetWayPointInside(endPointIn, out endPlanNode);

            long startToEndDistanceSqrd  = (endPointIn - startPointIn).LengthSquared();
            long moveStartInDistanceSqrd = (startPlanNode.Position - lastAddedNode.Position).LengthSquared();
            long moveEndInDistanceSqrd   = (endPlanNode.Position - lastToAddNode.Position).LengthSquared();

            if (startToEndDistanceSqrd < moveStartInDistanceSqrd ||
                startToEndDistanceSqrd < moveEndInDistanceSqrd)
            {
                return(true);
            }

            var crossings = new List <Tuple <int, int, IntPoint> >(BoundaryPolygons.FindCrossingPoints(lastAddedNode.Position, lastToAddNode.Position, BoundaryEdgeQuadTrees));

            crossings.Sort(new PolygonAndPointDirectionSorter(lastAddedNode.Position, lastToAddNode.Position));
            foreach (var crossing in crossings.SkipSame())
            {
                IntPointNode crossingNode = Waypoints.FindNode(crossing.Item3, findNodeDist);
                // for every crossing try to connect it up in the waypoint data
                if (crossingNode == null)
                {
                    crossingNode = AddTempWayPoint(removePointList, crossing.Item3);
                    // also connect it to the next and prev points on the polygon it came from
                    HookUpToEdge(crossingNode, crossing.Item1, crossing.Item2);
                }

                if (lastAddedNode != crossingNode &&
                    BoundaryPolygons.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2, BoundaryEdgeQuadTrees, BoundaryPointQuadTrees))
                {
                    Waypoints.AddPathLink(lastAddedNode, crossingNode);
                }
                else if (crossingNode.Links.Count == 0)
                {
                    // link it to the edge it is on
                    HookUpToEdge(crossingNode, crossing.Item1, crossing.Item2);
                }
                lastAddedNode = crossingNode;
            }

            if (lastAddedNode != lastToAddNode &&
                BoundaryPolygons.PointIsInside((lastAddedNode.Position + lastToAddNode.Position) / 2, BoundaryEdgeQuadTrees))
            {
                // connect the last crossing to the end node
                Waypoints.AddPathLink(lastAddedNode, lastToAddNode);
            }

            Path <IntPointNode> path = Waypoints.FindPath(startPlanNode, endPlanNode, true);

            foreach (var node in path.Nodes.SkipSamePosition())
            {
                pathThatIsInside.Add(new IntPoint(node.Position, z));
            }

            if (path.Nodes.Length == 0)
            {
                if (saveBadPathToDisk)
                {
                    WriteErrorForTesting(startPointIn, endPointIn, 0);
                }
                CalculatedPath?.Invoke(this, pathThatIsInside, startPointIn, endPointIn);
                return(false);
            }

            if (optomizePath)
            {
                OptomizePathPoints(pathThatIsInside);
            }

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

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