예제 #1
0
        private bool CalculateShortestPath()
        {
            Node currentNode;

            Position[] neighbors = new Position[8];

            heap.Add(startNode, 0);
            while (heap.Count > 0)
            {
                currentNode = heap.Remove();
                if (currentNode == targetNode)
                {
                    return(true);
                }

                currentNode.setClosed();
                var count = GetNeighbors(currentNode, ref neighbors);
                for (var i = 0; i < count; i++)
                {
                    var neighbor     = neighbors[i];
                    var neighborNode = GetNeighborNode(currentNode, neighbor);
                    if (neighborNode == null || neighborNode.isClosed())
                    {
                        continue;
                    }

                    int newGCost = NewGCost(currentNode, neighborNode);
                    if (newGCost < neighborNode.gCost || !neighborNode.isOpen())
                    {
#if DEBUG_PATHFINDING
                        if (showDebug)
                        {
                            DebugDrawer.Draw(new Vector2Int(currentNode.x, currentNode.y), new Vector2Int(neighborNode.x, neighborNode.y), Color.white);
                            DebugDrawer.DrawCube(new Vector2Int(neighborNode.x, neighborNode.y), Vector2Int.one, Color.white);
                        }
#endif

                        neighborNode.gCost  = newGCost;
                        neighborNode.hCost  = Heuristic(neighborNode, targetNode);
                        neighborNode.parent = currentNode;

                        if (!neighborNode.isOpen())
                        {
                            heap.Add(neighborNode, neighborNode.gCost + neighborNode.hCost);
                            neighborNode.setOpen();
                        }
                        else
                        {
                            heap.Update(neighborNode, neighborNode.gCost + neighborNode.hCost);
                        }
                    }
                }
            }
            return(false);
        }
예제 #2
0
        public static List <Subregion> GetPath(Subregion startSubregion, Subregion targetSubregion)
        {
            if (startSubregion.Region != targetSubregion.Region)
            {
                return(null);
            }

            if (startSubregion == targetSubregion)
            {
                List <Subregion> result = new List <Subregion>();
                result.Add(startSubregion);
                return(result);
            }

            MinHeap <Subregion> openSet   = new MinHeap <Subregion>(startSubregion.Region.Subregions.Count);
            HashSet <Subregion> closedSet = new HashSet <Subregion>();

            openSet.Add(startSubregion);
            while (openSet.Count > 0)
            {
                Subregion currentSubregion = openSet.RemoveFirst();

                closedSet.Add(currentSubregion);

                if (currentSubregion == targetSubregion)
                {
                    return(RetracePath(startSubregion, targetSubregion));
                }

                foreach (Subregion neighbour in currentSubregion.NeighbouringSubregions)
                {
                    if (closedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    int newMovementCostToNeighbour = currentSubregion.gCost + GetDistance(currentSubregion, neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost           = newMovementCostToNeighbour;
                        neighbour.hCost           = GetDistance(neighbour, targetSubregion);
                        neighbour.ParentSubregion = currentSubregion;

                        if (!openSet.Contains(neighbour))
                        {
                            openSet.Add(neighbour);
                        }
                    }
                }
            }

            return(null);
        }
예제 #3
0
        private void expandForwardFrontier(Node current, ref Neighbor[] neighbors)
        {
            current.setClosedA();

            var count = GetNeighbors(current, ref neighbors);

            for (var i = 0; i < count; i++)
            {
                var neighbor = neighbors[i].node;
                if (neighbor.isClosedA())
                {
                    continue;
                }

                var tentativeScore = current.costA + neighbors[i].cost;
                if (!neighbor.isOpenA())
                {
                    neighbor.setOpenA();
                    neighbor.costA   = tentativeScore;
                    neighbor.parentA = current;
                    openA.Add(neighbor, tentativeScore);
                    updateForwardFrontier(neighbor, tentativeScore);
                }
                else if (neighbor.costA > tentativeScore)
                {
                    neighbor.costA   = tentativeScore;
                    neighbor.parentA = current;
                    openA.Update(neighbor, tentativeScore);
                    updateForwardFrontier(neighbor, tentativeScore);
                }
            }
        }
예제 #4
0
        public virtual List <Node> GetPath(Vector2Int start, Vector2Int target, float upperBound = float.PositiveInfinity)
        {
            nodes = new Dictionary <long, Node>();

            var startNode = GetStartNode(start);
            var endNode   = GetEndNode(target);
            var neighbors = new Neighbor[8];

            middleNode     = null;
            bestPathLength = float.PositiveInfinity;

            openA = new MinHeap <Node, float>();
            openB = new MinHeap <Node, float>();

            openA.Add(startNode, 0);
            openB.Add(endNode, 0);

            var ticks = 0;

            while (openA.Count > 0 && openB.Count > 0)
            {
                ticks++;

                var mtmp = openA.Peek().costA + openB.Peek().costB;
                if (mtmp >= bestPathLength)
                {
                    Debug.Log("Found path in " + ticks + " ticks " + expandedNodes + " expanded nodes");
                    return(tracebackPath(middleNode));
                }
                if (mtmp >= upperBound)
                {
                    return(null);
                }

                expandForwardFrontier(openA.Remove(), ref neighbors);
                ticks++;

                mtmp = openA.Peek().costA + openB.Peek().costB;
                if (mtmp >= bestPathLength)
                {
                    Debug.Log("Found path in " + ticks + " ticks " + expandedNodes + " expanded nodes");
                    return(tracebackPath(middleNode));
                }
                if (mtmp >= upperBound)
                {
                    return(null);
                }

                expandBackwardFrontier(openB.Remove(), ref neighbors);
            }

            return(null);
        }
예제 #5
0
        public static List <Node> GetPath(Node startNode, Node targetNode)
        {
            if (startNode == targetNode)
            {
                return(new List <Node>());
            }

            if (startNode.Region != targetNode.Region)
            {
                return(null);
            }

            //Swap source with target for optimization of path retracing.
            Node tmp = startNode;

            startNode  = targetNode;
            targetNode = tmp;

            List <Subregion>  subregionPath = ASubregionSearch.GetPath(startNode.Subregion, targetNode.Subregion);
            Stack <Subregion> corridor      = new Stack <Subregion>(subregionPath);

            MinHeap <Node> openSet   = new MinHeap <Node>(Pathfinder.MapWidth * Pathfinder.MapHeight);
            HashSet <Node> closedSet = new HashSet <Node>();

            openSet.Add(startNode);

            while (openSet.Count > 0)
            {
                Node currentNode;
                do
                {
                    currentNode = openSet.RemoveFirst();
                    if (corridor.Count == 0 || currentNode.Subregion == corridor.Peek())
                    {
                        break;
                    }
                } while (true);

                closedSet.Add(currentNode);
                HandleAddToClosedSet?.Invoke(currentNode);                //visualization

                if (currentNode == targetNode)
                {
                    List <Node> path = RetracePath(startNode, targetNode);

                    foreach (Subregion subregion in subregionPath)
                    {
                        subregion.Child = null;
                    }

                    return(path);
                }

                foreach (Node neighbour in currentNode.GetAllNeighbours())
                {
                    if (!neighbour.IsTraversable || closedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    if (corridor.Count != 0 && corridor.Peek().Child == neighbour.Subregion)
                    {
                        corridor.Pop();
                    }

                    if (corridor.Count != 0 && !corridor.Peek().Nodes.Contains(neighbour))
                    {
                        continue;
                    }

                    int  newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
                    bool isInOpenSet        = openSet.Contains(neighbour);
                    if (newCostToNeighbour < neighbour.gCost || !isInOpenSet)
                    {
                        neighbour.gCost = newCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, targetNode);

                        if (neighbour.Subregion.Child != null)
                        {
                            neighbour.rCost = GetDistance(neighbour,
                                                          PathGrid.NodeAt(neighbour.Subregion.Child.AvergX,
                                                                          neighbour.Subregion.Child.AvergY));
                        }
                        else
                        {
                            neighbour.rCost = 0;
                        }

                        neighbour.Parent = currentNode;

                        if (!isInOpenSet)
                        {
                            openSet.Add(neighbour);
                        }
                    }
                }
            }

            foreach (Subregion subregion in subregionPath)
            {
                subregion.Child = null;
            }

            return(null);
        }
예제 #6
0
        public void FindPath(PathRequest request, Action <PathResult> callback)
        {
            Stopwatch sw = new Stopwatch();

            sw.Start();

            Vector3[] waypoints  = new Vector3[0];
            bool      pathSucces = false;

            Node startNode  = _grid.WorldPositionToNode(request.start);
            Node targetNode = _grid.WorldPositionToNode(request.end);

            if (startNode.IsWalkable && targetNode.IsWalkable)
            {
                MinHeap <Node> openSet   = new MinHeap <Node>(_grid.MaxSize);
                HashSet <Node> closedSet = new HashSet <Node>();

                openSet.Add(startNode);

                while (openSet.Count > 0)
                {
                    Node current = openSet.RemoveFirst();
                    closedSet.Add(current);

                    if (current == targetNode)
                    {
                        sw.Stop();
                        UnityEngine.Debug.Log("Path found " + sw.ElapsedMilliseconds + " ms");
                        pathSucces = true;
                        break;
                    }


                    foreach (Node neighbour in current.Neighbours)
                    {
                        if (neighbour == null || !neighbour.IsWalkable || closedSet.Contains(neighbour))
                        {
                            continue;
                        }

                        int newMovementCost = current.GCost + GetDistance(current, neighbour);

                        if (newMovementCost < neighbour.GCost || !openSet.Contains(neighbour))
                        {
                            neighbour.GCost  = newMovementCost;
                            neighbour.HCost  = GetDistance(neighbour, targetNode);
                            neighbour.Parent = current;

                            if (!openSet.Contains(neighbour))
                            {
                                openSet.Add(neighbour);
                            }
                            else
                            {
                                openSet.UpdateItem(neighbour);
                            }
                        }
                    }
                }
            }

            if (pathSucces)
            {
                waypoints = RetracePath(startNode, targetNode);
            }

            callback(new PathResult(waypoints, pathSucces, request.callback));
        }