A*算法的研究和应用

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

算法定义

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

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

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

效果预览

效果预览

A* 格子数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
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*寻路

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
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);
        }
    }
}

寻路测试

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
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*算法在线演示