Unity實現A*尋路演算法學習2.0

AlphaIcarus 發表於 2022-04-27
演算法 Unity

二叉樹儲存路徑節點

1.0中雖然實現了尋路的演算法,但是使用List<>來儲存節點效能並不夠強

尋路演算法學習1.0在這裡:https://www.cnblogs.com/AlphaIcarus/p/16185843.html

更好的方法是使用堆(或者叫樹)來代替列表儲存節點
注意:這裡使用陣列來實現堆,而非使用連結串列實現堆
這裡使用二叉樹的方式來儲存節點之間的關係

如果在樹的末尾新增了一個較小的值,
Unity實現A*尋路演算法學習2.0
那麼需要和父節點比較大小,如果更小,則交換位置

Unity實現A*尋路演算法學習2.0
然後再與父節點比較大小,如果小於父節點,則再次交換位置

Unity實現A*尋路演算法學習2.0
如果大於父節點,則停止交換

那如果較小的元素被移除了又怎麼排序呢?(之前說過因為Clsot值比較後有時需要重新設定父節點)
首先把樹末尾的節點資料新增到樹頂
Unity實現A*尋路演算法學習2.0
然後與兩個子節點比較大小,和更小的那一個交換位置
Unity實現A*尋路演算法學習2.0
然後再與兩個子節點比較大小,直到兩個子節點都更大
Unity實現A*尋路演算法學習2.0
那麼如何獲取相關的節點呢?這裡的數字表示第幾個節點而不是儲存的具體資料
Unity實現A*尋路演算法學習2.0
可以發現以下關係
如果獲取某個節點 n (第 n 個節點)

那麼該節點的父節點為 ( n - 1) / 2

左子節點為 2n + 1

右子節點為 2n + 2

在Unity中應用

新建指令碼Heap,這是一個資料型別,用來代替List型別,由陣列構成

public class Heap<T> where T : IHeapItem<T>	//繼承了該介面之後,需要實現資料型別T比較大小的方法
{
    T[] items;  //泛型陣列,可以為任何型別的資料 
    int currentItemCount;	//當前總共有多少元素
    public int Count		//屬性,訪問返回元素個數
    {
        get { return currentItemCount; }
    }
    public Heap(int maxHeapSize)    //構造器
    {
        items = new T[maxHeapSize];
    }
    public void Add(T item)     //新增新元素的方法
    {
        item.HeapIndex = currentItemCount;
        items[currentItemCount] = item;	//末尾新增新元素
        SortUp(item);				   //排序
        currentItemCount++;			    //元素總數+1
    }
    public T RemoveFirt()			//獲取堆頂部元素並移除
    {
        T firstItem = items[0];		//取得頂部元素
        currentItemCount--;			//元素總數-1
        items[0] = items[currentItemCount];	//將最後一個元素移到頂部
        items[0].HeapIndex = 0;				//將該元素的索引設為0,即第一個元素
        SortDown(items[0]);					//下沉元素
        return firstItem;			//返回取得的頂部元素
    }
    public void UpdateItem(T item)
    {
        SortUp(item);
    }
    public bool Contains(T item)	//判斷是否包含元素
    {
        return Equals(items[item.HeapIndex], item);
    }
    void SortDown(T item)     //下沉元素
    {
        while (true)	//一直迴圈,直到值小於左右子樹或到最後位置
        {
            int childIndexLeft = item.HeapIndex * 2 + 1;	//左子樹的索引
            int childIndexRight = item.HeapIndex * 2 + 2;	//右子樹的索引
            int swapIndex = 0;
            if (childIndexLeft < currentItemCount)	//
            {
                swapIndex = childIndexLeft;
                if (childIndexRight < currentItemCount)
                {
                    if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0)
                    {
                        swapIndex = childIndexRight; 
                    }
                }
                if (item.CompareTo(items[swapIndex]) < 0)
                {
                    Swap(item, items[swapIndex]);
                }
                else
                {
                    return;
                }
            }
            else
            {
                return;
            }
        }
    }
    private void SortUp(T item) //上浮元素
    {
        int parentIndex = (item.HeapIndex - 1) / 2;     //父節點的位置
        while (true)	//迴圈直至資料比父節點大
        {
            T parentItem = items[parentIndex];
            if (item.CompareTo(parentItem) > 0) //如果新的資料比父節點更小(f值更小)
            {
                Swap(item, parentItem);		//交換位置
            }
            else
            {
                break;
            }
            parentIndex = (item.HeapIndex - 1) / 2;	//下次迴圈前再次驗證父節點索引
        }
    }
    private void Swap(T itemA, T itemB)	//交換陣列中兩個元素的位置
    {
        items[itemA.HeapIndex] = itemB;
        items[itemB.HeapIndex] = itemA;
        int itemAIndex = itemA.HeapIndex;
        itemA.HeapIndex = itemB.HeapIndex;
        itemB.HeapIndex = itemAIndex;
    }
}
public interface IHeapItem<T> : IComparable<T> //介面,Heap必須實現該介面,IHeapItem繼承可比較介面
{
    int HeapIndex { get; set; }
}

接下來是Node

public class Node : IHeapItem<Node>	//繼承介面,需要實現能夠比較Node大小的方法
{
    ......
    private int heapIndex;	//宣告節點的索引
    ......
    public int HeapIndex	//屬性
    {
        get { return heapIndex; }
        set { heapIndex = value; }
    }
    public int CompareTo(Node needToCompare)	//實現的比較大小的方法
    {
        //通過比較 F 值的大小,來判斷節點的大小
        int compare = FCost.CompareTo(needToCompare.FCost);
        if (compare == 0)
        {
            compare = hCost.CompareTo(needToCompare.hCost);
        }
        return -compare;
    }
}

接下來是PathFinding

public class PathFinding : MonoBehaviour
{
    ......
    private void Update()
    {
        if (Input.GetKeyDown(KeyCode.Space))
        {
            //分別註釋方法,執行後比較兩個方法尋找相同路徑所需的時間
            //FindPath(seeker.position, target.position);	//之前使用List的方法
            FindPathHeap(seeker.position, target.position);	//新的使用Heap的方法
        }
    }
    ......
    private void FindPathHeap(Vector3 startPos, Vector3 targetPos)
    {
        Stopwatch sw = new Stopwatch();	//計時器
        sw.Start();
        Node startNode = grid.NodeFromWorldPos(startPos);   //輸入空間座標,計算出處於哪個節點位置
        Node targwtNode = grid.NodeFromWorldPos(targetPos);
		//這裡將List替換為了Heap,初始化Heap
        Heap<Node> openSet = new Heap<Node>(grid.MaxSize);          //用於儲存需要評估的節點
        HashSet<Node> closedSet = new HashSet<Node>();  //用於儲存已經評估的節點

        openSet.Add(startNode);

        while (openSet.Count > 0)   //如果還有待評估的節點
        {
            Node currentNode = openSet.RemoveFirt();  //獲取其中一個待評估的節點
            closedSet.Add(currentNode);     //將該節點加入已評估的節點,之後不再參與評估
            if (currentNode == targwtNode)  //如果該節點為目標終點,就計算出實際路徑並結束迴圈
            {
                sw.Stop();
                print("Path found: " + sw.ElapsedMilliseconds + "ms");
                RetracePath(startNode, targwtNode);
                return;
            }
            //如果該節點不是目標點,遍歷該點周圍的所有節點
            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                //如果周圍某節點不能行走 或 周圍某節點已經評估,為上一個節點,則跳過
                //                          說明某節點已經設定父節點
                if (!neighbor.walkable || closedSet.Contains(neighbor))
                {
                    continue;
                }
                //計算前起始點前往某節點的 gCost 值,起始點的 gCost 值就是0  
                //經過迴圈這裡會計算周圍所有節點的g值
                int newMovementCostToNeighbor = currentNode.gCost + GetDinstance(currentNode, neighbor);
                //如果新路線 gCost 值更小(更近), 或 某節點沒有評估過(為全新的節點)
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {

                    neighbor.gCost = newMovementCostToNeighbor;             //計算某節點gCost
                    neighbor.hCost = GetDinstance(neighbor, targwtNode);    //計算某節點hCost
                    neighbor.parent = currentNode;                          //將中間節點設為某節點的父節點
                                                                            //如果存在某節點gCost更小的節點,會重新將中間節點設為某節點父節點
                    if (!openSet.Contains(neighbor))    //如果某節點沒有評估過
                    {
                        openSet.Add(neighbor);          //將某節點加入待評估列表,在下一次迴圈進行評估,
                    }
                }
            }
        }
    }
    ......
}

接下來是MyGrid

public class MyGrid : MonoBehaviour
{
    public bool onlyPlayPathGizmos;	//是否只繪製路徑,便於觀察
    ......
    void Start()
    {
        nodeDiameter = nodeRadius * 2;
        gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter); //計算出x軸方向有多少個節點
        gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter); //計算出z軸方向有多少個節點
        CreateGrid();
    }
    public int MaxSize	//屬性,返回地圖路徑點的數量
    {
        get { return gridSizeX * gridSizeY; }
    }
    ......
    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
        if (grid != null)
        {
            Node playerNode = NodeFromWorldPos(player.position);
            if (onlyPlayPathGizmos)
            {
                //只繪製路徑
                if (path != null)
                {
                    foreach (Node node in path)
                    {
                        Gizmos.color = Color.yellow;
                        Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
                    }
                }
            }
            else	//繪製地圖所有點和路徑
            {
                foreach (Node node in grid)
                {
                   ......
                }
            }
        }
    }
}

接下來為了體現兩個方法的耗時差距

修改一下地圖的大小和節點的大小
Unity實現A*尋路演算法學習2.0
執行結果:

Unity實現A*尋路演算法學習2.0
使用Heap的耗時
Unity實現A*尋路演算法學習2.0
使用List的耗時
Unity實現A*尋路演算法學習2.0
可以看見速度大概提高了三倍