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); }