A* pathfinding algorithm, step by step, with complete source code

A* pathfinding algorithm - step by step (with complete source code)

use

​ The general purpose of the A* pathfinding algorithm is to flexibly find the shortest path from the initial point to the target point.

overview

​ Flexibility is a feature that the A* algorithm pays more attention to. Obstacles can be added arbitrarily, different weights can be given to the pathfinding loss of different terrains, priorities can be set, and so on. There are many algorithms for the shortest path, such as Dijkstra, BFS (breadth-first search), etc. This article focuses on the A-star pathfinding algorithm based on BFS.

Principle of BFS algorithm

​ Before introducing the A star algorithm, first introduce the method of BFS to find the shortest path. The breadth-first search mainly uses queues. The basic idea of ​​finding the shortest path is to traverse each step and add the grids that satisfy the conditions within the range of the current grid to the queue, and then continue the nine grids centered on the grids that satisfy the conditions. Expand outward, constantly remove the nodes that have been reached and add new nodes that can be reached, and repeat until the end is found. Note that in this process, the previous node of the current node should be recorded to facilitate the final search path.

Here is a moving picture made by another blogger, which is very clear. The dynamic display of the BFS maze process_Working hard for Lao Zhou's column-CSDN Blog

The grid has three states in total, being visited, waiting to be visited and already visited, corresponding to the process described above.

BFS algorithm template

public class Node
{
    
    
    public bool passable; //是否可通行
    public int row,col;
    public Node lastNode; //记录上一个节点
    public Node(bool passable,int row,int col)
    {
    
    
        this.passable = passable;
        this.row = row;
        this.col = col;
    }
}

public class BFSTest
{
    
    
    Node[,] nodeMap;
    List<Node> path = new List<Node>(); //记录路径
    bool[,] used; //记录已经遍历的节点，数组大小和nodeMap保持一致
    int maxWidth,maxHeight; //初始化时需一并记录最大值以判断边界
    //这里省略初始化地图的过程 默认nodeMap已经初始化
    public void InitMap(){
    
    } 
    public List<Node> findPath(int startRow,int startCol,int endRow,int endCol)
    {
    
    
        //每次寻路前都需要先清空path和used 在此省略
        
        //省略了合法性的检验，一定要验证所给索引的边界问题
        if(BFS(startRow,startCol,endRow,endCol))
        {
    
    
            //再根据lastNode构建出路径
            Node node = nodeMap[endRow,endCol];
            while(node!=null)
            {
    
    
                path.Add(node);
                node = node.lastNode;
            }
            path.Reverse(); //反转列表
            return path;
        }
        else
        {
    
    
            Console.WriteLine("无可行路径");
            return null;
        }
    }
    
    //返回值为是否能到达结尾点 --- 防止死路情况
    private bool BFS(int startRow,int startCol,int endRow,int endCol)
    {
    
    
        //定义队列
        Queue<Node> q = new Queue<Node>();
        //始发点入队
        q.Enqueue(nodeMap[startRow,startCol]);
        nodeMap[startRow,startCol].lastNode = null;
        //每次入队都要记录used
        used[startRow,startCol] = true;
        
        while(q.Count != 0)
        {
    
    
            //每次都取队首
            Node node = q.Dequeue();
            //构建偏移量 让遍历九宫格更加方便 从左上开始顺时针定义
            int[] dx = {
    
    -1,0,1,1,1,0,-1,-1};
            int[] dy = {
    
    -1,-1,-1,0,1,1,1,0};
            for(int i = 0; i < 8; i++)
            {
    
    
                int row = node.row + dy[i];
                int col = node.col + dx[i];
                //一定记得判断边界合法性  和  used情况
                if(used[row,col] || row < 0 || row >= maxHeight || col < 0 || col >= maxWidth)
                	continue;
                q.Enqueue(nodeMap[row,col]);
                used[row,col] = true;
               	nodeMap[row,col].lastNode = node;
                //到达终点提前结束即可
                if(row == endRow && col == endCol) return true;
            }
        }
        return false;
    }
}

This template is the basic template for implementing the A-star algorithm. Using this algorithm, the entry conditions can be flexibly formulated to meet the requirements of flexibility, but this algorithm still has a big drawback

It will traverse the grids waiting to be visited indiscriminately. Generally, the advanced team will visit first, and the loss will be greater for finding the end point.

​ So the A star algorithm adds a heuristic algorithm on this basis, and finds its pathfinding loss f for each grid. According to the pathfinding loss of f, select the grid with the lowest pathfinding loss to visit, so that the waste of accessing redundant grids can be greatly reduced.

Principle of A* Pathfinding Algorithm

​ The principle of the A* pathfinding algorithm, as mentioned above, is actually a heuristic optimization of BFS indiscriminate pathfinding, providing a pathfinding loss variable, and choosing the minimum pathfinding loss each time Node visits can greatly reduce the number of invalid visits.

Next, the calculation index of the pathfinding loss f is introduced.

g: Euclidean distance from the starting point to the current node

h: the Manhattan distance from the current node to the end point

f: Pathfinding loss of the current node f = g + h

Euclidean distance is the actual distance mentioned in daily mathematics, the formula is

The formula for Manhattan distance

Here is a chestnut

Take the above picture as an example. The red line is the Euclidean distance from the starting point to the current point. Generally, moving one grid horizontally is the unit distance 1, and moving obliquely is the square root of 2, which is approximately equal to 1.4. The Manhattan distance is a yellow arrow, which is the difference between the horizontal axis and the vertical axis. It can only move horizontally or vertically, and the unit distance is 1 at a time.

g = 1 + 1.4 = 2.4

h = 1 + 1 + 1 = 3

f = 2.4 +3 =5.4

​ After understanding this algorithm, you can calculate the f value of all nodes waiting to be visited, and select the value with the smallest pathfinding loss f for each BFS selection.

The A* algorithm is based on the change point of BFS

​ The basic A* algorithm is an improvement to BFS. It generally uses an open list and a closed list . The open list contains nodes waiting to be accessed, and the closed list contains nodes that have been visited.

Analyze related changes

Changes for Node nodes

1. Added the attribute of pathfinding loss f
2. Provides a method to calculate the loss f

Changes to BFS logic

1. There are nodes waiting to be accessed in the open list, which is the function of replacing the original queue Queue
2. The closed list saves the removed nodes in the open list, and cooperates with the open list to determine whether the element replaces the weight judgment function of the used array in these two lists.
3. The node selected in each cycle is not the leader of the team, but the node with the smallest f value should be selected. You can consider sorting the open list according to the f value every time and then taking the first element. (Priority queue can be used for optimization)

Optimization of A* Algorithm

• Encapsulating row and col with a structure is convenient for parameter passing, and the logic is clearer
• The list only stores the index, and the specific Node uses the index to search in the map to save memory space, and the reference of the same object is only unique in the map, which will not cause confusion.
• Use a priority queue to avoid having to sort the open list every time

The third optimization point is very important. When faced with a very large map, the time-consuming sorting is very serious. A priority queue, also called a binary heap, is actually a big (small) top heap. When storing elements, it always guarantees that the leader of the team must be the largest (smallest) . This property is very consistent with the use of the A* algorithm when selecting the smallest node of f, and it uses the characteristics of the binary heap. Compared with sorting, the speed is greatly improved, and this optimization is very necessary.

Priority queues are directly defined in languages ​​such as C++, priority_queue, etc., but C# does not have a pre-defined priority queue and needs to be implemented by itself.

If you are interested in the knowledge about the priority queue, you can learn it by yourself, mainly the knowledge points of heap sorting! The simple C# priority queue implemented by the author is pasted below.

PriorityQueue.cs

namespace Common
{
    
    
    /// <summary>
    /// 简易优先级队列的实现
    /// </summary>
    public class PriorityQueue<T>
    {
    
    
        //用一维数组存储元素
        T[] arr;
        //最大容量
        int capacity;
        public int Length
        {
    
    
            get
            {
    
    
                return length;
            }
        }
        //当前长度
        int length = 0;
        //自定义比较方式
        Func<T, T, bool> compareHandler;

		//这里没有提供自动扩容的方法，要求定义时必须指明最大容量
        public PriorityQueue(int capacity, Func<T, T, bool> compareHandler)
        {
    
    
            arr = new T[capacity];
            this.capacity = capacity;
            this.compareHandler = compareHandler;
        }

        /// <summary>
        /// 加入优先队列
        /// </summary>
        public void insert(T element)
        {
    
    
            //判断是否还有空余
            if (length + 1 > capacity) return;
            arr[length] = element;
            ModifyInsertElement(length);
            length++;
        }

        /// <summary>
        /// 调整新插入二叉堆的元素
        /// </summary>
        private void ModifyInsertElement(int index)
        {
    
    
            //边界
            if (index < 0) return;
            int father = (index - 1) / 2;
            if (compareHandler(arr[index], arr[father]))
            {
    
    
                swap(index, father);
                ModifyInsertElement(father);
            }
        }

        /// <summary>
        /// 获取队首元素
        /// </summary>
        public T top()
        {
    
    
            if (length == 0) return default(T);
            return arr[0];
        }

        /// <summary>
        /// 出队并获取队首
        /// </summary>
        /// <returns></returns>
        public T pop()
        {
    
    
            if (length == 0) return default(T);
            T tmp = arr[0];
            arr[0] = default(T);
            swap(0, length - 1);
            length--;
            ModifyPopElement(0);
            return tmp;
        }

        /// <summary>
        /// 调整出队时的二叉堆(堆顶开始从上至下)
        /// </summary>
        /// <param name="index"></param>
        private void ModifyPopElement(int index)
        {
    
    
            if (index >= length) return;
            int left = 2 * index + 1;
            int right = 2 * index + 2;

            if (left >= length && right >= length) return;
            if (left >= length)
            {
    
    
                if (compareHandler(arr[right], arr[index]))
                {
    
    
                    swap(right, index);
                    ModifyPopElement(right);
                }
                else return;
            }
            if (right >= length)
            {
    
    
                if (compareHandler(arr[left], arr[index]))
                {
    
    
                    swap(left, index);
                    ModifyPopElement(left);
                }
                else return;
            }

            //三者中取较大，若子节点较大则交换并递归向下继续查询比较
            T maxValue = compareHandler(arr[left], arr[right]) ? arr[left] : arr[right];
            maxValue = compareHandler(maxValue, arr[index]) ? maxValue : arr[index];
            if (maxValue.Equals(arr[index])) return;
            if (maxValue.Equals(arr[left]))
            {
    
    
                swap(left, index);
                ModifyPopElement(left);
            }
            if (maxValue.Equals(arr[right]))
            {
    
    
                swap(right, index);
                ModifyPopElement(right);
            }

        }

        private void swap(int a, int b)
        {
    
    
            T tmp = arr[a];
            arr[a] = arr[b];
            arr[b] = tmp;
        }

        //清空
        public void Clear()
        {
    
    
            arr = new T[capacity];
            length = 0;
        }



    }
}

A* algorithm detailed source code

AStarNode.csnode node class

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

namespace Common.AStar
{
    
    
    public struct NodePos
    {
    
    
        public int row;
        public int col;
        public NodePos(int row,int col)
        {
    
    
            this.row = row;
            this.col = col;
        }
    }

    public enum ANodeType
    {
    
    
        //节点可通行
        passable,
        //节点不可通行
        impassable
    }

    /// <summary>
    /// A*寻路系统的逻辑节点
    /// </summary>
    public class AStarNode
    {
    
    
        //逻辑位置 （索引行列号)
        public NodePos pos;

        //节点的类型
        public ANodeType nodeType;

        //A*算法的相关计算指标
        public float f;    //寻路消耗 f = g + h
        public float g;    //起点到当前节点的移动距离
        public float h;    //当前节点到终点的曼哈顿距离

        //A*节点的上一步
        public AStarNode lastNode;
        

        public AStarNode(NodePos pos,ANodeType nodeType)
        {
    
    
            this.pos = pos;
            this.nodeType = nodeType;
        }

        /// <summary>
        /// 计算寻路消耗,使用前务必设置lastNode
        /// </summary>
        /// <param name="currentPos">当前节点</param>
        public void CalculateF(NodePos endPos)
        {
    
    
            //如果是起点 则为空
            if(lastNode == null)
            {
    
    
                g = 0;
                h = CalculateMHDis(pos, endPos);
                f = g + h;
                return;
            }
            //判断是否为上下左右 而非斜
            bool isStraight = Mathf.Abs(pos.row - lastNode.pos.row) + Mathf.Abs(pos.col - lastNode.pos.col) == 1;
            //计算g 当前节点到起点的距离
            g = lastNode.g + (isStraight ? 1 : 1.4f);

            //计算h 当前节点到终点的曼哈顿距离
            h = CalculateMHDis(pos, endPos);

            //计算f f = g + h
            f = g + h;
        }

        /// <summary>
        /// 计算当前节点到终点的曼哈顿距离
        /// </summary>
        /// <param name="currentPos">当前节点位置</param>
        /// <param name="endPos">终点位置</param>
        /// <returns></returns>
        private int CalculateMHDis(NodePos currentPos, NodePos endPos)
        {
    
    
            return Mathf.Abs(endPos.row - currentPos.row) + Mathf.Abs(endPos.col - currentPos.col);
        }
    }
}

AStarManager.csA* pathfinding system manager

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

namespace Common.AStar
{
    
    
    /// <summary>
    /// A*寻路系统管理器 单例
    /// </summary>
    public class AStarManager : Singleton<AStarManager>
    {
    
    
        //逻辑地图 (节点容器)
        public AStarNode[,] nodeMap;

        //开启列表 - 存储当前可以走到的点
        //private List<NodePos> openList; 用优先队列进行优化
        private PriorityQueue<NodePos> openList;
        //关闭列表 - 存储当前已经走完的点
        private List<NodePos> closeList;

        //判重数组
        private bool[,] used;

        //逻辑地图的尺寸 (节点容器大小)
        private int logicWidth;
        private int logicHeight;

        //实际地图节点尺寸 --- 逻辑节点默认 单位长度的正方体
        private float realNodeWidth = 1; //根据实际对地图的切割情况决定 demo默认为1
        private float realNodeHeight = 1;

        /// <summary>
        /// 单例提供的初始化函数
        /// </summary>
        protected override void Init()
        {
    
    
            base.Init();
            //初始化相关数据结构 构造小顶堆的优先级队列
            openList = new PriorityQueue<NodePos>(100,(a,b)=>
            {
    
    
                return nodeMap[a.row, a.col].f < nodeMap[b.row, b.col].f;
            }
            );
            closeList = new List<NodePos>();

            
        }

        /// <summary>
        /// 初始化节点容器
        /// </summary>
        /// <param name="logicWidth">逻辑宽(容器的列数)</param>
        /// <param name="logicHeight">逻辑高(容器的行数)</param>
        public void InitNodeMap(int logicWidth,int logicHeight)
        {
    
    
            //初始化容器
            nodeMap = new AStarNode[logicHeight, logicWidth];
            this.logicWidth = logicWidth;
            this.logicHeight = logicHeight;

            //创建节点并放入容器中
            for(int i = 0; i < logicHeight; i++)
                for(int j = 0; j < logicWidth; j++)
                {
    
    
                    //这里使用随机设置节点类型，实际需按地图配置文件进行配置
                    AStarNode node = new AStarNode(new NodePos(i, j),
                        UnityEngine.Random.Range(0,100) > 20 ? ANodeType.passable : ANodeType.impassable );

                    nodeMap[i, j] = node;
                }
            used = new bool[logicHeight, logicWidth];
        }

        /// <summary>
        /// A*核心算法 找寻最短路径
        /// </summary>
        /// <param name="startPos">起点位置(实际位置)</param>
        /// <param name="endPos">终点位置(实际位置)</param>
        /// <returns></returns>
        public List<AStarNode> FindPath(Vector2 startPos,Vector2 endPos)
        {
    
    
            //存储最短路径
            List<AStarNode> path = new List<AStarNode>();
            //清空双列表和判重数组
            openList.Clear();
            closeList.Clear();
            used = new bool[logicHeight, logicWidth];

            //实际位置转换到逻辑位置 --这里仍需完善,负值的情况
            int startRow = (int)(startPos.y / realNodeHeight);
            int startCol = (int)(startPos.x / realNodeWidth);
            int endRow = (int)(endPos.y / realNodeHeight);
            int endCol = (int)(endPos.x / realNodeWidth);

            //判断逻辑位置是否合法
            if (startRow < 0 || startRow >= logicHeight
                || startCol < 0 || startCol >= logicWidth
                || endRow < 0 || endRow >= logicHeight
                || endCol < 0 || endCol >= logicWidth)

                return null;

            //特判如果起点或终点有阻挡 直接返回空
            AStarNode startNode = nodeMap[startRow, startCol];
            AStarNode endNode = nodeMap[endRow, endCol];
            if (startNode.nodeType == ANodeType.impassable || endNode.nodeType == ANodeType.impassable) return null;

            //bfs广度优先搜索实现A*寻路算法
            bool isSuccess = bfs(new NodePos(startRow, startCol), 
                new NodePos(endRow, endCol));

            //如果搜索到最短路则返回
            if (isSuccess)
            {
    
    
                return CalculatePath(new NodePos(startRow, startCol), new NodePos(endRow, endCol));
            }
            else return null;
        
        }

        private bool bfs(NodePos startPos,NodePos endPos)
        {
    
    
            //每次从开启队列中移除最优的一个节点加入关闭列表，将移除节点周围的新节点加入开启列表
            //初始状态 先初始化起点相关 在加入开启列表
            
            
            AStarNode startNode = nodeMap[startPos.row, startPos.col];
            startNode.lastNode = null; //起点设为空 因为寻路可能多次 此步不能省略
            startNode.CalculateF(endPos);
            openList.insert(startPos);
            used[startPos.row, startPos.col] = true;

            while (openList.Length > 0)
            {
    
    
                //对openList进行排序 每次取f最小的移除
                //openList.Sort(CustomSortFunc);  优化成优先队列后无需进行排序

                //取出List的首位
                NodePos topPos = openList.pop();
                AStarNode currentNode = nodeMap[topPos.row,topPos.col];
                closeList.Add(topPos);

                //定义偏移量 从左下开始顺时针
                int[] dx = {
    
     -1, -1, -1, 0, 1, 1, 1, 0 };
                int[] dy = {
    
     -1, 0, 1, 1, 1, 0, -1, -1 };

                for(int i = 0; i < 8; i++)
                {
    
    
                    int row = currentNode.pos.row + dx[i];
                    int col = currentNode.pos.col + dy[i];
                    //根据边界进行筛选
                    if (row < 0 || row >= logicHeight || col < 0 || col >= logicWidth) 
                        continue;
                    //根据openList和closeList判断重复 --- 单独使用used判重 空间换时间
                    //if (openList.Contains(new NodePos(row, col)) || closeList.Contains(new NodePos(row, col)))
                    //continue;
                    //判重
                    if (used[row, col]) continue;

                    AStarNode node = nodeMap[row, col];
                    //还需判断节点是否可通过
                    if (node.nodeType == ANodeType.impassable) continue;

                    //筛选成功的节点设置lastNode 计算f 加入openList
                    node.lastNode = currentNode;
                    node.CalculateF(endPos);
                    //添加进开启列表
                    openList.insert(node.pos);
                    used[row, col] = true;
                    //如果结尾添加进来 可提前结束
                    if (node.pos.Equals(endPos)) return true;

                }
            }

            return false;
            
        }

        private int CustomSortFunc(NodePos x, NodePos y)
        {
    
    
            AStarNode nodeX = nodeMap[x.row, x.col];
            AStarNode nodeY = nodeMap[y.row, y.col];

            if (nodeX.f > nodeY.f) return 1;
            else if (nodeX.f == nodeY.f) return 1;
            else return -1;
        }

        
        
        
        /// <summary>
        /// 根据关闭列表计算路径
        /// </summary>
        /// <param name="startPos">起始逻辑位置</param>
        /// <param name="endPos">终点逻辑位置</param>
        /// <returns></returns>
        private List<AStarNode> CalculatePath(NodePos startPos,NodePos endPos)
        {
    
    
            List<AStarNode> path = new List<AStarNode>();
            AStarNode endNode = nodeMap[endPos.row, endPos.col];
            AStarNode startNode = nodeMap[startPos.row, endPos.col];
            AStarNode node = endNode;
            while(node!=null && !node.Equals(startNode))
            {
    
    
                path.Add(node);
                node = node.lastNode;
            }
            //翻转列表
            path.Reverse();

            return path;
        }



    }
}

need attention

1. The unit distance 1 mentioned in this article is a logical distance. The actual map size needs to be loaded according to the actual map data. Finally, the actual distance is converted to the logical distance for calculation, and the result is converted back for use.
2. The initialization of the map is directly random initialization, and the actual development needs to be initialized and generated according to the map data.
3. Before testing this module, you need to call the method once InitNodeMapto use FindPaththe function.
4. This manager also uses the singleton mode, interested readers can refer to it by themselves, and the code of the singleton mode is also posted below

namespace Common
{
    
    
    /// <summary>
    /// 饿汉式单例模式
    /// </summary>
    /// <typeparam name="T"></typeparam>
    public class Singleton<T> where T : Singleton<T> //注意此约束为T必须为其本身或子类
    {
    
    
        protected Singleton() {
    
     }
        public static T Instance {
    
     get; private set; }

        static Singleton()
        {
    
    
            Instance = Activator.CreateInstance(typeof(T), true) as T;
            Instance.Init(); //初始化一次
        }

        /// <summary>
        /// 可选初始化函数
        /// </summary>
        protected virtual void Init()
        {
    
    

        }
    }
}