A*寻路算法实现

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Linq;
namespace AStar
{
    class Program
    {
        class Point
        {
            public int x;
            public int y;
            public double h;
            public double g;
            public Point parent;
            public Point(int x, int y)
            {
                this.x = x;
                this.y = y;
            }

            public double F { get { return h + g; } }

        }
        public const int BOUND_X = 10;
        public const int BOUND_Y = 10;
        public static int[,] map = new int[BOUND_X, BOUND_Y] {
            {1, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            {0, 0, 3, 0, 0, 0, 0, 0, 0, 0 },
            {0, 0, 3, 0, 0, 0, 0, 0, 0, 0 },
            {3, 3, 0, 0, 3, 3, 0, 0, 0, 0 },
            {0, 0, 0, 0, 3, 3, 0, 0, 0, 0 },
            {0, 0, 0, 0, 3, 3, 3, 3, 3, 0 },
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            {0, 0, 0, 0, 0, 0, 0, 0, 2, 0 },
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
        };
        private static void InitGraph()
        {
            for (int xIndex = 0; xIndex < 10; xIndex++)
            {
                for (int yIndex = 0; yIndex < 10; yIndex++)
                {
                    pList[xIndex, yIndex] = new Point(xIndex, yIndex);
                }
            }
        }
        private static Point[,] pList = new Point[10, 10];
        static void Main(string[] args)
        {
            InitGraph();
            var start = pList[0, 0];
            var end = pList[9, 9];
            CalPath(start, end);
            Console.ReadLine();
        }
        /// <summary>
        /// 判断该点是否可以通过
        /// </summary>
        /// <param name="curPoint"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        static bool IsObstacle(Point curPoint, Point point) {
            if (3 == map[point.x, point.y]) {
                return true;
            }
            //对角
            if ((1 == Math.Abs(curPoint.x - point.x) && 1 == Math.Abs(curPoint.y - point.y)))
            {
                int maxPX = curPoint.x > point.x ? curPoint.x : point.x;
                int maxPY = curPoint.y > point.y ? curPoint.y : point.y;
                int minPX = curPoint.x > point.x ? point.x: curPoint.x;
                int minPY = curPoint.y > point.y ? point.y: curPoint.y;
                return 3 == map[minPX, maxPY] || 3 == map[maxPX, minPY];
            }
            return false;
        }
        static void CalPath(Point start, Point end)
        {
            List<Point> openList = new List<Point>();
            List<Point> closeList = new List<Point>();
            openList.Add(start);
            while (openList.Count > 0)
            {
                Point curPoint = openList[0];
                openList.RemoveAt(0);
                if (curPoint == end)
                {
                    Stack<Point> stack = new Stack<Point>();

                    StringBuilder strBuilder = new StringBuilder();
                    while (null != curPoint) {
                        stack.Push(curPoint);
                        curPoint = curPoint.parent;
                    }
                    bool first = true;
                    while (stack.Count > 0) {
                        var p = stack.Pop();
                        if (first)
                        {
                            first = false;
                            strBuilder.Append($"({p.x},{p.y})");
                        }
                        else {
                            strBuilder.Append($"->({p.x},{p.y})");
                        }
                    }
                    Console.WriteLine($"路径已找到: 
 {strBuilder}");
                    break;
                }
                //------获取当前点周围的八个点-------
                var roundPoints = GetRoundPoints(curPoint);
                foreach (var p in roundPoints)
                {
                    if (closeList.Contains(p) || IsObstacle(curPoint, p))
                    {
                        continue;
                    }
                    if (!openList.Contains(p))
                    {
                        openList.Add(p);
                        p.g = curPoint.g + Math.Sqrt(Math.Pow(curPoint.x - p.x, 2) + Math.Pow(curPoint.y - p.y, 2));
                        p.h = Math.Abs(start.x - end.x) + Math.Abs(start.y - end.y);
                        p.parent = curPoint;
                    }
                    else
                    {
                        var newG = curPoint.g + Math.Sqrt(Math.Pow(curPoint.x - p.x, 2) + Math.Pow(curPoint.y - p.y, 2));
                        if (p.g > newG)
                        {
                            p.g = newG;
                            p.parent = curPoint;
                        }
                    }
                }
                closeList.Add(curPoint);
                openList.Sort((a, b) =>
                {
                    if (a.F < b.F)
                    {
                        return -1;
                    }
                    else if (a.F > b.F)
                    {
                        return 1;
                    }
                    return 0;
                });
            }
        }
        static bool PointVaild(int x, int y)
        {
            return x >= 0 && x < BOUND_X && y >= 0 && y < BOUND_Y;
        }
        static List<Point> GetRoundPoints(Point point)
        {
            List<Point> list = new List<Point>();
            int xindex = point.x;
            int yindex = point.y;
            //左边三个
            int newXIndex = xindex - 1;
            int newYIndex = yindex - 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            newXIndex = xindex - 1;
            newYIndex = yindex;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            newXIndex = xindex - 1;
            newYIndex = yindex + 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            //中间两个
            newXIndex = xindex;
            newYIndex = yindex - 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            newXIndex = xindex;
            newYIndex = yindex + 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            //右边三个
            newXIndex = xindex + 1;
            newYIndex = yindex - 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            //右边三个
            newXIndex = xindex + 1;
            newYIndex = yindex;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            //右边三个
            newXIndex = xindex + 1;
            newYIndex = yindex + 1;
            if (PointVaild(newXIndex, newYIndex))
            {
                list.Add(pList[newXIndex, newYIndex]);
            }
            return list;
        }
    }
}

原理参照：https://www.jianshu.com/p/274bbb6598df