编辑代码

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

#define N 5
#define M 5
#define OPEN_SET_SIZE 1000

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

typedef struct {
    Node nodes[OPEN_SET_SIZE];
    int size;
} OpenSet;

bool isValid(int maze[N][M], int x, int y) {
    return x >= 0 && x < N && y >= 0 && y < M && maze[x][y] == 0;
}

int heuristicManhattan(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

int heuristicZero(int x1, int y1, int x2, int y2) {
    return 0;
}

void pushOpenSet(OpenSet* openSet, Node node) {
    for (int i = 0; i < openSet->size; i++) {
        if (openSet->nodes[i].f > node.f) {
            for (int j = openSet->size - 1; j > i; j--) {
                openSet->nodes[j] = openSet->nodes[j - 1];
            }
            openSet->nodes[i] = node;
            openSet->size++;
            return;
        }
    }
    openSet->nodes[openSet->size++] = node;
}

Node popOpenSet(OpenSet* openSet) {
    Node minNode = openSet->nodes[0];
    for (int i = 1; i < openSet->size; i++) {
        if (openSet->nodes[i].f < minNode.f) {
            minNode = openSet->nodes[i];
        }
    }
    for (int i = 0; i < openSet->size - 1; i++) {
        openSet->nodes[i] = openSet->nodes[i + 1];
    }
    openSet->size--;
    return minNode;
}

bool isInClosedSet(Node* closedSet, int size, int x, int y) {
    for (int i = 0; i < size; i++) {
        if (closedSet[i].x == x && closedSet[i].y == y) {
            return true;
        }
    }
    return false;
}

void aStar(int maze[N][M], int startX, int startY, int endX, int endY, int (*heuristic)(int, int, int, int)) {
    Node startNode = {.x = startX, .y = startY, .g = 0, .h = heuristic(startX, startY, endX, endY), .f = heuristic(startX, startY, endX, endY), .parent = NULL};
    Node endNode = {.x = endX, .y = endY};
    Node* closedSet = (Node*)calloc(N * M, sizeof(Node));
    OpenSet openSet = {.size = 0};
    pushOpenSet(&openSet, startNode);

    int expandedNodes = 0;
    int generatedNodes = 1; // Including the start node

    while (openSet.size > 0) {
        Node currentNode = popOpenSet(&openSet);

        if (currentNode.x == endNode.x && currentNode.y == endNode.y) {
            // Reconstruct path
            Node* path = (Node*)malloc((generatedNodes + 1) * sizeof(Node));
            Node* temp = &currentNode;
            for (int i = 0; temp != NULL; i++) {
                path[i] = *temp;
                temp = temp->parent;
            }
            path[generatedNodes] = endNode; // Append end node to the path (optional, for visualization)

            // Print path
            printf("Shortest path:\n");
            for (int i = generatedNodes; i >= 0; i--) {
                printf("(%d, %d) ", path[i].x, path[i].y);
            }
            printf("\n");

            // Print expanded and generated nodes
            printf("Expanded nodes: %d\n", expandedNodes);
            printf("Generated nodes: %d\n", generatedNodes);

            free(path);
            free(closedSet);
            return;
        }

        // Mark current node as closed
        Node closedNode = currentNode;
        closedNode.parent = NULL; // Don't need parent in closed set
        for (int i = 0; i < closedSetSize; i++) {
            if (closedSet[i].x == INT_MIN && closedSet[i].y == INT_MIN) {
                closedSet[i] = closedNode;
                break;
            }
        }

        // Explore neighbors
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};
        for (int i = 0; i < 4; i++) {
            int newX = currentNode.x + dx[i];
            int newY = currentNode.y + dy[i];
            if (isValid(maze, newX, newY) && !isInClosedSet(closedSet, N * M, newX, newY)) {
                Node neighbor = {.x = newX, .y = newY, .g = currentNode.g + 1, .h = heuristic(newX, newY, endX, endY), .f = currentNode.g + 1 + heuristic(newX, newY, endX, endY), .parent = &currentNode};

                // Check if neighbor is already in open set
                bool isInOpenSet = false;
                for (int j = 0; j < openSet.size; j++) {
                    if (openSet.nodes[j].x == newX && openSet.nodes[j].y == newY && openSet.nodes[j].g <= neighbor.g) {
                        isInOpenSet = true;
                        break;
                    }
                }

                if (!isInOpenSet) {
                    pushOpenSet(&openSet, neighbor);
                    generatedNodes++;
                }

                expandedNodes++;
            }
        }
    }

    printf("No path found.\n");
    free(closedSet);
}

int main() {
    int maze[N][M] = {
        {0, 0, 0, 0, 0},
        {1, 0, 1, 0, 1},
        {0, 0, 1, 1, 1},
        {0, 1, 0, 0, 0},
        {0, 0, 0, 1, 0}
    };

    printf("Using heuristic Manhattan distance:\n");
    aStar(maze, 0, 0, 4, 4, heuristicManhattan);

    printf("\nUsing heuristic h(n)=0 (breadth-first):\n");
    aStar(maze, 0, 0, 4, 4, heuristicZero);

    return 0;
}