A*寻路算法——初版

企业开发 2023-05-17 06:01:05 阅读次数: 0
  1. 格子节点类
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public enum E_Node_Type
{
    
    
    Stop, Pass, Boundary
}

public class AStarNode
{
    
    
    public float f;
    public float g;
    public float h;
    public int x;
    public int y;
    public E_Node_Type type;
    public AStarNode nodeParent;

    public AStarNode(int x,int y,E_Node_Type type)
    {
    
    
        this.x = x;
        this.y = y;
        this.type = type;
    }

}
  1. 格子节点管理寻路类（寻路主要逻辑）
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;
using UnityEngine.UI;

public class AStarMgr : BaseSingleton<AStarMgr>
{
    
    
    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_CalculateOrderNum;//计算OpenList的排序次数，用来比较优化
    /// <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];

        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_OpenList.Clear();
        m_CloseList.Clear();

        pStartNode.nodeParent = null;
        pStartNode.f = 0;
        pStartNode.g = 0;
        pStartNode.h = 0;
        //首先把起点放到关闭列表
        m_CloseList.Add(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;
            }
            m_OpenList.Sort((x, y) => x.f.CompareTo(y.f)); //升序排序
            m_CalculateOrderNum++;
            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_OpenList.Contains(pNode) && !m_CloseList.Contains(pNode))
            {
    
    
                pNode.nodeParent = parentNode;
                pNode.g = parentNode != null ? g + parentNode.g : g;
                pNode.h = _GetManhattanDistance(x, y, endPos);
                pNode.f = pNode.g + pNode.h;
                m_OpenList.Add(pNode);
            }
        }
    }


}
  1. 测试A*表现类
//#define AStar
//#define AStarOptimize
#define JPS                                                   

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

public class StartAStarFindPathTest : MonoBehaviour
{
    
    
    //地图宽高
    public int mapWidth = 20;
    public int mapHeight = 20;
    //开始寻路按钮
    public Button findPathBtn;
    //重置地图按钮
    public Button resetMapBtn;
    //色块用来区分格子
    public Color passColor;
    public Color stopColor;
    public Color startPosColor;
    public Color endPosColor;
    public Color pathLineColor;

    //地点，终点
    private Vector2Int startPos;
    private Vector2Int endPos;

    private Transform m_CanvasTrans;
    private Transform m_ParentTrans;
    private float m_CanvasX;
    private float m_CanvasY;
    private int m_RecordClickNum;//记录点击方框的次数，区分选择 1 起点和 2 终点
    private float m_GridWidth;//格子宽
    private float m_GridHeight;//格子高
    private Dictionary<Vector2, GameObject> m_GridObjDict = new Dictionary<Vector2, GameObject>();//缓存地图格子，坐标：格子实体

    // Start is called before the first frame update
    void Start()
    {
    
    
        //测试用，默认左下角起点，右上角终点
        startPos = new Vector2Int(0, 0);
        endPos = new Vector2Int(mapWidth - 1, mapHeight - 1);
        //地图所挂父节点
        m_ParentTrans = GameObject.Find("NodeParent").transform;
        m_CanvasTrans = GameObject.Find("Canvas").transform;
        m_CanvasX = m_CanvasTrans.GetComponent<RectTransform>().anchoredPosition.x;
        m_CanvasY = m_CanvasTrans.GetComponent<RectTransform>().anchoredPosition.y;

        _InitDrawMap();
        findPathBtn.onClick.AddListener(_OnClickFindPath);
        resetMapBtn.onClick.AddListener(_ResetMap);
    }

    private void _OnClickFindPath()
    {
    
    
        Debug.Log("点击开始寻路");
        if (startPos == Vector2Int.zero && endPos == Vector2Int.zero || startPos == endPos)
        {
    
    
            Debug.Log("请选择起点和终点或不符合规范请重选!!!");
            return;
        }
#if AStar
        List<AStarNode> lstNode = AStarMgr.Instance.CalculateFindPath(startPos, endPos);
#elif AStarOptimize
        List<AStarNode> lstNode = AStarOptimizeMgr.Instance.CalculateFindPath(startPos, endPos);
#elif JPS
        List<AStarNode> lstNode = JPSMgr.Instance.CalculateFindPath(startPos, endPos);
#endif
        if (lstNode == null)
        {
    
    
            Debug.Log("没有可走路径");
            return;
        }
        Debug.Log("节点数量：" + lstNode.Count);
        //foreach (var node in lstNode)
        //{
    
    
        //    Debug.LogFormat("x:{0};y:{1}", node.x, node.y);
        //}

        _DrawPath(lstNode);
    }

    //初始化绘制地图
    private void _InitDrawMap()
    {
    
    
        foreach (GameObject obj in m_GridObjDict.Values)
        {
    
    
            Destroy(obj);
        }
        m_GridObjDict.Clear();
#if AStar
        AStarMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#elif AStarOptimize
        AStarOptimizeMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#elif JPS
        JPSMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#endif
        AStarNode pNode;
        for (int i = 0; i < mapWidth; i++)
        {
    
    
            for (int j = 0; j < mapHeight; j++)
            {
    
    
#if AStar
                pNode = AStarMgr.Instance.Nodes[i, j];
#elif AStarOptimize
                pNode = AStarOptimizeMgr.Instance.Nodes[i, j];
#elif JPS
                pNode = JPSMgr.Instance.Nodes[i, j];
#endif
                //模拟显示地图
                GameObject obj = GameObject.Instantiate(Resources.Load<GameObject>("path_node"));
                if (pNode.type == E_Node_Type.Stop)
                {
    
    
                    obj.GetComponent<Image>().color = stopColor;
                    obj.name = "stop";
                }
                else
                {
    
    
                    obj.GetComponent<Image>().color = passColor;
                    obj.name = "pass";
                }
                if (m_GridWidth == 0 || m_GridHeight == 0)
                {
    
    
                    m_GridWidth = obj.GetComponent<RectTransform>().rect.width;
                    m_GridHeight = obj.GetComponent<RectTransform>().rect.height;
                }

                obj.transform.position = new Vector3(pNode.x * m_GridWidth, pNode.y * m_GridHeight);
                obj.transform.SetParent(m_ParentTrans);

                m_GridObjDict[new Vector2(pNode.x, pNode.y)] = obj;
            }
        }
    }

    //重置地图
    private void _ResetMap()
    {
    
    
        //测试用，默认左下角起点，右上角终点
        startPos = new Vector2Int(0, 0);
        endPos = new Vector2Int(mapWidth - 1, mapHeight - 1);

        m_RecordClickNum = 0;
#if AStar
        AStarMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#elif AStarOptimize
        AStarOptimizeMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#elif JPS
        JPSMgr.Instance.InitMapInfo(mapWidth, mapHeight);
#endif
        AStarNode pNode;
        for (int i = 0; i < mapWidth; i++)
        {
    
    
            for (int j = 0; j < mapHeight; j++)
            {
    
    
#if AStar
                pNode = AStarMgr.Instance.Nodes[i, j];
#elif AStarOptimize
                pNode = AStarOptimizeMgr.Instance.Nodes[i, j];
#elif JPS
                pNode = JPSMgr.Instance.Nodes[i, j];
#endif
                //模拟显示地图
                GameObject obj = m_GridObjDict[new Vector2(pNode.x, pNode.y)];
                if (pNode.type == E_Node_Type.Stop)
                {
    
    
                    obj.GetComponent<Image>().color = stopColor;
                    obj.name = "stop";
                }
                else
                {
    
    
                    obj.GetComponent<Image>().color = passColor;
                    obj.name = "pass";
                }
                obj.transform.position = new Vector3(pNode.x * m_GridWidth, pNode.y * m_GridHeight);
                obj.transform.SetParent(m_ParentTrans);

                m_GridObjDict[new Vector2(pNode.x, pNode.y)] = obj;
            }
        }
    }

    //绘制路径
    private void _DrawPath(List<AStarNode> lstPathNode)
    {
    
    
        foreach (var node in lstPathNode)
        {
    
    
            if (node.x == startPos.x && node.y == startPos.y || node.x == endPos.x && node.y == endPos.y)
                continue;
            Vector2 key = new Vector2(node.x, node.y);
            if (m_GridObjDict.ContainsKey(key))
            {
    
    
                m_GridObjDict[key].GetComponent<Image>().color = pathLineColor;
            }
        }
    }

    void Update()
    {
    
    
        if (Input.GetMouseButtonDown(0))
        {
    
    
            GameObject obj = GetFirstPickGameObject(Input.mousePosition);
            if (m_RecordClickNum < 2 && obj != null)
            {
    
    
                if (obj.tag != "PathNode")
                    return;

                Vector3 objPos = obj.transform.GetComponent<RectTransform>().anchoredPosition;
                m_RecordClickNum++;
                if (m_RecordClickNum == 1)
                {
    
    
                    startPos = new Vector2Int((int)((objPos.x + m_CanvasX) / m_GridWidth), (int)((objPos.y + m_CanvasY) / m_GridHeight));
                    obj.GetComponent<Image>().color = startPosColor;
                    Debug.Log("选择起点：" +objPos+",数组坐标："+ startPos);
                }
                else
                {
    
    
                    endPos = new Vector2Int((int)((objPos.x + m_CanvasX) / m_GridWidth), (int)((objPos.y + m_CanvasY) / m_GridHeight));
                    obj.GetComponent<Image>().color = endPosColor;
                    Debug.Log("选择终点：" + objPos + ",数组坐标：" + endPos);
                }
            }
        }
    }

    /// <summary>
    /// 点击屏幕坐标
    /// </summary>
    /// <param name="position"></param>
    /// <returns></returns>
    public GameObject GetFirstPickGameObject(Vector2 position)
    {
    
    
        EventSystem eventSystem = EventSystem.current;
        PointerEventData pointerEventData = new PointerEventData(eventSystem);
        pointerEventData.position = position;
        //射线检测ui
        List<RaycastResult> uiRaycastResultCache = new List<RaycastResult>();
        eventSystem.RaycastAll(pointerEventData, uiRaycastResultCache);
        if (uiRaycastResultCache.Count > 0)
            return uiRaycastResultCache[0].gameObject;
        return null;
    }
}

猜你喜欢

转载自blog.csdn.net/weixin_42205218/article/details/114805163
今日推荐
LFOSSA 源来如此公开课 | 掌握云原生未来：CNCF 认证全面攻略与备考秘籍
国产云输入法——仅华为无云端数据上传安全问题
开源日报 | 工业开源项目OGG 1.0；姐姐，你要和我一起配置火狐吗；苹果AI遥遥落后？Fedora 40
开放签电子签章：停止新增，优化体验，前进更进（五一假期前工作）
开源日报 | 中学生开源前端动画引擎；全球首个Llama3 8B中文版开源模型；联想电脑恐出局；Linus讽刺AI炒作
“百模大战”必有一战 | 2024中国“百模大战”竞争格局分析
周排行
Family Tree 题解
BZOJ 1093 最大半连通子图 SCC + DP
幂等处理
Spring----学习（2）----XML 配置Bean 自动装配
SQL Server 远程更新目标表数据
HIbernate3.6 环境搭建
特殊符号正则表达式
【Linux】第一章 进程的理解
843. n-皇后问题（dfs+输出各种情况）
空间数据库2
每日归档
更多
2024-04-26(39)
2024-04-25(22)
2024-04-24(36)
2024-04-23(26)
2024-04-22(39)
2024-04-21(0)
2024-04-20(6)
2024-04-19(5)
2024-04-18(0)
2024-04-17(5)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022