Thuật toán Dijkstra

1dist[start] = 0;  // Khoảng cách điểm đầu = 02pq.add(new int[]{0, start});  // Đưa điểm đầu vào hàng đợi ưu tiên3while (!pq.isEmpty()) {  // Đến khi hàng đợi rỗng4  int[] top = pq.poll(); int node = top[1];  // Lấy ô gần nhất5  if (settled[node]) continue;  // Bỏ qua bản trùng cũ6  settled[node] = true;  // Chốt khoảng cách của nó7  if (node == goal) return buildPath();  // Tới đích — truy vết đường8  for (int neighbor : neighbors(node)) {  // Duyệt các ô kề đi được9    int alt = dist[node] + cost(neighbor);  // Khoảng cách khi đi qua ô này10    if (alt < dist[neighbor]) {  // Tìm được đường ngắn hơn11      dist[neighbor] = alt; parent[neighbor] = node;  // Cập nhật và lưu cha12      pq.add(new int[]{alt, neighbor});  // Xếp lại với khóa mới13    }14  }15}16 17List<Integer> neighbors(int cell) {  // Các ô kề đi được18  int r = cell / COLS, c = cell % COLS;19  int[][] dirs = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};20  List<Integer> out = new ArrayList<>();21  for (int[] d : dirs) {22    int nr = r + d[0], nc = c + d[1];23    if (nr < 0 || nr >= ROWS || nc < 0 || nc >= COLS) continue;24    int nbr = nr * COLS + nc;25    if (!walls.contains(nbr)) out.add(nbr);26  }27  return out;28}29 30int cost(int cell) {  // Chi phí bước vào một ô31  return weights.contains(cell) ? WEIGHT_COST : 1;32}33 34List<Integer> buildPath() {  // Lần theo cha từ đích35  List<Integer> path = new ArrayList<>();36  for (int at = goal; at != -1; at = parent[at]) path.add(at);37  Collections.reverse(path);38  return path;39}