【Unity】自己写的AStar寻路

经过测试,效率还可以

使用


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

            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
{
    /// 
    /// 坐标数据
    /// 
    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
{
    /// 
    /// 节点数据
    /// 
    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
{
    /// 
    /// 地图数据
    /// 
    public class MapGrid
    {
        /// 
        /// 开启列表树,key为FCose
        /// 
        private SortedDictionary> openTree = new SortedDictionary>();

        /// 
        /// 开启列表
        /// 
        private HashSet openSet = new HashSet();

        /// 
        /// 关闭列表
        /// 
        private HashSet closeSet = new HashSet();

        /// 
        /// 所有的节点
        /// 
        private Dictionary allNodes = new Dictionary();

        /// 
        /// 寻路终点
        /// 
        private Point2 endPos;

        /// 
        /// 网格大小
        /// 
        private Point2 gridSize;

        /// 
        /// 当前寻路数据
        /// 
        private List currentPath;

        /// 
        /// 新建一个PathGrid,包含了网格大小和障碍物信息
        /// 
        /// 
        /// 
        /// 
        public MapGrid(int x, int y, List 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));
                }
            }
        }

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

            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;
        }

        /// 
        /// 寻路
        /// 
        /// 
        private void UpdatePath(Point2 currentPos)
        {
            //关闭该节点
            closeSet.Add(currentPos);
            RemoveOpen(currentPos, allNodes[currentPos]);

            //继续寻找周围节点
            List 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);
                }
            }
        }

        /// 
        /// 将旧节点更新为新节点
        /// 
        /// 
        /// 
        private void UpdateNode(PathNode newNode, PathNode oldNode)
        {
            Point2 nodePos = newNode.position;
            int oldCost = oldNode.fCost;
            allNodes[nodePos] = newNode;
            List 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 { nodePos };
                openTree.Add(newNode.fCost, sameCost);
            }
        }

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

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

        /// 
        /// 找到某节点的所有相邻节点
        /// 
        /// 
        /// 
        private List FindNeighbor(Point2 nodePos)
        {
            List result = new List();

            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寻路)