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 分钟前
WEB攻防-通用漏洞&文件上传&js验证&mime&user.ini&语言特性
前端
鸽鸽程序猿9 分钟前
【前端】CSS
前端·css
ggdpzhk10 分钟前
VUE：基于MVVN的前端js框架
前端·javascript·vue.js
盼海14 分钟前
排序算法(五)--归并排序
数据结构·算法·排序算法
学不会•2 小时前
css数据不固定情况下，循环加不同背景颜色
前端·javascript·html
Theodore_10223 小时前
4 设计模式原则之接口隔离原则
java·开发语言·设计模式·java-ee·接口隔离原则·javaee
网易独家音乐人Mike Zhou4 小时前
【卡尔曼滤波】数据预测Prediction观测器的理论推导及应用 C语言、Python实现（Kalman Filter）
c语言·python·单片机·物联网·算法·嵌入式·iot
冰帝海岸4 小时前
01-spring security认证笔记
java·笔记·spring
活宝小娜5 小时前
vue不刷新浏览器更新页面的方法
前端·javascript·vue.js
世间万物皆对象5 小时前
Spring Boot核心概念：日志管理
java·spring boot·单元测试
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09RAG 实践- Ollama+RagFlow 部署本地知识库10怎样让音频速度变慢？请跟随以下方法进行操作