[C言語] a*(A Star)探索アルゴリズムを実装する方法

typedef struct Node {
    int x; // ノードのx座標
    int y; // ノードのy座標
    float g; // 開始ノードからのコスト
    float h; // ヒューリスティックコスト
    float f; // 総コスト
    struct Node* parent; // 親ノードへのポインタ
} Node;
Node* startNode; // スタートノード
Node* goalNode; // ゴールノード

#define MAX_NODES 100 // 最大ノード数
Node* openList[MAX_NODES]; // オープンリスト
Node* closedList[MAX_NODES]; // クローズドリスト
int openListSize = 0; // オープンリストのサイズ
int closedListSize = 0; // クローズドリストのサイズ

Node* getLowestFNode() {
    int lowestIndex = 0;
    for (int i = 1; i < openListSize; i++) {
        if (openList[i]->f < openList[lowestIndex]->f) {
            lowestIndex = i;
        }
    }
    Node* lowestNode = openList[lowestIndex];
    // オープンリストから削除
    for (int i = lowestIndex; i < openListSize - 1; i++) {
        openList[i] = openList[i + 1];
    }
    openListSize--;
    return lowestNode;
}

if (currentNode->x == goalNode->x && currentNode->y == goalNode->y) {
    // ゴールノードに到達した場合の処理
}

for (int direction = 0; direction < 4; direction++) { // 4方向を探索
    Node* adjacentNode =
        getAdjacentNode(currentNode, direction); // 隣接ノードを取得

    // グリッドの範囲内か確認
    if (adjacentNode->x >= 0 && adjacentNode->x < GRID_SIZE &&
        adjacentNode->y >= 0 && adjacentNode->y < GRID_SIZE) {
        // クローズドリストに存在する場合はスキップ
        if (nodeInList(adjacentNode, closedList, closedListSize)) {
            free(adjacentNode);
            continue;
        }

        // g値、h値、f値の計算
        adjacentNode->g = currentNode->g + 1; // コストを加算
        adjacentNode->h = calculateHeuristic(
            adjacentNode, goalNode); // ヒューリスティック計算
        adjacentNode->f =
            adjacentNode->g + adjacentNode->h; // 総コスト計算
        adjacentNode->parent = currentNode;    // 親ノードを設定

        // オープンリストに存在しない場合は追加
        if (!nodeInList(adjacentNode, openList, openListSize)) {
            openList[openListSize++] = adjacentNode;
        } else {
            free(adjacentNode); // 既に存在する場合は解放
        }
    } else {
        free(adjacentNode); // 範囲外のノードは解放
    }
}

Node* current = goalNode; // ゴールノードから開始
while (current != NULL) {
    // 経路を記録
    current = current->parent; // 親ノードに移動
}

printf("経路: ");
while (current != NULL) {
    printf("(%d, %d) ", current->x, current->y); // 各ノードの座標を表示
    current = current->parent;                   // 親ノードに移動
}
printf("\n");

for (int i = 0; i < openListSize; i++) {
    free(openList[i]); // オープンリストのノードを解放
}
for (int i = 0; i < closedListSize; i++) {
    free(closedList[i]); // クローズドリストのノードを解放
}

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#define CRT_SECURE_NO_WARNINGS
#define MAX_NODES 100 // 最大ノード数
#define GRID_SIZE 5   // グリッドのサイズ

typedef struct Node {
    int x;               // ノードのx座標
    int y;               // ノードのy座標
    float g;             // 開始ノードからのコスト
    float h;             // ヒューリスティックコスト
    float f;             // 総コスト
    struct Node* parent; // 親ノードへのポインタ
} Node;

Node* openList[MAX_NODES];   // オープンリスト
Node* closedList[MAX_NODES]; // クローズドリスト
int openListSize = 0;        // オープンリストのサイズ
int closedListSize = 0;      // クローズドリストのサイズ

// ヒューリスティック関数(マンハッタン距離)
float calculateHeuristic(Node* a, Node* b) {
    return fabs(a->x - b->x) + fabs(a->y - b->y);
}

// 隣接ノードを取得する関数
Node* getAdjacentNode(Node* current, int direction) {
    Node* adjacent = (Node*)malloc(sizeof(Node));
    adjacent->x = current->x + (direction == 0 ? 1 : (direction == 1 ? -1 : 0)); // x座標の変更
    adjacent->y = current->y + (direction == 2 ? 1 : (direction == 3 ? -1 : 0)); // y座標の変更
    return adjacent;
}

// 最小f値のノードをオープンリストから取得し削除
Node* getLowestFNode() {
    int lowestIndex = 0;
    for (int i = 1; i < openListSize; i++) {
        if (openList[i]->f < openList[lowestIndex]->f) {
            lowestIndex = i;
        }
    }
    Node* lowestNode = openList[lowestIndex];
    // オープンリストから削除
    for (int i = lowestIndex; i < openListSize - 1; i++) {
        openList[i] = openList[i + 1];
    }
    openListSize--;
    return lowestNode;
}

// 経路を復元する関数
void reconstructPath(Node* goalNode) {
    Node* current = goalNode; // ゴールノードから開始
    printf("経路: ");
    while (current != NULL) {
        printf("(%d, %d) ", current->x, current->y); // 各ノードの座標を表示
        current = current->parent;                   // 親ノードに移動
    }
    printf("\n");
}

// ノードがリストに存在するか確認
int nodeInList(Node* node, Node* list[], int listSize) {
    for (int i = 0; i < listSize; i++) {
        if (list[i]->x == node->x && list[i]->y == node->y) {
            return 1;
        }
    }
    return 0;
}

int main() {
    // スタートノードとゴールノードの設定
    Node* startNode = (Node*)malloc(sizeof(Node));
    startNode->x = 0; // スタートノードのx座標
    startNode->y = 0; // スタートノードのy座標
    startNode->g = 0; // スタートノードからのコスト
    startNode->h = calculateHeuristic(startNode, &(Node){GRID_SIZE - 1, GRID_SIZE - 1}); // ゴールノードまでのヒューリスティック
    startNode->f = startNode->g + startNode->h; // 総コスト
    startNode->parent = NULL;                   // 親ノードはなし

    Node* goalNode = (Node*)malloc(sizeof(Node));
    goalNode->x = GRID_SIZE - 1; // ゴールノードのx座標
    goalNode->y = GRID_SIZE - 1; // ゴールノードのy座標

    // オープンリストにスタートノードを追加
    openList[openListSize++] = startNode;

    // メインループ
    while (openListSize > 0) {
        Node* currentNode = getLowestFNode(); // 最小f値のノードを取得
        if (currentNode->x == goalNode->x && currentNode->y == goalNode->y) {
            // ゴールノードに到達した場合
            reconstructPath(currentNode); // 経路を復元
            break;
        }

        // 現在のノードをクローズドリストに移動
        closedList[closedListSize++] = currentNode;

        // 隣接ノードの探索
        for (int direction = 0; direction < 4; direction++) { // 4方向を探索
            Node* adjacentNode = getAdjacentNode(currentNode, direction); // 隣接ノードを取得

            // グリッドの範囲内か確認
            if (adjacentNode->x >= 0 && adjacentNode->x < GRID_SIZE &&
                adjacentNode->y >= 0 && adjacentNode->y < GRID_SIZE) {

                // クローズドリストに存在する場合はスキップ
                if (nodeInList(adjacentNode, closedList, closedListSize)) {
                    free(adjacentNode);
                    continue;
                }

                // g値、h値、f値の計算
                adjacentNode->g = currentNode->g + 1; // コストを加算
                adjacentNode->h = calculateHeuristic(adjacentNode, goalNode); // ヒューリスティック計算
                adjacentNode->f = adjacentNode->g + adjacentNode->h; // 総コスト計算
                adjacentNode->parent = currentNode;    // 親ノードを設定

                // オープンリストに存在しない場合は追加
                if (!nodeInList(adjacentNode, openList, openListSize)) {
                    openList[openListSize++] = adjacentNode;
                } else {
                    free(adjacentNode); // 既に存在する場合は解放
                }
            } else {
                free(adjacentNode); // 範囲外のノードは解放
            }
        }
    }

    // メモリの解放
    for (int i = 0; i < openListSize; i++) {
        free(openList[i]); // オープンリストのノードを解放
    }
    for (int i = 0; i < closedListSize; i++) {
        free(closedList[i]); // クローズドリストのノードを解放
    }
    free(startNode); // スタートノードを解放
    free(goalNode);  // ゴールノードを解放

    return 0;        // プログラムの終了
}

経路: (4, 4) (4, 3) (4, 2) (4, 1) (4, 0) (3, 0) (2, 0) (1, 0) (0, 0)