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($"路径已找到: \n {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