Thuật toán tìm đường A* (A-Star)

// A* là một thuật toán phức tạp, đây là một phiên bản giả mã/đơn giản hóa để hiểu ý tưởng chính.
using System.Collections.Generic;

public class AStarPathfinder {
    public List<Node> FindPath(Node startNode, Node targetNode) {
        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);

        while (openSet.Count > 0) {
            Node currentNode = openSet[0];
            // Tìm node có F-cost thấp nhất trong openSet
            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];
                }
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if (currentNode == targetNode) {
                return RetracePath(startNode, targetNode);
            }

            foreach (Node neighbour in GetNeighbours(currentNode)) {
                if (!neighbour.isWalkable || closedSet.Contains(neighbour)) continue;

                int newMoveCost = currentNode.gCost + GetDistance(currentNode, neighbour);
                if (newMoveCost < neighbour.gCost || !openSet.Contains(neighbour)) {
                    neighbour.gCost = newMoveCost;
                    neighbour.hCost = GetDistance(neighbour, targetNode); // H-cost: chi phí ước tính
                    neighbour.parent = currentNode;

                    if (!openSet.Contains(neighbour)) openSet.Add(neighbour);
                }
            }
        }
        return null; // Không tìm thấy đường đi
    }
    // Các hàm phụ trợ như RetracePath, GetNeighbours, GetDistance cần được triển khai...
}

public class Node {
    public bool isWalkable; public Vector3 worldPosition;
    public int gCost, hCost; public Node parent;
    public int FCost { get { return gCost + hCost; } }
}