private static IList <Point> ReconstructPath(Point goal, ExpansionMatrixConteiner expansionMatrixConteiner, IEnumerable <Point> allPoints)
{
var path = new List <Point>()
{
goal
};
var currentPoint = goal;
while (expansionMatrixConteiner.ExpansionMatrix[currentPoint] > 0)
{
Point closestNeighbour = null;
var minCost = double.MaxValue;
foreach (var neihgbour in currentPoint.GetNeighbors(allPoints))
{
if (expansionMatrixConteiner.ExpansionMatrix[neihgbour] < minCost)
{
minCost = expansionMatrixConteiner.ExpansionMatrix[neihgbour];
closestNeighbour = neihgbour;
}
}
currentPoint = closestNeighbour;
path.Add(closestNeighbour);
}
return(path);
}
/// <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);
}