unity使用Dijkstra算法实现自动寻路 unity自动寻路代码

这是我用Unity实现的A*寻路算法，参考了许多大神的代码架构终于写出。现成列出来，供各位学习交流。

A*寻路算法 这是寻路的地图，地图由100个小方格组成，每个方格有对应的编号。

方格地图的编号

详细代码在此，代码都有十分详细的注释

A*节点的数据结构：

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

//A*节点的数据结构
public class AStarPoint
{
    public AStarPoint parentPoint { get; set; }//父节点
    public GameObject gameObject { get; set; }//节点的游戏物体

    //F，G，H值
    public float F { get; set; }
    public float G { get; set; }
    public float H { get; set; }

    public Vector2 position { get; set; }//当前节点所处于的位置
    public int posX { get; set; }
    public int posY { get; set; }

    public bool isObstacle { get; set; }//是否是障碍物

    /// <summary>
    /// 构造函数
    /// </summary>
    /// <param name="X">该节点的X坐标</param>
    /// <param name="Y">该节点的Y坐标</param>
    public AStarPoint(int X,int Y)
    {
        posX = X;
        posY = Y;
        position = new Vector2(posX, posY);
        parentPoint = null;
        gameObject = GameObject.Find(X + "," + Y);//根据坐标绑定到场景中的游戏物体
    }
}

A*寻路算法本身

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

public class AStarAlgorithm
{
    //地图的长宽的格子数量
    const int XLength = 10;
    const int YLength = 10;

    //整张地图的节点集合
    public AStarPoint[,] pointGrid = new AStarPoint[XLength,YLength];

    //存放最终寻路结果的栈
    public Stack<AStarPoint> pathPosStack = new Stack<AStarPoint>();

    public static AStarAlgorithm Instance;

    /// <summary>
    /// 实例化A*算法
    /// </summary>
    public static AStarAlgorithm GetInstance
    {
        get
        {
            if(Instance==null)
            {
                Instance = new AStarAlgorithm();
            }
            return Instance;
        }
    }

    /// <summary>
    /// 构造函数，实例化的时候会调用用于初始化整张地图
    /// </summary>
    public AStarAlgorithm()
    {
        InitPoint();
    }
    /// <summary>
    /// 初始化游戏地图集合
    /// </summary>
    private void InitPoint()
    {
        for(int i=0;i<XLength;i++)
        {
            for(int j=0;j<YLength;j++)
            {
                pointGrid[i,j] = new AStarPoint(i,j);
            }
        }
    }
    /// <summary>
    /// 清除节点与节点之间的父子关系
    /// </summary>
    void ClearGrid()
    {
        for(int i=0;i<XLength;i++)
        {
            for(int j=0;j<YLength;j++)
            {
                if(!pointGrid[i,j].isObstacle)
                {
                    if(pointGrid[i,j].gameObject!=null)
                    {
                        pointGrid[i, j].parentPoint = null;
                    }
                }
            }
        }
    }

    /// <summary>
    /// 设置障碍物，被设置为障碍物的格子会升到高处
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    public void SetObstacle(int x,int y)
    {
        pointGrid[x, y].isObstacle = true;
        Vector3 pointPos = pointGrid[x, y].gameObject.transform.position;
        pointGrid[x,y].gameObject.transform.SetPositionAndRotation(new Vector3(pointPos.x,100,pointPos.z),Quaternion.identity);
    }
    /// <summary>
    /// A*寻路算法本体
    /// </summary>
    /// <param name="startPoint">寻路起点</param>
    /// <param name="endPoint">寻路终点</param>
    /// <returns></returns>
    public Stack<AStarPoint> FindPath(AStarPoint startPoint,AStarPoint endPoint)
    {
        //清除上一次算法留下的节点与节点之间的父子关系
        ClearGrid();

        //初始化Open表和Close表
        List<AStarPoint> openList = new List<AStarPoint>();
        List<AStarPoint> closeList = new List<AStarPoint>();

        //开始时将起点加入Open表
        openList.Add(startPoint);

        while(openList.Count>0)
        {
            //寻找Open表中F值最小的节点
            AStarPoint minPoint = FindMinPoint(openList); 

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

            //寻找minPoint周围的点（边界和障碍物不会算在内）
            List<AStarPoint> surroundPoints = FindSurroundPoints(minPoint);

            //如果surroundPoints中的点在Close表中出现过，则移除这些点
            foreach (AStarPoint closePoint in closeList)
            {
                if (surroundPoints.Contains(closePoint))
                {
                    surroundPoints.Remove(closePoint);
                }
            }

           //遍历surroundPoints中的点
            foreach (AStarPoint point in surroundPoints)
            {
                //若该点在Open表中出现过，则检查这条路径是否更优，
                //也就是说经由当前方格(我们选中的方格) 到达那个方格是否具有更小的 G 值。
                if (openList.Contains(point))
                {
                    float newPathG = CalcG(point,minPoint);
                    //如果 G 值更小，则把那个方格的父亲设为当前方格 ( 我们选中的方格 ) ，
                    //然后重新计算那个方格的 F 值和 G 值
                    if (newPathG<point.G)
                    {
                        point.parentPoint = minPoint;
                        point.G = newPathG;
                        point.F = point.G + point.H;
                    }
                    //如果没有，不做任何操作。
                }
                else
                {
                    //若该点没有在Open表中出现过，则直接计算F值存入点内，且将该点的父亲设置为minPoint
                    CalcF(point, endPoint);
                    point.parentPoint = minPoint;
                    openList.Add(point);
                }
            }

            //若已经到达终点，则退出循环
            if(openList.IndexOf(endPoint)>-1)
            {
                break;
            }
        }
        //返回寻路结果
        return GetPathWay(startPoint, endPoint);
    }
    
    /// <summary>
    /// 将寻路结果装入pathStack栈中
    /// </summary>
    /// <param name="startPoint">起点</param>
    /// <param name="endPoint">终点</param>
    /// <returns></returns>
    Stack<AStarPoint> GetPathWay(AStarPoint startPoint,AStarPoint endPoint)
    {
        pathPosStack.Clear();

        AStarPoint temp = endPoint;
        while(temp.parentPoint!=null)
        {
            pathPosStack.Push(temp);
            temp = temp.parentPoint;
        }
        return pathPosStack;
    }

    /// <summary>
    /// 寻找list表中F值最小的节点
    /// </summary>
    /// <param name="list"></param>
    /// <returns></returns>
    AStarPoint FindMinPoint(List<AStarPoint> list)
    {
        float F = list[0].F;
        AStarPoint ret=null;
        foreach(AStarPoint point in list)
        {
            if(point.F<=F)
            {
                F = point.F;
                ret = point;
            }
        }
        return ret;
    }
    /// <summary>
    /// 寻找point周围的节点加入List中，包括垂直方向和斜向共八个方向
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    List<AStarPoint> FindSurroundPoints(AStarPoint point)
    {
        List<AStarPoint> ret=new List<AStarPoint>();
        AStarPoint up=null, down = null, left = null, right = null;
        AStarPoint lu = null, ru = null, ld = null, rd = null;

        //如果是边界，就不加入List中
        if(point.posY<YLength-1)
        {
            up = pointGrid[point.posX, point.posY + 1];
        }
        if(point.posY>0)
        {
            down = pointGrid[point.posX, point.posY-1];
        }
        if(point.posX<XLength-1)
        {
            right = pointGrid[point.posX + 1, point.posY];
        }
        if(point.posX>0)
        {
            left = pointGrid[point.posX - 1, point.posY];
        }
        
        if(left!=null && down!=null)
        {
            ld = pointGrid[point.posX - 1, point.posY - 1];
        }
        if(left!=null && up!=null)
        {
            lu = pointGrid[point.posX - 1, point.posY + 1];
        }
        if(right!=null && down!=null)
        {
            rd = pointGrid[point.posX + 1, point.posY - 1];
        }
        if(right!=null && up!=null)
        {
            ru = pointGrid[point.posX + 1, point.posY + 1];
        }

        //上下左右方向，如果是障碍物就不加入list中
        if(left!=null && left.isObstacle==false)
        {
            ret.Add(left);
        }
        if(right!=null && right.isObstacle==false)
        {
            ret.Add(right);
        }
        if(up!=null && up.isObstacle==false)
        {
            ret.Add(up);
        }
        if(down!=null && down.isObstacle==false)
        {
            ret.Add(down);
        }
        //这里规定了如果上下左右方向有障碍，则斜方向不能寻路过去，读者可以不加入这个条件
        if (lu != null && lu.isObstacle == false && ret.Contains(left) && ret.Contains(up))
        {
            ret.Add(lu);
        }
        if(ld!=null && ld.isObstacle==false && ret.Contains(left) && ret.Contains(down))
        {
            ret.Add(ld);
        }
        if(ru!=null && ru.isObstacle==false && ret.Contains(right) && ret.Contains(up))
        {
            ret.Add(ru);
        }
        if(rd!=null && rd.isObstacle==false && ret.Contains(right) && ret.Contains(down))
        {
            ret.Add(rd);
        }
        return ret;

    }

    //计算G值
    float CalcG(AStarPoint surroundPoint, AStarPoint minPoint)
    {
        return Vector2.Distance(surroundPoint.position, minPoint.position) + minPoint.G;
    }
    //计算F值
    void CalcF(AStarPoint nowPoint,AStarPoint endPoint)
    {
        float H = Mathf.Abs(endPoint.posX - nowPoint.posX) + Mathf.Abs(endPoint.posY - nowPoint.posY);
        float G = 0;
        if(nowPoint.parentPoint==null)
        {
            G = 0;
        }
        else
        {
            G = Vector2.Distance(nowPoint.parentPoint.position, nowPoint.position) + nowPoint.parentPoint.G;
        }
        nowPoint.G = G;
        nowPoint.H = H;
        nowPoint.F = G + H;
    }
}

方块移动方法：

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

public class Cube : MonoBehaviour
{
    AStarAlgorithm myAlgorithm;
    AStarPoint[,] pointGrid;
    Stack<AStarPoint> pathPosStack;

    AStarPoint startPos;//开始位置
    AStarPoint endPos;//结束位置

    GameObject player;//玩家
    void Start()
    {
        myAlgorithm = AStarAlgorithm.GetInstance;//获取算法实例
        pointGrid = myAlgorithm.pointGrid;//获取地图点集合
        player = GameObject.FindGameObjectWithTag("Player");//获取玩家

        //设置障碍物
        myAlgorithm.SetObstacle(5, 3);
        myAlgorithm.SetObstacle(6, 3);
        myAlgorithm.SetObstacle(5, 4);
        myAlgorithm.SetObstacle(6, 4);
        myAlgorithm.SetObstacle(1, 7);
        myAlgorithm.SetObstacle(2, 7);
        myAlgorithm.SetObstacle(3, 7);
        myAlgorithm.SetObstacle(4, 7);
        //开始定位玩家和cube所在的位置进行移动
        InvokeRepeating("Relocate", 0,1f);
    }

    static Coroutine C;
    /// <summary>
    /// 定位cube和玩家的位置作为起始点和终点
    /// </summary>
    void Relocate()
    {
        //使用射线检测的方法定位两者的位置，赋值给startPos和endPos
        RaycastHit hit1, hit2;
        if (Physics.Raycast(new Ray(gameObject.transform.position, Vector3.down), out hit1)
            && Physics.Raycast(new Ray(player.transform.position, Vector3.down), out hit2))
        {
            GameObject startObject = hit1.collider.gameObject;
            GameObject endObject = hit2.collider.gameObject;
            foreach (AStarPoint point in pointGrid)
            {
                if (point.gameObject == startObject)
                {
                    startPos = point;
                }
                if (point.gameObject == endObject)
                {
                    endPos = point;
                }
            }
        }
        //获取寻路路径
        pathPosStack = myAlgorithm.FindPath(startPos, endPos);
        //开始寻路
        try { StopCoroutine(C); } catch { }//停止之前的协程
        C = StartCoroutine(Walk());
    }

    /// <summary>
    /// cube移动的协程
    /// </summary>
    /// <returns></returns>
    IEnumerator Walk()
    {
        AStarPoint targetPos = startPos;
        while (true)
        {
            if (targetPos.gameObject.transform.position.x==gameObject.transform.position.x
                && targetPos.gameObject.transform.position.z==gameObject.transform.position.z && pathPosStack.Count > 0)
            {
                targetPos = pathPosStack.Peek();
                pathPosStack.Pop();
            }
            Vector3 dis = new Vector3(targetPos.gameObject.transform.position.x,
            gameObject.transform.position.y,
            targetPos.gameObject.transform.position.z);
            gameObject.transform.position = Vector3.MoveTowards(gameObject.transform.position, dis, 5*Time.deltaTime);
            yield return null;
        }
    }
}

运行游戏，可以看见寻路算法十分有效，Cube可以完美避开障碍物，无论Player在哪里都能被找到