用队列实现广度优先搜索（BFS），找出最短路径。用栈保存走过的路径，并输出路径和标识最短路径的地图。
输入用例： 0：路 1：墙壁
24 24
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0
0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0
0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0
0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0
0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0
1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0
0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0
0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0
0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0
0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0
1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0
0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0 0 1 1 1 1 0 0 0
0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0
0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0
0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0 0 1 1 1 0 1 1 0
1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
这里需要在头文件中重新定义队列和栈的数据元素类型 SElemType, QElemType。
define.h
// define.h
#ifndef __MENGQL_DEFINE__
#define __MENGQL_DEFINE__
#define C_LOG_DBG(format, ...)
//printf("[%s@%s,%d] " format ,__FUNCTION__, __FILE__, __LINE__, ##__VA_ARGS__);
#define C_LOG_ERR(format, ...) printf("[%s@%s,%d] " format ,__FUNCTION__, __FILE__, __LINE__, ##__VA_ARGS__);
typedef enum EStatus {ERROR, OK} Status;
typedef struct
{
int x;
int y;
}PosType;
typedef struct
{
PosType seat;
PosType parent;
}SElemType, QElemType;
#endif
Maze.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include "define.h"
4 #include "SqQueue.h"
5 #include "SqStack.h"
6
7 int **g_MazeMap;
8 int g_m, g_n;//g_m:列数 g_n:行数
9
10 #define SHORTPRINT 5
11 #define FOOTPRINT 2
12
13 void MazePrint()
14 {
15 int i, j;
16 for(i=0; i<g_m; ++i)
17 {
18 for(j=0; j<g_n; ++j)
19 {
20 printf("%d ", g_MazeMap[i][j]);
21 }
22 printf("\n");
23 }
24 }
25 void MarkShortPrint(PosType pos)
26 {
27 g_MazeMap[pos.y][pos.x] = SHORTPRINT;
28 }
29 void MazePrintRes(SqStack *S, int x, int y)
30 {
31 SElemType e;
32 while(StackEmpty(S) != OK)
33 {
34 PopStack(S, &e);
35 if(e.seat.x == x && e.seat.y == y)
36 {
37 MazePrintRes(S, e.parent.x, e.parent.y);
38 MarkShortPrint(e.seat);
39 printf("[%d][%d]\n", x, y);
40 }
41 }
42 return;
43 }
44 Status MazePass(PosType pos)
45 {
46 if(pos.x>=0 && pos.y>=0 && pos.x<g_m && pos.y<g_n && g_MazeMap[pos.y][pos.x] == 0)
47 {
48 return OK;
49 }
50 return ERROR;
51 }
52 void MazeFootPrint(PosType pos)
53 {
54 g_MazeMap[pos.y][pos.x] = FOOTPRINT;
55 }
56 void NextPos(PosType *pos, int di)
57 {
58 switch(di)
59 {
60 case 1:
61 pos->y = pos->y-1;
62 break;
63 case 2:
64 pos->x = pos->x-1;
65 break;
66 case 3:
68 pos->y = pos->y+1;
69 break;
70 case 4:
71 pos->x = pos->x+1;
72 break;
73 defult:
74 break;
75 }
76 }
77 Status MazeShortPath(PosType *start, PosType *end)
78 {
79 PosType curPos, nextPos;
80 SElemType e;
81 SqQueue Q;
82 SqStack resS;//保存遍历过的节点，用于输出结果
83 int i;
84
85 e.parent.x = -1;
86 e.parent.y = -1;
87 e.seat.x = start->x;
88 e.seat.y = start->y;
89
90 if(InitQueue(&Q) != OK || InitStack(&resS) != OK)
91 {
92 return ERROR;
93 }
94 do
95 {
96 PushStack(&resS, e);
97 curPos.x = e.seat.x;
98 curPos.y = e.seat.y;
99 for(i=1; i<5; ++i)
100 {
101 C_LOG_DBG("[%d][%d][%d]\n", curPos.x, curPos.y, i);
102 nextPos.x = curPos.x;
103 nextPos.y = curPos.y;
104 NextPos(&nextPos, i);
105 if(MazePass(nextPos) == OK)
106 {
107 C_LOG_DBG("[%d][%d][%d][%d][%d]\n", curPos.x, curPos.y, nextPos.x, nextPos.y);
108 e.parent.x = curPos.x;
109 e.parent.y = curPos.y;
110 e.seat.x = nextPos.x;
111 e.seat.y = nextPos.y;
112
113 EnQueue(&Q, e);
114 MazeFootPrint(nextPos);
115
116 if(nextPos.x == end->x && nextPos.y == end->y)
117 {
118 C_LOG_DBG("%s\n", "DEBUG");
119 PushStack(&resS, e);
120 MazePrintRes(&resS, nextPos.x, nextPos.y);
121 MazePrint();
122 return OK;
123 }
124 }
125 }
126 }while(DeQueue(&Q, &e) == OK);
127 MazePrint();
128 DestoryStack(&resS);
129 DestroyQueue(&Q);
130 return ERROR;
131 }
132
133 int main()
134 {
135 int i, j;
136 PosType start;
137 PosType end;
138
139 scanf("%d %d", &g_m, &g_n);
140 start.x = 0;
141 start.y = 0;
142 end.x = g_m-1;
143 end.y = g_n-1;
144
145 g_MazeMap=(int**)malloc(g_m*sizeof(int*));
146 for(i=0; i<g_m; ++i)
147 {
148 g_MazeMap[i] = (int*)malloc(g_n*sizeof(int));
149 }
150 for(i=0; i<g_m; ++i)
151 {
152 for(j=0; j<g_n; ++j)
153 {
154 scanf("%d", &g_MazeMap[i][j]);
155 }
156 }
157 MazeShortPath(&start, &end);
158 }
输出结果（路径坐标）：
[0][0]
[0][1]
[0][2]
[0][3]
[0][4]
[1][4]
[2][4]
[2][5]
[3][5]
[4][5]
[4][6]
[4][7]
[5][7]
[6][7]
[7][7]
[7][8]
[7][9]
[7][10]
[7][11]
[7][12]
[8][12]
[9][12]
[10][12]
[10][13]
[11][13]
[12][13]
[12][14]
[12][15]
[13][15]
[14][15]
[15][15]
[15][16]
[15][17]
[15][18]
[15][19]
[15][20]
[16][20]
[17][20]
[18][20]
[18][21]
[19][21]
[20][21]
[20][22]
[20][23]
[21][23]
[22][23]
[23][23]
输出结果（路径地图）：0：没走过的路 1：墙壁 2：走过的路 5：走过的路中的最短路径
5 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2
5 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2
5 2 2 2 1 1 2 2 2 2 2 2 1 1 2 2 2 2 2 2 1 1 2 2
5 1 1 1 1 2 2 2 2 1 1 1 1 2 2 2 2 1 1 1 1 2 2 2
5 5 5 1 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 1 2 2 2 2
2 1 5 5 5 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2
2 1 1 1 5 1 1 2 2 1 1 1 2 1 1 2 2 1 1 1 2 1 1 2
1 2 2 2 5 5 5 5 1 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2
2 2 1 2 2 2 1 5 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2
2 2 1 2 2 2 1 5 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 2
2 2 2 2 1 1 2 5 2 2 2 2 1 1 2 2 2 2 2 2 1 1 2 2
2 1 1 1 1 2 2 5 2 1 1 1 1 2 2 2 2 1 1 1 1 2 2 2
2 2 2 1 2 2 2 5 5 5 5 1 2 2 2 2 2 2 2 1 2 2 2 2
2 1 2 2 2 1 2 2 2 1 5 5 5 1 2 2 2 1 2 2 2 1 2 2
2 1 1 1 2 1 1 2 2 1 1 1 5 1 1 2 2 1 1 1 2 1 1 2
1 2 2 2 2 2 2 2 1 2 2 2 5 5 5 5 1 2 2 2 2 2 2 2
2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 5 2 2 1 2 2 2 1 2
2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 5 2 2 1 2 2 2 1 2
2 2 2 2 1 1 2 2 2 2 2 2 1 1 2 5 2 2 2 2 1 1 2 2
2 1 1 1 1 2 2 2 2 1 1 1 1 2 2 5 2 1 1 1 1 2 2 2
2 2 2 1 2 2 2 2 2 2 2 1 2 2 2 5 5 5 5 1 2 2 2 2
2 1 2 2 2 1 2 2 2 1 2 2 2 1 2 2 2 1 5 5 5 1 2 2
2 1 1 1 2 1 1 2 2 1 1 1 2 1 1 2 2 1 1 1 5 1 1 0
1 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 1 2 2 2 5 5 5 5