用C++写一个A*寻路算法

好的，以下是一个简单的C语言实现的A*寻路算法：

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

#define ROW 5
#define COL 5

typedef struct {
    int x, y;
} Point;

typedef struct {
    Point parent;
    int f, g, h;
} Node;

Node map[ROW][COL];
Point start, end;

bool is_valid(int x, int y) {
    return x >= 0 && x < ROW && y >= 0 && y < COL;
}

bool is_obstacle(int x, int y) {
    return map[x][y].f == -1;
}

int heuristic(int x, int y) {
    return abs(x - end.x) + abs(y - end.y);
}

void print_path() {
    Point path[ROW * COL];
    int count = 0;
    Point current = end;
    while (current.x != start.x || current.y != start.y) {
        path[count++] = current;
        current = map[current.x][current.y].parent;
    }
    path[count++] = start;
    printf("Path: ");
    for (int i = count - 1; i >= 0; i--) {
        printf("(%d, %d) ", path[i].x, path[i].y);
    }
    printf("\n");
}

void a_star() {
    int dx[8] = {-1, -1, 0, 1, 1, 1, 0, -1};
    int dy[8] = {0, 1, 1, 1, 0, -1, -1, -1};
    map[start.x][start.y].f = 0;
    map[start.x][start.y].g = 0;
    map[start.x][start.y].h = heuristic(start.x, start.y);
    Node open_set[ROW * COL];
    int open_set_size = 1;
    open_set[0] = map[start.x][start.y];
    while (open_set_size > 0) {
        int current_index = 0;
        for (int i = 1; i < open_set_size; i++) {
            if (open_set[i].f < open_set[current_index].f) {
                current_index = i;
            }
        }
        Node current = open_set[current_index];
        if (current.parent.x == end.x && current.parent.y == end.y) {
            print_path();
            return;
        }
        open_set[current_index] = open_set[open_set_size - 1];
        open_set_size--;
        for (int i = 0; i < 8; i++) {
            int new_x = current.parent.x + dx[i];
            int new_y = current.parent.y + dy[i];
            if (!is_valid(new_x, new_y) || is_obstacle(new_x, new_y)) {
                continue;
            }
            int new_g = current.g + 1;
            int new_h = heuristic(new_x, new_y);
            int new_f = new_g + new_h;
            if (map[new_x][new_y].f == -2 || new_f < map[new_x][new_y].f) {
                map[new_x][new_y].parent = current.parent;
                map[new_x][new_y].f = new_f;
                map[new_x][new_y].g = new_g;
                map[new_x][new_y].h = new_h;
                if (map[new_x][new_y].f == -2) {
                    open_set[open_set_size++] = map[new_x][new_y];
                }
            }
        }
    }
    printf("No path found!\n");
}

int main() {
    // initialize map
    for (int i = 0; i < ROW; i++) {
        for (int j = 0; j < COL; j++) {
            map[i][j].parent.x = i;
            map[i][j].parent.y = j;
            map[i][j].f = -2;
            map[i][j].g = -2;
            map[i][j].h = -2;
        }
    }
    // set obstacles
    map[1][1].f = -1;
    map[1][2].f = -1;
    map[1][3].f = -1;
    map[2][3].f = -1;
    map[3][3].f = -1;
    map[3][2].f = -1;
    // set start and end points
    start.x = 0;
    start.y = 0;
    end.x = 4;
    end.y = 4;
    // run A* algorithm
    a_star();
    return 0;
}

这个程序实现了一个简单的A*寻路算法，可以在一个5x5的地图上找到从起点到终点的最短路径。其中，-1表示障碍物，-2表示未访问过的节点，其它数字表示已访问过的节点的f值。