用c语言编写batch Informed Trees（(BIT*）在二维地图空间中寻找路径的代码

我可以回答这个问题。BIT* 是一种路径规划算法，可以在二维地图空间中寻找路径。以下是使用 C 语言编写 BIT* 算法的示例代码：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define MAP_WIDTH 100
#define MAP_HEIGHT 100
#define OBSTACLE_COST 100000

typedef struct {
int x;
int y;
} Point;

typedef struct {
Point parent;
double g;
double h;
} Node;

typedef struct {
Node *nodes;
int count;
int capacity;
} NodeList;

typedef struct {
int **map;
int width;
int height;
} Map;

double euclidean_distance(Point a, Point b) {
return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

double manhattan_distance(Point a, Point b) {
return abs(a.x - b.x) + abs(a.y - b.y);
}

void add_node(NodeList *list, Node node) {
if (list->count == list->capacity) {
list->capacity *= 2;
list->nodes = (Node *)realloc(list->nodes, list->capacity * sizeof(Node));
}
list->nodes[list->count++] = node;
}

void remove_node(NodeList *list, int index) {
list->nodes[index] = list->nodes[--list->count];
}

int is_obstacle(Map *map, Point point) {
if (point.x < || point.x >= map->width || point.y < || point.y >= map->height) {
return 1;
}
return map->map[point.y][point.x] == OBSTACLE_COST;
}

NodeList *get_neighbors(Map *map, Node node) {
NodeList *neighbors = (NodeList *)malloc(sizeof(NodeList));
neighbors->nodes = (Node *)malloc(8 * sizeof(Node));
neighbors->count = ;
neighbors->capacity = 8;
for (int dx = -1; dx <= 1; dx++) {
for (int dy = -1; dy <= 1; dy++) {
if (dx == && dy == ) {
continue;
}
Point neighbor_point = {node.parent.x + dx, node.parent.y + dy};
if (is_obstacle(map, neighbor_point)) {
continue;
}
double neighbor_g = node.g + euclidean_distance(node.parent, neighbor_point);
double neighbor_h = euclidean_distance(neighbor_point, (Point){map->width - 1, map->height - 1});
Node neighbor_node = {{neighbor_point.x, neighbor_point.y}, neighbor_g, neighbor_h};
add_node(neighbors, neighbor_node);
}
}
return neighbors;
}

NodeList *get_path(Map *map, Point start, Point end) {
NodeList *open_list = (NodeList *)malloc(sizeof(NodeList));
open_list->nodes = (Node *)malloc(sizeof(Node));
open_list->count = ;
open_list->capacity = 1;
Node start_node = {{start.x, start.y}, , euclidean_distance(start, end)};
add_node(open_list, start_node);
NodeList *closed_list = (NodeList *)malloc(sizeof(NodeList));
closed_list->nodes = (Node *)malloc(sizeof(Node));
closed_list->count = ;
closed_list->capacity = 1;
while (open_list->count > ) {
int current_index = ;
for (int i = ; i < open_list->count; i++) {
if (open_list->nodes[i].g + open_list->nodes[i].h < open_list->nodes[current_index].g + open_list->nodes[current_index].h) {
current_index = i;
}
}
Node current_node = open_list->nodes[current_index];
if (current_node.parent.x == end.x && current_node.parent.y == end.y) {
NodeList *path = (NodeList *)malloc(sizeof(NodeList));
path->nodes = (Node *)malloc(sizeof(Node));
path->count = ;
path->capacity = 1;
add_node(path, current_node);
while (current_node.parent.x != start.x || current_node.parent.y != start.y) {
for (int i = ; i < closed_list->count; i++) {
if (closed_list->nodes[i].parent.x == current_node.parent.x && closed_list->nodes[i].parent.y == current_node.parent.y) {
current_node = closed_list->nodes[i];
add_node(path, current_node);
break;
}
}
}
return path;
}
remove_node(open_list, current_index);
add_node(closed_list, current_node);
NodeList *neighbors = get_neighbors(map, current_node);
for (int i = ; i < neighbors->count; i++) {
Node neighbor_node = neighbors->nodes[i];
int in_open_list = ;
for (int j = ; j < open_list->count; j++) {
if (open_list->nodes[j].parent.x == neighbor_node.parent.x && open_list->nodes[j].parent.y == neighbor_node.parent.y) {
in_open_list = 1;
if (open_list->nodes[j].g > neighbor_node.g) {
open_list->nodes[j].g = neighbor_node.g;
open_list->nodes[j].parent = neighbor_node.parent;
}
break;
}
}
if (!in_open_list) {
int in_closed_list = ;
for (int j = ; j < closed_list->count; j++) {
if (closed_list->nodes[j].parent.x == neighbor_node.parent.x && closed_list->nodes[j].parent.y == neighbor_node.parent.y) {
in_closed_list = 1;
if (closed_list->nodes[j].g > neighbor_node.g) {
closed_list->nodes[j].g = neighbor_node.g;
closed_list->nodes[j].parent = neighbor_node.parent;
add_node(open_list, closed_list->nodes[j]);
remove_node(closed_list, j);
}
break;
}
}
if (!in_closed_list) {
add_node(open_list, neighbor_node);
}
}
}
free(neighbors->nodes);
free(neighbors);
}
return NULL;
}

int main() {
Map map = {(int **)malloc(MAP_HEIGHT * sizeof(int *)), MAP_WIDTH, MAP_HEIGHT};
for (int i = ; i < MAP_HEIGHT; i++) {
map.map[i] = (int *)malloc(MAP_WIDTH * sizeof(int));
for (int j = ; j < MAP_WIDTH; j++) {
map.map[i][j] = ;
}
}
map.map[20][30] = OBSTACLE_COST;
map.map[21][30] = OBSTACLE_COST;
map.map[22][30] = OBSTACLE_COST;
map.map[23][30] = OBSTACLE_COST;
map.map[24][30] = OBSTACLE_COST;
map.map[25][30] = OBSTACLE_COST;
map.map[26][30] = OBSTACLE_COST;
map.map[27][30] = OBSTACLE_COST;
map.map[28][30] = OBSTACLE_COST;
map.map[29][30] = OBSTACLE_COST;
NodeList *path = get_path(&map, (Point){, }, (Point){MAP_WIDTH - 1, MAP_HEIGHT - 1});
for (int i = path->count - 1; i >= ; i--) {
printf("(%d, %d)\n", path->nodes[i].parent.x, path->nodes[i].parent.y);
}
return ;
}