/// <summary>
/// Получаем все точки от конца до начала
/// </summary>
/// <param name="node">Узел от которого получаем весь путь</param>
/// <returns></returns>
public List <LinkedAPoint> GetPathForNode(LinkedPathNode node)
{
var path = new List <LinkedAPoint>();
while (node != null)
{
path.Add(node.Point);
// Получаем предшествующий узел, который может быть null
node = node.ComeFrom;
}
return(path);
}
/// <summary>
/// Получает всех соседей узла
/// </summary>
/// <param name="node">Текущий узел</param>
/// <param name="goal">Конечная точка</param>
public List <LinkedPathNode> GetNodeNeighbours(LinkedPathNode node, LinkedAPoint goal)
{
var result = new List <LinkedPathNode>();
var neighbours = new LinkedAPoint[4];
neighbours[0] = node.Point.Left;
neighbours[1] = node.Point.Right;
neighbours[2] = node.Point.Top;
neighbours[3] = node.Point.Bottom;
for (var i = 0; i < neighbours.Length; i++)
{
var point = neighbours[i];
if (point == null)
{
continue;
}
if (point.IsWalkable == false)
{
continue;
}
var neighbour = new LinkedPathNode()
{
Point = point,
ComeFrom = node,
PathLengthFromStart = node.PathLengthFromStart + GetDistanceBetweenNeighbours(),
HeuristicPathLength = GetHeuristicPathLength(point, goal)
};
result.Add(neighbour);
}
return(result);
}
/// <summary>
/// Поиск оптимального пути
/// </summary>
/// <param name="start">Стартовая точка</param>
/// <param name="goal">Конечная точка</param>
/// <exception cref="Exception">Появляется при больших рассчетах</exception>
public List <LinkedAPoint> FindPath(LinkedAPoint start, LinkedAPoint goal)
{
var closedSet = new List <LinkedPathNode>();
var openSet = new List <LinkedPathNode>();
var startNode = new LinkedPathNode()
{
Point = start,
ComeFrom = null,
PathLengthFromStart = 0,
HeuristicPathLength = GetHeuristicPathLength(start, goal)
};
openSet.Add(startNode);
int exception = 0;
while (openSet.Count > 0)
{
exception++;
if (exception > 10000)
{
throw new System.Exception("Поиск пути был обработан более 10_000 тысяч раз");
}
// Потенциально ближайший узел
var nearestNodeIndex = openSet.MinIndex(x
=> x.FullPathLength);
LinkedPathNode nearestNode = openSet[nearestNodeIndex];
// Если ближайший узел является нашей целью
if (goal.Equals(nearestNode.Point))
{
var path = GetPathForNode(nearestNode);
path.Reverse();
return(path);
}
// Убираем узел из открытого списка и помещаем в закрытый
openSet.Remove(nearestNode);
closedSet.Add(nearestNode);
// Получаем всех соседей
var neighbours = GetNodeNeighbours(nearestNode, goal);
for (var i = 0; i < neighbours.Count; i++)
{
var neighbourNode = neighbours[i];
// Если данный узел был рассмотрен ранее
if (closedSet.FirstOrDefault(x
=> x.Point == neighbourNode.Point) != null)
{
continue;
}
// Находится ли сосед в открытом списке
var coincidenceNode = openSet.FirstOrDefault(x
=> x.Point == neighbourNode.Point);
// Если сосед не был на рассмотрении (не существует в открытом списке)
if (coincidenceNode == null)
{
openSet.Add(neighbourNode);
}
else
{
// Если совпадение не было на рассмотрении, но предыдуший путь был больше текущего.
// Точка у coincidence и neighbour одна, но путь и рассчеты могут быть разными
if (coincidenceNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
{
coincidenceNode.ComeFrom = neighbourNode.ComeFrom;
coincidenceNode.PathLengthFromStart = neighbourNode.PathLengthFromStart;
}
}
}
}
// Пути не существует
return(new List <LinkedAPoint>());
}
