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

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

#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` 为空，说明无法到达终点，算法结束。最后，可以通过遍历每个节点的父节点，从终点到起点构造出一条路径。