用c++写一个A*寻路算法
时间: 2023-05-15 18:07:22 浏览: 69
好的,以下是一个简单的用 C 语言实现的 A* 寻路算法:
```c
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#define ROWS 10
#define COLS 10
typedef struct {
int row, col;
} Point;
typedef struct {
Point parent;
double f, g, h;
} Node;
bool is_valid(Point p) {
return p.row >= 0 && p.row < ROWS && p.col >= 0 && p.col < COLS;
}
bool is_obstacle(Point p) {
return p.row == 2 && p.col == 2 ||
p.row == 2 && p.col == 3 ||
p.row == 2 && p.col == 4 ||
p.row == 3 && p.col == 4 ||
p.row == 4 && p.col == 4 ||
p.row == 5 && p.col == 4 ||
p.row == 6 && p.col == 4 ||
p.row == 7 && p.col == 4 ||
p.row == 7 && p.col == 5 ||
p.row == 7 && p.col == 6;
}
double heuristic(Point p1, Point p2) {
return abs(p1.row - p2.row) + abs(p1.col - p2.col);
}
void print_path(Node nodes[ROWS][COLS], Point start, Point end) {
Point path[ROWS * COLS];
int len = 0;
Point p = end;
while (p.row != start.row || p.col != start.col) {
path[len++] = p;
p = nodes[p.row][p.col].parent;
}
path[len++] = start;
printf("Path: ");
for (int i = len - 1; i >= 0; i--) {
printf("(%d, %d) ", path[i].row, path[i].col);
}
printf("\n");
}
void a_star(Point start, Point end) {
Node nodes[ROWS][COLS];
for (int i = 0; i < ROWS; i++) {
for (int j = 0; j < COLS; j++) {
nodes[i][j].parent = (Point) {-1, -1};
nodes[i][j].f = nodes[i][j].g = nodes[i][j].h = -1;
}
}
nodes[start.row][start.col].f = 0;
nodes[start.row][start.col].g = 0;
nodes[start.row][start.col].h = heuristic(start, end);
Point open_set[ROWS * COLS];
int open_set_len = 1;
open_set[0] = start;
while (open_set_len > 0) {
int best_index = 0;
for (int i = 1; i < open_set_len; i++) {
if (nodes[open_set[i].row][open_set[i].col].f <
nodes[open_set[best_index].row][open_set[best_index].col].f) {
best_index = i;
}
}
Point current = open_set[best_index];
if (current.row == end.row && current.col == end.col) {
print_path(nodes, start, end);
return;
}
open_set[best_index] = open_set[--open_set_len];
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
if (i == 0 && j == 0) {
continue;
}
Point neighbor = (Point) {current.row + i, current.col + j};
if (!is_valid(neighbor) || is_obstacle(neighbor)) {
continue;
}
double tentative_g = nodes[current.row][current.col].g + heuristic(current, neighbor);
if (nodes[neighbor.row][neighbor.col].g == -1 ||
tentative_g < nodes[neighbor.row][neighbor.col].g) {
nodes[neighbor.row][neighbor.col].parent = current;
nodes[neighbor.row][neighbor.col].g = tentative_g;
nodes[neighbor.row][neighbor.col].h = heuristic(neighbor, end);
nodes[neighbor.row][neighbor.col].f = nodes[neighbor.row][neighbor.col].g +
nodes[neighbor.row][neighbor.col].h;
bool found = false;
for (int k = 0; k < open_set_len; k++) {
if (open_set[k].row == neighbor.row && open_set[k].col == neighbor.col) {
found = true;
break;
}
}
if (!found) {
open_set[open_set_len++] = neighbor;
}
}
}
}
}
printf("No path found\n");
}
int main() {
Point start = (Point) {0, 0};
Point end = (Point) {ROWS - 1, COLS - 1};
a_star(start, end);
return 0;
}
```
这个算法使用了一个二维数组 `nodes` 来存储每个节点的信息,包括父节点、f 值、g 值和 h 值。其中 f 值是 g 值和 h 值的和,g 值表示从起点到当前节点的距离,h 值表示从当前节点到终点的估计距离。在算法开始时,将起点的 f 值设为 0,g 值设为 0,h 值设为起点到终点的估计距离。然后将起点加入一个开放列表 `open_set` 中。接下来,每次从 `open_set` 中选取 f 值最小的节点作为当前节点,如果当前节点是终点,则找到了一条路径,否则将当前节点从 `open_set` 中移除,并将它的所有邻居加入 `open_set` 中。对于每个邻居,如果它还没有被访问过,或者从起点到它的距离比之前更短,就更新它的信息,并将它加入 `open_set` 中。如果 `open_set` 为空,说明无法到达终点,算法结束。最后,可以通过遍历每个节点的父节点,从终点到起点构造出一条路径。
相关推荐
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![7z](https://img-home.csdnimg.cn/images/20210720083312.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)