C言語で書いてみました

■ 経路探索1／スタック／キュー（平成16年午後2問1設問1～4）

#include <stdio.h>

/* 平成16年午後2 */

#define USESTACK    0   /* 1:stack, 0:queue */

#define AVAIL   -1
#define OBST    -2
#define n        7
#define m        9
#define MAXSIZE ((n+2)*(m+2))

int cell[MAXSIZE] = {
    -2,-2,-2,-2,-2,-2,-2,-2,-2,-2,-2,
    -2,-1,-1,-1,-1,-1,-1,-1,-2,-2,-2,
    -2,-1,-1,-1,-1,-2,-1,-1,-2,-2,-2,
    -2,-1,-1,-1,-1,-2,-1,-2,-2,-1,-2,
    -2,-1,-2,-1,-1,-2,-1,-1,-1,-1,-2,
    -2,-2,-2,-2,-1,-2,-1,-2,-1,-1,-2,
    -2,-1,-2,-1,-1,-1,-1,-1,-2,-2,-2,
    -2,-1,-1,-1,-1,-1,-1,-1,-2,-2,-2,
    -2,-2,-2,-2,-2,-2,-2,-2,-2,-2,-2,
};
int d[4+1];
int s = 26, t = 42;
#if USESTACK==1
    int stack[MAXSIZE+1];
    int top;
#else
    int queue[MAXSIZE+1];
    int nq, rear, front;
#endif

void insert(int x);
int pick_cell();
void print(const char* str);
void init();
void find_path(int s, int t);
void back_trace(int x, int y);
void initW();

void init()
{
    initW();
    d[1] = -(m+2);
    d[2] = m+2;
    d[3] = -1;
    d[4] = 1;
}

void find_path(int s, int t)
{
    int i, p;
    cell[s] = 0;
    insert(s);
    p = pick_cell();
    while (p > 0 && cell[t] == AVAIL) {
        for (i = 1; i <= 4; i++) {
            if (cell[p+d[i]] == AVAIL) {
                cell[p+d[i]] = cell[p] + 1;
                insert(p+d[i]);
            }
        }
        p = pick_cell();
    }
    if (cell[t] != AVAIL)
        back_trace(t, s);
    else
        print("S と T の間には経路がない");
}

void back_trace(int x, int y)
{
    int i, p;
    printf("経路：%d", x);
    p = x;
    while (p != y) {
        for (i = 1; i <= 4; i++) {
            if (cell[p+d[i]] == cell[p] - 1) {
                p += d[i];
                printf("←%d", p);
                break;
            }
        }
    }
    putchar('\n');
}

void initW()
{
#if USESTACK==1
    top = 0;
#else
    nq = rear = front = 0;
#endif
}

void insert(int x)
{
#if USESTACK==1
    if (top >= MAXSIZE) {
        print("スタックが満杯です");
    } else {
        top++;
        stack[top] = x;
    }
#else
    if (nq == MAXSIZE) {
        print("キューが満杯です");
    } else {
        nq++;
        rear = (rear % MAXSIZE) + 1;
        queue[rear] = x;
    }
#endif
}

int pick_cell()
{
#if USESTACK==1
    if (top <= 0) {
        return 0;
    } else {
        top--;
        return stack[top+1];
    }
#else
    if (nq == 0) {
        return 0;
    } else {
        nq--;
        front = (front % MAXSIZE) + 1;
        return queue[front];
    }
#endif
    return 0;
}

void print(const char* str)
{
    puts(str);
}

void print_array()
{
    int i;
    for (i = 0; i < MAXSIZE; i++) {
        if (i == s)
            printf(" S ");
        else if (i == t)            
            printf(" T ");
        else if (cell[i] == OBST)
            printf("■ ");
        else if (cell[i] == AVAIL)
            printf("□ ");
        else
            printf("%2d ", cell[i]);
        if (i % (m+2) == m+1) putchar('\n');
    }
    putchar('\n');
}

int main()
{
#if USESTACK==1
    print("スタック方式");
#else
    print("キュー方式");
#endif

    init();
    find_path(s, t);
    print_array();
    return 0;
}