c++写一段动态寻路的代码

以下是一段简单的动态寻路的代码，使用了 A* 算法来进行路径搜索：

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

#define ROWS 5
#define COLS 5

struct node {
    int x;
    int y;
    int f;
    int g;
    int h;
    struct node *parent;
};

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

bool is_obstacle(int x, int y, int maze[ROWS][COLS]) {
    return (maze[x][y] == 1);
}

bool is_destination(int x, int y, int dest_x, int dest_y) {
    return (x == dest_x && y == dest_y);
}

int calculate_h(int x, int y, int dest_x, int dest_y) {
    return abs(dest_x - x) + abs(dest_y - y);
}

void print_path(struct node *node) {
    if (node == NULL) {
        return;
    }
    print_path(node->parent);
    printf("(%d, %d) ", node->x, node->y);
}

int find_path(int maze[ROWS][COLS], int start_x, int start_y, int dest_x, int dest_y) {
    struct node open_list[ROWS * COLS];
    struct node closed_list[ROWS * COLS];
    int open_count = 0;
    int closed_count = 0;
    int directions[4][2] = {{-1, 0}, {0, -1}, {1, 0}, {0, 1}};
    struct node current_node = {start_x, start_y, 0, 0, 0, NULL};
    open_list[open_count++] = current_node;

    while (open_count > 0) {
        int current_index = 0;
        for (int i = 0; i < open_count; i++) {
            if (open_list[i].f < open_list[current_index].f) {
                current_index = i;
            }
        }
        current_node = open_list[current_index];

        if (is_destination(current_node.x, current_node.y, dest_x, dest_y)) {
            print_path(&current_node);
            return 0;
        }

        for (int i = 0; i < 4; i++) {
            int new_x = current_node.x + directions[i][0];
            int new_y = current_node.y + directions[i][1];

            if (is_valid(new_x, new_y) && !is_obstacle(new_x, new_y, maze)) {
                struct node child_node = {new_x, new_y, 0, 0, 0, &current_node};
                child_node.g = current_node.g + 1;
                child_node.h = calculate_h(new_x, new_y, dest_x, dest_y);
                child_node.f = child_node.g + child_node.h;

                bool in_open_list = false;
                for (int j = 0; j < open_count; j++) {
                    if (open_list[j].x == child_node.x && open_list[j].y == child_node.y) {
                        in_open_list = true;
                        if (child_node.g < open_list[j].g) {
                            open_list[j] = child_node;
                        }
                        break;
                    }
                }
                if (!in_open_list) {
                    bool in_closed_list = false;
                    for (int j = 0; j < closed_count; j++) {
                        if (closed_list[j].x == child_node.x && closed_list[j].y == child_node.y) {
                            in_closed_list = true;
                            break;
                        }
                    }
                    if (!in_closed_list) {
                        open_list[open_count++] = child_node;
                    }
                }
            }
        }

        closed_list[closed_count++] = current_node;

        for (int i = current_index; i < open_count - 1; i++) {
            open_list[i] = open_list[i + 1];
        }
        open_count--;
    }

    printf("No path found!");
    return 1;
}

int main() {
    int maze[ROWS][COLS] = {{0, 0, 0, 0, 0},
                            {0, 1, 1, 0, 0},
                            {0, 0, 0, 0, 1},
                            {0, 1, 1, 1, 0},
                            {0, 0, 0, 0, 0}};

    find_path(maze, 0, 0, 4, 4);

    return 0;
}

该代码使用了一个二维数组来表示地图，其中 0 表示可以通过的区域，1 表示障碍物。在 find_path 函数中，首先将起始节点加入到 open_list 中，然后不断从 open_list 中选取 f 值最小的节点进行扩展，直到找到目标节点或者 open_list 为空。在扩展一个节点时，会计算其每个相邻节点的 f 值，并将其加入到 open_list 中（如果不在 open_list 中的话），或者更新 open_list 中已有的相邻节点的 f 值（如果已经在 open_list 中但路径更优）。同时，会将已扩展的节点加入到 closed_list 中，以避免重复扩展。最后如果找到了目标节点，会从目标节点开始，通过递归回溯的方式打印出路径。如果无法找到路径，则输出 "No path found!"。