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));
                }
            }
        }
    }
}
相关推荐
customer081 分钟前
【开源免费】基于SpringBoot+Vue.JS周边产品销售网站（JAVA毕业设计）
java·vue.js·spring boot·后端·spring cloud·java-ee·开源
全栈开发圈3 分钟前
新书速览|Java网络爬虫精解与实践
java·开发语言·爬虫
WaaTong5 分钟前
《重学Java设计模式》之 单例模式
java·单例模式·设计模式
面试鸭7 分钟前
离谱！买个人信息买到网安公司头上？？？
java·开发语言·职场和发展
qq_3901617717 分钟前
防抖函数--应用场景及示例
前端·javascript
John.liu_Test1 小时前
js下载excel示例demo
前端·javascript·excel
沈询-阿里1 小时前
java-智能识别车牌号_基于spring ai和开源国产大模型_qwen vl
java·开发语言
AaVictory.1 小时前
Android 开发 Java中 list实现 按照时间格式 yyyy-MM-dd HH:mm 顺序
android·java·list
南宫生1 小时前
贪心算法习题其四【力扣】【算法学习day.21】
学习·算法·leetcode·链表·贪心算法
热门推荐
01【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总02(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明03组基轨迹建模 GBTM的介绍与实现（Stata 或 R）04【AIGC】重塑未来的科技巨轮05全面解析：构建基于深度学习的安全帽检测系统（UI界面+YOLO代码+数据集）06【经验分享】Ubuntu22.04安装微信(linux官方版)07基于YOLOv10深度学习的CT扫描图像肾结石智能检测系统【python源码+Pyqt5界面+数据集+训练代码】深度学习实战、目标检测08Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO09RAG 实践- Ollama+RagFlow 部署本地知识库10【TC3xx芯片】TC3xx芯片电源管理系统PMS详解