/// <summary>
/// Получения маршрута. Маршрут представлен в виде списка координат точек.
/// </summary>
/// <param name="pathNode"></param>
/// <returns></returns>
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;
}
/// <summary>
/// Создаем класс для вычисления маршрута. Основной метод вычисления маршрута будет выглядеть так:
/// </summary>
/// <param name="field"></param>
/// <param name="startCorner"></param>
/// <param name="goalCorner"></param>
/// <returns></returns>
public static List<Point> FindPath(Map map, Point startCorner, Point goalCorner)
{
// Шаг 1. Создается 2 списка вершин – ожидающие рассмотрения и уже рассмотренные.
// В ожидающие добавляется точка старта, список рассмотренных пока пуст.
var closedSet = new Collection<PathNode>();
var openSet = new Collection<PathNode>();
// Шаг 2. Для каждой точки рассчитывается F = G + H. G – расстояние от старта
// до точки, H – примерное расстояние от точки до цели. Так же каждая
// точка хранит ссылку на точку, из которой в нее пришли.
PathNode startNode = new PathNode()
{
Position = startCorner,
CameFrom = null,
PathLengthFromStart = 0,
HeuristicEstimatePathLength = GetHeuristicPathLength(startCorner, goalCorner)
};
openSet.Add(startNode);
while (openSet.Count > 0)
{
// Шаг 3. Из списка точек на рассмотрение выбирается точка X с наименьшим F.
var currentNode = openSet.OrderBy(node =>
node.EstimateFullPathLength).First();
// Шаг 4. Если точка X – цель, то мы нашли маршрут.
if (currentNode.Position == goalCorner)
return GetPathForNode(currentNode);
// Шаг 5. Переносим точку X из списка ожидающих рассмотрения в список уже
// рассмотренных.
openSet.Remove(currentNode);
closedSet.Add(currentNode);
// Шаг 6. Для каждой из точек, соседних для X (обозначим эту соседнюю точку Y),
// делаем следующее
foreach (var neighbourNode in GetNeighbours(currentNode, goalCorner, map))
{
// Шаг 7. Если Y уже находится в рассмотренных – пропускаем ее.
if (closedSet.Count(node => node.Position == neighbourNode.Position) > 0)
continue;
var openNode = openSet.FirstOrDefault(node =>
node.Position == neighbourNode.Position);
// Шаг 8. Если Y еще нет в списке на ожидание – добавляем ее туда, запомнив ссылку
// на X и рассчитав Y.G (это X.G + расстояние от X до Y) и Y.H.
if (openNode == null)
openSet.Add(neighbourNode);
else
if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
{
// Шаг 9. Если же Y в списке на рассмотрение – проверяем, если X.G + расстояние
// от X до Y < Y.G, значит мы пришли в точку Y более коротким путем, заменяем
// Y.G на X.G + расстояние от X до Y, а точку, из которой пришли в Y на X.
openNode.CameFrom = currentNode;
openNode.PathLengthFromStart = neighbourNode.PathLengthFromStart;
}
}
}
// Шаг 10. Если список точек на рассмотрение пуст, а до цели мы так и не дошли – значит маршрут
// не существует.
return null;
}
/// <summary>
/// Получение списка соседей для точки:
/// </summary>
/// <param name="pathNode"></param>
/// <param name="goalCorner"></param>
/// <param name="field"></param>
/// <returns></returns>
private static Collection<PathNode> GetNeighbours(PathNode pathNode, Point goalCorner, Map map)
{
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 >= map.Columns)
continue;
if (point.Y < 0 || point.Y >= map.Rows)
continue;
// Проверяем, что по клетке можно ходить.
if (map.IsCornerBusy(point))
continue;
// Заполняем данные для точки маршрута.
var neighbourNode = new PathNode()
{
Position = point,
CameFrom = pathNode,
PathLengthFromStart = pathNode.PathLengthFromStart +
GetDistanceBetweenNeighbours(),
HeuristicEstimatePathLength = GetHeuristicPathLength(point, goalCorner)
};
result.Add(neighbourNode);
}
return result;
}
