使用c++写出一段A*寻路算法

由于A*算法涉及到估价函数的设计，因此需要先定义地图和估价函数。以下是一个简单的示例代码：

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

#define MAP_WIDTH 10
#define MAP_HEIGHT 10

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

Node *openList[MAP_WIDTH * MAP_HEIGHT];
Node *closeList[MAP_WIDTH * MAP_HEIGHT];

int map[MAP_WIDTH][MAP_HEIGHT] = {
    {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, 1, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};

int heuristic(int x1, int y1, int x2, int y2) {
    // 曼哈顿距离
    return abs(x1 - x2) + abs(y1 - y2);
}

int isInList(Node **list, int len, int x, int y) {
    for (int i = 0; i < len; i++) {
        if (list[i]->x == x && list[i]->y == y) {
            return i;
        }
    }
    return -1;
}

void insertToList(Node **list, int *len, Node *node) {
    list[*len] = node;
    (*len)++;
}

void removeFromList(Node **list, int *len, int index) {
    for (int i = index; i < *len - 1; i++) {
        list[i] = list[i + 1];
    }
    (*len)--;
}

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

void printMap(int sx, int sy, int ex, int ey) {
    for (int y = 0; y < MAP_HEIGHT; y++) {
        for (int x = 0; x < MAP_WIDTH; x++) {
            if (x == sx && y == sy) {
                printf("S");
            } else if (x == ex && y == ey) {
                printf("E");
            } else if (map[x][y] == 1) {
                printf("#");
            } else {
                printf(".");
            }
        }
        printf("\n");
    }
}

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

Node *findPath(int sx, int sy, int ex, int ey) {
    int openListLen = 0;
    int closeListLen = 0;
    Node *startNode = (Node *) malloc(sizeof(Node));
    Node *endNode = (Node *) malloc(sizeof(Node));
    startNode->x = sx;
    startNode->y = sy;
    startNode->f = 0;
    startNode->g = 0;
    startNode->h = 0;
    startNode->parent = NULL;
    insertToList(openList, &openListLen, startNode);

    while (openListLen > 0) {
        // 找出f值最小的节点
        Node *currentNode = openList[0];
        int currentIndex = 0;
        for (int i = 1; i < openListLen; i++) {
            if (openList[i]->f < currentNode->f) {
                currentNode = openList[i];
                currentIndex = i;
            }
        }

        // 从openList中移除
        removeFromList(openList, &openListLen, currentIndex);

        // 加入closeList
        insertToList(closeList, &closeListLen, currentNode);

        // 找到了目标节点，返回路径
        if (currentNode->x == ex && currentNode->y == ey) {
            endNode = currentNode;
            break;
        }

        // 扩展节点
        for (int i = -1; i <= 1; i++) {
            for (int j = -1; j <= 1; j++) {
                int x = currentNode->x + i;
                int y = currentNode->y + j;

                // 节点不能越界，不能是障碍物，不能与当前节点重合，不能在closeList中
                if (x >= 0 && x < MAP_WIDTH && y >= 0 && y < MAP_HEIGHT
                    && !(i == 0 && j == 0) && map[x][y] != 1
                    && isInList(closeList, closeListLen, x, y) == -1) {
                    // 计算g、h、f值
                    int g = currentNode->g + abs(i) + abs(j);
                    int h = heuristic(x, y, ex, ey);
                    int f = g + h;

                    // 判断新节点是否在openList中
                    int index = isInList(openList, openListLen, x, y);
                    if (index != -1) {
                        Node *node = openList[index];
                        if (g < node->g) {
                            node->g = g;
                            node->f = f;
                            node->parent = currentNode;
                        }
                    } else {
                        Node *node = (Node *) malloc(sizeof(Node));
                        node->x = x;
                        node->y = y;
                        node->g = g;
                        node->h = h;
                        node->f = f;
                        node->parent = currentNode;
                        insertToList(openList, &openListLen, node);
                    }
                }
            }
        }
    }

    clearList(openList, &openListLen);
    clearList(closeList, &closeListLen);

    return endNode;
}

int main() {
    Node *endNode = findPath(0, 0, 9, 9);
    if (endNode != NULL) {
        printMap(0, 0, 9, 9);
        printPath(endNode);
    } else {
        printf("No path found.\n");
    }
    return 0;
}

该代码实现了一个简单的A*寻路算法，能够在地图中找到起点和终点之间的路径。可以通过修改地图和起点终点坐标来测试算法的正确性。