C言語で書いてみました

■ 経路探索2（平成16年午後2問1設問5）

#include <stdio.h>

/* 平成16年午後2問1設問5 */

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

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

int cell[MAXSIZE] = {
     OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST,
     OBST,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL, OBST, OBST, OBST,
     OBST,AVAIL,AVAIL,AVAIL,AVAIL, OBST,AVAIL,AVAIL, OBST, OBST, OBST,
     OBST,AVAIL,AVAIL,AVAIL,AVAIL, OBST,AVAIL, OBST, OBST,AVAIL, OBST,
     OBST,AVAIL, OBST,AVAIL,AVAIL, OBST,AVAIL,AVAIL,AVAIL,AVAIL, OBST,
     OBST, OBST, OBST, OBST,AVAIL, OBST,AVAIL, OBST,AVAIL,AVAIL, OBST,
     OBST,AVAIL, OBST,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL, OBST, OBST, OBST,
     OBST,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL,AVAIL, OBST, OBST, OBST,
     OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST, OBST,
};
int d[4+1] = { 0, -(m+2), m+2, -1, 1 };
int s = 26, t = 42;
#if USESTACK==1
    int Ws_stack[MAXSIZE+1], Wt_stack[MAXSIZE+1];
    int Ws_top, Wt_top;
#else
    int Ws_queue[MAXSIZE+1], Wt_queue[MAXSIZE+1];
    int Ws_nq, Ws_rear, Ws_front;
    int Wt_nq, Wt_rear, Wt_front;
#endif

void Ws_insert(int x);
void Wt_insert(int x);
int Ws_pick_cell();
int Wt_pick_cell();
void print(const char* str);
void find_path(int s, int t);
void back_trace(int x, int y);
void initW();

void find_path(int s, int t)
{
    int i, j, p, q, met_s, met_t;

    cell[s] = 1; cell[t] = -n*m; met_s = met_t = 0;
    initW();
    Ws_insert(s); Wt_insert(t);
    while (1) {
        p = Ws_pick_cell();
        if (p != 0) {
            for (i = 1; i <= 4; i++) {
                if (-n*m <= cell[p+d[i]] && cell[p+d[i]] <= -1) {
                    met_s = p;
                    break;
                } else if (cell[p+d[i]] == AVAIL) {
                    cell[p+d[i]] = cell[p] + 1;
                    Ws_insert(p+d[i]);
                }
            }
        }
        if (met_s != 0) break;

        q = Wt_pick_cell();
        if (q != 0) {
            for (j = 1; j <= 4; j++) {
                if (1 <= cell[q+d[j]] && cell[q+d[j]] <= n*m) {
                    met_t = q;
                    break;
                } else if (cell[q+d[j]] == AVAIL) {
                    cell[q+d[j]] = cell[q] + 1;
                    Wt_insert(q+d[j]);
                }
            }
        }
        if (met_t != 0) break;

        if (p == 0 && q == 0) break;
    }
    
    if (met_s > 0) {
        back_trace(met_s+d[i], t);
        back_trace(met_s, s);
    } else if (met_t > 0) {
        back_trace(met_t, t);
        back_trace(met_t+d[j], 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
    Ws_top = Wt_top = 0;
#else
    Ws_nq = Ws_rear = Ws_front = 0;
    Wt_nq = Wt_rear = Wt_front = 0;
#endif
}

void Ws_insert(int x)
{
#if USESTACK==1
    if (Ws_top >= MAXSIZE) {
        print("スタックが満杯です");
    } else {
        Ws_top++;
        Ws_stack[Ws_top] = x;
    }
#else
    if (Ws_nq == MAXSIZE) {
        print("キューが満杯です");
    } else {
        Ws_nq++;
        Ws_rear = (Ws_rear % MAXSIZE) + 1;
        Ws_queue[Ws_rear] = x;
    }
#endif
}

void Wt_insert(int x)
{
#if USESTACK==1
    if (Wt_top >= MAXSIZE) {
        print("スタックが満杯です");
    } else {
        Wt_top++;
        Wt_stack[Wt_top] = x;
    }
#else
    if (Wt_nq == MAXSIZE) {
        print("キューが満杯です");
    } else {
        Wt_nq++;
        Wt_rear = (Wt_rear % MAXSIZE) + 1;
        Wt_queue[Wt_rear] = x;
    }
#endif
}

int Ws_pick_cell()
{
#if USESTACK==1
    if (Ws_top <= 0) {
        return 0;
    } else {
        Ws_top--;
        return Ws_stack[Ws_top+1];
    }
#else
    if (Ws_nq == 0) {
        return 0;
    } else {
        Ws_nq--;
        Ws_front = (Ws_front % MAXSIZE) + 1;
        return Ws_queue[Ws_front];
    }
#endif
    return 0;
}

int Wt_pick_cell()
{
#if USESTACK==1
    if (Wt_top <= 0) {
        return 0;
    } else {
        Wt_top--;
        return Wt_stack[Wt_top+1];
    }
#else
    if (Wt_nq == 0) {
        return 0;
    } else {
        Wt_nq--;
        Wt_front = (Wt_front % MAXSIZE) + 1;
        return Wt_queue[Wt_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("%+3d", cell[i]);
        if (i % (m+2) == m+1) putchar('\n');
    }
    putchar('\n');
}

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

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