Ejemplo n.º 1
0
    /// <summary>
    /// A* algorithm
    /// </summary>
    /// <param name="startPos">Node at which to start the pathat</param>
    /// <param name="targetPos">Target node</param>
    void FindPath(Node startPos, Node targetPos)
    {
        Heap <Node>    openSet   = new Heap <Node>(grid.MaxSize);
        HashSet <Node> closedSet = new HashSet <Node>();

        openSet.Add(startPos);

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

            // Path found
            if (currentNode == targetPos)
            {
                RetracePath(startPos, targetPos);
                return;
            }

            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                if (!neighbor.walkable || closedSet.Contains(neighbor))
                {
                    continue;
                }

                int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost  = newMovementCostToNeighbor;
                    neighbor.hCost  = GetDistance(neighbor, targetPos);
                    neighbor.parent = currentNode;

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