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