[Unity算法]A星尋路(一)：基礎版本

參考鏈接：

https://www.cnblogs.com/yangyxd/articles/5447889.html

 

一.原理

1.將場景簡化，分割為一個個正方形格子，這些格子稱之為節點(node)，從一個節點到另一個節點的距離稱之為代價(cost)。一個節點與水平/垂直方向的相鄰節點的代價是1，與對角節點的代價是1.4。這里引用公式f = g + h，f表示該節點的總代價，g表示該節點與上一路徑節點的代價，h表示該節點與目標節點的代價。

2.需要兩個列表，開啟列表(openList)和關閉列表(closeList)。開啟列表用來記錄需要考慮的節點，關閉列表用來記錄不會再考慮的節點。

3.在開啟列表中添加起始節點。

4.在開啟列表中找到總代價最低的節點，然后從開啟列表中移除該節點，從關閉列表中添加該節點，把與該節點相鄰的可通行的節點添加到開啟列表，並且更新這些節點的代價。

5.循環第四步，如果當前節點等於目標節點，則退出循環。

 

二.實現

FindWayNode.cs

using UnityEngine;

public class FindWayNode {

    public bool isObstacle;//是否是障礙物
    public Vector3 scenePos;//場景位置
    public int x, y;//坐標

    public int gCost;//與起始點的距離
    public int hCost;//與目標點的距離
    public int fCost {
        get { return gCost + hCost; }
    }//總距離

    public FindWayNode parentNode;//父節點

    public FindWayNode(bool isObstacle, Vector3 scenePos, int x, int y)
    {
        this.isObstacle = isObstacle;
        this.scenePos = scenePos;
        this.x = x;
        this.y = y;
    }
}

FindWayGrid.cs

using System.Collections.Generic;
using UnityEngine;

public class FindWayGrid {

    public int width;//格子水平方向個數
    public int height;//格子垂直方向個數
    public float nodeLength;//格子長度

    private FindWayNode[,] findWayNodes;//格子數組
    private float halfNodeLength;//格子長度的一半
    private Vector3 startPos;//場景坐標起點

    public FindWayGrid(int width, int height, float nodeLength = 1f)
    {
        this.width = width;
        this.height = height;
        this.nodeLength = nodeLength;

        findWayNodes = new FindWayNode[width, height];
        halfNodeLength = nodeLength / 2;
        startPos = new Vector3(-width / 2 * nodeLength + halfNodeLength, 0, -height / 2 * nodeLength + halfNodeLength);

        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
            {
                Vector3 pos = CoordinateToScenePos(x, y);
                findWayNodes[x, y] = new FindWayNode(false, pos, x, y);
            }
        }
    }

    //坐標轉場景坐標
    public Vector3 CoordinateToScenePos(int x, int y)
    {
        Vector3 pos = new Vector3(startPos.x + x * nodeLength, startPos.y, startPos.z + y * nodeLength);
        return pos;
    }

    //根據場景坐標獲取節點
    public FindWayNode GetNode(Vector3 pos)
    {
        int x = (int)(Mathf.RoundToInt(pos.x - startPos.x) / nodeLength);
        int y = (int)(Mathf.RoundToInt(pos.z - startPos.z) / nodeLength);
        x = Mathf.Clamp(x, 0, width - 1);
        y = Mathf.Clamp(y, 0, height - 1);
        return GetNode(x, y);
    }

    //根據坐標獲取節點
    public FindWayNode GetNode(int x, int y)
    {
        return findWayNodes[x, y];
    }

    //獲取相鄰節點列表
    public List<FindWayNode> GetNearbyNodeList(FindWayNode node)
    {
        List<FindWayNode> list = new List<FindWayNode>();
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0)
                {
                    continue;
                }
                int x = node.x + i;
                int y = node.y + j;
                if (x >= 0 && x < width && y >= 0 && y < height)
                {
                    list.Add(findWayNodes[x, y]);
                }
            }
        }
        return list;
    }

    //獲取兩個節點之間的距離
    int GetDistance(FindWayNode nodeA, FindWayNode nodeB)
    {
        int countX = Mathf.Abs(nodeA.x - nodeB.x);
        int countY = Mathf.Abs(nodeA.y - nodeB.y);
        if (countX > countY)
        {
            return 14 * countY + 10 * (countX - countY);
        }
        else
        {
            return 14 * countX + 10 * (countY - countX);
        }
    }

    //找出起點到終點的最短路徑
    public List<FindWayNode> FindWay(Vector3 startPos, Vector3 endPos)
    {
        FindWayNode startNode = GetNode(startPos);
        FindWayNode endNode = GetNode(endPos);

        List<FindWayNode> openList = new List<FindWayNode>();
        List<FindWayNode> closeList = new List<FindWayNode>();
        openList.Add(startNode);

        while (openList.Count > 0)
        {
            FindWayNode nowNode = openList[0];

            //選擇花費最低的
            for (int i = 0; i < openList.Count; i++)
            {
                if (openList[i].fCost <= nowNode.fCost &&
                    openList[i].hCost < nowNode.hCost)
                {
                    nowNode = openList[i];
                }
            }

            openList.Remove(nowNode);
            closeList.Add(nowNode);

            //找到目標節點
            if (nowNode == endNode)
            {
                return GeneratePath(startNode, endNode);
            }

            List<FindWayNode> nearbyNodeList = GetNearbyNodeList(nowNode);
            for (int i = 0; i < nearbyNodeList.Count; i++)
            {
                FindWayNode node = nearbyNodeList[i];
                //如果是牆或者已經在關閉列表中
                if (node.isObstacle || closeList.Contains(node))
                {
                    continue;
                }
                //計算當前相鄰節點與開始節點的距離
                int gCost = nowNode.gCost + GetDistance(nowNode, node);
                //如果距離更小，或者原來不在打開列表
                if (gCost < node.gCost || !openList.Contains(node))
                {
                    //更新與開始節點的距離
                    node.gCost = gCost;
                    //更新與結束節點的距離
                    node.hCost = GetDistance(node, endNode);
                    //更新父節點為當前選定的節點
                    node.parentNode = nowNode;
                    //加入到打開列表
                    if (!openList.Contains(node))
                    {
                        openList.Add(node);
                    }
                }
            }
        }

        return null;
    }

    //生成路徑
    public List<FindWayNode> GeneratePath(FindWayNode startNode, FindWayNode endNode)
    {
        List<FindWayNode> nodeList = new List<FindWayNode>();
        if (endNode != null)
        {
            FindWayNode tempNode = endNode;
            while (tempNode != startNode)
            {
                nodeList.Add(tempNode);
                tempNode = tempNode.parentNode;
            }
            nodeList.Reverse();//反轉路徑
        }
        return nodeList;
    }
}

TestFindWay.cs

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

public class TestFindWay : MonoBehaviour {
    
    public Transform startTra;//起點tra
    public Transform endTra;//終點tra
    public Transform floorTra;//地板tra

    public GameObject obstacleGridPrefab;//障礙物格子
    public GameObject pathGridPrefab;//路徑格子
    public LayerMask obstacleLayer;//障礙物所在的層
    
    private FindWayGrid findWayGrid;

    private GameObject obstacleRootGo;//障礙物格子的父go
    private GameObject pathRootGo;//路徑格子的父go
    private List<GameObject> pathGridGoList;//路徑格子go列表

    void Start ()
    {
        MeshFilter meshFilter = floorTra.GetComponent<MeshFilter>();
        int width = Mathf.CeilToInt(meshFilter.mesh.bounds.size.x) * (int)floorTra.localScale.x;
        int height = Mathf.CeilToInt(meshFilter.mesh.bounds.size.z) * (int)floorTra.localScale.z;

        findWayGrid = new FindWayGrid(width, height);

        obstacleRootGo = new GameObject("ObstacleRoot");
        pathRootGo = new GameObject("PathRoot");
        pathGridGoList = new List<GameObject>();

        ShowObstacle();
    }
    
    void Update ()
    {
        List<FindWayNode> nodeList = findWayGrid.FindWay(startTra.position, endTra.position);
        if (nodeList != null)
        {
            ShowPath(nodeList);
        }
    }

    //展示路徑
    public void ShowPath(List<FindWayNode> list)
    {
        for (int i = 0; i < list.Count; i++)
        {
            if (i < pathGridGoList.Count)
            {
                pathGridGoList[i].transform.position = list[i].scenePos;
                pathGridGoList[i].SetActive(true);
            }
            else
            {
                GameObject go = Instantiate(pathGridPrefab);
                go.transform.SetParent(pathRootGo.transform);
                go.transform.position = list[i].scenePos;
                pathGridGoList.Add(go);
            }
        }

        for (int i = list.Count; i < pathGridGoList.Count; i++)
        {
            pathGridGoList[i].SetActive(false);
        }
    }

    //展示障礙物
    public void ShowObstacle()
    {
        int width = findWayGrid.width;
        int height = findWayGrid.height;
        float halfNodeLength = findWayGrid.nodeLength / 2;

        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
            {
                FindWayNode node = findWayGrid.GetNode(x, y);
                bool isObstacle = Physics.CheckSphere(node.scenePos, halfNodeLength, obstacleLayer);
                if (isObstacle)
                {
                    GameObject go = GameObject.Instantiate(obstacleGridPrefab, node.scenePos, Quaternion.identity) as GameObject;
                    go.transform.SetParent(obstacleRootGo.transform);
                }
                node.isObstacle = isObstacle;
            }
        }
    }
}

 

三.使用

把TestFindWay.cs掛上，然后對public變量進行拖拽賦值即可。效果如下：