22.C#写算法之A*寻路算法的实现

AStarSearch

每一个格子具有F、G、H这3个属性，如下图：

G：从起点走到当前格子的成本，也就是已经花费了多少步。

H：在不考虑障碍的情况下，从当前格子走到目标格子的距离，也就是离目标还有多远。

F：G和H的综合评估，也就是从起点到达当前格子，再从当前格子到达目标格子的总步数。

A星寻路算法代码如下：

using System;
using System.Collections.Generic;

namespace Test01
{
    class Grid {
        public int x;
        public int y;
        public int f;
        public int g;
        public int h;
        public Grid parent;

        public Grid(int x, int y){
            this.x = x;
            this.y = y;
        }
        public void InitGrid(Grid parent, Grid end)
        {
            this.parent = parent;
            if (parent != null)
                this.g = parent.g + 1;
            else
                this.g = 1;
            this.h = Math.Abs(this.x - end.x) + Math.Abs(this.y - end.y);
            this.f = this.g + this.h;
        }
    }

    class AStarPathFinding
    {
        public static int[,] MAZE = new int[,] {
            {0,0,0,0,0,0,0},
            {0,0,1,0,0,0,0},
            {0,0,1,0,0,0,0},
            {0,0,1,0,0,0,0},
            {0,0,1,0,0,0,0},
        };

        private static bool ContainGrid(List<Grid> grids, int x, int y)
        {
            foreach (var item in grids)
            {
                if (item.x == x && item.y == y)
                    return true;
            }
            return false;
        }

        private static bool IsValidGrid(int x, int y, List<Grid> openList, List<Grid> closeList)
        {
            //是否超过边界
            if (x < 0 || x >= MAZE.GetLength(0) || y < 0 || y >= MAZE.GetLength(1))
                return false;
            //是否有障碍物
            if (MAZE[x,y] == 1)
                return false;
            //是否已经在openList中
            if (ContainGrid(openList, x, y))
                return false;
            //是否已经在closeList中
            if (ContainGrid(closeList, x, y))
                return false;
            return true;
        }

        private static List<Grid> FindNeighbors(Grid grid, List<Grid> openList, List<Grid> closeList)
        {
            List<Grid> gridList = new List<Grid>();
            if (IsValidGrid(grid.x, grid.y - 1, openList, closeList))
                gridList.Add(new Grid(grid.x, grid.y - 1));
            if(IsValidGrid(grid.x,grid.y+1,openList,closeList))
                gridList.Add(new Grid(grid.x, grid.y + 1));
            if (IsValidGrid(grid.x - 1, grid.y, openList, closeList))
                gridList.Add(new Grid(grid.x - 1, grid.y));
            if (IsValidGrid(grid.x + 1, grid.y, openList, closeList))
                gridList.Add(new Grid(grid.x + 1, grid.y));
            return gridList;
        }

        private static Grid FindMinGrid(List<Grid> openList)
        {
            Grid tempGrid = openList[0];
            foreach (var item in openList)
            {
                if (item.f < tempGrid.f)
                    tempGrid = item;
            }
            return tempGrid;
        }

        /// <summary>
        /// A*寻路主逻辑
        /// </summary>
        /// <param name="star">迷宫起点</param>
        /// <param name="end">迷宫终点</param>
        /// <returns></returns>
        public static Grid AStarSearch(Grid start, Grid end)
        {
            List<Grid> openList = new List<Grid>();
            List<Grid> closeList = new List<Grid>();
            //把起点加入openList
            openList.Add(start);
            //主循环，每一轮检查1个当前方格节点
            while (openList.Count > 0)
            {
                //在openList中查找F值最小的节点，将其作为当前方格节点
                Grid currentGrid = FindMinGrid(openList);
                //将当前节点从openList中移除
                openList.Remove(currentGrid);
                //当前方格节点进入closeList
                closeList.Add(currentGrid);
                //找到所有邻近节点
                List<Grid> neighbors = FindNeighbors(currentGrid, openList, closeList);
                foreach (var item in neighbors)
                {
                    if (!openList.Contains(item))
                    {
                        //邻近节点不在openList中，标记“父节点”、G、H、F,并放入openList
                        item.InitGrid(currentGrid, end);
                        openList.Add(item);
                    }
                }
                //如果终点在openList中，直接返回终点格子
                foreach (var item in openList)
                {
                    if (item.x == end.x && item.y == end.y)
                        return item;
                }
            }
            //openList用尽，仍然找不到终点，说明终点不可到达，返回空
            return null;
        }
        

        static void Main(string[] args)
        {
            Console.WriteLine("Start");
            //设置起点和终点
            Grid starGrid = new Grid(2, 1);
            Grid endGrid = new Grid(2, 5);
            //搜索迷宫终点
            Grid resultGrid = AStarSearch(starGrid, endGrid);
            //回溯迷宫路径
            List<Grid> path = new List<Grid>();
            while (resultGrid != null)
            {
                path.Add(new Grid(resultGrid.x, resultGrid.y));
                resultGrid = resultGrid.parent;
            }
            //输出迷宫和路径，路径用*表示
            for (int i = 0; i < MAZE.GetLength(0); i++)
            {
                for (int j = 0; j < MAZE.GetLength(1); j++)
                {
                    if (ContainGrid(path, i, j))
                        Console.Write("*, ");
                    else
                        Console.Write(MAZE[i, j] + ", ");
                }
                Console.WriteLine();
            }
            Console.WriteLine();
        }
    }
}