public AStarSearch(Battlefield graph, Cell start, Cell goal) { _start = start; _goal = goal; var frontier = new PriorityQueue <Cell>(); frontier.Enqueue(start, 0); cameFrom[start] = start; costSoFar[start] = 0; while (frontier.Count > 0) { var current = frontier.Dequeue(); if (current.Equals(goal)) { break; } foreach (var next in graph.Neighbors(current)) { double newCost = costSoFar[current] + 1; if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { costSoFar[next] = newCost; double priority = newCost + Cell.Distance(next.position, goal.position); frontier.Enqueue(next, priority); cameFrom[next] = current; } } } }