A*算法是一种常用的启发式搜索算法，常用于路径规划等问题。下面是一个简单的C语言实现：
```c
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#define ROW 10 // 地图行数
#define COL 10 // 地图列数
#define INF 0x3f3f3f3f // 无穷大
typedef struct node {
int x; // 当前节点的x坐标
int y; // 当前节点的y坐标
int f; // 当前节点的f值
int g; // 当前节点的g值
int h; // 当前节点的h值
struct node *parent; // 当前节点的父节点
} Node;
int map[ROW][COL] = { // 地图，0表示可通过，1表示障碍物
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
};
// 计算节点的h值，曼哈顿距离
int calc_h(int x, int y, int target_x, int target_y) {
return abs(target_x - x) + abs(target_y - y);
}
// 判断某个节点是否是障碍物
bool is_obstacle(int x, int y) {
if (x < 0 || x >= ROW || y < 0 || y >= COL) {
return true;
}
if (map[x][y] == 1) {
return true;
}
return false;
}
// 判断某个节点是否已经在open或closed列表中
bool is_in_list(Node *node, Node **list, int len) {
for (int i = 0; i < len; i++) {
if (node->x == list[i]->x && node->y == list[i]->y) {
return true;
}
}
return false;
}
// 在open列表中找到f值最小的节点
Node *find_min_f_node(Node **open, int len) {
Node *min_node = open[0];
for (int i = 1; i < len; i++) {
if (open[i]->f < min_node->f) {
min_node = open[i];
}
}
return min_node;
}
// 获取当前节点的邻居节点
Node **get_neighbors(Node *current, Node *target, int *len) {
Node **neighbors = (Node **)malloc(8 * sizeof(Node *));
*len = 0;
int x = current->x;
int y = current->y;
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
if (i == 0 && j == 0) {
continue;
}
if (is_obstacle(x + i, y + j)) {
continue;
}
Node *node = (Node *)malloc(sizeof(Node));
node->x = x + i;
node->y = y + j;
node->parent = current;
node->g = current->g + 1;
node->h = calc_h(node->x, node->y, target->x, target->y);
node->f = node->g + node->h;
neighbors[(*len)++] = node;
}
}
return neighbors;
}
// 释放内存
void free_nodes(Node **nodes, int len) {
for (int i = 0; i < len; i++) {
free(nodes[i]);
}
free(nodes);
}
// A*算法
Node *a_star(int start_x, int start_y, int target_x, int target_y) {
Node *start = (Node *)malloc(sizeof(Node));
start->x = start_x;
start->y = start_y;
start->g = 0;
start->h = calc_h(start_x, start_y, target_x, target_y);
start->f = start->g + start->h;
start->parent = NULL;
Node *target = (Node *)malloc(sizeof(Node));
target->x = target_x;
target->y = target_y;
target->g = INF;
target->h = 0;
target->f = target->g + target->h;
target->parent = NULL;
Node *open[ROW * COL] = {NULL}; // open列表
int open_len = 0;
Node *closed[ROW * COL] = {NULL}; // closed列表
int closed_len = 0;
open[open_len++] = start;
while (open_len > 0) {
Node *current = find_min_f_node(open, open_len);
if (current->x == target->x && current->y == target->y) { // 找到了目标节点
return current;
}
open_len--;
closed[closed_len++] = current;
int neighbors_len = 0;
Node **neighbors = get_neighbors(current, target, &neighbors_len);
for (int i = 0; i < neighbors_len; i++) {
Node *neighbor = neighbors[i];
if (is_in_list(neighbor, closed, closed_len)) { // neighbor已经在closed列表中
free(neighbor);
continue;
}
int tentative_g = current->g + 1;
if (!is_in_list(neighbor, open, open_len)) { // neighbor不在open列表中
open[open_len++] = neighbor;
} else if (tentative_g >= neighbor->g) { // neighbor在open列表中，但tentative_g不是更优的路径
free(neighbor);
continue;
}
neighbor->parent = current;
neighbor->g = tentative_g;
neighbor->f = neighbor->g + neighbor->h;
}
free_nodes(neighbors, neighbors_len);
}
return NULL; // 没有找到路径
}
int main() {
int start_x = 0;
int start_y = 0;
int target_x = 9;
int target_y = 9;
Node *result = a_star(start_x, start_y, target_x, target_y);
if (result == NULL) {
printf("No path found.\n");
return 0;
}
int path_len = 0;
Node *node = result;
while (node != NULL) {
path_len++;
node = node->parent;
}
Node **path = (Node **)malloc(path_len * sizeof(Node *));
node = result;
for (int i = path_len - 1; i >= 0; i--) {
path[i] = node;
node = node->parent;
}
printf("Path: ");
for (int i = 0; i < path_len; i++) {
printf("(%d, %d)%s", path[i]->x, path[i]->y, i == path_len - 1 ? "\n" : "->");
}
free_nodes(path, path_len);
return 0;
}
```
上面的代码中，定义了一个Node结构体表示节点。在A*算法中，每个节点都有一个f值，由g值和h值相加而来。g值表示从起点到当前节点的距离，h值表示从当前节点到目标节点的曼哈顿距离。启发式函数使用曼哈顿距离，这是因为在网格地图中，只能沿着横向或纵向移动，不能斜着走。在计算h值时，使用曼哈顿距离可以更加符合实际情况。
A*算法的具体步骤如下：
1. 将起点加入open列表。
2. 从open列表中找到f值最小的节点，将它从open列表中移除并加入closed列表。
3. 获取当前节点的所有邻居节点，计算它们的g值、h值和f值。
4. 对于每个邻居节点，如果它已经在closed列表中，则跳过；如果它不在open列表中，则将它加入open列表；如果它已经在open列表中，且新的g值不是更优的路径，则跳过。
5. 如果目标节点在open列表中，则返回它；否则，重复步骤2-4，直到open列表为空。
在代码中，map数组表示地图，0表示可通过，1表示障碍物。is_obstacle函数用来判断某个节点是否是障碍物。get_neighbors函数用来获取当前节点的邻居节点。is_in_list函数用来判断某个节点是否已经在open或closed列表中。find_min_f_node函数用来在open列表中找到f值最小的节点。a_star函数是A*算法的具体实现。最后，从终点开始，一直往父节点回溯，直到回溯到起点，就得到了一条路径。
扫一扫
1107
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
