A* Pathfinding — Dijkstra + Heuristic

PriorityQueue pq = new PriorityQueue<>((a, b) -> a.f - b.f);
Map gScore = new HashMap<>();
gScore.put(start, 0);
pq.offer(new Node(start, heuristic(start, goal)));
while (!pq.isEmpty()) {
  Node cur = pq.poll();
  if (cur.pos == goal) return reconstruct(cur);
  for (Node next : neighbors(cur)) {
    int tentativeG = gScore.get(cur.pos) + dist(cur, next);
    if (tentativeG < gScore.getOrDefault(next.pos, INF)) {
      gScore.put(next.pos, tentativeG);
      next.f = tentativeG + heuristic(next.pos, goal);
      pq.offer(next);
    }
  }
}