/// <summary>
/// Восстановление оптимального пути
/// </summary>
/// <param name="goal">Целевая точка</param>
/// <returns>Оптимальный путь до целевой точки</returns>
private static IList <APoint> ReconstructPath(APoint goal)
{
var resultList = new List <APoint>();
var currentPoint = goal;
while (currentPoint != null)
{
resultList.Add(currentPoint);
currentPoint = currentPoint.CameFromAPoint;
}
resultList.Reverse();
return(resultList);
}
/// <summary>
/// Поиск точки с минимальной эврестической функцией (F)
/// </summary>
/// <param name="points">Список точек</param>
/// <returns>Точка с минимальной эврестической функцией</returns>
private static APoint GetPointWithMinF(IEnumerable <APoint> points)
{
if (!points.Any())
{
throw new Exception("Пустой список точек");
}
var minF = double.MaxValue;
APoint resultAPoint = null;
foreach (var point in points)
{
if (point.F < minF)
{
minF = point.F;
resultAPoint = point;
}
}
return(resultAPoint);
}
/// <summary>
/// Расчет матрицы распространения
/// </summary>
/// <param name="start">Точка, для которой рассчитывается матрица распространения</param>
/// <param name="goal">
/// Целевая точка. Если null, то матрица распространения рассчитывается от стартовой точки до всех
/// остальных точек сети
/// </param>
/// <param name="allPoints">Все точки сети</param>
/// <returns>Матрица распространения</returns>
private static ExpansionMatrixConteiner GetExpansionMatrix(APoint start, APoint goal, IEnumerable <APoint> allPoints)
{
foreach (var point in allPoints)
{
point.CameFromAPoint = null;
}
var emc = new ExpansionMatrixConteiner
{
ExpansionMatrix = new Dictionary <APoint, double>()
//Path = new Dictionary<APoint, IList<APoint>>()
};
var closedSet = new HashSet <APoint>();
var openSet = new HashSet <APoint> {
start
};
start.G = 0d;
start.H = goal == null ? 0d : start.GetHeuristicCost(goal);
var pathFound = false;
while (openSet.Count > 0)
{
var x = GetPointWithMinF(openSet);
if (goal != null && x == goal)
{
pathFound = true;
break;
}
openSet.Remove(x);
closedSet.Add(x);
emc.ExpansionMatrix.Add(x, x.G);
//emc.Path.Add(x, ReconstructPath(x));
var neighbors = x.GetNeighbors(allPoints);
foreach (var y in neighbors)
{
if (closedSet.Contains(y))
{
continue;
}
var tentativeGScore = x.G + x.GetCost(y);
bool tentativeIsBetter;
if (!openSet.Contains(y))
{
openSet.Add(y);
tentativeIsBetter = true;
}
else
{
tentativeIsBetter = tentativeGScore < y.G;
}
if (tentativeIsBetter)
{
y.CameFromAPoint = x;
y.G = tentativeGScore;
y.H = goal == null ? 0d : y.GetHeuristicCost(goal);
}
}
}
if (goal != null && !pathFound)
{
throw new Exception("Путь до конечной точки не найден");
}
return(emc);
}
/// <summary>
/// Расчет оптимального пути до целевой точки
/// </summary>
/// <param name="start">Стартовая точка пути</param>
/// <param name="goal">Целевая точка пути</param>
/// <param name="allPoints">Все точки сети</param>
/// <returns>Оптимальный путь от стартовой точки до целевой</returns>
public static IList <APoint> GetPath(APoint start, APoint goal, IEnumerable <APoint> allPoints)
{
GetExpansionMatrix(start, goal, allPoints);
return(ReconstructPath(goal));
}