【Unity】【C#】A*寻路算法-学习心得_c# unityengine 根据坐标查找最近的点-优快云博客

本文链接：https://blog.youkuaiyun.com/Terrell21/article/details/82584634

昨天在阅读《U3d人工智能编程精粹》第三章的A*寻路算法的时候，由于书本只是粗略地用图片于伪代码展示了一遍A*寻路。，我便从网络中查找解读。经过翻阅多篇文章，似乎只有这位大大的博客文章（https://blog.youkuaiyun.com/yiyikela/article/details/46134339）写得较为详细，易于理解。

然后参考了（https://www.cnblogs.com/yangyxd/articles/5447889.html）[UNITY]A-star（A星）寻路这篇文章，在Unity中编写了A*寻路。

以下整理一下本人学习A*寻路时，了解到的知识：

启发式搜索：

启发式搜索是通过利用问题所拥有的启发信息来指导搜索往最有希望的方向去，以达到减少搜索范围和降低目的复杂度的方法。

估价函数：

用于评估节点代价的函数： $f(X)=g(x)+h(x)$

g(x)：初始节点到X节点的实际代价（起点到当前点的代价）

h(x)：X节点到目标节点的预估代价（当前点到终点的代价）

f(x)：初始节点到X节点再到目标节点的代价估计（起点到终点的代价估计）

实现A*寻路的思路（个人见解）：

《U3d人工智能编程精粹》里提及实现A*寻路有三种工作方式：

创建基于单元的导航图
创建可视点导航图
创建导航网格

由于查阅资料以及书本提供的内容都是创建基于单元的导航图的工作方式来实现A*寻路，

我作为初学者也只能先写下通过创建基于单元的导航图实现方法的个人见解。（有机会可以自己摸索一下下面两种工作方式）

创建单元导航图：

创建基于单元的导航图，需要条件：

构成导航图的点以及点的相关信息
地图自身的相关信息

点信息类：

需要记录点在地图上的坐标 x ， z

以及在世界坐标上的位置position

  public class AsNode
    {
        //点信息位置
        public Vector3 position;
        //点信息坐标 由于我采用三维坐标，所以采用 X Z方便自己读懂
        public int x,z;
        //构造点信息
        public AsNode(int x,int z)
        {
            this.x = x;
            this.z = z;
            this.position = new Vector3(x, 0, z);
        }
    }

地图类：

需要创建地图的宽高大小

以及需要记录地图上的点信息----纵横相邻的点相距距离为1

public class MapGrid : MonoBehaviour {
    //地图大小(宽高)
    public const int mGridWidth = 10;
    public const int mGridHeigh = 10;
    //地图点信息
    public AsNode[,] mPointGrid = new AsNode[mGridWidth,mGridHeigh];

    //初始化地图上的点信息
    public void InitMapInfo()
    {
        //读取地图点的信息
        for (int i = 0; i < mGridHeigh; i++)
        {
            for (int j = 0; j < mGridWidth; j++)
            {
                mPointGrid[i, j] = new AsNode(i,j);
            }
        }
    }
    //绘制地图
    private void OnDrawGizmos()
    {
        for (int i = 0; i < mGridHeigh; i++)
        {
            for (int j = 0; j < mGridWidth; j++)
            {
               
                Gizmos.DrawWireCube(mPointGrid[i, j].position, Vector3.one);

            }
        }
    }
}

绘制导航图：

地图中还需要记录初始节点目标节点障碍物节点：

由于点还区分是否为障碍物，所以为点添加属性

public class MapGrid : MonoBehaviour { 
    //起点
    public GameObject m_gameObject;
    //终点
    public GameObject target;
    //墙壁点
    public GameObject[] walls;
}

public class AsNode{
        public bool isWall;
        public AsNode(int x, int z,bool isWall)
        {
            this.x = x;
            this.z = z;
            this.isWall = isWall;
            this.position = new Vector3(x, 0, z);
        }
}

获取这些点信息需要重新改写我们的初始地图的方法：

创建基于单元的导航图类的代码：

public class MapGrid : MonoBehaviour {
    //地图大小(宽高)
    public const int mGridWidth = 10;
    public const int mGridHeigh = 10;    
    //地图上点的信息
    public class AsNode
    {
        public Vector3 position;
        //点信息坐标
        public int x,z;
        //地图点的状态
        public AsNode parent;
        public bool isWall;
        public AsNode(int x,int z)
        {
            this.x = x;
            this.z = z;
            this.position = new Vector3(x, 0, z);
        }

        public AsNode(int x, int z,bool isWall)
        {
            this.x = x;
            this.z = z;
            this.isWall = isWall;
            this.position = new Vector3(x, 0, z);
        }
    }
    //起点
    public GameObject m_gameObject;
    //终点
    public GameObject target;
    //墙壁点
    public GameObject[] walls;
    //地图点信息
    public AsNode[,] mPointGrid = new AsNode[mGridWidth,mGridHeigh];

    private void Start()
    {
        InitMapInfo();
    }


    public void InitMapInfo()
    {
        //读取地图点的信息
        for (int i = 0; i < mGridHeigh; i++)
        {
            for (int j = 0; j < mGridWidth; j++)
            {
                mPointGrid[i, j] = new AsNode(i,j);
                foreach(var wall in walls)
                { 
                    if (mPointGrid[i, j].position == wall.transform.position)
                        mPointGrid[i, j].isWall = true;
                }
            }
        }
    }

    //画地图
    private void OnDrawGizmos()
    {
        for (int i = 0; i < mGridHeigh; i++)
        {
            for (int j = 0; j < mGridWidth; j++)
            {
               
                Gizmos.DrawWireCube(mPointGrid[i, j].position, Vector3.one);

            }
        }
    }
}

此地图信息的起点、终点、障碍点，需要自行在U3D添加，然后挂载到脚本上（物体名字不需要关注，懒修改..）

效果图如下：

实现寻路算法：

（寻路代码引用https://www.cnblogs.com/yangyxd/articles/5447889.html）

实现寻路算法前，由于A*寻路是启发式搜索算法，通过估价函数来计算预期代价，来进行判断最终寻路路径。

并且寻路需要记录节点关系。

所以为点添加属性 f g h 和 parent；

以下为AsNode完整代码：

//地图上点的信息
public class AsNode
{
        public Vector3 position;
        //点信息坐标
        public int x,z;
        //地图点的状态
        public AsNode parent;
        public bool isWall;
        //估价函数
        public int g;//(起点到当前点)
        public int h;//(当前点到终点)
        public int f ;//f=g+h;

        public AsNode(int x,int z)
        {
            this.x = x;
            this.z = z;
            this.position = new Vector3(x, 0, z);
        }

        public AsNode(int x, int z,bool isWall)
        {
            this.x = x;
            this.z = z;
            this.isWall = isWall;
            this.position = new Vector3(x, 0, z);
        }
}

导航图创建完毕，开始思考寻路的实现。

既然是寻路，我们就需要吧已经走过的点，已经需要判断的点记录下来。

我们可以通过两个表来记录这两个信息：

openList：记录当前需要判断的点（考虑代价，是否能成为下一个路径点）
closedList：记录已经确定的点（已经走过，成为路径点）

①通过对当前点进行查找邻近点

查找邻近点代码实现：

//取得周围的节点
    public List<AsNode> getNeibourhood(AsNode node)
    {
        List<AsNode> list = new List<AsNode>();
        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 z = node.z+j;
                if (x < mGridWidth && x >= 0 && z < mGridHeigh && z >= 0)
                    list.Add(mPointGrid[x, z]); 
            }
        }
        return list;
    }

②将符合以下条件的点加入到openList中

不是障碍物
还没加入openList中

然后对openList中的邻近点进行估价计算(本案例采用了对角算法),F值小的成为下一个中间节点，并加入closedList中,并从openList中移除。

估价算法（对角算法）：

黑线长度是通过三角形斜边公式计算出来的 $\sqrt{1^{2}+1^{2}}=\sqrt{2}\approx 1.414$

以上述内容作为此寻路算法中的路径代价：

水平，垂直移动代价为1
斜向移动代价为1.4

通过判断两点之间的X差值，Z差值，若

X差值较大，则Y差值采用斜向移动，而X差值比Y差值多出来的部分使用水平/垂直移动

（由于斜向移动后的位置相等于分别进行一次水平移动和垂直移动）

（移动到相同位置，斜向移动代价为1.4，反之需要1+1=2）

（所以将差值较小的次数作为斜向移动次数计算）

所以预期代价为

$14*(a.z-b.z)+10*(a.x-b.x)$ 或

$14*(a.x-b.x)+10*(a.z-b.z)$

实现代码：

public int getDistanceNodes(MapGrid.AsNode a,MapGrid.AsNode b)
{
        //采用对角算法 X(Y)差值较小的，采取对角
        //对角代价14
        //水平 垂直 代价10
        int cntX = Mathf.Abs(a.x - b.x);
        int cntZ = Mathf.Abs(a.z - b.z);
        if (cntX > cntZ)
            return 14 * cntZ + 10 * (cntX - cntZ);
        else
            return 14 * cntX + 10 * (cntZ - cntX);
}

获得新的路径点后，通过closeList的点，进行对路径进行更新。

直到查找到终点为止，否则一直进行邻居点查找，并比较估计代价的运算。

案例代码将获取的路径使用了点来打印出来。

寻路算法完整代码(参考博客)：

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

public class FindPath : MonoBehaviour {
    public MapGrid grid;
	// Use this for initialization
	void Start () {
        grid = GetComponent<MapGrid>();
	}
	
	void Update () {
        FindingPath(grid.m_gameObject.transform.position, grid.target.transform.position);
	}

    //获取两个节点之间的距离
    public int getDistanceNodes(MapGrid.AsNode a,MapGrid.AsNode b)
    {
        //采用对角算法 X(Y)差值较小的，采取对角
        //对角代价14
        //水平 垂直 代价10
        int cntX = Mathf.Abs(a.x - b.x);
        int cntZ = Mathf.Abs(a.z - b.z);
        if (cntX > cntZ)
            return 14 * cntZ + 10 * (cntX - cntZ);
        else
            return 14 * cntX + 10 * (cntZ - cntX);
    }

    void FindingPath(Vector3 startPos, Vector3 endPos)
    {
        //通过坐标获取地图上的起点和终点
        MapGrid.AsNode startNode = grid.getItem(startPos);
        MapGrid.AsNode endNode = grid.getItem(endPos);
        //打开列表 存放邻近点
        List<MapGrid.AsNode> openList = new List<MapGrid.AsNode>();
        //关闭列表 存放选定的路径点
        List<MapGrid.AsNode> closedList = new List<MapGrid.AsNode>();
        openList.Add(startNode);
      
        while (openList.Count > 0)
        {
            MapGrid.AsNode curNode = openList[0];
            //（第一次运算）是用来将起点加入到关闭列表中
            //(后面的运算)通过判断还没到达终点,继续下面的执行过程获得新的打开列表（当前点的邻居点）
            for(int i = 0, max = openList.Count; i <max; i++)
            {
                //比较开启列表里的点 估价总值F 和点到终点的估价H
                //若列表中的点估价更低，替换为当前点
                if (openList[i].f <= curNode.f && openList[i].h < curNode.h)
                {
                    curNode = openList[i];
                }
            }
            //选择到点，然后加入到关闭列表中
            openList.Remove(curNode);
            closedList.Add(curNode);

            //直到找到终点为止
            if (curNode == endNode)
            {
                generatePath(startNode, endNode);
                return;
            }
         
            foreach (var item in grid.getNeibourhood(curNode))
            {
                //如果该点是障碍点，或者关闭列表已经有该点，跳过
                if (item.isWall || closedList.Contains(item))
                    continue;
                //可移动点，进行估价
                int newCost = curNode.g + getDistanceNodes(curNode, item);
                //如果距离更小，或者原来不在开始列表中
                if (newCost < item.g || !openList.Contains(item))
                {
                    //更新与开始节点的距离
                    item.g = newCost;
                    //更新与终点的距离
                    item.h = getDistanceNodes(item, endNode);
                    //更新父节点为当前选定的节点
                    item.parent = curNode;
                    //如果节点是新加入的，将它加入打开列表中
                    if (!openList.Contains(item))
                        openList.Add(item);
                }
            }
        }
        generatePath(startNode, null);
    }

    void generatePath(MapGrid.AsNode startNode,MapGrid.AsNode endNote)
    {
        List<MapGrid.AsNode> path = new List<MapGrid.AsNode>();
        if (endNote != null)
        {
            MapGrid.AsNode temp = endNote;
            while (temp != startNode)
            {
                path.Add(temp);
                temp = temp.parent;
            }
            //反转队列内容
            path.Reverse();
        }
        //更新路径
        grid.updatePath(path);
    }

}

地图信息映射完整代码（个人编写）：

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

public class MapGrid : MonoBehaviour {
    //生成路径点预载体
    public GameObject Node;
    //地图大小(宽高)
    public const int mGridWidth = 10;
    public const int mGridHeigh = 10;
    //地图上点的信息
    public class AsNode
    {
        public Vector3 position;
        //点信息坐标
        public int x,z;
        //地图点的状态
        public AsNode parent;
        public bool isWall;
        //估价函数
        public int g;//(起点到当前点)
        public int h;//(当前点到终点)
        public int f ;//f=g+h;

        public AsNode(int x,int z)
        {
            this.x = x;
            this.z = z;
            this.position = new Vector3(x, 0, z);
        }
        public AsNode(int x, int z,bool isWall)
        {
            this.x = x;
            this.z = z;
            this.isWall = isWall;
            this.position = new Vector3(x, 0, z);
        }
    }

    //起点
    public GameObject m_gameObject;
    //终点
    public GameObject target;
    //墙壁点
    public GameObject[] walls;
    //地图点信息
    public AsNode[,] mPointGrid = new AsNode[mGridWidth,mGridHeigh];
    //路径目标
    private List<GameObject> pathObj = new List<GameObject>();
  
    private void Start()
    {
        InitMapInfo();
    }

    public void InitMapInfo()
    {
        //读取地图点的信息
        for (int i = 0; i < mGridHeigh; i++)
        {
            for (int j = 0; j < mGridWidth; j++)
            {
                mPointGrid[i, j] = new AsNode(i,j);
                foreach(var wall in walls)
                {
                    if (mPointGrid[i, j].position == wall.transform.position)
                        mPointGrid[i, j].isWall = true;
                }
            }
        }
    }

    //画地图
    private void OnDrawGizmos()
    {
        for (int i = 0; i < mGridHeigh; i++)
            for (int j = 0; j < mGridWidth; j++)
                Gizmos.DrawWireCube(mPointGrid[i, j].position, Vector3.one);
    }

    //取得周围的节点
    public List<AsNode> getNeibourhood(AsNode node)
    {
        List<AsNode> list = new List<AsNode>();
        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 z = node.z+j;
                if (x < mGridWidth && x >= 0 && z < mGridHeigh && z >= 0)
                    list.Add(mPointGrid[x, z]); 
            }
        }
        return list;
    }

    //更新路径
    public void updatePath(List<AsNode> lines)
    {
        int curListSize = pathObj.Count;
        for(int i = 0, max = lines.Count; i < max; i++)
        {
            if (i < curListSize)
            {
                pathObj[i].transform.position = lines[i].position;
                pathObj[i].SetActive(true);
            }
            else
            {
                GameObject obj = GameObject.Instantiate(Node, lines[i].position, Quaternion.identity);
                pathObj.Add(obj);
            }
        }
    }
    //通过坐标找地图上的点
    public AsNode getItem(Vector3 pos)
    {
        int x = Mathf.RoundToInt(pos.x);
        int z = Mathf.RoundToInt(pos.z);
        return mPointGrid[x, z];
    }
}

A*寻路实现效果图：

【白点为起点（初始节点）】

【绿点为阻碍点（不可移动节点）】

【红点为终点（目标节点）】

【蓝点为路径点（中间节点X）】