A*算法的简单实现

A*算简单实现

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

#define ROW 5
#define COL 5

// 结点表示
typedef struct Node {
    int row, col;       // 结点在地图中的坐标
    int f, g, h;        // 用于计算路径的代价
    struct Node* parent; // 用于回溯路径
} Node;

Node* createNode(int row, int col) {
    Node* newNode = (Node*)malloc(sizeof(Node));
    newNode->row = row;
    newNode->col = col;
    newNode->f = newNode->g = newNode->h = 0;
    newNode->parent = NULL;
    return newNode;
}

// 是否为有效结点
bool isValid(int row, int col) {
    return (row >= 0) && (row < ROW) && (col >= 0) && (col < COL);
}

// 是否为障碍物
bool isObstacle(int map[ROW][COL], int row, int col) {
    return map[row][col] == 1; // 1表示障碍物
}

// 是否为终点
bool isDestination(int row, int col, Node* dest) {
    return (row == dest->row) && (col == dest->col);
}

// 计算H值（启发函数，估计从当前结点到目标结点的代价）
int calculateHValue(int row, int col, Node* dest) {
    return abs(row - dest->row) + abs(col - dest->col);
}

// A*算法实现
void AStarSearch(int map[ROW][COL], Node* start, Node* dest) {
    // 待访问结点集合
    Node* openList[ROW * COL];
    int openListSize = 0;

    // 已访问结点集合
    bool closedList[ROW][COL];
    memset(closedList, false, sizeof(closedList));

    // 添加起点到待访问集合
    openList[openListSize++] = start;

    // 定义相邻结点的行列偏移
    int rowOffset[] = {-1, 0, 1, 0};
    int colOffset[] = {0, 1, 0, -1};

    while (openListSize > 0) {
        // 从待访问集合中选择f值最小的结点
        int currentIndex = 0;
        for (int i = 1; i < openListSize; ++i) {
            if (openList[i]->f < openList[currentIndex]->f) {
                currentIndex = i;
            }
        }

        // 当前结点
        Node* current = openList[currentIndex];

        // 从待访问集合中移除当前结点
        openList[currentIndex] = openList[openListSize - 1];
        --openListSize;

        // 将当前结点标记为已访问
        closedList[current->row][current->col] = true;

        // 遍历当前结点的相邻结点
        for (int i = 0; i < 4; ++i) {
            int newRow = current->row + rowOffset[i];
            int newCol = current->col + colOffset[i];

            // 检查相邻结点是否为有效结点
            if (isValid(newRow, newCol)) {
                // 如果相邻结点是目标结点，结束搜索
                if (isDestination(newRow, newCol, dest)) {
                    dest->parent = current;
                    return;
                }

                // 如果相邻结点不是障碍物且未被访问过
                if (!closedList[newRow][newCol] && !isObstacle(map, newRow, newCol)) {
                    Node* adjacentNode = createNode(newRow, newCol);
                    adjacentNode->g = current->g + 1;
                    adjacentNode->h = calculateHValue(newRow, newCol, dest);
                    adjacentNode->f = adjacentNode->g + adjacentNode->h;
                    adjacentNode->parent = current;

                    // 如果相邻结点不在待访问集合中，将其添加进去
                    bool inOpenList = false;
                    for (int j = 0; j < openListSize; ++j) {
                        if (openList[j]->row == adjacentNode->row && openList[j]->col == adjacentNode->col) {
                            inOpenList = true;
                            break;
                        }
                    }

                    if (!inOpenList) {
                        openList[openListSize++] = adjacentNode;
                    }
                }
            }