fork download
copy 
  1. #include<bits/stdc++.h>
  2. using namespace std;
  3.  
  4. // Định nghĩa kích thước lưới
  5. #define ROW 9
  6. #define COL 10
  7.  
  8. // Cấu trúc tọa độ (Point)
  9. typedef pair<int, int> Pair;
  10.  
  11. // Cấu trúc lưu trữ thông tin của một ô (Node)
  12. struct cell {
  13.     // Tọa độ cha (parent) để truy vết
  14.     // pair<int, int> parent;
  15.     int parent_i, parent_j;
  16.     // f = g + h
  17.     double f, g, h;
  18. };
  19.  
  20. // --- Các hàm hỗ trợ ---
  21.  
  22. // Kiểm tra xem ô (row, col) có nằm trong lưới không
  23. bool isValid(int row, int col) {
  24.     return (row >= 0) && (row < ROW) && (col >= 0) && (col < COL);
  25. }
  26.  
  27. // Kiểm tra xem ô có phải là tường (bị chặn) không
  28. bool isUnBlocked(const vector<vector<int>>& grid, int row, int col) {
  29.     // 1 = bị chặn, 0 = không bị chặn
  30.     return grid[row][col] == 0;
  31. }
  32.  
  33. // Kiểm tra xem có phải là ô đích không
  34. bool isDestination(int row, int col, Pair dest) {
  35.     return (row == dest.first && col == dest.second);
  36. }
  37.  
  38. // Tính Heuristic (h) - dùng khoảng cách Manhattan
  39. double calculateHValue(int row, int col, Pair dest) {
  40.     // return sqrt(pow(row - dest.first, 2) + pow(col - dest.second, 2)); // Euclidean
  41.     return abs(row - dest.first) + abs(col - dest.second); // Manhattan
  42. }
  43.  
  44. // Truy vết đường đi từ đích về
  45. void tracePath(const vector<vector<cell>>& cellDetails, Pair dest) {
  46.     cout << "Đường đi là: " << endl;
  47.     stack<Pair> path;
  48.  
  49.     int row = dest.first;
  50.     int col = dest.second;
  51.  
  52.     while (!(cellDetails[row][col].parent_i == row && cellDetails[row][col].parent_j == col)) {
  53.         path.push({row, col});
  54.         int temp_row = cellDetails[row][col].parent_i;
  55.         int temp_col = cellDetails[row][col].parent_j;
  56.         row = temp_row;
  57.         col = temp_col;
  58.     }
  59.  
  60.     path.push({row, col}); // Thêm nút Start
  61.  
  62.     while (!path.empty()) {
  63.         Pair p = path.top();
  64.         path.pop();
  65.         cout << "-> (" << p.first << ", " << p.second << ") ";
  66.     }
  67.     cout << endl;
  68. }
  69.  
  70. // --- Hàm A* chính ---
  71.  
  72. // Kiểu dữ liệu cho Hàng đợi ưu tiên (OPEN list)
  73. // pair<f_score, pair<x, y>>
  74. typedef pair<double, Pair> pPair;
  75.  
  76. void aStarSearch(const vector<vector<int>>& grid, Pair src, Pair dest) {
  77.  
  78.     // 1. Kiểm tra tính hợp lệ của Start và Goal
  79.     if (!isValid(src.first, src.second) || !isValid(dest.first, dest.second)) {
  80.         cout << "Start hoặc Goal không hợp lệ" << endl;
  81.         return;
  82.     }
  83.  
  84.     if (!isUnBlocked(grid, src.first, src.second) || !isUnBlocked(grid, dest.first, dest.second)) {
  85.         cout << "Start hoặc Goal bị chặn" << endl;
  86.         return;
  87.     }
  88.  
  89.     if (isDestination(src.first, src.second, dest)) {
  90.         cout << "Chúng ta đã ở đích" << endl;
  91.         return;
  92.     }
  93.  
  94.     // 2. Khởi tạo
  95.     // Mảng 2D lưu chi tiết các ô
  96.     vector<vector<cell>> cellDetails(ROW, vector<cell>(COL));
  97.  
  98.     // Mảng 2D cho CLOSE list (đã xử lý hay chưa)
  99.     vector<vector<bool>> closedList(ROW, vector<bool>(COL, false));
  100.  
  101.     // Khởi tạo tất cả các ô
  102.     for (int i = 0; i < ROW; ++i) {
  103.         for (int j = 0; j < COL; ++j) {
  104.             cellDetails[i][j].f = FLT_MAX;
  105.             cellDetails[i][j].g = FLT_MAX;
  106.             cellDetails[i][j].h = FLT_MAX;
  107.             cellDetails[i][j].parent_i = -1;
  108.             cellDetails[i][j].parent_j = -1;
  109.         }
  110.     }
  111.  
  112.     // Khởi tạo ô bắt đầu
  113.     int i = src.first, j = src.second;
  114.     cellDetails[i][j].g = 0.0;
  115.     cellDetails[i][j].h = calculateHValue(i, j, dest);
  116.     cellDetails[i][j].f = cellDetails[i][j].g + cellDetails[i][j].h;
  117.     cellDetails[i][j].parent_i = i;
  118.     cellDetails[i][j].parent_j = j;
  119.  
  120.     // Hàng đợi ưu tiên (OPEN list)
  121.     // Sắp xếp theo f tăng dần (min-heap)
  122.     priority_queue<pPair, vector<pPair>, greater<pPair>> openList;
  123.  
  124.     // Thêm nút bắt đầu vào OPEN list
  125.     openList.push({cellDetails[i][j].f, {i, j}});
  126.  
  127.     bool foundDest = false;
  128.  
  129.     // 3. Vòng lặp chính
  130.     while (!openList.empty()) {
  131.         // Lấy nút có f thấp nhất
  132.         pPair p = openList.top();
  133.         openList.pop();
  134.  
  135.         // Lấy tọa độ
  136.         i = p.second.first;
  137.         j = p.second.second;
  138.  
  139.         // Đánh dấu là đã xử lý
  140.         closedList[i][j] = true;
  141.  
  142.         // 4. Duyệt các hàng xóm (4 hướng)
  143.         // (Thay đổi để duyệt 8 hướng nếu cần)
  144.         int dRow[] = {-1, 1, 0, 0};
  145.         int dCol[] = {0, 0, 1, -1};
  146.  
  147.         for (int k = 0; k < 4; ++k) {
  148.             int n_i = i + dRow[k];
  149.             int n_j = j + dCol[k];
  150.  
  151.             // Nếu hàng xóm hợp lệ
  152.             if (isValid(n_i, n_j)) {
  153.                 // 5. Kiểm tra Đích
  154.                 if (isDestination(n_i, n_j, dest)) {
  155.                     // Đặt cha cho ô đích và truy vết
  156.                     cellDetails[n_i][n_j].parent_i = i;
  157.                     cellDetails[n_i][n_j].parent_j = j;
  158.                     cout << "Tìm thấy đường đi!" << endl;
  159.                     tracePath(cellDetails, dest);
  160.                     foundDest = true;
  161.                     return; // Thoát hàm
  162.                 }
  163.  
  164.                 // Nếu hàng xóm chưa xử lý VÀ không bị chặn
  165.                 else if (!closedList[n_i][n_j] && isUnBlocked(grid, n_i, n_j)) {
  166.  
  167.                     // 6. Tính toán chi phí
  168.                     double gNew = cellDetails[i][j].g + 1.0; // Chi phí mỗi bước là 1
  169.                     double hNew = calculateHValue(n_i, n_j, dest);
  170.                     double fNew = gNew + hNew;
  171.  
  172.                     // 7. Cập nhật đường đi tốt hơn
  173.                     if (cellDetails[n_i][n_j].f == FLT_MAX || cellDetails[n_i][n_j].f > fNew) {
  174.  
  175.                         // Thêm vào OPEN list
  176.                         openList.push({fNew, {n_i, n_j}});
  177.  
  178.                         // Cập nhật chi tiết
  179.                         cellDetails[n_i][n_j].f = fNew;
  180.                         cellDetails[n_i][n_j].g = gNew;
  181.                         cellDetails[n_i][n_j].h = hNew;
  182.                         cellDetails[n_i][n_j].parent_i = i;
  183.                         cellDetails[n_i][n_j].parent_j = j;
  184.                     }
  185.                 }
  186.             }
  187.         }
  188.     }
  189.  
  190.     // 8. Không tìm thấy đường
  191.     if (!foundDest) {
  192.         cout << "Không tìm thấy đường đi" << endl;
  193.     }
  194. }
  195.  
  196. // --- Hàm main để chạy ---
  197. int main() {
  198.     /* Bản đồ
  199.     0 = Đường đi
  200.     1 = Tường (Obstacle)
  201.     */
  202.     vector<vector<int>> grid = {
  203.         {0, 0, 1, 0, 0, 0, 0, 1, 0, 0},
  204.         {0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
  205.         {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
  206.         {0, 0, 0, 0, 1, 0, 0, 1, 0, 0},
  207.         {0, 0, 1, 1, 1, 0, 0, 1, 0, 0},
  208.         {0, 0, 0, 0, 1, 0, 0, 1, 0, 0},
  209.         {0, 0, 0, 0, 1, 0, 0, 1, 0, 0},
  210.         {0, 0, 0, 0, 1, 0, 0, 1, 0, 0},
  211.         {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
  212.     };
  213.  
  214.     // Điểm bắt đầu (Start)
  215.     Pair src = {8, 0};
  216.  
  217.     // Điểm kết thúc (Goal)
  218.     Pair dest = {0, 9};
  219.  
  220.     aStarSearch(grid, src, dest);
  221.  
  222.     return 0;
  223. }
  224.  
Success #stdin #stdout 0.01s 5320KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Tìm thấy đường đi!
Đường đi là: 
-> (8, 0) -> (8, 1) -> (8, 2) -> (8, 3) -> (8, 4) -> (8, 5) -> (7, 5) -> (6, 5) -> (5, 5) -> (4, 5) -> (3, 5) -> (2, 5) -> (2, 6) -> (2, 7) -> (2, 8) -> (1, 8) -> (0, 8) -> (0, 9)
https://ideone.com/EYbOsZ
language:
C++14 (gcc 8.3)
created:
1 day ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!