Unity A*尋路算法

看了下蠻牛教育的A*尋路視頻後整理的代碼，下面就直接貼代碼了

首先是Node類的聲明：定義一些節點所需的變量

using UnityEngine;
using System.Collections;

public class Node  
{
    //節點索引
    public int gridX, gridY;

    //節點所在位置
    public Vector3 worldPos;

    //節點是否可以行走
    public bool walkable;
    
    //評估參數： gConst-到起點的距離  hCost-到終點 的距離
    public int gCost;
    public int hCost;

    //評估和：越小則最優
    public int fConst
    {
        get { return gCost + hCost; }
    }

    //用來尋路結束後反向尋找路徑節點
    public Node parent;

    public Node(bool walkable, Vector3 pos, int x, int y)
    {
        this.walkable = walkable;
        this.worldPos = pos;
        this.gridX = x;
        this.gridY = y;
    }
}

然後是Grid類，這是用來畫節點的，以及存儲節點信息的

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class Grid : MonoBehaviour 
{
    //平面的大小
    public Vector2 gridSize;

    //節點集合，會把平面劃分成節點分佈
    private Node[,] grid;

    //節點的半徑
    public float nodeRadius;

    //節點的直徑
    private float nodeDiameter;

    //障礙物所在的層級
    public LayerMask whatLayer;

    //根據平面的大小以及節點的半徑可以算出平面上一行和一列的節點的個數
    public int gridCntX, gridCntY;

    public Transform player;

    //路徑節點數列
    public List<Node> path = new List<Node>();

    void Start()
    {
        //直徑可以根據半徑得出
        nodeDiameter = nodeRadius * 2;

        //算出平面中節點的行數和列數
        gridCntX = Mathf.RoundToInt(gridSize.x / nodeDiameter);
        gridCntY = Mathf.RoundToInt(gridSize.y / nodeDiameter);

        //初始化節點集合
        grid = new Node[gridCntX, gridCntY];

        creatGrid();
    }

    void OnDrawGizmos()
    {
        //畫邊框
        Gizmos.DrawWireCube(transform.position, new Vector3(gridSize.x, 1, gridSize.y));

        if (grid == null)
            return;

        //把所有的節點畫出來
        foreach (var node in grid)
        {
            Gizmos.color = node.walkable ? Color.white : Color.red;
            Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }

        //畫出路徑
        if (path != null)
        {
            foreach (var node in path)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
            }
        }

        //標出玩家的位置
        Node playerNode = GetFromPostion(player.position);
        if (playerNode != null && playerNode.walkable)
        {
            Gizmos.color = Color.black;
            Gizmos.DrawCube(playerNode.worldPos, Vector3.one * (nodeDiameter - 0.1f));
        }
    }

    /// <summary>
    /// 創建節點
    /// </summary>
    private void creatGrid()
    {
        //首先計算出起始點位置
        Vector3 startPoint = transform.position - (gridSize.x / 2) * Vector3.right - (gridSize.y / 2) * Vector3.forward;

        for(int i =0;i<gridCntX;i++)
        {
            for(int j = 0;j<gridCntY;j++)
            {
                //獲取每個節點的位置
                Vector3 worPos = startPoint + Vector3.right * (i * nodeDiameter + nodeRadius) + Vector3.forward * (j * nodeDiameter + nodeRadius);

                //檢測該節點是否可以行走
                bool walkable = !Physics.CheckSphere(worPos, nodeRadius, whatLayer);

                //初始化每個節點
                grid[i, j] = new Node(walkable, worPos, i, j);
            }
        }
    }

    /// <summary>
    /// 根據position獲取該位置的節點
    /// </summary>
    /// <param name="pos"></param>
    /// <returns></returns>
    public Node GetFromPostion(Vector3 pos)
    {
        //因爲grid的中心點是原點(0,0,0)，所以(pos.x + gridSize.x / 2)爲相對grid的長度
        float percentX = (pos.x + gridSize.x / 2) / gridSize.x;
        float percentY = (pos.z + gridSize.y / 2) / gridSize.y;

        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        //總長度爲gridCntX，因爲x爲索引，範圍爲0 - gridCntX-1，所以要gridCntX-1
        int x = Mathf.RoundToInt((gridCntX - 1) * percentX);
        int y = Mathf.RoundToInt((gridCntY - 1) * percentY);

        return grid[x, y];
    }

    /// <summary>
    /// 獲取某個節點的相鄰的節點
    /// </summary>
    /// <param name="node"></param>
    /// <returns></returns>
    public List<Node> GetNeibourhood(Node node)
    {
        List<Node> neibourhood = new List<Node>();

        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                    continue;

                int tempX = node.gridX + i;
                int tempY = node.gridY + j;

                if (tempX < gridCntX && tempX > 0 && tempY > 0 && tempY < gridCntY)
                    neibourhood.Add(grid[tempX, tempY]);
            }
        }

        return neibourhood;
    }
	
}

下面就是FindPath類，尋路的關鍵算法就在裏面哦

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class FindPath : MonoBehaviour 
{
    //玩家和終點的位置
    public Transform player, endPoint;

    private Grid _grid;

    void Start()
    {
        _grid = GetComponent<Grid>();
    }

    void Update()
    {
        findingPath(player.position, endPoint.position);
    }
	
    /// <summary>
    /// 尋找路徑
    /// </summary>
    /// <param name="startPos"></param>
    /// <param name="endPos"></param>
    private void findingPath(Vector3 startPos, Vector3 endPos)
    {
        //獲取起點和終點的節點信息
        Node startNode = _grid.GetFromPostion(startPos);
        Node endNode = _grid.GetFromPostion(endPos);

        //建立開啓列表和關閉列表，並把起點加到開啓列表裏面
        List<Node> openList = new List<Node>();
        HashSet<Node> closeList = new HashSet<Node>();
        openList.Add(startNode);

        while (openList.Count > 0)
        {
            //首先獲取到開啓列表裏面最優的節點
            Node currentNode = openList[0];
            for (int i = 0; i < openList.Count; i++)
            {
                if (openList[i].fConst < currentNode.fConst ||
                    openList[i].fConst == currentNode.fConst && openList[i].hCost < currentNode.hCost)
                {
                    currentNode = openList[i];
                }
            }

            //然後把該節點加入到關閉列表中
            openList.Remove(currentNode);
            closeList.Add(currentNode);

            //如果當前節點爲終點說明尋路完成
            if (currentNode == endNode)
            {
                generatePath(startNode, endNode);
                return;
            }

            //刷新該節點附近一圈的節點的估值信息
            foreach(var node in _grid.GetNeibourhood(currentNode))
            {
                if (!node.walkable || closeList.Contains(node))
                    continue;

                int newCont = currentNode.gCost + getDistanceNodes(currentNode, node);
                if(newCont < node.gCost || !openList.Contains(node))
                {
                    node.gCost = newCont;
                    node.hCost = getDistanceNodes(node, endNode);
                    node.parent = currentNode;

                    if (!openList.Contains(node))
                        openList.Add(node);
                }
            }
        }
    }


    /// <summary>
    /// 獲取兩節點之間距離，即估價
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <returns></returns>
    private int getDistanceNodes(Node a, Node b)
    {
        int cntX = Mathf.Abs(a.gridX - b.gridX);
        int cntY = Mathf.Abs(a.gridY - b.gridY);

        if(cntX >= cntY)
            return 14 * cntY + 10 * (cntX - cntY);
        else
            return 14 * cntX + 10 * (cntY - cntX);
    }

    /// <summary>
    /// 當尋路完成後反向獲取路徑節點信息
    /// </summary>
    /// <param name="startNode"></param>
    /// <param name="endNode"></param>
    private void generatePath(Node startNode, Node endNode)
    {
        List<Node> path = new List<Node>();
        Node temp = endNode;

        while(temp != startNode)
        {
            path.Add(temp);
            temp = temp.parent;
        }

        path.Reverse();
        _grid.path = path;
    }
}

不過上面的算法只是初級的，沒有做過優化，當行走範圍大了之後，就會遇到性能瓶頸，運算量以幾何增長

至於優化有二叉堆，有興趣的人可以先自行研究，等我瞭解透了優化之後再繼續更新。。。

下面附上工程文件地址：http://download.csdn.net/download/liujunjie612/9784183