Unity3d利用A*寻路算法实现寻路模拟

原文地址：blog.liujunliang.com.cn

这里我先引用一篇详细文章来介绍A*算法

本文源码链接：点击打开链接

文章内容如下

简易地图

 

如图所示简易地图, 其中绿色方块的是起点 (用 A 表示), 中间蓝色的是障碍物, 红色的方块 (用 B 表示) 是目的地. 为了可以用一个二维数组来表示地图, 我们将地图划分成一个个的小方块.

二维数组在游戏中的应用是很多的, 比如贪吃蛇和俄罗斯方块基本原理就是移动方块而已. 而大型

 游戏的地图, 则是将各种"地貌"铺在这样的小方块上.

寻路步骤

1. 从起点 A 开始, 把它作为待处理的方格存入一个"开启列表", 开启列表就是一个等待检查方格

 的列表.

2. 寻找起点 A 周围可以到达的方格, 将它们放入"开启列表", 并设置它们的"父方格"为 A.

3. 从"开启列表"中删除起点 A, 并将起点 A 加入"关闭列表", "关闭列表"中存放的都是不需要再次检查的方格

 

图中浅绿色描边的方块表示已经加入 "开启列表" 等待检查. 淡蓝色描边的起点 A 表示已经放入

 "关闭列表" , 它不需要再执行检查.

从 "开启列表" 中找出相对最靠谱的方块, 什么是最靠谱? 它们通过公式 F=G+H 来计算.

F = G + H 

G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动).

 H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).

我们假设横向移动一个格子的耗费为 10, 为了便于计算, 沿斜方向移动一个格子耗费是 14. 为了

更直观的展示如何运算 FGH, 图中方块的左上角数字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心里想的结果一样?

从 "开启列表" 中选择 F 值最低的方格 C (绿色起始方块 A 右边的方块), 然后对它进行如下处

理:

4. 把它从 "开启列表" 中删除, 并放到 "关闭列表" 中.

5. 检查它所有相邻并且可以到达 (障碍物和 "关闭列表" 的方格都不考虑) 的方格. 如果这些方格

还不在 "开启列表" 里的话, 将它们加入 "开启列表", 计算这些方格的 G, H 和 F 值各是多少, 并设置它们的 "父方格" 为 C.

6. 如果某个相邻方格 D 已经在 "开启列表" 里了, 检查如果用新的路径 (就是经过 C 的路径) 到达它的话, G 值是否会更低一些, 如果新的 G 值更低, 那就把它的 "父方格" 改为目前选中的方格 C, 然后重新计算它的 F 值和 G 值 (H 值不需要重新计算, 因为对于每个方块, H 值是不变的). 如果新的 G 值比较高, 就说明经过 C 再到达 D 不是一个明智的选择, 因为它需要更远的路, 这时我们什么也不做.

 

如图, 我们选中了 C 因为它的 F 值最小, 我们把它从 "开启列表" 中删除, 并把它加入 "关闭列表". 它右边上下三个都是墙, 所以不考虑它们. 它左边是起始方块, 已经加入到 "关闭列表" 了, 也不考虑. 所以它周围的候选方块就只剩下 4 个. 让我们来看看 C 下面的那个格子, 它目前的 G 是 14, 如

果通过 C 到达它的话, G 将会是 10 + 10, 这比 14 要大, 因此我们什么也不做.

然后我们继续从 "开启列表" 中找出 F 值最小的, 但我们发现 C 上面的和下面的同时为 54, 这

时怎么办呢? 这时随便取哪一个都行, 比如我们选择了 C 下面的那个方块 D.

D 右边已经右上方的都是墙, 所以不考虑, 但为什么右下角的没有被加进 "开启列表" 呢? 

因为如果 C 下面的那块也不可以走, 想要到达 C 右下角的方块就需要从 "方块的角" 走了, 在程序中设置是否允许这样走. (图中的示例不允许这样走)

就这样, 我们从 "开启列表" 找出 F 值最小的, 将它从 "开启列表" 中移掉, 添加到 "关闭列表".

再继续找出它周围可以到达的方块, 如此循环下去...

那么什么时候停止呢? —— 当我们发现 "开始列表" 里出现了目标终点方块的时候, 说明路径已经

被找到.

如何找回路径

如上图所示, 除了起始方块, 每一个曾经或者现在还在 "开启列表" 里的方块, 它都有一个 "父方块", 通过 "父方块" 可以索引到最初的 "起始方块", 这就是路径.

Unity代码实现

算法类

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

public class AStarAlgorithm 
{
    private const int mGridWidth = 20;    
    private const int mGridHeight = 10;

    //使用二维数组存储点网格    
    public AStarPoint[,] mPointGrid = new AStarPoint[mGridWidth,mGridHeight];
    //存储路径方格子
    public List<AStarPoint> mPathPosList = new List<AStarPoint>();

    private static AStarAlgorithm _instance;
    public static AStarAlgorithm GetInsatnce
    {
        get
        {
            if (_instance == null)
            {
                _instance = new AStarAlgorithm();
            }

            return _instance;
        }
    }

    public AStarAlgorithm()
    {
        InitPoint();
    }

    //在网格上设置点的信息
    private void InitPoint()
    {
        for (int i = 0; i < mGridWidth; i++)
        {
            for (int j = 0; j < mGridHeight; j++)
            {                
                mPointGrid[i, j] = new AStarPoint(i, j);
            }
        }

        //设置障碍物
        mPointGrid[4, 2].mIsObstacle = true;
        mPointGrid[4, 3].mIsObstacle = true;
        mPointGrid[4, 4].mIsObstacle = true;
        mPointGrid[4, 5].mIsObstacle = true;
        mPointGrid[4, 6].mIsObstacle = true;

        //显示障碍物
        for (int x = 0; x < mGridWidth; x++)
        {
            for (int y = 0; y < mGridHeight; y++)
            {
                if (mPointGrid[x, y].mIsObstacle)
                {
                    CreatePath(x, y, Color.blue);
                }
            }
        }
    }

    public void ClearGrid()
    {
        for (int x = 0; x < mGridWidth; x++)
        {
            for (int y = 0; y < mGridHeight; y++)
            {
                if (!mPointGrid[x, y].mIsObstacle)
                {
                    if (mPointGrid[x, y].mGameObject != null)
                    {
                        GameObject.Destroy(mPointGrid[x, y].mGameObject);
                        mPointGrid[x, y].mGameObject = null;

                        //重新设置父节点
                        mPointGrid[x, y].mParentPoint = null;
                    }
                }
            }
        }
    }

    //寻路
    public List<AStarPoint> FindPath(AStarPoint mStartPoint, AStarPoint mEndPoint)
    {
        if (mEndPoint.mIsObstacle || mStartPoint.mPosition == mEndPoint.mPosition)
        {
            return  null;
        }

        //开启列表
        List<AStarPoint> openPointList = new List<AStarPoint>();
        //关闭列表
        List<AStarPoint> closePointList = new List<AStarPoint>();

        openPointList.Add(mStartPoint);

        while (openPointList.Count > 0)
        {
            //寻找开启列表中最小预算值的表格
            AStarPoint minFPoint = FindPointWithMinF(openPointList);
            //将当前表格从开启列表移除 在关闭列表添加
            openPointList.Remove(minFPoint);
            closePointList.Add(minFPoint);
            //找到当前点周围的全部点
            List<AStarPoint> surroundPoints = FindSurroundPoint(minFPoint);            
            //在周围的点中，将关闭列表里的点移除掉
            SurroundPointsFilter(surroundPoints, closePointList);
            //寻路逻辑
            foreach (var surroundPoint in surroundPoints)
            {
                if (openPointList.Contains(surroundPoint))
                {
                    //计算下新路径下的G值（H值不变的，比较G相当于比较F值）
                    float newPathG = CalcG(surroundPoint, minFPoint);
                    if (newPathG < surroundPoint.mG)
                    {
                        surroundPoint.mG = newPathG;
                        surroundPoint.mF = surroundPoint.mG + surroundPoint.mH;
                        surroundPoint.mParentPoint = minFPoint;
                    }
                }
                else
                {
                    //将点之间的
                    surroundPoint.mParentPoint = minFPoint;
                    CalcF(surroundPoint, mEndPoint);
                    openPointList.Add(surroundPoint);
                }
            }

            //如果开始列表中包含了终点，说明找到路径
            if (openPointList.IndexOf(mEndPoint) > -1)
            {
                break;
            }
        }

        return ShowPath(mStartPoint, mEndPoint);
    }

    private List<AStarPoint> ShowPath(AStarPoint start, AStarPoint end)
    {
        mPathPosList.Clear();

        AStarPoint temp = end;
        while (true)
        {
            mPathPosList.Add(temp);

            Color c = Color.white;
            if (temp == start)
            {
                c = Color.green;
            }
            else if (temp == end)
            {
                c = Color.red;
            }
            CreatePath(temp.mPositionX, temp.mPositionY, c);

            if (temp.mParentPoint == null)
                break;
            temp = temp.mParentPoint;
        }

        return mPathPosList;
    }

    private void CreatePath(int x, int y, Color color)
    {
        GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cube);
        go.transform.position = new Vector3(x, y, 0);
        go.GetComponent<Renderer>().material.color = color;
        go.transform.SetParent(GameObject.Find("Path").transform);

        if (mPointGrid[x, y].mGameObject != null)
        {
            GameObject.Destroy(mPointGrid[x, y].mGameObject);             
        }
        mPointGrid[x, y].mGameObject = go;
    }

    //寻找预计值最小的格子
    private AStarPoint FindPointWithMinF(List<AStarPoint> openPointList)
    {
        float f = float.MaxValue;
        AStarPoint temp = null;
        foreach (AStarPoint p in openPointList)
        {
            if (p.mF < f)
            {
                temp = p;
                f = p.mF;
            }
        }
        return temp;
    }

    //寻找周围的全部点
    private List<AStarPoint> FindSurroundPoint(AStarPoint point)
    {
        List<AStarPoint> list = new List<AStarPoint>();

        判断周围的八个点是否在网格内/
        AStarPoint up = null, down = null, left = null, right = null;
        AStarPoint lu = null, ru = null, ld = null, rd = null;
        if (point.mPositionY < mGridHeight - 1)
        {
            up = mPointGrid[point.mPositionX, point.mPositionY + 1];
        }
        if (point.mPositionY > 0)
        {
            down = mPointGrid[point.mPositionX, point.mPositionY - 1];
        }
        if (point.mPositionX > 0)
        {
            left = mPointGrid[point.mPositionX - 1, point.mPositionY];
        }
        if (point.mPositionX < mGridWidth - 1)
        {
            right = mPointGrid[point.mPositionX + 1, point.mPositionY];
        }
        if (up != null && left != null)
        {
            lu = mPointGrid[point.mPositionX - 1, point.mPositionY + 1];
        }
        if (up != null && right != null)
        {
            ru = mPointGrid[point.mPositionX + 1, point.mPositionY + 1];
        }
        if (down != null && left != null)
        {
            ld = mPointGrid[point.mPositionX - 1, point.mPositionY - 1];
        }
        if (down != null && right != null)
        {
            rd = mPointGrid[point.mPositionX + 1, point.mPositionY - 1];
        }
        

        /将可以经过的表格添加到开启列表中/
        if (down != null && down.mIsObstacle == false)
        {
            list.Add(down);
        }
        if (up != null && up.mIsObstacle == false)
        {
            list.Add(up);
        }
        if (left != null && left.mIsObstacle == false)
        {
            list.Add(left);
        }
        if (right != null && right.mIsObstacle == false)
        {
            list.Add(right);
        }
        if (lu != null && lu.mIsObstacle == false && left.mIsObstacle == false && up.mIsObstacle == false)
        {
            list.Add(lu);
        }
        if (ld != null && ld.mIsObstacle == false && left.mIsObstacle == false && down.mIsObstacle == false)
        {
            list.Add(ld);
        }
        if (ru != null && ru.mIsObstacle == false && right.mIsObstacle == false && up.mIsObstacle == false)
        {
            list.Add(ru);
        }
        if (rd != null && rd.mIsObstacle == false && right.mIsObstacle == false && down.mIsObstacle == false)
        {
            list.Add(rd);
        }

        return list;
    }

    //将关闭带你从周围点列表中关闭
    private void SurroundPointsFilter(List<AStarPoint> surroundPoints, List<AStarPoint> closePoints)
    {
        foreach (var closePoint in closePoints)
        {
            if (surroundPoints.Contains(closePoint))
            {
                Debug.Log("将关闭列表的点移除");
                surroundPoints.Remove(closePoint);
            }
        }
    }

    //计算最小预算值点G值
    private float CalcG(AStarPoint surround, AStarPoint minFPoint)
    {
        return Vector3.Distance(surround.mPosition, minFPoint.mPosition) + minFPoint.mG;
    }

    //计算该点到终点的F值
    private void CalcF(AStarPoint now, AStarPoint end)
    {
        //F = G + H
        float h = Mathf.Abs(end.mPositionX - now.mPositionX) + Mathf.Abs(end.mPositionY - now.mPositionY);
        float g = 0;
        if (now.mParentPoint == null)
        {
            g = 0;
        }
        else
        {
            g = Vector2.Distance(new Vector2(now.mPositionX, now.mPositionY), new Vector2(now.mParentPoint.mPositionX, now.mParentPoint.mPositionY)) + now.mParentPoint.mG;
        }
        float f = g + h;
        now.mF = f;
        now.mG = g;
        now.mH = h;
    }
}

其中AStarPoint是存储点的信息（位置、父格子、实体对象）

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


/// <summary>
/// 存储寻路点信息
/// </summary>
public class AStarPoint
{
    //父“格子”
    public AStarPoint mParentPoint { get; set; }
    //格子显示对象
    public GameObject mGameObject { get; set; }

    public float mF { get; set; }
    public float mG { get; set; }
    public float mH { get; set; }
    //点的位置
    public Vector2 mPosition { get; set; }
    public int mPositionX { get; set; }
    public int mPositionY { get; set; }
    //该点是否处于障碍物
    public bool mIsObstacle { get; set; }

    public AStarPoint(int positionX,int positionY)
    {
        this.mPositionX = positionX;
        this.mPositionY = positionY;
        this.mPosition = new Vector2(mPositionX, mPositionY);
        this.mParentPoint = null;
    }
}

寻路模拟

创建一个DoPlayer测试类，用于模仿一个物体的寻路

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

public class DoPlayer : MonoBehaviour
{
    private GameObject mCubeParent;

    //存储路径点
    private List<AStarPoint> mPathPosList;

    //网格大小
    private const int mGridWidth = 20;
    private const int mGridHeight = 10;

    private AStarPoint[,] mPointGrid;

    private AStarPoint mStartPos;
    private AStarPoint mEndPos { get; set; }

    //每一秒发生位移
    private float mTime = 0.7f;
    private float mTimer = 0.0f;

    //目标点
    private int mTargetX { get; set; }
    private int mTargetY { get; set; }

    private void Start()
    {
        mCubeParent = GameObject.Find("Plane");

        mPointGrid = AStarAlgorithm.GetInsatnce.mPointGrid;
        mStartPos = mPointGrid[0, 0];

        InitBG();
    }

    private void Update()
    {
        mTimer += Time.deltaTime;
        if (mTimer >= mTime)
        {
            mTimer = 0;
            Walk();
        }
    }

    private void Walk()
    {     
        if (mPathPosList != null && mPathPosList.Count > 1)
        {
            mStartPos = mPathPosList[mPathPosList.Count - 1];
            Color color = mStartPos.mGameObject.GetComponent<Renderer>().material.color;
            mPathPosList.Remove(mStartPos);
            Destroy(mStartPos.mGameObject);
            mStartPos.mGameObject = null;
            
            mStartPos = mPathPosList[mPathPosList.Count - 1];
            mStartPos.mGameObject.GetComponent<Renderer>().material.color = color;
            
        }
    }

    private void InitBG()
    {
        for (int i = 0; i < mGridWidth; i++)
        {
            for (int j = 0; j < mGridHeight; j++)
            {
                CreateCube(i, j, Color.gray);
            }
        }
    }


    private void CreateCube(int x, int y, Color color)
    {
        GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cube);
        go.transform.SetParent(mCubeParent.transform);
        go.transform.position = new Vector3(x, y, 0);
        go.transform.localScale = new Vector3(0.9f, 0.9f, 0.9f);
        go.GetComponent<Renderer>().material.color = color;

        go.AddComponent<Cube>().FindPath = FindPath;
    }

    public void FindPath(int mTargetX, int mTargetY)
    {
        if (mPathPosList != null)
        {
            mPathPosList.Clear();
        }
        AStarAlgorithm.GetInsatnce.ClearGrid();

        //网格点对象重新刷新了  需要使用网格来索引到点 mPathPosList存储的点是之前的AStarPoint
        this.mEndPos = mPointGrid[mTargetX, mTargetY];
        this.mStartPos = mPointGrid[mStartPos.mPositionX, mStartPos.mPositionY];

        mPathPosList = AStarAlgorithm.GetInsatnce.FindPath(this.mStartPos, mEndPos);
    }
}

其中创建的Cube网格类代码如下

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

public class Cube : MonoBehaviour
{
    public delegate void VoidDelegate(int x, int y);
    public VoidDelegate FindPath;

    private void OnMouseDown()
    {
        if (FindPath != null)
        {
            FindPath((int)this.transform.position.x, (int)this.transform.position.y);
        }
    }
}

效果展示

效果显示正常，绿色方块代表主角位置，红色方块代表目标点，白色方块代表路径

原文地址：blog.liujunliang.com.cn