A*寻路算法-C#版

喜欢ヅ旅行 2021-09-17 00:18 272阅读 0赞

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70][]

1.  从起点 A 开始, 把它作为待处理的方格存入一个"开启列表", 开启列表就是一个等待检查方格的列表.
2.  寻找起点 A 周围可以到达的方格, 将它们放入"开启列表", 并设置它们的"父方格"为 A.
3.  从"开启列表"中删除起点 A, 并将起点 A 加入"关闭列表", "关闭列表"中存放的都是不需要再次检查的方格

![20181113175428744.jpeg][]

图中浅绿色描边的方块表示已经加入 "开启列表" 等待检查. 淡蓝色描边的起点 A 表示已经放入"关闭列表" , 它不需要再执行检查.从 "开启列表" 中找出相对最靠谱的方块, 什么是最靠谱? 它们通过公式 F=G+H 来计算.

**G** 表示从起点A移动到网格上指定方格的移动耗费(可沿斜方向移动).

**H** 表示从指定的方格移动到终点B的预计耗费(H有很多计算方法,这里我们设定只可以上下左右移动).

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 1][]

我们假设横向移动一个格子的耗费为 10, 为了便于计算, 沿斜方向移动一个格子耗费是 14. 为了更直观的展示如何运算 FGH, 图中方块的左上角数字表示 F, 左下角表示 G, 右下角表示 H.

从 "开启列表" 中选择 F 值最低的方格 C (绿色起始方块 A 右边的方块), 然后对它进行如下处理:

1.  把它从 "开启列表" 中删除, 并放到 "关闭列表" 中.
2.  检查它所有相邻并且可以到达 (障碍物和 "关闭列表" 的方格都不考虑) 的方格. 如果这些方格

还不在 "开启列表" 里的话, 将它们加入 "开启列表", 计算这些方格的 G, H 和 F 值各是多少, 并设置它们的 "父方格" 为 C.

1.  如果某个相邻方格 D 已经在 "开启列表" 里了, 检查如果用新的路径 (就是经过 C 的路径) 到达它的话, G 值是否会更低一些, 如果新的 G 值更低, 那就把它的 "父方格" 改为目前选中的方格 C, 然后重新计算它的 F 值和 G 值 (H 值不需要重新计算, 因为对于每个方块, H 值是不变的). 如果新的 G 值比较高, 就说明经过 C 再到达 D 不是一个明智的选择, 因为它需要更远的路, 这时我们什么也不做.

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 1][]

如图, 我们选中了 C 因为它的 F 值最小, 我们把它从 "开启列表" 中删除, 并把它加入 "关闭列表". 它右边上下三个都是墙, 所以不考虑它们. 它左边是起始方块, 已经加入到 "关闭列表" 了, 也不考虑. 所以它周围的候选方块就只剩下 4 个. 让我们来看看 C 下面的那个格子, 它目前的 G 是 14, 如果通过 C 到达它的话, G 将会是 10 + 10, 这比 14 要大, 因此我们什么也不做.

然后我们继续从 "开启列表" 中找出 F 值最小的, 但我们发现 C 上面的和下面的同时为 54, 这时怎么办呢? 这时随便取哪一个都行, 比如我们选择了 C 下面的那个方块 D.

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 2][]

1.  右边已经右上方的都是墙, 所以不考虑, 但为什么右下角的没有被加进 "开启列表" 呢? 因为如

*  C 下面的那块也不可以走, 想要到达 C 右下角的方块就需要从 "方块的角" 走了, 在程序中设置是否允许这样走. (图中的示例不允许这样走)

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 3][]

就这样, 我们从 "开启列表" 找出 F 值最小的, 将它从 "开启列表" 中移掉, 添加到 "关闭列表".

再继续找出它周围可以到达的方块, 如此循环下去...

那么什么时候停止呢? —— 当我们发现 "开始列表" 里出现了目标终点方块的时候, 说明路径已经被找到.

**如何找回路径**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 4][]

如上图所示, 除了起始方块, 每一个曾经或者现在还在 "开启列表" 里的方块, 它都有一个 "父方块", 通过 "父方块" 可以索引到最初的 "起始方块", 这就是路径.

using System;
    using System.Collections.Generic;
    
    namespace ConsoleApplication2
    {
        class Program
        {
            static void Main(string[] args)
            {
                test mytest = new test();
    
                //定义出发位置
                Point pa = new Point();
                pa.x = 1;
                pa.y = 3;
    
                //定义目的地
                Point pb = new Point();
                pb.x = 8;
                pb.y = 8;
    
                mytest.FindWay(pa, pb);
    
                mytest.PrintMap();
                Console.ReadKey();
            }
        }
        class test
        {
            //数组用1表示可通过，0表示障碍物
            byte[,] R = new byte[10, 10] {
                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
                { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
    
                };
    
            //开启列表
            List<Point> Open_List = new List<Point>();
    
            //关闭列表
            List<Point> Close_List = new List<Point>();
    
            //从开启列表查找F值最小的节点
            private Point GetMinFFromOpenList()
            {
                Point Pmin = null;
                foreach (Point p in Open_List)
                    if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H)
                        Pmin = p;
                return Pmin;
            }
    
            //判断一个点是否为障碍物
            private bool IsBar(Point p, byte[,] map)
            {
                if (map[p.y, p.x] == 0) return true;
                return false;
            }
    
            //判断关闭列表是否包含一个坐标的点
            private bool IsInCloseList(int x, int y)
            {
                foreach (Point p in Close_List) if (p.x == x && p.y == y) return true;
                return false;
            }
            //从关闭列表返回对应坐标的点
            private Point GetPointFromCloseList(int x, int y)
            {
                foreach (Point p in Close_List) if (p.x == x && p.y == y) return p;
                return null;
            }
    
            //判断开启列表是否包含一个坐标的点
            private bool IsInOpenList(int x, int y)
            {
                foreach (Point p in Open_List) if (p.x == x && p.y == y) return true;
                return false;
            }
    
            //从开启列表返回对应坐标的点
            private Point GetPointFromOpenList(int x, int y)
            {
                foreach (Point p in Open_List) if (p.x == x && p.y == y) return p;
                return null;
            }
    
            //计算某个点的G值
            private int GetG(Point p)
            {
                if (p.father == null) return 0;
                if (p.x == p.father.x || p.y == p.father.y) return p.father.G + 10;
                else return p.father.G + 14;
            }
    
            //计算某个点的H值
            private int GetH(Point p, Point pb)
            {
                return Math.Abs(p.x - pb.x) + Math.Abs(p.y - pb.y);
            }
    
            //检查当前节点附近的节点
            private void CheckP8(Point p0, byte[,] map, Point pa, ref Point pb)
            {
                for (int xt = p0.x - 1; xt <= p0.x + 1; xt++)
                {
                    for (int yt = p0.y - 1; yt <= p0.y + 1; yt++)
                    {
                        //排除超过边界和等于自身的点
                        if ((xt >= 0 && xt < 10 && yt >= 0 && yt < 10) && !(xt == p0.x && yt == p0.y))
                        {
                            //排除障碍点和关闭列表中的点
                            if (map[yt, xt] != 0 && !IsInCloseList(xt, yt))
                            {
                                if (IsInOpenList(xt, yt))
                                {
                                    Point pt = GetPointFromOpenList(xt, yt);
                                    int G_new = 0;
                                    if (p0.x == pt.x || p0.y == pt.y) G_new = p0.G + 10;
                                    else G_new = p0.G + 14;
                                    if (G_new < pt.G)
                                    {
                                        Open_List.Remove(pt);
                                        pt.father = p0;
                                        pt.G = G_new;
                                        Open_List.Add(pt);
                                    }
                                }
                                else
                                {
                                    //不在开启列表中
                                    Point pt = new Point();
                                    pt.x = xt;
                                    pt.y = yt;
                                    pt.father = p0;
                                    pt.G = GetG(pt);
                                    pt.H = GetH(pt, pb);
                                    Open_List.Add(pt);
                                }
                            }
                        }
                    }
                }
            }
    
            public void FindWay(Point pa, Point pb)
            {
                Open_List.Add(pa);
                while (!(IsInOpenList(pb.x, pb.y) || Open_List.Count == 0))
                {
                    Point p0 = GetMinFFromOpenList();
                    if (p0 == null) return;
                    Open_List.Remove(p0);
                    Close_List.Add(p0);
                    CheckP8(p0, R, pa, ref pb);
                }
    
                Point p = GetPointFromOpenList(pb.x, pb.y);
                while (p.father != null)
                {
                    p = p.father;
                    R[p.y, p.x] = 3;
                }
            }
    
            public void SaveWay(Point pb)
            {
                Point p = pb;
                while (p.father != null)
                {
                    p = p.father;
                    R[p.y, p.x] = 3;
                }
            }
    
            public void PrintMap()
            {
                for (int a = 0; a < 10; a++)
                {
                    for (int b = 0; b < 10; b++)
                    {
                        if (R[a, b] == 1) Console.Write("█");
                        else if (R[a, b] == 3) Console.Write("★");
                        else if (R[a, b] == 4) Console.Write("○");
    
                        else Console.Write("  ");
                    }
                    Console.Write("\n");
                }
            }
        }
    
        class Point
        {
            public int y;
            public int x;
            public int G;
            public int H;
    
            public Point() { }
            public Point(int x0, int y0, int G0, int H0, Point F)
            {
                x = x0;
                y = y0;
                G = G0;
                H = H0;
                father = F;
            }
    
            public Point father;
        }
    }

![20181113182032728.png][]

**或者：**

using System;
    using System.Collections.Generic;
    using System.Linq;
    
    namespace Maze
    {
        class Program
        {
            static void Main(string[] args)
            {
                int[,] array = {
                               { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                               { 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
                               { 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
                               { 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
                               { 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
                               { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
                               { 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
                               { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
                               };
                Maze maze = new Maze(array);
                Point start = new Point(1, 1);
                Point end = new Point(6, 10);
                var parent = maze.FindPath(start, end, false);
    
                Console.WriteLine("Print path:");
                while (parent != null)
                {
                    Console.WriteLine(parent.X + ", " + parent.Y);
                    parent = parent.ParentPoint;
                }
                Console.ReadKey();
            }
        }
        class Maze
        {
            public const int OBLIQUE = 14;
            public const int STEP = 10;
            public int[,] MazeArray { get; private set; }
            List<Point> CloseList;
            List<Point> OpenList;
    
            public Maze(int[,] maze)
            {
                this.MazeArray = maze;
                OpenList = new List<Point>(MazeArray.Length);
                CloseList = new List<Point>(MazeArray.Length);
            }
    
            public Point FindPath(Point start, Point end, bool IsIgnoreCorner)
            {
                OpenList.Add(start);
                while (OpenList.Count != 0)
                {
                    //找出F值最小的点
                    var tempStart = OpenList.MinPoint();
                    OpenList.RemoveAt(0);
                    CloseList.Add(tempStart);
                    //找出它相邻的点
                    var surroundPoints = SurrroundPoints(tempStart, IsIgnoreCorner);
                    foreach (Point point in surroundPoints)
                    {
                        if (OpenList.Exists(point))
                            //计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
                            FoundPoint(tempStart, point);
                        else
                            //如果它们不在开始列表里, 就加入, 并设置父节点,并计算GHF
                            NotFoundPoint(tempStart, end, point);
                    }
                    if (OpenList.Get(end) != null)
                        return OpenList.Get(end);
                }
                return OpenList.Get(end);
            }
    
            private void FoundPoint(Point tempStart, Point point)
            {
                var G = CalcG(tempStart, point);
                if (G < point.G)
                {
                    point.ParentPoint = tempStart;
                    point.G = G;
                    point.CalcF();
                }
            }
    
            private void NotFoundPoint(Point tempStart, Point end, Point point)
            {
                point.ParentPoint = tempStart;
                point.G = CalcG(tempStart, point);
                point.H = CalcH(end, point);
                point.CalcF();
                OpenList.Add(point);
            }
    
            private int CalcG(Point start, Point point)
            {
                int G = (Math.Abs(point.X - start.X) + Math.Abs(point.Y - start.Y)) == 2 ? STEP : OBLIQUE;
                int parentG = point.ParentPoint != null ? point.ParentPoint.G : 0;
                return G + parentG;
            }
    
            private int CalcH(Point end, Point point)
            {
                int step = Math.Abs(point.X - end.X) + Math.Abs(point.Y - end.Y);
                return step * STEP;
            }
    
            //获取某个点周围可以到达的点
            public List<Point> SurrroundPoints(Point point, bool IsIgnoreCorner)
            {
                var surroundPoints = new List<Point>(9);
    
                for (int x = point.X - 1; x <= point.X + 1; x++)
                    for (int y = point.Y - 1; y <= point.Y + 1; y++)
                    {
                        if (CanReach(point, x, y, IsIgnoreCorner))
                            surroundPoints.Add(x, y);
                    }
                return surroundPoints;
            }
    
            //在二维数组对应的位置不为障碍物
            private bool CanReach(int x, int y)
            {
                return MazeArray[x, y] == 0;
            }
    
            public bool CanReach(Point start, int x, int y, bool IsIgnoreCorner)
            {
                if (!CanReach(x, y) || CloseList.Exists(x, y))
                    return false;
                else
                {
                    if (Math.Abs(x - start.X) + Math.Abs(y - start.Y) == 1)
                        return true;
                    //如果是斜方向移动, 判断是否 "拌脚"
                    else
                    {
                        if (CanReach(Math.Abs(x - 1), y) && CanReach(x, Math.Abs(y - 1)))
                            return true;
                        else
                            return IsIgnoreCorner;
                    }
                }
            }
        }
    
        //Point 类型
        public class Point
        {
            public Point ParentPoint { get; set; }
            public int F { get; set; }  //F=G+H
            public int G { get; set; }
            public int H { get; set; }
            public int X { get; set; }
            public int Y { get; set; }
    
            public Point(int x, int y)
            {
                this.X = x;
                this.Y = y;
            }
            public void CalcF()
            {
                this.F = this.G + this.H;
            }
        }
    
        //对 List<Point> 的一些扩展方法
        public static class ListHelper
        {
            public static bool Exists(this List<Point> points, Point point)
            {
                foreach (Point p in points)
                    if ((p.X == point.X) && (p.Y == point.Y))
                        return true;
                return false;
            }
    
            public static bool Exists(this List<Point> points, int x, int y)
            {
                foreach (Point p in points)
                    if ((p.X == x) && (p.Y == y))
                        return true;
                return false;
            }
    
            public static Point MinPoint(this List<Point> points)
            {
                points = points.OrderBy(p => p.F).ToList();
                return points[0];
            }
            public static void Add(this List<Point> points, int x, int y)
            {
                Point point = new Point(x, y);
                points.Add(point);
            }
    
            public static Point Get(this List<Point> points, Point point)
            {
                foreach (Point p in points)
                    if ((p.X == point.X) && (p.Y == point.Y))
                        return p;
                return null;
            }
    
            public static void Remove(this List<Point> points, int x, int y)
            {
                foreach (Point point in points)
                {
                    if (point.X == x && point.Y == y)
                        points.Remove(point);
                }
            }
        }
    }

![20181113181926373.png][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70]: /images/20210725/98c57cedd1044d0d92f7ee2532b56023.png
[20181113175428744.jpeg]: /images/20210725/ad60159580eb4fff9529aac9361e7196.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 1]: /images/20210725/7649d6ec8e4a48b0b6cc51e0af04df58.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 2]: /images/20210725/a068725572fb4ee0b735c881f323dfdf.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 3]: /images/20210725/3b70d5403a3e4e51870cbf8b07b39180.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwMzIzMjU2_size_16_color_FFFFFF_t_70 4]: /images/20210725/adae28761ac144b4bc1470b17c8a3b26.png
[20181113182032728.png]: /images/20210725/3f01325f29384a7c8e22e33d569d9628.png
[20181113181926373.png]: /images/20210725/02c12ea243fe4ee0b1ddcf2eabc130f9.png