static void Main(string[] args)
{
WeightedGraph weightedGraph = new WeightedGraph();
weightedGraph.readArcsFromFile("test.txt"); // считываем узлы и веса с файла
weightedGraph.showSquareGraph(); // вывод весов
Node start = new Node(0, 0);
Node finish = new Node(4, 4);
AStar alg = new AStar();
List <Node> path = alg.findPath(start, finish, weightedGraph);
weightedGraph.ShowPath(path, start, finish);
foreach (var step in path)
{
Console.WriteLine("x: " + step.x + " y: " + step.y);
}
}
public List <Node> findPath(Node startNode, Node finishNode, WeightedGraph graph)
{
Dictionary <Node, int> pricesToVisitedNodes = new Dictionary <Node, int>();
Dictionary <Node, Node> paths = new Dictionary <Node, Node>();
QueueWithPriority queue = new QueueWithPriority();
queue.Put(startNode, 0);
pricesToVisitedNodes.Add(startNode, 0);
while (!queue.IsEmpty())
{
Node current = queue.PopMostPriorityElement();
if (finishNode.Equals(current))
{
break;
}
foreach (var neighbor in graph.GetNeighbors(current))
{
int costToNeighbor = pricesToVisitedNodes[current] + graph.getWeight(current, neighbor);
bool condition = false;
// если соседний узел еще не посещался или новая цена перехода из старта до него меньше чем была, то
if (!pricesToVisitedNodes.ContainsKey(neighbor))
{
condition = true;
}
else
if (pricesToVisitedNodes[neighbor] > costToNeighbor)
{
condition = true;
}
if (condition)
{
// обновляем цену перехода до соседнего узла
pricesToVisitedNodes[neighbor] = costToNeighbor;
queue.Put(neighbor, costToNeighbor + heuristic(current, neighbor));
if (!paths.ContainsKey(neighbor))
{
paths.Add(neighbor, current);
}
else
{
paths[neighbor] = current;
}
}
}
}
// выбираем цепочку , которая привела к цели
Node next = finishNode;
List <Node> path = new List <Node>();
path.Add(next);
while (!startNode.Equals(next))
{
next = paths[next];
path.Add(next);
}
return(path);
}
public Queue <I> Solve <I>(WeightedGraph <T> Graph, System.Func <T, I> ReturnConverter, T Start, T Goal, bool IncludeStart = false, bool IncludeGoal = false, int MaxIterations = int.MaxValue)
{
Dictionary <T, T> CameFrom = new Dictionary <T, T>(); // initialize dictionarys
Dictionary <T, float> CostSoFar = new Dictionary <T, float>(); // initialize dictionaries
PriorityQueue <AStarNode <T> > Frontier = new PriorityQueue <AStarNode <T> > ();
Frontier.Insert(new AStarNode <T>(Start, 0)); // insert start
CameFrom [Start] = Start;
CostSoFar [Start] = 0;
int IterationCount = 0;
bool FoundPath = false;
while (Frontier.Count > 0) // while there are still items in the frontier
{
T Current = Frontier.RemoveRoot().Data; // remove most prior item
if (EqualsGoal(Current, Goal))
{
Goal = Current; FoundPath = true; break;
} // found the goal
foreach (T Next in Graph.Neighbors(Current))
{
float NewCost = CostSoFar [Current] + Graph.Cost(Current, Next); // calculate the new cost of the next node. Cost of last item + cost to go from last to next
if (!CostSoFar.ContainsKey(Next) || NewCost < CostSoFar [Next]) // if next item isnt already in the cost so far dictionary or new cost is lower the prev cost
{
CostSoFar [Next] = NewCost; // update the new cost
float priority = NewCost + Heuristic(Next, Goal); // new priority is Newcost + distance between next and goal (AKA heuristic)
Frontier.Insert(new AStarNode <T>(Next, priority)); // add item to the frontiers
CameFrom [Next] = Current; // the parent of next is the last one
}
}
// max iterations
IterationCount++;
if (IterationCount >= MaxIterations)
{
return(null);
} // could not find path. Timed out
}
if (FoundPath) // there is a path found
// build back path
{
Queue <I> Path = new Queue <I> ();
T current = Goal;
Path.Enqueue(ReturnConverter(current));
int count = 0; // iteration count
while (!Equals(current, Start) && count < CameFrom.Count)
{
current = CameFrom [current];
if (!Equals(current, Start))
{
Path.Enqueue(ReturnConverter(current));
}
count++; // add up iteration count
}
if (!Equals(current, Start))
{
return(null);
} // if current isnt equal to the start. No path is found
if (IncludeStart)
{
Path.Enqueue(ReturnConverter(Start));
} // include start
return(Path); // return valid path
}
return(null);
}
