赞
踩
A* C++代码实现【AI自动编程】
#include "pch.h" #include "A_Start.h" #include <iostream> #include <vector> #include <queue> #include <cmath> // 定义节点类型 struct Node { int x; // 节点的 x 坐标 int y; // 节点的 y 坐标 double g; // 从起点到该节点的路径代价 double h; // 从该节点到目标节点的启发式估计代价 double f; // 该节点的估价函数值 Node* parent; // 指向该节点的父节点的指针 Node(int x_, int y_, double g_, double h_, Node* parent_ = nullptr) : x(x_), y(y_), g(g_), h(h_), f(g_ + h_), parent(parent_) {} }; // 定义比较函数,用于在 priority_queue 中比较节点的估价函数值 struct CompareNodes { bool operator()(Node* n1, Node* n2) const { return n1->f > n2->f; } }; // 定义 A* 算法函数 std::vector<Node*> astar(int start_x, int start_y, int goal_x, int goal_y, std::vector<std::vector<int>>& grid) { // 定义起点和终点节点 Node* start = new Node(start_x, start_y, 0, 0); Node* goal = new Node(goal_x, goal_y, 0, 0); // 定义 open 和 closed 列表 std::priority_queue<Node*, std::vector<Node*>, CompareNodes> open; std::vector<Node*> closed; // 将起点加入 open 列表 open.push(start); // 当 open 列表非空时循环 while (!open.empty()) { // 取出 open 列表中估价函数值最小的节点 Node* current = open.top(); open.pop(); // 如果该节点是目标节点,返回路径 if (current->x == goal->x && current->y == goal->y) { std::vector<Node*> path; while (current) { path.push_back(current); current = current->parent; } return path; } // 将该节点加入 closed 列表 closed.push_back(current); // 遍历当前节点的邻居 for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { // 跳过当前节点 if (i == 0 && j == 0) continue; // 计算邻居节点的坐标 int x = current->x + i; int y = current->y + j; // 如果邻居节点越界或为障碍物或已在 closed 列表中,跳过 if (x < 0 || x >= grid.size() || y < 0 || y >= grid[0].size() || grid[x][y] == 1) continue; bool in_closed = false; for (Node* n : closed) { if (n->x == x && n->y == y) { in_closed = true; break; } } if (in_closed) continue; // 计算从起点到该邻居节点的路径代价 double g = current->g + std::sqrt(i * i + j * j); // 如果邻居节点不在 open 列表中,或者新的路径代价更小 bool in_open = false; Node* neighbor = nullptr; for (Node* n : open._Get_container()) { if (n->x == x && n->y == y) { in_open = true; neighbor = n; break; } } if (!in_open || g < neighbor->g) { // 更新或插入邻居节点 double h = std::sqrt((x - goal->x) * (x - goal->x) + (y - goal->y) * (y - goal->y)); if (in_open) { open._Get_container().erase(std::find(open._Get_container().begin(), open._Get_container().end(), neighbor)); } Node* new_node = new Node(x, y, g, h, current); open.push(new_node); } } } } // 如果找不到路径,返回空向量 return std::vector<Node*>(); } // 测试 A* 算法函数 int main() { std::vector<std::vector<int>> grid = { {0, 0, 1, 0, 0}, {0, 0, 1, 0, 0}, {0, 0, 0, 0, 0}, {0, 0, 1, 0, 0}, {0, 0, 0, 0, 0}, }; int start_x = 0; int start_y = 0; int goal_x = 4; int goal_y = 4; std::vector<Node*> path = astar(start_x, start_y, goal_x, goal_y, grid); if (path.empty()) { std::cout << "No path found!" << std::endl; } else { for (auto it = path.rbegin(); it != path.rend(); ++it) { std::cout << "(" << (*it)->x << ", " << (*it)->y << ") "; } std::cout << std::endl; } return 0; }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。