美文网首页
【Unity】自己写的AStar寻路

【Unity】自己写的AStar寻路

作者: 木心Sepith | 来源:发表于2022-05-13 18:23 被阅读0次

经过测试,效率还可以

使用


        public void Test()
        {
            var pathGrid = new MapGrid(100, 100, new List<Point2>());

            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

            sw.Start();

            var path = pathGrid.FindPath(new Point2(0, 0), new Point2(99, 99));

            sw.Stop();
            Debug.LogError("total second:" + sw.ElapsedMilliseconds);
        }

Point2

namespace AStar
{
    /// <summary>
    /// 坐标数据
    /// </summary>
    public class Point2
    {
        public Point2(int x, int y)
        {
            this.x = x;
            this.y = y;
        }

        public int x { get; set; }

        public int y { get; set; }

        public override bool Equals(object obj)
        {
            return this.x == (obj as Point2).x && this.y == (obj as Point2).y;
        }

        public override int GetHashCode()
        {
            return x ^ (y * 256);
        }

        public override string ToString()
        {
            return x + "," + y;
        }

        public static bool operator ==(Point2 a, Point2 b)
        {
            return a.Equals(b);
        }

        public static bool operator !=(Point2 a, Point2 b)
        {
            return !a.Equals(b);
        }
    }
}

PathNode

namespace AStar
{
    /// <summary>
    /// 节点数据
    /// </summary>
    public class PathNode
    {
        public PathNode(bool isWall, Point2 position)
        {
            this.isWall = isWall;
            this.position = position;
        }

        public readonly Point2 position;

        public bool isWall { get; set; }

        public PathNode parent { get; set; }

        public int gCost { get; set; }

        public int hCost { get; set; }

        public int fCost
        {
            get
            {
                return gCost + hCost;
            }
        }

        public override bool Equals(object obj)
        {
            PathNode node = obj as PathNode;
            return node.isWall == this.isWall && node.gCost == this.gCost && node.hCost == this.hCost && node.parent == this.parent && this.position == node.position;
        }

        public override int GetHashCode()
        {
            return gCost ^ (hCost * 128) + position.GetHashCode();
        }

        public static bool operator ==(PathNode a, PathNode b)
        {
            return a.Equals(b);
        }

        public static bool operator !=(PathNode a, PathNode b)
        {
            return !a.Equals(b);
        }
    }
}

MapGrid

using System.Collections.Generic;
using System.Linq;
using System;

namespace AStar
{
    /// <summary>
    /// 地图数据
    /// </summary>
    public class MapGrid
    {
        /// <summary>
        /// 开启列表树,key为FCose
        /// </summary>
        private SortedDictionary<int, List<Point2>> openTree = new SortedDictionary<int, List<Point2>>();

        /// <summary>
        /// 开启列表
        /// </summary>
        private HashSet<Point2> openSet = new HashSet<Point2>();

        /// <summary>
        /// 关闭列表
        /// </summary>
        private HashSet<Point2> closeSet = new HashSet<Point2>();

        /// <summary>
        /// 所有的节点
        /// </summary>
        private Dictionary<Point2, PathNode> allNodes = new Dictionary<Point2, PathNode>();

        /// <summary>
        /// 寻路终点
        /// </summary>
        private Point2 endPos;

        /// <summary>
        /// 网格大小
        /// </summary>
        private Point2 gridSize;

        /// <summary>
        /// 当前寻路数据
        /// </summary>
        private List<Point2> currentPath;

        /// <summary>
        /// 新建一个PathGrid,包含了网格大小和障碍物信息
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="walls"></param>
        public MapGrid(int x, int y, List<Point2> walls)
        {
            gridSize = new Point2(x, y);
            for (int i = 0; i < x; i++)
            {
                for (int j = 0; j < y; j++)
                {
                    Point2 newPos = new Point2(i, j);
                    allNodes.Add(newPos, new PathNode(walls.Contains(newPos), newPos));
                }
            }
        }

        /// <summary>
        /// 寻路主要逻辑,通过调用该方法来获取路径信息,由一串Point2代表
        /// </summary>
        /// <param name="beginPos"></param>
        /// <param name="endPos"></param>
        /// <returns></returns>
        public List<Point2> FindPath(Point2 beginPos, Point2 endPos)
        {
            List<Point2> result = new List<Point2>();

            this.endPos = endPos;
            Point2 currentPos = beginPos;

            //把起点加入开启列表
            openSet.Add(currentPos);

            while (!currentPos.Equals(this.endPos))
            {
                UpdatePath(currentPos);

                //未找到路径
                if (openSet.Count == 0) return null;

                currentPos = openTree.First().Value.First();
            }

            //找到了路径,返回路径
            Point2 path = currentPos;

            while (!path.Equals(beginPos))
            {
                result.Add(path);
                path = allNodes[path].parent.position;
                currentPath = result;
            }

            result.Add(beginPos);
            return result;
        }

        /// <summary>
        /// 寻路
        /// </summary>
        /// <param name="currentPos"></param>
        private void UpdatePath(Point2 currentPos)
        {
            //关闭该节点
            closeSet.Add(currentPos);
            RemoveOpen(currentPos, allNodes[currentPos]);

            //继续寻找周围节点
            List<Point2> neighborNodes = FindNeighbor(currentPos);
            foreach (Point2 nodePos in neighborNodes)
            {
                PathNode newNode = new PathNode(false, nodePos);
                newNode.parent = allNodes[currentPos];

                int g;
                int h;

                //计算当前代价gCost,距离起点的距离
                {
                    //直行移动耗费设为10,斜行移动耗费设为14
                    g = currentPos.x == nodePos.x || currentPos.y == nodePos.y ? 10 : 14;

                    newNode.gCost = g + newNode.parent.gCost;
                }

                //计算预估代价hCost,预估终点的距离
                {
                    int xMoves = Math.Abs(nodePos.x - endPos.x);
                    int yMoves = Math.Abs(nodePos.y - endPos.y);

                    int min = Math.Min(xMoves, yMoves);
                    int max = Math.Max(xMoves, yMoves);
                    h = min * 14 + (max - min) * 10;

                    //欧拉距离
                    //h = (int)Math.Pow(nodePos.x - endPos.x, 2) + (int)Math.Pow(nodePos.y - endPos.y, 2);

                    //曼哈顿距离
                    //h = Math.Abs(nodePos.x - endPos.x) + Math.Abs(nodePos.y - endPos.y);

                    newNode.hCost = h;
                }



                PathNode originNode = allNodes[nodePos];

                if (openSet.Contains(nodePos))
                {
                    //如果新节点的f值小于老节点的f值,用新节点替换老节点
                    if (newNode.fCost < originNode.fCost)
                    {
                        UpdateNode(newNode, originNode);
                    }
                }
                else
                {
                    allNodes[nodePos] = newNode;
                    AddOpen(nodePos, newNode);
                }
            }
        }

        /// <summary>
        /// 将旧节点更新为新节点
        /// </summary>
        /// <param name="newNode"></param>
        /// <param name="oldNode"></param>
        private void UpdateNode(PathNode newNode, PathNode oldNode)
        {
            Point2 nodePos = newNode.position;
            int oldCost = oldNode.fCost;
            allNodes[nodePos] = newNode;
            List<Point2> sameCost;

            if (openTree.TryGetValue(oldCost, out sameCost))
            {
                sameCost.Remove(nodePos);
                if (sameCost.Count == 0) openTree.Remove(oldCost);
            }

            if (openTree.TryGetValue(newNode.fCost, out sameCost))
            {
                sameCost.Add(nodePos);
            }
            else
            {
                sameCost = new List<Point2> { nodePos };
                openTree.Add(newNode.fCost, sameCost);
            }
        }

        /// <summary>
        /// 将目标节点移出开启列表
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="node"></param>
        private void RemoveOpen(Point2 pos, PathNode node)
        {
            openSet.Remove(pos);
            List<Point2> sameCost;
            if (openTree.TryGetValue(node.fCost, out sameCost))
            {
                sameCost.Remove(pos);
                if (sameCost.Count == 0) openTree.Remove(node.fCost);
            }
        }

        /// <summary>
        /// 将目标节点加入开启列表
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="node"></param>
        private void AddOpen(Point2 pos, PathNode node)
        {
            openSet.Add(pos);
            List<Point2> sameCost;
            if (openTree.TryGetValue(node.fCost, out sameCost))
            {
                sameCost.Add(pos);
            }
            else
            {
                sameCost = new List<Point2> { pos };
                openTree.Add(node.fCost, sameCost);
            }
        }

        /// <summary>
        /// 找到某节点的所有相邻节点
        /// </summary>
        /// <param name="nodePos"></param>
        /// <returns></returns>
        private List<Point2> FindNeighbor(Point2 nodePos)
        {
            List<Point2> result = new List<Point2>();

            for (int x = -1; x < 2; x++)
            {
                for (int y = -1; y < 2; y++)
                {
                    if (x == 0 && y == 0) continue;

                    Point2 currentPos = new Point2(nodePos.x + x, nodePos.y + y);

                    if (currentPos.x >= gridSize.x || currentPos.y >= gridSize.y || currentPos.x < 0 || currentPos.y < 0) continue; //out of bondary
                    if (closeSet.Contains(currentPos)) continue; // already in the close list
                    if (allNodes[currentPos].isWall) continue;  // the node is a wall

                    result.Add(currentPos);
                }
            }

            return result;
        }
    }
}

相关文章

  • 【Unity】自己写的AStar寻路

    经过测试,效率还可以 使用 Point2 PathNode MapGrid

  • Astar寻路

    大家好,这次给大家分享最近学习A* 这款插件的心得,前面我一点点介绍,到最终我们实现一个多个小队寻路的效果。 首先...

  • AStar 寻路算法

    A*(A-Star)算法是一种静态路网中求解最短路最有效的直接搜索方法。 注意是最有效的直接搜索算法。之后涌现了很...

  • Astar 寻路模块

    直接干这个是因为此模块比较独立 直接进入正题 Astar 干啥的?就是在一张图上搜寻两个点的可行路径 第一步、建立...

  • A星(A*,AStar)寻路

    概念 node:节点,格子 cost:代价 F:预计的总路径的代价 G:从起点到点前节点的代价 H:从当前节点到终...

  • cocos creator Astar寻路导航与地图编辑

    1、插件或者TileMap工具生成地图json文件 2、astar寻路算法 3、将json文件与寻路算法结合,获得...

  • AStar-寻路原理

    本文转自Unity Connect博主majorWu 起源 由于一次面试被问起AStar算法原理,我当场面...

  • 游戏里的跨地图寻路算法

    前段时间遇到一个跨地图寻路的需求,需要在任意两个地图之间自动寻路。我们的寻路算法用的是AStar,每个地图都有一份...

  • 游戏算法(1):实现AStar寻路算法

    本文从项目从2D项目寻路需求做介绍。实现了Astar的带权宽搜算法。 本文链接游戏算法(1):实现2D寻路算法[h...

  • 使用Javascript实现的astar寻路算法

    最近在研究游戏相关的,于是就使用js撸了个a星寻路算法顺便撸了个算法的工作原理过程演示 话不多说,直接上地址。 h...

网友评论

      本文标题:【Unity】自己写的AStar寻路

      本文链接:https://www.haomeiwen.com/subject/bcjuurtx.html