public static IList <AStarPoint> ReconstructPath(AStarPoint goal,
ExpansionMatrixContainer expansionMatrixContainer)
{
var path = new List <AStarPoint>()
{
goal
};
var currentPoint = goal;
while (expansionMatrixContainer.ExpansionMatrix[currentPoint] > 0)
{
AStarPoint closestNeighbour = null;
var minCost = double.MaxValue;
foreach (var neihgbour in currentPoint.GetNeighbors())
{
if (expansionMatrixContainer.ExpansionMatrix[neihgbour] < minCost)
{
minCost = expansionMatrixContainer.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 ExpansionMatrixContainer GetExpansionMatrix(AStarPoint start, AStarPoint goal,
IEnumerable <AStarPoint> allPoints)
{
foreach (var point in allPoints)
{
point.CameFromPoint = null;
}
var emc = new ExpansionMatrixContainer
{
ExpansionMatrix = new Dictionary <AStarPoint, double>(),
//Path = new Dictionary<Point, IList<Point>>()
};
var closedSet = new HashSet <AStarPoint>();
var openSet = new HashSet <AStarPoint> {
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();
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.CameFromPoint = x;
y.G = tentativeGScore;
y.H = goal == null ? 0d : y.GetHeuristicCost(goal);
}
}
}
if (goal != null && !pathFound)
{
throw new Exception("���� �� �������� ����� �� ������");
}
return(emc);
}
