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