Пример #1
0
        private IntPointNode GetWayPointInside(IntPoint position, out IntPointNode waypointAtPosition)
        {
            waypointAtPosition = null;
            OutlineData.Polygons.MovePointInsideBoundary(position, out (int polyIndex, int pointIndex, IntPoint position)foundPolyPointPosition, OutlineData.EdgeQuadTrees, OutlineData.PointKDTrees, OutlineData.PointIsInside);
            if (foundPolyPointPosition.polyIndex == -1)
            {
                // The point is already inside
                var existingNode = OutlineData.Waypoints.FindNode(position, FindNodeDist);
                if (existingNode == null)
                {
                    waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
                    return(waypointAtPosition);
                }

                waypointAtPosition = existingNode;
                return(waypointAtPosition);
            }
            else             // The point had to be moved inside the polygon
            {
                if (position == foundPolyPointPosition.position)
                {
                    var existingNode = OutlineData.Waypoints.FindNode(position, FindNodeDist);
                    if (existingNode != null)
                    {
                        waypointAtPosition = existingNode;
                        return(waypointAtPosition);
                    }
                    else
                    {
                        // get the way point that we need to insert
                        waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
                        HookUpToEdge(waypointAtPosition, foundPolyPointPosition.polyIndex, foundPolyPointPosition.pointIndex);
                        return(waypointAtPosition);
                    }
                }
                else                 // the point was outside, hook it up to the nearest edge
                {
                    // find the start node if we can
                    IntPointNode startNode = OutlineData.Waypoints.FindNode(foundPolyPointPosition.position, FindNodeDist);

                    // After that create a temp way point at the current position
                    waypointAtPosition = AddTempWayPoint(OutlineData.RemovePointList, position);
                    if (startNode != null)
                    {
                        OutlineData.Waypoints.AddPathLink(startNode, waypointAtPosition);
                    }
                    else
                    {
                        // get the way point that we need to insert
                        startNode = AddTempWayPoint(OutlineData.RemovePointList, foundPolyPointPosition.position);
                        HookUpToEdge(startNode, foundPolyPointPosition.polyIndex, foundPolyPointPosition.pointIndex);
                        OutlineData.Waypoints.AddPathLink(startNode, waypointAtPosition);
                    }

                    return(startNode);
                }
            }
        }
Пример #2
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);
                }
            }
        }
Пример #3
0
 public virtual float DistanceTo(Pathfinding.IPoint pPoint)
 {
     if (pPoint is IntPointNode)
     {
         IntPointNode otherNode = pPoint as IntPointNode;
         return((Position - otherNode.Position).Length());
     }
     else
     {
         throw new NotImplementedException();
     }
 }
Пример #4
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);
        }
Пример #5
0
        private void HookUpToEdge(IntPointNode crossingNode, int polyIndex, int pointIndex)
        {
            int count = OutlineData.Polygons[polyIndex].Count;

            if (count > 0)
            {
                pointIndex = (pointIndex + count) % count;
                IntPointNode prevPolyPointNode = OutlineData.Waypoints.FindNode(OutlineData.Polygons[polyIndex][pointIndex]);
                OutlineData.Waypoints.AddPathLink(crossingNode, prevPolyPointNode);
                IntPointNode nextPolyPointNode = OutlineData.Waypoints.FindNode(OutlineData.Polygons[polyIndex][(pointIndex + 1) % count]);
                OutlineData.Waypoints.AddPathLink(crossingNode, nextPolyPointNode);
            }
        }
Пример #6
0
 private bool SegmentIsAllInside(IntPointNode lastAddedNode, IntPointNode crossingNode)
 {
     if (OutlineData.DistanceFromOutside == null)
     {
         // check just the center point
         return(OutlineData.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside);
     }
     else
     {
         // check many points along the line
         return(OutlineData.PointIsInside((lastAddedNode.Position + crossingNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside &&
                OutlineData.PointIsInside(lastAddedNode.Position + (crossingNode.Position - lastAddedNode.Position) / 4) == QTPolygonsExtensions.InsideState.Inside &&
                OutlineData.PointIsInside(lastAddedNode.Position + (crossingNode.Position - lastAddedNode.Position) * 3 / 4) == QTPolygonsExtensions.InsideState.Inside);
     }
 }
Пример #7
0
        public int CompareTo(object obj)
        {
            IntPointNode target      = obj as IntPointNode;
            float        targetValue = target.PathCostHere + target.DistanceToGoal;
            float        thisValue   = PathCostHere + DistanceToGoal;

            if (targetValue > thisValue)
            {
                return(1);
            }
            else if (targetValue == thisValue)
            {
                return(0);
            }
            else
            {
                return(-1);
            }
        }
Пример #8
0
        private bool CalculatePath(IntPoint startPointIn, IntPoint endPointIn, Polygon pathThatIsInside, int layerIndex)
        {
            double z = startPointIn.Z;

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

            // neither needed to be moved
            if (OutlineData.Polygons.FindIntersection(startPointIn, endPointIn, OutlineData.EdgeQuadTrees) == Intersection.None &&
                OutlineData.PointIsInside((startPointIn + endPointIn) / 2) == QTPolygonsExtensions.InsideState.Inside)
            {
                return(true);
            }

            OutlineData.RemovePointList.Dispose();

            pathThatIsInside.Clear();

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

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

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

            // if we move both points less than the distance of this segment
            if (startToEndDistanceSqrd < moveStartInDistanceSqrd &&
                startToEndDistanceSqrd < moveEndInDistanceSqrd)
            {
                // then go ahead and say it is a good path
                return(true);
            }

            var crossings = new List <(int polyIndex, int pointIndex, IntPoint position)>(OutlineData.Polygons.FindCrossingPoints(lastAddedNode.Position, lastToAddNode.Position, OutlineData.EdgeQuadTrees));

            if (crossings.Count == 0)
            {
                return(true);
            }

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

                if (lastAddedNode != crossingNode &&
                    (SegmentIsAllInside(lastAddedNode, crossingNode) || lastAddedNode.Links.Count == 0))
                {
                    OutlineData.Waypoints.AddPathLink(lastAddedNode, crossingNode);
                }
                else if (crossingNode.Links.Count == 0)
                {
                    // link it to the edge it is on
                    HookUpToEdge(crossingNode, polyIndex, pointIndex);
                }

                lastAddedNode = crossingNode;
            }

            if (lastAddedNode != lastToAddNode &&
                (OutlineData.PointIsInside((lastAddedNode.Position + lastToAddNode.Position) / 2) == QTPolygonsExtensions.InsideState.Inside ||
                 lastToAddNode.Links.Count == 0))
            {
                // connect the last crossing to the end node
                OutlineData.Waypoints.AddPathLink(lastAddedNode, lastToAddNode);
            }

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

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

            if (path.Nodes.Length == 0 && saveBadPathToDisk)
            {
                WriteErrorForTesting(layerIndex, startPointIn, endPointIn, 0);
                return(false);
            }

            return(true);
        }
Пример #9
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);
        }
Пример #10
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);
        }