A* C++代码实现【AI自动编程】

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;
}