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

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

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

#define ROW 10
#define COL 10

typedef struct Node {
    int x, y;
    int f, g, h;
    struct Node *parent;
} Node;

Node *openList[ROW * COL];
Node *closeList[ROW * COL];
int openLen = 0, closeLen = 0;
int map[ROW][COL] = {
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};

int isObstacle(int x, int y) {
    if (x < 0 || x >= ROW || y < 0 || y >= COL) {
        return 1;
    }
    return map[x][y];
}

int calcG(Node *node) {
    int g = 0;
    while (node->parent) {
        g += abs(node->x - node->parent->x) + abs(node->y - node->parent->y);
        node = node->parent;
    }
    return g;
}

int calcH(Node *node, Node *end) {
    return abs(node->x - end->x) + abs(node->y - end->y);
}

int calcF(Node *node, Node *end) {
    return calcG(node) + calcH(node, end);
}

void insertOpenList(Node *node) {
    int i;
    for (i = 0; i < openLen; i++) {
        if (openList[i]->f > node->f) {
            break;
        }
    }
    for (int j = openLen; j > i; j--) {
        openList[j] = openList[j - 1];
    }
    openList[i] = node;
    openLen++;
}

Node *popOpenList() {
    Node *node = openList[0];
    for (int i = 0; i < openLen - 1; i++) {
        openList[i] = openList[i + 1];
    }
    openLen--;
    return node;
}

void insertCloseList(Node *node) {
    closeList[closeLen++] = node;
}

int isInCloseList(Node *node) {
    for (int i = 0; i < closeLen; i++) {
        if (closeList[i]->x == node->x && closeList[i]->y == node->y) {
            return 1;
        }
    }
    return 0;
}

void freeList(Node **list, int len) {
    for (int i = 0; i < len; i++) {
        free(list[i]);
    }
}

void printPath(Node *node) {
    while (node) {
        printf("(%d, %d) ", node->x, node->y);
        node = node->parent;
    }
    printf("\n");
}

void aStar(int startX, int startY, int endX, int endY) {
    Node *start = (Node *)malloc(sizeof(Node));
    start->x = startX;
    start->y = startY;
    start->f = 0;
    start->g = 0;
    start->h = 0;
    start->parent = NULL;
    insertOpenList(start);

    while (openLen > 0) {
        Node *cur = popOpenList();
        if (cur->x == endX && cur->y == endY) {
            printPath(cur);
            freeList(openList, openLen);
            freeList(closeList, closeLen);
            return;
        }
        insertCloseList(cur);

        for (int i = -1; i <= 1; i++) {
            for (int j = -1; j <= 1; j++) {
                if (i == 0 && j == 0) {
                    continue;
                }
                int x = cur->x + i;
                int y = cur->y + j;
                if (isObstacle(x, y) || isInCloseList(&(Node){x, y})) {
                    continue;
                }
                Node *node = (Node *)malloc(sizeof(Node));
                node->x = x;
                node->y = y;
                node->parent = cur;
                node->g = calcG(node);
                node->h = calcH(node, &(Node){endX, endY});
                node->f = calcF(node, &(Node){endX, endY});
                insertOpenList(node);
            }
        }
    }

    printf("No path found.\n");
    freeList(openList, openLen);
    freeList(closeList, closeLen);
}

int main() {
    map[1][1] = 1;
    map[2][1] = 1;
    map[3][1] = 1;
    map[4][1] = 1;
    map[5][1] = 1;
    map[6][1] = 1;
    map[7][1] = 1;
    map[8][1] = 1;
    map[9][1] = 1;

    aStar(0, 0, 9, 9);

    return 0;
}

这是一个简单的A*寻路算法实现，可以在一个10x10的地图上找到起点到终点的最短路径。