Реализация алгоритма поиска А* на C#
Поиск пути из точки А в точку Б — одна из самых распространенных задач при разработке игр. Для решения этой задачи есть множество алгоритмов, но самым часто используемым является A* (A star). Ему и посвящен сегодняшний пост.
Алгоритм.
- Создается 2 списка вершин — ожидающие рассмотрения и уже рассмотренные. В ожидающие добавляется точка старта, список рассмотренных пока пуст.
- Для каждой точки рассчитывается F = G + H. G — расстояние от старта до точки, H — примерное расстояние от точки до цели. О расчете этой величины я расскажу позднее. Так же каждая точка хранит ссылку на точку, из которой в нее пришли.
- Из списка точек на рассмотрение выбирается точка с наименьшим F. Обозначим ее X.
- Если X — цель, то мы нашли маршрут.
- Переносим X из списка ожидающих рассмотрения в список уже рассмотренных.
- Для каждой из точек, соседних для X (обозначим эту соседнюю точку Y), делаем следующее:
- Если Y уже находится в рассмотренных — пропускаем ее.
- Если Y еще нет в списке на ожидание — добавляем ее туда, запомнив ссылку на X и рассчитав Y.G (это X.G + расстояние от X до Y) и Y.H.
- Если же Y в списке на рассмотрение — проверяем, если X.G + расстояние от X до Y < Y.G, значит мы пришли в точку Y более коротким путем, заменяем Y.G на X.G + расстояние от X до Y, а точку, из которой пришли в Y на X.
- Если список точек на рассмотрение пуст, а до цели мы так и не дошли — значит маршрут не существует.
Функция примерной оценки расстояния до цели.
Эта функция должна выполнять несколько условий:
- Функция никогда не переоценивает расстояние до цели.
- Для это функции расстояния выполняется неравенство треугольника. Поясню подробнее: предположим у нас есть три точки — A, B и C. Для путей A-B B-C и A-C должно быть верно следующее неравенство: A-B + B-C >= A-C.
Реализация.
Я реализовал алгоритм для прямоугольной карты, состоящей из клеток.
Для начала создаем класс точки:
public class PathNode { // Координаты точки на карте. public Point Position { get; set; } // Длина пути от старта (G). public int PathLengthFromStart { get; set; } // Точка, из которой пришли в эту точку. public PathNode CameFrom { get; set; } // Примерное расстояние до цели (H). public int HeuristicEstimatePathLength { get; set; } // Ожидаемое полное расстояние до цели (F). public int EstimateFullPathLength { get { return this.PathLengthFromStart + this.HeuristicEstimatePathLength; } } }
Описывать код не буду, комментариев достаточно. Идем дальше.
Создаем класс для вычисления маршрута. Основной метод вычисления маршрута будет выглядеть так:
public static List<Point> FindPath(int[,] field, Point start, Point goal) { // Шаг 1. var closedSet = new Collection<PathNode>(); var openSet = new Collection<PathNode>(); // Шаг 2. PathNode startNode = new PathNode() { Position = start, CameFrom = null, PathLengthFromStart = 0, HeuristicEstimatePathLength = GetHeuristicPathLength(start, goal) }; openSet.Add(startNode); while (openSet.Count > 0) { // Шаг 3. var currentNode = openSet.OrderBy(node => node.EstimateFullPathLength).First(); // Шаг 4. if (currentNode.Position == goal) return GetPathForNode(currentNode); // Шаг 5. openSet.Remove(currentNode); closedSet.Add(currentNode); // Шаг 6. foreach (var neighbourNode in GetNeighbours(currentNode, goal, field)) { // Шаг 7. if (closedSet.Count(node => node.Position == neighbourNode.Position) > 0) continue; var openNode = openSet.FirstOrDefault(node => node.Position == neighbourNode.Position); // Шаг 8. if (openNode == null) openSet.Add(neighbourNode); else if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart) { // Шаг 9. openNode.CameFrom = currentNode; openNode.PathLengthFromStart = neighbourNode.PathLengthFromStart; } } } // Шаг 10. return null; }
Осталось добавить несколько служебных функций.
Первая из них — функция расстояния от X до Y:
private static int GetDistanceBetweenNeighbours() { return 1; }
Расстояние между соседними клетками у меня всегда равно 1.
Функция примерной оценки расстояния до цели:
private static int GetHeuristicPathLength(Point from, Point to) { return Math.Abs(from.X - to.X) + Math.Abs(from.Y - to.Y); }
Для оценки расстояния я использую длину пути без препятствий.
Получение списка соседей для точки:
private static Collection<PathNode> GetNeighbours(PathNode pathNode, Point goal, int[,] field) { var result = new Collection<PathNode>(); // Соседними точками являются соседние по стороне клетки. Point[] neighbourPoints = new Point[4]; neighbourPoints[0] = new Point(pathNode.Position.X + 1, pathNode.Position.Y); neighbourPoints[1] = new Point(pathNode.Position.X - 1, pathNode.Position.Y); neighbourPoints[2] = new Point(pathNode.Position.X, pathNode.Position.Y + 1); neighbourPoints[3] = new Point(pathNode.Position.X, pathNode.Position.Y - 1); foreach (var point in neighbourPoints) { // Проверяем, что не вышли за границы карты. if (point.X < 0 || point.X >= field.GetLength(0)) continue; if (point.Y < 0 || point.Y >= field.GetLength(1)) continue; // Проверяем, что по клетке можно ходить. if ((field[point.X, point.Y] != 0) && (field[point.X, point.Y] != 1)) continue; // Заполняем данные для точки маршрута. var neighbourNode = new PathNode() { Position = point, CameFrom = pathNode, PathLengthFromStart = pathNode.PathLengthFromStart + GetDistanceBetweenNeighbours(), HeuristicEstimatePathLength = GetHeuristicPathLength(point, goal) }; result.Add(neighbourNode); } return result; }
Получения маршрута. Маршрут представлен в виде списка координат точек.
private static List<Point> GetPathForNode(PathNode pathNode) { var result = new List<Point>(); var currentNode = pathNode; while (currentNode != null) { result.Add(currentNode.Position); currentNode = currentNode.CameFrom; } result.Reverse(); return result; }
Именно для этого мы и хранили ссылки на точку, из которой пришли.
Чтобы переделать алгоритм на другой способ представления точек (например граф расстояний между городами), нужно переделать получение соседей и расчет расстояний.
Вот, что в итоге получилось:
Кстати квадратики уже умеют ходить и драться :-)