C++ A*算法
#include "pch.h"
#include "A_Start.h"
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
using namespace std;
// 定义节点结构体
struct Node {
int x, y; // 节点在地图中的坐标
int g, h; // g: 起点到该节点的距离,h: 该节点到终点的距离
Node* parent; // 指向该节点的父节点
Node(int x, int y, int g, int h, Node* parent) : x(x), y(y), g(g), h(h), parent(parent) {}
};
// 定义比较函数,用于priority_queue
struct CompareNode {
bool operator()(Node* a, Node* b) {
return (a->g + a->h) > (b->g + b->h); // f = g + h
}
};
// A*算法实现函数
vector<vector<int>> AStar(vector<vector<int>> map, int start_x, int start_y, int end_x, int end_y) {
int rows = map.size();
int cols = map[0].size();
vector<vector<Node*>> nodes(rows, vector<Node*>(cols, nullptr)); // 地图中所有节点
priority_queue<Node*, vector<Node*>, CompareNode> openList; // 开放列表,按f = g + h的大小排序
vector<Node*> closeList; // 关闭列表
// 创建所有节点
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
nodes[i][j] = new Node(i, j, INT_MAX, 0, nullptr); // 初始化g为无穷大
}
}
// 起点
Node* startNode = nodes[start_x][start_y];
startNode->g = 0;
startNode->h = abs(start_x - end_x) + abs(start_y - end_y); // 曼哈顿距离
openList.push(startNode);
// A*算法
while (!openList.empty()) {
Node* current = openList.top();
openList.pop();
closeList.push_back(current);
// 到达终点
if (current->x == end_x && current->y == end_y) {
vector<vector<int>> path(rows, vector<int>(cols, 0));
Node* node = current;
while (node) {
path[node->x][node->y] = 1;
node = node->parent;
}
return path;
}
// 遍历相邻节点
int dx[] = { -1, 0, 1, 0 };
int dy[] = { 0, 1, 0, -1 };
for (int i = 0; i < 4; i++) {
int x = current->x + dx[i];
int y = current->y + dy[i];
// 节点不在地图内或者为障碍物,跳过
if (x < 0 || x = rows || y < 0 || y >= cols || map[x][y] == 1)
{
continue;
}
Node* neighbor = nodes[x][y];
// 如果该节点已在关闭列表中,跳过
if (find(closeList.begin(), closeList.end(), neighbor) != closeList.end()) {
continue;
}
// 计算新的g值
int g = current->g + 1;
// 如果新的g值比该节点原本的g值小,则更新该节点
if (g < neighbor->g) {
neighbor->g = g;
neighbor->parent = current;
neighbor->h = abs(x - end_x) + abs(y - end_y); // 曼哈顿距离
if (find(openList.begin(), openList.end(), neighbor) == openList.end()) {
openList.push(neighbor);
}
}
}
}
// 没有到达终点,返回空
return vector<vector<int>>();
}
//// 示例
//int main() {
// vector<vector<int>> map = {
// {0, 0, 0, 0, 0, 0, 0},
// {0, 0, 0, 0, 1, 0, 0},
// {0, 0, 1, 0, 1, 0, 0},
// {0, 0, 1, 0, 0, 0, 0},
// {0, 0, 1, 1, 1, 0, 0},
// {0, 0, 0, 0, 0, 0, 0}
// };
//
//vector<vector<int>> path = AStar(map, 0, 0, 5, 6);
//
//for (auto row : path) {
// for (auto cell : row) {
// cout << cell << " ";
// }
// cout << endl;
//}
//
//return 0;
//这个示例代码实现了一个简单的A* 算法,输入一张地图和起点、终点的坐标,输出一条从起点到终点的最短路径。
//其中,地图是一个二维矩阵,0表示可以通过的位置,1表示障碍物;路径是一个二维矩阵,1表示在路径上,0表示不在路径上。
本文详细探讨了如何使用C++实现经典的A*寻路算法,涵盖了A*算法的基本原理、关键数据结构以及C++代码实现的细节,旨在帮助读者深入理解并能够应用该算法解决实际路径规划问题。
348

被折叠的 条评论
为什么被折叠?



