fork download
  1. #include <bits/stdc++.h>
  2. using namespace std;
  3.  
  4. struct Node {
  5. int x, y;
  6. int g, h;
  7. Node(int _x, int _y, int _g, int _h) {
  8. x = _x; y = _y; g = _g; h = _h;
  9. }
  10. int f() const { return g + h; }
  11. };
  12.  
  13. // Hàm so sánh để priority_queue lấy node có f() nhỏ nhất
  14. struct cmp {
  15. bool operator()(const Node& a, const Node& b) {
  16. return a.f() > b.f();
  17. }
  18. };
  19.  
  20. int n, m;
  21. int sx, sy, tx, ty; // start, target
  22. int a[105][105]; // bản đồ: 0 đi được, 1 là vật cản
  23. bool visited[105][105];
  24.  
  25. int dx[] = {1, -1, 0, 0};
  26. int dy[] = {0, 0, 1, -1};
  27.  
  28. int heuristic(int x, int y) {
  29. return abs(x - tx) + abs(y - ty); // Manhattan
  30. }
  31.  
  32. int Astar() {
  33. priority_queue<Node, vector<Node>, cmp> pq;
  34. pq.push(Node(sx, sy, 0, heuristic(sx, sy)));
  35.  
  36. while (!pq.empty()) {
  37. Node cur = pq.top(); pq.pop();
  38. int x = cur.x, y = cur.y;
  39.  
  40. if (visited[x][y]) continue;
  41. visited[x][y] = true;
  42.  
  43. // đến đích
  44. if (x == tx && y == ty)
  45. return cur.g;
  46.  
  47. // duyệt 4 hướng
  48. for (int k = 0; k < 4; k++) {
  49. int nx = x + dx[k];
  50. int ny = y + dy[k];
  51.  
  52. if (nx < 0 || ny < 0 || nx >= n || ny >= m) continue;
  53. if (a[nx][ny] == 1) continue;
  54. if (!visited[nx][ny]) {
  55. int ng = cur.g + 1;
  56. int nh = heuristic(nx, ny);
  57. pq.push(Node(nx, ny, ng, nh));
  58. }
  59. }
  60. }
  61. return -1; // không tìm được đường
  62. }
  63.  
  64. int main() {
  65. cin >> n >> m;
  66. cin >> sx >> sy >> tx >> ty;
  67.  
  68. for (int i = 0; i < n; i++)
  69. for (int j = 0; j < m; j++)
  70. cin >> a[i][j];
  71.  
  72. int res = Astar();
  73. if (res == -1) cout << "Khong tim duoc duong\n";
  74. else cout << "Do dai duong di ngan nhat: " << res << "\n";
  75. }
  76.  
Success #stdin #stdout 0.01s 5296KB
stdin
Standard input is empty
stdout
Do dai duong di ngan nhat: 0