示例#1
0
    // string, vector3
    public Vector3 FindPath(Vector3 startPos, Vector3 targetPos)
    {
        // Node startNode = grid.NodeFromWorldPoint(startPos);
        // Node targetNode = grid.NodeFromWorldPoint(targetPos);

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


        List <Node>    openSet   = new List <Node>();
        HashSet <Node> closedSet = new HashSet <Node>();

        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].F_value < currentNode.F_value || (openSet[i].F_value == currentNode.F_value && openSet[i].H_value < currentNode.H_value))
                {
                    currentNode = openSet[i];
                }
            }
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);



            if (currentNode == targetNode)
            {
                // return RetracePath_S(startNode, targetNode);
                return(RetracePath_V(startNode, targetNode));
            }

            foreach (Node neighbour in grid.GetNeighbours(currentNode, FourDir))
            {
                if (neighbour.Type == Node.tile_type.WALL || closedSet.Contains(neighbour))
                {
                    continue;
                }

                int newMovementCostToNeighbour = currentNode.G_value + GetDistance(currentNode, neighbour);
                if (newMovementCostToNeighbour < neighbour.G_value || !openSet.Contains(neighbour))
                {
                    neighbour.G_value = newMovementCostToNeighbour;
                    neighbour.H_value = GetDistance(neighbour, targetNode);

                    neighbour.parent = currentNode;

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

        //return "NONE";
        return(startPos);
    }