A* Pathfinding Algorithm - Optimization

1. Grid node management class (main pathfinding logic)

1. Optimize the sorting times of OpenList to reduce the number of sorting times.
2. Optimize the judgment of whether the node exists in the open list OpenList and the closed list CloseList, and use an array to record the grids already in the list, because the efficiency of array search is relatively high, and space is exchanged for time.

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

public class AStarOptimizeMgr : BaseSingleton<AStarOptimizeMgr>
{
    
    
    private int m_Width;
    private int m_Height;

    private AStarNode[,] m_Nodes;

    public AStarNode[,] Nodes
    {
    
    
        get {
    
     return m_Nodes; }
    }
    private List<AStarNode> m_OpenList = new List<AStarNode>();
    private List<AStarNode> m_CloseList = new List<AStarNode>();
    private int m_RecordSortNum;//记录排序的数量
    private int m_CalculateOrderNum;//计算OpenList的排序次数，用来比较优化
    private AStarNode[,] m_RecordNodes;//记录已经在m_OpenList或者在m_CloseList中的节点
    /// <summary>
    /// 初始化地图信息
    /// </summary>
    /// <param name="width"></param>
    /// <param name="height"></param>
    public void InitMapInfo(int width, int height)
    {
    
    
        Debug.Log("InitMapInfo");
        m_Width = width;
        m_Height = height;
        if (m_Nodes == null)
        {
    
    
            m_Nodes = new AStarNode[width, height];
            m_RecordNodes = new AStarNode[width, height];
        }

        for (int i = 0; i < width; i++)
        {
    
    
            for (int j = 0; j < height; j++)
            {
    
    
                AStarNode pNode;
                if (i == 0 || i == width - 1 || j == 0 || j == height - 1)
                {
    
    
                    pNode = new AStarNode(i, j, E_Node_Type.Boundary);
                }
                else
                {
    
    
                    E_Node_Type eType = _GetRandRomNodeType();
                    pNode = new AStarNode(i, j, eType);
                }
                m_Nodes[i, j] = pNode;

            }
        }
    }

    /// <summary>
    /// 计算寻路
    /// </summary>
    /// <param name="startPos"></param>
    /// <param name="endPos"></param>
    /// <returns></returns>
    public List<AStarNode> CalculateFindPath(Vector2Int startPos, Vector2Int endPos)
    {
    
    
        //目前只考虑坐标都是整数情况

        int iStartX = startPos.x;
        int iStartY = startPos.y;
        int iEndX = endPos.x;
        int iEndY = endPos.y;
        //如果超出界限
        if (_IsOverBound(iStartX, iStartY) || _IsOverBound(iEndX, iEndY))
        {
    
    
            Debug.LogError("所在点超出界限");
            return null;
        }
        AStarNode pStartNode = m_Nodes[iStartX, iStartY];
        AStarNode pEndNode = m_Nodes[iEndX, iEndY];
        //如果是阻挡
        if (pStartNode.type == E_Node_Type.Stop || pEndNode.type == E_Node_Type.Stop)
        {
    
    
            Debug.LogError("所在点是阻挡点，不合规范");
            return null;
        }

        //寻路逻辑
        m_CalculateOrderNum = 0;
        m_RecordSortNum = 0;
        m_OpenList.Clear();
        m_CloseList.Clear();

        pStartNode.nodeParent = null;
        pStartNode.f = 0;
        pStartNode.g = 0;
        pStartNode.h = 0;
        //首先把起点放到关闭列表
        m_CloseList.Add(pStartNode);
        m_RecordNodes[iStartX, iStartY] = pStartNode;
        AStarNode pCurrenNode = pStartNode;
        float fTime = Time.realtimeSinceStartup;
        while (pCurrenNode != pEndNode)
        {
    
    
            //左上
            _AddNearNodeToOpenList(pCurrenNode.x - 1, pCurrenNode.y - 1, 1.4f, pCurrenNode, endPos);
            //上
            _AddNearNodeToOpenList(pCurrenNode.x - 1, pCurrenNode.y, 1.0f, pCurrenNode, endPos);
            //右上
            _AddNearNodeToOpenList(pCurrenNode.x - 1, pCurrenNode.y + 1, 1.4f, pCurrenNode, endPos);
            //左
            _AddNearNodeToOpenList(pCurrenNode.x, pCurrenNode.y - 1, 1.0f, pCurrenNode, endPos);
            //右
            _AddNearNodeToOpenList(pCurrenNode.x, pCurrenNode.y + 1, 1.0f, pCurrenNode, endPos);
            //左下
            _AddNearNodeToOpenList(pCurrenNode.x + 1, pCurrenNode.y - 1, 1.4f, pCurrenNode, endPos);
            //下
            _AddNearNodeToOpenList(pCurrenNode.x + 1, pCurrenNode.y, 1.0f, pCurrenNode, endPos);
            //右下
            _AddNearNodeToOpenList(pCurrenNode.x + 1, pCurrenNode.y + 1, 1.4f, pCurrenNode, endPos);

            if (m_OpenList.Count == 0)
            {
    
    
                Debug.LogError("死路");
                return null;
            }
            if (m_RecordSortNum <= 0)//当排序数量小于等于零时
            {
    
    
                m_OpenList.Sort((x, y) => x.f.CompareTo(y.f)); //升序排序
                m_CalculateOrderNum++;
                m_RecordSortNum = m_OpenList.Count;
            }
            else
            {
    
    
                m_RecordSortNum--;
            }

            pCurrenNode = m_OpenList[0];
            m_CloseList.Add(pCurrenNode);
            m_OpenList.RemoveAt(0);
        }
        Debug.LogFormat("用时：{0}", Time.realtimeSinceStartup - fTime);
        Debug.Log("排序次数" + m_CalculateOrderNum);
        List<AStarNode> lstPathNode = new List<AStarNode>();
        AStarNode pNode = pEndNode;
        while (pNode != null)
        {
    
    
            lstPathNode.Add(pNode);
            pNode = pNode.nodeParent;
        }
        lstPathNode.Reverse();
        return lstPathNode;
    }

    /// <summary>
    /// 随机地块类型
    /// </summary>
    /// <returns></returns>
    private E_Node_Type _GetRandRomNodeType()
    {
    
    
        int iRandNum = UnityEngine.Random.Range(0, 10);
        if (iRandNum >= 0 && iRandNum < 8)
            return E_Node_Type.Pass;
        else
            return E_Node_Type.Stop;
    }

    /// <summary>
    /// 判断是否超出边界
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    private bool _IsOverBound(int x, int y)
    {
    
    
        if (x < 0 || x >= m_Width || y < 0 || y >= m_Height)
            return true;
        else
            return false;
    }

    /// <summary>
    /// 判断是否是阻碍
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    private bool _IsCanPass(int x, int y)
    {
    
    
        E_Node_Type type = m_Nodes[x, y].type;
        if (type == E_Node_Type.Stop)
            return false;
        else
            return true;
    }

    /// <summary>
    /// 获取曼哈顿距离
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="endPos"></param>
    /// <returns></returns>
    private int _GetManhattanDistance(int x, int y, Vector2Int endPos)
    {
    
    
        int iHorizontal = Mathf.Abs(endPos.x - x);
        int iVertical = Mathf.Abs(endPos.y - y);
        return iHorizontal + iVertical;
    }

    /// <summary>
    ///  把周围节点添加到开放列表中
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="parentNode"></param>
    private void _AddNearNodeToOpenList(int x, int y, float g, AStarNode parentNode, Vector2Int endPos)
    {
    
    
        if (!_IsOverBound(x, y) && _IsCanPass(x, y))
        {
    
    
            AStarNode pNode = m_Nodes[x, y];
            if (m_RecordNodes[x, y] != pNode)
            {
    
    
                pNode.nodeParent = parentNode;
                pNode.g = parentNode != null ? g + parentNode.g : g;
                pNode.h = _GetManhattanDistance(x, y, endPos);
                pNode.f = pNode.g + pNode.h;

                if (m_RecordSortNum > 0)
                {
    
    
                    if (pNode.f < m_OpenList[0].f)
                        m_OpenList.Insert(0, pNode);
                    else
                        m_OpenList.Add(pNode);
                }
                else
                {
    
    
                    m_OpenList.Add(pNode);
                }
                m_RecordNodes[x, y] = pNode;
            }
        }
    }


}