Exemple #1
0
        public virtual List <Node> GetPath(IGrid grid, Vector2Int start, Vector2Int target, bool greedy = true)
        {
            this.grid  = grid;
            sizeY      = grid.GetSize().y;
            heap       = new MinHeap <Node, int>();
            nodes      = new Dictionary <long, Node>();
            startNode  = GetNodeFromIndexUnchecked(start.x, start.y);
            targetNode = GetNodeFromIndexUnchecked(target.x, target.y);

            if (greedy && findPathGreedy())
            {
                return(RetracePath());
            }

            if (CalculateShortestPath())
            {
                return(RetracePath());
            }

            return(null);
        }
Exemple #2
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);
        }
        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));
        }