コード例 #1
0
ファイル: PF_Pathfinding.cs プロジェクト: Sauroctone/CRAB
    public Vector3[] FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Vector3[] waypoints   = new Vector3[0];
        bool      pathSuccess = false;

        Node startNode  = grid.GetNodeFromWorldPoint(startPos);
        Node targetNode = grid.GetNodeFromWorldPoint(targetPos);

        if (!targetNode.walkable)
        {
            targetNode = GetClosestValidNode(targetNode, targetPos);
        }

        if (startNode.walkable && targetNode.walkable)
        {
            Heap <Node>    openSet   = new Heap <Node> (grid.MaxSize); //Nodes to be evaluated (check if it is the closest to target)
            HashSet <Node> closedSet = new HashSet <Node> ();          //Nodes already evaluated (is used to add his neighbours to the openSet)
            openSet.Add(startNode);

            while (openSet.Count > 0)
            {
                Node currentNode = openSet.RemoveFirst();

                /*//Find the node with the lowest fCost (picks the one with the lowest hcost if there are several)
                 * for (int i = 1; i < openSet.Count; i++)
                 * {
                 *      if (openSet [i].fCost < currentNode.fCost || openSet [i].fCost == currentNode.fCost && openSet [i].hCost < currentNode.hCost)
                 *      {
                 *              currentNode = openSet [i];
                 *      }
                 * }
                 *
                 * //The node is eveluated, so we pass it to the closedSet
                 * openSet.Remove (currentNode);*/
                closedSet.Add(currentNode);

                //Checks if the node is the target
                if (currentNode == targetNode)
                {
                    pathSuccess = true;
                    break;
                }

                //Check the neighbours of the current node to add them to the open set
                //where they will be evaluted in the next loop
                foreach (Node neighbour in grid.GetNeighbours(currentNode))
                {
                    // pass the node if it's not walkable or has already been evaluted
                    if (!neighbour.walkable || closedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    //update or set gCost and hCost of the node
                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, targetNode);
                        //set the currentNode as "parent" (it's a variable) of the neighbour node
                        //(the "parenting" hierachy will help trace the path from the target to the start)
                        neighbour.parent = currentNode;

                        //Add to open set if it's not already in
                        if (!openSet.Contains(neighbour))
                        {
                            openSet.Add(neighbour);
                            openSet.UpdateItem(neighbour);
                        }
                    }
                }
            }
        }

        if (pathSuccess)
        {
            waypoints = RetracePath(startNode, targetNode);
            return(RetracePath(startNode, targetNode));
        }
        else
        {
            return(null);
        }
    }
コード例 #2
0
    IEnumerator FindPath(Vector3 startPos_, Vector3 endPos_)
    {
        Stopwatch sw = new Stopwatch();

        sw.Start();

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

        // Get the start and end nodes.
        PF_Node startNode = grid.GetNodeFromWorldPoint(startPos_);
        PF_Node endNode   = grid.GetNodeFromWorldPoint(endPos_);

        startNode.parentNode = startNode;

        // Only search if start and end are both walkable nodes.
        if (startNode.walkable && endNode.walkable)
        {
            // Create two lists of nodes.
            // Open list will contain nodes to be evaluated.
            // Closed list will contain nodes that have already been evaluated.
            PF_Heap <PF_Node> openSet   = new PF_Heap <PF_Node>(grid.MaxSize);
            HashSet <PF_Node> closedSet = new HashSet <PF_Node>();

            // Add the starting node to our open list.
            openSet.Add(startNode);

            // Loop through the openSet.
            while (openSet.Count > 0)
            {
                // Begin with the first element in the list.
                PF_Node currentNode = openSet.RemoveFirst();

                // Add the starting node to the closed set.
                closedSet.Add(currentNode);

                // Path has been found.
                if (currentNode == endNode)
                {
                    sw.Stop();

                    //UnityEngine.Debug.Log("Path found: " + sw.ElapsedMilliseconds + "ms");

                    pathSuccess = true;

                    break;
                }

                foreach (PF_Node neighbour in grid.GetNeighbourNodes(currentNode))
                {
                    // Check if the neighbour is not walkable, or already in the closed list.
                    if (!neighbour.walkable || closedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    // Get the cost to move to the neighbour.
                    int newNeighbourMoveCost = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;

                    // If the new move cost is smaller than the current, or the open list does not contain this neighbour.
                    if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        // Update the gCost and hCost.
                        neighbour.gCost = newNeighbourMoveCost;
                        neighbour.hCost = GetDistance(neighbour, endNode);

                        // Update the parent node.
                        neighbour.parentNode = currentNode;

                        // Add it to the open set if it is not currently there.
                        if (!openSet.Contains(neighbour))
                        {
                            openSet.Add(neighbour);
                        }
                        // Otherwise update it.
                        else
                        {
                            openSet.UpdateItem(neighbour);
                        }
                    }
                }
            }
        }

        yield return(null);

        if (pathSuccess)
        {
            waypoints = CalculatePath(startNode, endNode);
        }

        requestManager.FinishedProcessingPath(waypoints, pathSuccess);
    }