public List <Vector2Int> PathResult(Vector2Int start, Vector2Int end) { MinHeap <int, Vector2Int> pointsQueue = new MinHeap <int, Vector2Int>(); HashSet <Vector2Int> closedPoints = new HashSet <Vector2Int>(); Dictionary <Vector2Int, Vector2Int> backwardPointDict = new Dictionary <Vector2Int, Vector2Int>(); Dictionary <Vector2Int, int> distanceToPointDict = new Dictionary <Vector2Int, int>(); //Наверно можно backwardPoint и distanceToPoint объединить, или использовать вместо них массив двумерный. //Но, думаю, для тестового и так сойдет :) var heuristic = _graph2D.HeuristicWayCost(start, end); pointsQueue.Add(heuristic, start); distanceToPointDict[start] = 0; while (!pointsQueue.IsEmpty()) { var currentPoint = pointsQueue.Pop().m_value; var currentPointDistance = distanceToPointDict[currentPoint]; if (currentPoint == end) { var result = new List <Vector2Int>(); var backwardPoint = end; while (backwardPoint != start) { result.Add(backwardPoint); backwardPoint = backwardPointDict[backwardPoint]; } result.Reverse(); return(result); } closedPoints.Add(currentPoint); foreach (var neighbor in _graph2D.GetNeighbors(currentPoint)) { if (closedPoints.Contains(neighbor)) { continue; } var distanceToNeighbor = currentPointDistance + _graph2D.WayCost(currentPoint, neighbor); if (!distanceToPointDict.TryGetValue(neighbor, out int currentDistanceToNeighbor) || distanceToNeighbor < currentDistanceToNeighbor) { backwardPointDict[neighbor] = currentPoint; distanceToPointDict[neighbor] = distanceToNeighbor; var fValue = distanceToNeighbor + _graph2D.HeuristicWayCost(currentPoint, end); pointsQueue.Add(fValue, neighbor); //В этом месте может случится так, что neighbor продублируется в pointsQueue. //Поэтому нужно усовершенствовать MinHeap. добавить метод, который будет изменять вес у уже добавленного элемента и обновлять структуру MinHeap, //чтобы сохранялись свойства двоичной кучи. //Но в нашем конкретном случае этого не случится, потому что функции WayCost и HeuristicWayCost являются консистентными. //Из-за этого условие (distanceToNeighbor < currentDistanceToNeighbor) всегда будет == False. //Для тестового решил не делать :) } } } return(null); }