A*寻路算法

qq_292410132024-02-21 20:30

教程网上都有，BiliBili上有一个讲的比较好的，但是地址忘了，有兴趣了解原理的可以去找一下。下面我就贴一下代码，注释都有，基本功能都有，需要扩展的话也方便。

cs 复制代码 
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace AI
{
    /// <summary>
    /// A* 寻路的点
    /// </summary>
    [Serializable]
    public class Point
    {
        /// <summary>
        /// 父节点，用于寻找总体的代价Cost
        /// </summary>
        public Point Parent { get; set; }
        /// <summary>
        ///  总消耗
        ///  F = G + H
        /// </summary>
        public float F { get; set; }
        /// <summary>
        /// 表示从起点移动到网格上指定方格的移动耗费
        /// </summary>
        public float G { get; set; }
        /// <summary>
        /// 当前点到终点的消耗
        /// </summary>
        public float H { get; set; }
        /// <summary>
        /// 行
        /// </summary>
        public int X { get; set; }
        /// <summary>
        /// 列
        /// </summary>
        public int Y { get; set; }
        /// <summary>
        /// 判断是否是障碍物
        /// 动态设置，当这个点上面有车的时候就设置为true
        /// </summary>
        public bool isObstacle { get; set; }
        /// <summary>
        /// 坐标，AI寻路所走的点
        /// </summary>
        public Vector3 position;

        public Point(int x,int y,Point parent = null,Vector3 position = default)
        {
            X = x;
            Y = y;
            Parent = parent;
            this.position = position;
            isObstacle = false;
        }

        public void UpdateParent(Point parent,float g)
        {
            Parent = parent;
            G = g;
            F = G + H;
        }
    }
}

上面是定义的点的坐标，下面是逻辑代码。

cs 复制代码 
using AI;
using System.Collections;
using System.Collections.Generic;
using UnityEditor.Experimental.GraphView;
using UnityEngine;

public class AStarWrapperMono : MonoBehaviour
{
    /// <summary>
    /// 起点
    /// </summary>
    public Vector3 start = Vector3.zero;
    /// <summary>
    /// 终点
    /// </summary>
    public Vector3 end = Vector3.zero;
    /// <summary>
    /// 起点
    /// </summary>
    public Point startPoint;
    /// <summary>
    /// 终点
    /// </summary>
    public Point endPoint;
    /// <summary>
    /// 地图行列宽高
    /// </summary>
    public int mapRow, mapCol, mapWidth = 4, mapHeight = 5;
    /// <summary>
    /// 地图容器
    /// </summary>
    private Point[,] map;
    /// <summary>
    /// 开列表（周围的点列表）
    /// </summary>
    private List<Point> openList = new List<Point>();
    /// <summary>
    /// 关列表（每次比较后得到的最小的F的点列表）
    /// </summary>
    private List<Point> closeList = new List<Point>();
    /// <summary>
    /// 路径点
    /// </summary>
    private List<Point> path = new List<Point>();

    public Collider car;

    private void Start()
    {
        Init();
    }



#if UNITY_EDITOR
    private void Update()
    {
        //设置障碍物，可以自己使用一帧去获取就行了，我只是动态展示用的
        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                if (car.bounds.Contains(map[i, j].position))
                {
                    map[i, j].isObstacle = true;
                    if (path != null)
                    {
                        path.Clear();
                    }
                    path = FindPath();
                }
                else
                {
                    map[i, j].isObstacle = false;
                }
            }
        }
        //if (Input.GetKeyDown(KeyCode.Space))
        //{
        //    path = FindPath();
        //}
        //if (Input.GetKeyDown(KeyCode.R))
        //{
        //    path.Clear();
        //}
    }

    #region 测试
    private void Init()
    {
        InitMap();
    }
    #endregion
#endif

    public void Init(Vector3 m_start, Vector3 m_end)
    {
        InitPosition(m_start, m_end);
        InitMap();
        path = FindPath();
    }

    private void InitPosition(Vector3 m_start, Vector3 m_end)
    {
        start = m_start;
        end = m_end;
    }

    /// <summary>
    /// 初始化网格
    /// </summary>
    private void InitMap()
    {
        map = new Point[mapRow, mapCol];
        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                map[i, j] = new Point(i, j, null, GetMapPosition(i, j));
            }
        }
        startPoint = map[0, 0];
    }

    #region 方法2
    private List<Point> FindPath()
    {
        //判断是否越界
        if (startPoint.X < 0 || startPoint.X >= mapRow || startPoint.Y < 0 || startPoint.Y >= mapCol || endPoint.X < 0 || endPoint.X >= mapRow || endPoint.Y < 0 || endPoint.Y >= mapCol)
        {
            return null;
        }

        Point mStart = map[startPoint.X, startPoint.Y];
        Point mEnd = map[endPoint.X, endPoint.Y];

        //判断起点和终点是否是障碍物点
        if (mStart.isObstacle || mEnd.isObstacle)
        {
            return null;
        }
        closeList.Clear();
        openList.Clear();

        mStart.Parent = null;
        mStart.F = 0;
        mStart.G = 0;
        mStart.H = 0;

        closeList.Add(mStart);

        while (true)
        {
            //左下
            FindNearlyToOpenList(mStart.X - 1, mStart.Y - 1, 1.4f, mStart, mEnd);
            //正下
            FindNearlyToOpenList(mStart.X, mStart.Y - 1, 1, mStart, mEnd);
            //右下
            FindNearlyToOpenList(mStart.X + 1, mStart.Y - 1, 1.4f, mStart, mEnd);
            //正左
            FindNearlyToOpenList(mStart.X - 1, mStart.Y, 1, mStart, mEnd);
            //正右
            FindNearlyToOpenList(mStart.X + 1, mStart.Y, 1, mStart, mEnd);
            //左上
            FindNearlyToOpenList(mStart.X - 1, mStart.Y + 1, 1.4f, mStart, mEnd);
            //正上
            FindNearlyToOpenList(mStart.X, mStart.Y + 1, 1, mStart, mEnd);
            //右上
            FindNearlyToOpenList(mStart.X + 1, mStart.Y + 1, 1.4f, mStart, mEnd);

            if (openList.Count == 0)
            {
                return null;
            }

            //排序选出最小的点
            openList.Sort(SortOpenList);
            //放入关闭列表，然后从开启列表中移除
            closeList.Add(openList[0]);
            //找到这个点，进行下一次寻路
            mStart = openList[0];
            openList.RemoveAt(0);
            if (mStart == mEnd)
            {
                //结束
                List<Point> path = new List<Point>();
                path.Add(mEnd);
                while (mEnd.Parent != null)
                {
                    path.Add(mEnd.Parent);
                    mEnd = mEnd.Parent;
                }
                path.Reverse();
                return path;
            }
        }
    }

    /// <summary>
    /// 查找就近点
    /// </summary>
    private void FindNearlyToOpenList(int x, int y, float g, Point parent, Point end)
    {
        if (x < 0 || x >= mapRow || y < 0 || y >= mapCol)
        {
            return;
        }

        Point point = map[x, y];
        if (point == null || point.isObstacle || closeList.Contains(point) || openList.Contains(point))
        {
            return;
        }

        point.Parent = parent;
        point.G = parent.G + g;
        point.H = Mathf.Abs(end.X - point.X) + Mathf.Abs(end.Y - point.Y);
        point.F = point.G + point.H;
        openList.Add(point);
    }

    /// <summary>
    /// 排序
    /// </summary>
    private int SortOpenList(Point a, Point b)
    {
        if (a.F > b.F)
        {
            return 1;
        }
        else if (a.F == b.F)
        {
            return 1;
        }
        else
        {
            return -1;
        }
    }
    #endregion

    /// <summary>
    /// 获取地图上的点对应的坐标
    /// </summary>
    /// <param name="i"></param>
    /// <param name="j"></param>
    /// <returns></returns>
    private Vector3 GetMapPosition(int i, int j)
    {
        float width, height;
        width = Mathf.Abs(start.x - end.x);
        height = Mathf.Abs(start.z - end.z);
        float divideX = 1;
        float divideZ = 1;
        if (width < mapWidth)
        {
            width = mapWidth;
            divideX = 2;
        }
        if (height < mapHeight)
        {
            height = mapHeight;
            divideZ = 2;
        }
        //单元长度
        float unitWidth = width / mapRow;
        float unitHeight = height / mapCol;

        float x, z;
        if (start.x > end.x)
        {
            x = start.x - unitWidth * i;
        }
        else
        {
            x = start.x + unitWidth * i;
        }
        if (start.z > end.z)
        {
            z = start.z - unitHeight * j;
        }
        else
        {
            z = start.z + unitHeight * j;
        }
        Vector3 result = new Vector3(x, start.y, z);
        if (Mathf.Abs(x - end.x) <= unitWidth/ divideX && Mathf.Abs(z - end.z) <= unitHeight/ divideZ)
        {
            endPoint = new Point(i, j, null, result);
        }
        return result;
    }

    private void OnDrawGizmos()
    {
        float width, height;
        width = Mathf.Abs(start.x - end.x);
        height = Mathf.Abs(start.z - end.z);
        if (width < mapWidth)
        {
            width = mapWidth;
        }
        if (height < mapHeight)
        {
            height = mapHeight;
        }
        //单元长度
        float unitWidth = width / mapRow;
        float unitHeight = height / mapCol;

        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                if ((i == 0 && j == 0))
                {
                    Gizmos.color = Color.red;
                }
                else
                {
                    Gizmos.color = Color.yellow;

                    if (path != null)
                    {
                        for (int m = 0; m < path.Count; m++)
                        {
                            if (path[m].X == i && path[m].Y == j)
                            {
                                Gizmos.color = Color.green;
                            }
                        }
                    }
                }
                if (map != null && map.Length > 0)
                {
                    if (map[i, j].isObstacle)
                    {
                        Gizmos.color = Color.black;
                    }
                }
                if (endPoint != null)
                {
                    if (i == endPoint.X && j == endPoint.Y)
                    {
                        Gizmos.color = Color.red;
                    }
                }
                Gizmos.DrawCube(GetMapPosition(i, j), new Vector3(unitWidth / 1.2f, 1, unitHeight / 1.2f));
            }
        }
    }
}

上面的是测试代码，下面的就是供外部调用的代码，初始化使用

AStarWrapper astar = new AStarWrapper();

然后获取里面的方法就行了，可以直接拿来用。

cs 复制代码 
using AI;
using System.Collections;
using System.Collections.Generic;
using UnityEditor.Experimental.GraphView;
using UnityEngine;

public class AStarWrapper
{
    /// <summary>
    /// 起点
    /// </summary>
    public Vector3 start = Vector3.zero;
    /// <summary>
    /// 终点
    /// </summary>
    public Vector3 end = Vector3.zero;
    /// <summary>
    /// 起点
    /// </summary>
    public Point startPoint;
    /// <summary>
    /// 终点
    /// </summary>
    public Point endPoint;
    /// <summary>
    /// 地图行列宽高
    /// </summary>
    public int mapRow, mapCol, mapWidth = 8, mapHeight = 5;
    /// <summary>
    /// 地图容器
    /// </summary>
    private Point[,] map;
    /// <summary>
    /// 开列表（周围的点列表）
    /// </summary>
    private List<Point> openList = new List<Point>();
    /// <summary>
    /// 关列表（每次比较后得到的最小的F的点列表）
    /// </summary>
    private List<Point> closeList = new List<Point>();
    /// <summary>
    /// 路径点
    /// </summary>
    private List<Point> path = new List<Point>();
    /// <summary>
    /// 显示Gizmos
    /// </summary>
    private bool isShowGizmos = false;

    public Collider car;

    public void Init(int mMapRow, int mMapCol, Vector3 m_start, Vector3 m_end)
    {
        isShowGizmos = true;
        mapRow = mMapRow;
        mapCol = mMapCol;
        InitPosition(m_start, m_end);
        InitMap();
    }

    private void InitPosition(Vector3 m_start, Vector3 m_end)
    {
        start = m_start;
        end = m_end;
    }

    /// <summary>
    /// 初始化网格
    /// </summary>
    private void InitMap()
    {
        map = new Point[mapRow, mapCol];
        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                map[i, j] = new Point(i, j, null, GetMapPosition(i, j));
            }
        }
        startPoint = map[0, 0];
        float distance = Vector3.Distance(startPoint.position, new Vector3(end.x, start.y, end.z));
        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                float temp = Vector3.Distance(map[i, j].position, new Vector3(end.x, start.y, end.z));
                if (temp < distance)
                {
                    distance = temp;
                    endPoint = map[i, j];
                }
            }
        }
    }

    #region 
    /// <summary>
    /// 寻找路径
    /// </summary>
    /// <returns></returns>
    private List<Point> FindPath()
    {
        //判断是否越界
        if (startPoint.X < 0 || startPoint.X >= mapRow || startPoint.Y < 0 || startPoint.Y >= mapCol || endPoint.X < 0 || endPoint.X >= mapRow || endPoint.Y < 0 || endPoint.Y >= mapCol)
        {
            return null;
        }

        Point mStart = map[startPoint.X, startPoint.Y];
        Point mEnd = map[endPoint.X, endPoint.Y];

        //判断起点和终点是否是障碍物点
        if (mStart.isObstacle || mEnd.isObstacle)
        {
            return null;
        }
        closeList.Clear();
        openList.Clear();

        mStart.Parent = null;
        mStart.F = 0;
        mStart.G = 0;
        mStart.H = 0;

        closeList.Add(mStart);

        while (true)
        {
            //左下
            FindNearlyToOpenList(mStart.X - 1, mStart.Y - 1, 1.4f, mStart, mEnd);
            //正下
            FindNearlyToOpenList(mStart.X, mStart.Y - 1, 1, mStart, mEnd);
            //右下
            FindNearlyToOpenList(mStart.X + 1, mStart.Y - 1, 1.4f, mStart, mEnd);
            //正左
            FindNearlyToOpenList(mStart.X - 1, mStart.Y, 1, mStart, mEnd);
            //正右
            FindNearlyToOpenList(mStart.X + 1, mStart.Y, 1, mStart, mEnd);
            //左上
            FindNearlyToOpenList(mStart.X - 1, mStart.Y + 1, 1.4f, mStart, mEnd);
            //正上
            FindNearlyToOpenList(mStart.X, mStart.Y + 1, 1, mStart, mEnd);
            //右上
            FindNearlyToOpenList(mStart.X + 1, mStart.Y + 1, 1.4f, mStart, mEnd);

            if (openList.Count == 0)
            {
                return null;
            }

            //排序选出最小的点
            openList.Sort(SortOpenList);
            //放入关闭列表，然后从开启列表中移除
            closeList.Add(openList[0]);
            //找到这个点，进行下一次寻路
            mStart = openList[0];
            openList.RemoveAt(0);
            if (mStart == mEnd)
            {
                //结束
                List<Point> path = new List<Point>();
                path.Add(mEnd);
                while (mEnd.Parent != null)
                {
                    path.Add(mEnd.Parent);
                    mEnd = mEnd.Parent;
                }
                path.Reverse();
                return path;
            }
        }
    }

    /// <summary>
    /// 查找就近点
    /// </summary>
    private void FindNearlyToOpenList(int x, int y, float g, Point parent, Point end)
    {
        if (x < 0 || x >= mapRow || y < 0 || y >= mapCol)
        {
            return;
        }

        Point point = map[x, y];
        if (point == null || point.isObstacle || closeList.Contains(point) || openList.Contains(point))
        {
            return;
        }

        point.Parent = parent;
        point.G = parent.G + g;
        point.H = Mathf.Abs(end.X - point.X) + Mathf.Abs(end.Y - point.Y);
        point.F = point.G + point.H;
        openList.Add(point);
    }

    /// <summary>
    /// 排序
    /// </summary>
    private int SortOpenList(Point a, Point b)
    {
        if (a.F > b.F)
        {
            return 1;
        }
        else if (a.F == b.F)
        {
            return 1;
        }
        else
        {
            return -1;
        }
    }

    public void SetObstacle(Collider collider)
    {
        for (int i = 0; i < mapRow; i++)
        {
            for (int j = 0; j < mapCol; j++)
            {
                if (collider.bounds.Contains(map[i, j].position))
                {
                    map[i, j].isObstacle = true;
                    if (path != null)
                    {
                        path.Clear();
                    }
                    path = FindPath();
                }
                else
                {
                    map[i, j].isObstacle = false;
                }
            }
        }
    }

    public List<Point> GetPath()
    {
        return path;
    }
    #endregion

    /// <summary>
    /// 获取地图上的点对应的坐标
    /// </summary>
    /// <param name="i"></param>
    /// <param name="j"></param>
    /// <returns></returns>
    private Vector3 GetMapPosition(int i, int j)
    {
        float width, height;
        width = Mathf.Abs(start.x - end.x);
        height = Mathf.Abs(start.z - end.z);
        float divideX = 1;
        float divideZ = 1;
        if (width < mapWidth)
        {
            width = mapWidth;
            divideX = 2;
        }
        if (height < mapHeight)
        {
            height = mapHeight;
            divideZ = 2;
        }
        //单元长度
        float unitWidth = width / mapRow;
        float unitHeight = height / mapCol;

        float x, z;
        if (start.x > end.x)
        {
            x = start.x - unitWidth * i;
        }
        else
        {
            x = start.x + unitWidth * i;
        }
        if (start.z > end.z)
        {
            z = start.z - unitHeight * j;
        }
        else
        {
            z = start.z + unitHeight * j;
        }
        Vector3 result = new Vector3(x, start.y, z);
        //if (Mathf.Abs(x - end.x) <= unitWidth / divideX && Mathf.Abs(z - end.z) <= unitHeight / divideZ)
        //{
        //    endPoint = new Point(i, j, null, result);
        //}
        return result;
    }

    public void Reset()
    {
        isShowGizmos = false;
    }

    public void OnDrawGizmos()
    {
        if (isShowGizmos)
        {
            float width, height;
            width = Mathf.Abs(start.x - end.x);
            height = Mathf.Abs(start.z - end.z);
            if (width < mapWidth)
            {
                width = mapWidth;
            }
            if (height < mapHeight)
            {
                height = mapHeight;
            }
            //单元长度
            float unitWidth = width / mapRow;
            float unitHeight = height / mapCol;

            for (int i = 0; i < mapRow; i++)
            {
                for (int j = 0; j < mapCol; j++)
                {
                    if ((i == 0 && j == 0))
                    {
                        Gizmos.color = Color.red;
                    }
                    else
                    {
                        Gizmos.color = Color.yellow;

                        if (path != null)
                        {
                            for (int m = 0; m < path.Count; m++)
                            {
                                if (path[m].X == i && path[m].Y == j)
                                {
                                    Gizmos.color = Color.green;
                                }
                            }
                        }
                    }
                    if (map != null && map.Length > 0)
                    {
                        if (map[i, j].isObstacle)
                        {
                            Gizmos.color = Color.black;
                        }
                    }
                    if (endPoint != null)
                    {
                        if (i == endPoint.X && j == endPoint.Y)
                        {
                            Gizmos.color = Color.red;
                        }
                    }
                    Gizmos.DrawCube(GetMapPosition(i, j), new Vector3(unitWidth / 1.2f, 1, unitHeight / 1.2f));
                }
            }
        }
    }
}
相关推荐
鸽鸽程序猿7 分钟前
【算法】【优选算法】宽搜（BFS）中队列的使用
算法·宽度优先·队列
Jackey_Song_Odd7 分钟前
C语言 单向链表反转问题
c语言·数据结构·算法·链表
ProtonBase10 分钟前
如何从 0 到 1 ，打造全新一代分布式数据架构
java·网络·数据库·数据仓库·分布式·云原生·架构
Watermelo61711 分钟前
详解js柯里化原理及用法，探究柯里化在Redux Selector 的场景模拟、构建复杂的数据流管道、优化深度嵌套函数中的精妙应用
开发语言·前端·javascript·算法·数据挖掘·数据分析·ecmascript
m0_7482489413 分钟前
HTML5系列（11）-- Web 无障碍开发指南
前端·html·html5
乐之者v16 分钟前
leetCode43.字符串相乘
java·数据结构·算法
m0_7482356124 分钟前
从零开始学前端之HTML（三）
前端·html
A懿轩A1 小时前
C/C++ 数据结构与算法【数组】 数组详细解析【日常学习，考研必备】带图+详细代码
c语言·数据结构·c++·学习·考研·算法·数组
古希腊掌管学习的神1 小时前
[搜广推]王树森推荐系统——矩阵补充&最近邻查找
python·算法·机器学习·矩阵
云边有个稻草人1 小时前
【优选算法】—复写零（双指针算法）
笔记·算法·双指针算法
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02ARM学习（31）编译器对overlay方式的支持03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08基于大语言模型（LLM）的合成数据生成、策展和评估的综述09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10树莓派 5 AI 套件（Hailo-8L）使用教程