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));
                }
            }
        }
    }
}
相关推荐
mghio31 分钟前
Dubbo 中的集群容错
java·微服务·dubbo
范文杰2 小时前
AI 时代如何更高效开发前端组件？21st.dev 给了一种答案
前端·ai编程
拉不动的猪2 小时前
刷刷题50（常见的js数据通信与渲染问题）
前端·javascript·面试
拉不动的猪3 小时前
JS多线程Webworks中的几种实战场景演示
前端·javascript·面试
FreeCultureBoy3 小时前
macOS 命令行 原生挂载 webdav 方法
前端
uhakadotcom4 小时前
Astro 框架：快速构建内容驱动型网站的利器
前端·javascript·面试
uhakadotcom4 小时前
了解Nest.js和Next.js：如何选择合适的框架
前端·javascript·面试
uhakadotcom4 小时前
React与Next.js：基础知识及应用场景
前端·面试·github
uhakadotcom4 小时前
Remix 框架：性能与易用性的完美结合
前端·javascript·面试
热门推荐
01我决定放弃搞 Java 了02DeepSeek各版本说明与优缺点分析03如何在WPS和Word/Excel中直接使用DeepSeek功能04【AUTOSAR 基础软件】Can模块详解（Can栈之驱动模块）05RAG 实践- Ollama+RagFlow 部署本地知识库06从零安装 LLaMA-Factory 微调 Qwen 大模型成功及所有的坑07Mac cursor设置jdk、Maven版本08本地化部署AI知识库：基于Ollama+DeepSeek+AnythingLLM保姆级教程09如何本地部署AI智能体平台，带你手搓一个AI Agent10纯离线部署本地知识库LLM大模型