对A*算法早有耳闻,但一直未细致去分析过,最近重新看到这个概念,来尝试分析一波

算法定义

A* 算法常被表示为 f(n)=g(n)+h(n)

f(n)表示从状态n到达目标状态的花费的预测值
g(n)表示从初始状态到达状态n的花费
h(n)表示从状态n到达目标状态的花费预测值(最佳路径)

拖了蛮久了,差点我都忘记了我还有个博客等着我每周一更呢。

效果预览


A* 格子数据

namespace FastyTools.Astar
{
    /// <summary>
    /// 格子类型
    /// </summary>
    public enum NodeType
    {
        Walk,
        /// <summary>
        /// 障碍物
        /// </summary>
        Obstacle
    }

    /// <summary>
    /// A*格子
    /// </summary>
    public class AstarNode
    {
        public NodeType Type { get; set; }
        /// <summary>
        /// 总花费
        /// </summary>
        public float F => G + H;
        /// <summary>
        /// 从起点到当前格子的花费
        /// </summary>
        public float G { get;  set; }
        /// <summary>
        /// 从当前格子到终点的花费
        /// </summary>
        public float H { get; set; }

        /// <summary>
        /// 父节点
        /// </summary>
        public AstarNode Father { get; set; }
        public int X { get; }
        public int Y { get; }

        public AstarNode(int x, int y, NodeType type)
        {
            X = x;
            Y = y;
            Type = type;
        }
    }
}

A*寻路

using System.Collections.Generic;
using FastyTools.Singleton;
using UnityEngine;

namespace FastyTools.Astar
{
    [SerializeField]
    public struct Vec2
    {
        public int X { get; set; }
        public int Y { get; set; }

        public Vec2(int x, int y)
        {
            X = x;
            Y = y;
        }


        //运算符重载
        public static Vec2 operator +(Vec2 a, Vec2 b)
        {
            return new Vec2(a.X + b.X, a.Y + b.Y);
        }

        public static Vec2 operator -(Vec2 a, Vec2 b)
        {
            return new Vec2(a.X - b.X, a.Y - b.Y);
        }
    }

    /// <summary>
    /// A星寻路管理器
    /// </summary>
    public class AstarManager : Singleton<AstarManager>
    {
        private int mapW;
        private int mapH;
        public AstarNode[,] map;
        private List<AstarNode> openList = new List<AstarNode>();
        private List<AstarNode> closeList = new List<AstarNode>();


        /// <summary>
        /// 初始化地图信息
        /// </summary>
        /// <param name="w"></param>
        /// <param name="h"></param>
        public void InitMapInfo(int w, int h)
        {
            mapW = w;
            mapH = h;
            map = new AstarNode[w, h];
            for (int i = 0; i < w; i++)
            {
                for (int j = 0; j < h; j++)
                {
                    //随机阻挡
                    map[i, j] = new AstarNode(i, j, Random.Range(0, 100) < 20 ? NodeType.Obstacle : NodeType.Walk);
                }
            }
        }

        /// <summary>
        /// 寻路
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public List<AstarNode> FindWay(Vec2 start, Vec2 end)
        {
            // Debug.Log($"开始点({start.X},{start.Y}) 结束点({end.X},{end.Y})");
            //1.是否在地图范围内
            if (!(IsInMap(start) && IsInMap(end)))
            {
                Debug.LogError("A* 寻路 起点或终点不在地图范围内");
                return null;
            }

            //2.可达
            var s = map[start.X, start.Y];
            var e = map[end.X, end.Y];

            if (s.Type == NodeType.Obstacle || e.Type == NodeType.Obstacle)
            {
                Debug.LogError("A* 寻路起点或 终点不可达");
                return null;
            }

            //清空关闭和开启列表
            openList.Clear();
            closeList.Clear();

            var path = new List<AstarNode>();
            //3.计算
            s.Father = null;
            s.G = 0;
            s.H = 0;
            closeList.Add(s); //开始点放入


            while (s != e)
            {
                //开始写入
                WriteNear2OpenList(start + new Vec2(-1, -1), s, e, 1.4f); //左上
                WriteNear2OpenList(start + new Vec2(0, -1), s, e); //上
                WriteNear2OpenList(start + new Vec2(1, -1), s, e, 1.4f); //右上
                WriteNear2OpenList(start + new Vec2(-1, 0), s, e); //左
                WriteNear2OpenList(start + new Vec2(1, 0), s, e); //右
                WriteNear2OpenList(start + new Vec2(-1, 1), s, e, 1.4f); //左下
                WriteNear2OpenList(start + new Vec2(0, 1), s, e); //下
                WriteNear2OpenList(start + new Vec2(1, 1), s, e, 1.4f);


                //不存在任何路径
                if (openList.Count == 0)
                {
                    Debug.Log("A* 死路");
                    foreach (var cc in closeList)
                    {
                        Debug.Log(cc.X + "-" + cc.Y);
                    }

                    return null;
                }

                //选择消耗最小的点(根据F)
                openList.Sort((a, b) =>
                    {
                        if (a.F > b.F)
                        {
                            return 1;
                        }
                        else if (a.F == b.F)
                        {
                            return 1;
                        }
                        else
                        {
                            return -1;
                        }
                    }
                );
                //放入关闭
                closeList.Add(openList[0]);
                //设置新起点,继续下一次寻路
                s = openList[0];
                start = new Vec2(s.X, s.Y);
                openList.RemoveAt(0);
            }
            //回溯路径
            path.Add(e);
            while (e.Father != null)
            {
                path.Add(e.Father);
                e = e.Father;
            }

            //翻转path
            path.Reverse();
            return path;
        }

        /// <summary>
        /// 是否在地图中
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        private bool IsInMap(Vec2 pos)
        {
            if (pos.X < 0 || pos.X >= mapW || pos.Y < 0 || pos.Y >= mapH)
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// 取得node
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        private AstarNode GetNode(Vec2 pos)
        {
            if (IsInMap(pos))
            {
                return map[pos.X, pos.Y];
            }

            return null;
        }

        /// <summary>
        /// 将周围点数据计算并写入openList
        /// </summary>
        /// <param name="pos">当前位置</param>
        /// <param name="father">父对象</param>
        /// <param name="end">终点</param>
        /// <param name="g">离上一个点的距离(移动花费)</param>
        private void WriteNear2OpenList(Vec2 pos, AstarNode father, AstarNode end, float g = 1f)
        {
            var node = GetNode(pos);
            if (node == null || node.Type == NodeType.Obstacle || openList.Contains(node) || closeList.Contains(node))
            {
                return;
            }

            node.Father = father;
            node.H = Mathf.Abs(end.X - node.X) + Mathf.Abs(end.Y - node.Y);
            node.G = node.G + g;
            openList.Add(node);
        }
    }
}

寻路测试

using System;
using System.Collections.Generic;
using FastyTools.Astar;
using UnityEngine;

namespace FastyTools.Test
{
    public class AStarTest : MonoBehaviour
    {

        public int beginX;
        public int beginY;

        public Vector2 offset;
        public int mapW;
        public int mapH;

        public GameObject prefab;

        private Dictionary<string,GameObject> dictionary=new Dictionary<string, GameObject>();

        public Vector2 start = Vector2.right * -1;

        public Vector2 end;
        private void Start()
        {
            start = Vector2.right * -1;
            AstarManager.Instance.InitMapInfo(mapW,mapH);
            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    var go= Instantiate(prefab, new Vector3(beginX + offset.x * i, 0,beginY+offset.y * j),Quaternion.identity);
                    go.transform.SetParent(transform);
                    go.name = i + "_" + j;
                    if (AstarManager.Instance.map[i,j].Type==NodeType.Obstacle)
                    {
                        go.GetComponent<MeshRenderer>().material.color=Color.red;
                    }
                    dictionary.Add(go.name,go);
                }
            }
        }

        private void Update()
        {
            if (Input.GetKeyDown(KeyCode.K))
            {
                var path = AstarManager.Instance.FindWay(new Vec2((int) start.x, (int) start.y),
                    new Vec2((int) end.x, (int) end.y));
                foreach (var c in path)
                {
                    print(c.X+"  "+c.Y);
                }
            }

            if (Input.GetMouseButtonDown(0))
            {
                var ray= Camera.main.ScreenPointToRay(Input.mousePosition);
                RaycastHit hit;
                if (Physics.Raycast(ray,out hit,1000))
                {
                    var c = hit.collider.gameObject;
                    var xy = c.name.Split('_');
                    //开始终点
                    if (start==Vector2.right*-1)
                    {
                        c.GetComponent<MeshRenderer>().material.color=Color.yellow;
                        start=new Vector2(int.Parse(xy[0]),int.Parse(xy[1]));
                    }
                    else if (end==Vector2.zero)
                    {
                        // 终点
                        c.GetComponent<MeshRenderer>().material.color=Color.blue;
                            end=new Vector2(int.Parse(xy[0]),int.Parse(xy[1]));
                    }
                    else
                    {
                        print("开始寻路");
                        //寻路
                        var path = AstarManager.Instance.FindWay(new Vec2((int) start.x, (int) start.y),
                            new Vec2((int) end.x, (int) end.y));
                        foreach (var cc in path)
                        {
                            print(cc.X+"  "+cc.Y);
                            dictionary[cc.X+"_"+cc.Y].GetComponent<MeshRenderer>().material.color=Color.green;
                        }
                    }
                }
            }
        }
    }
}

相关阅读

路径规划之 A* 算法
迪杰斯特拉算法
A*算法在线演示