• 类似于推箱子的小游戏 寻找 最短路径

    实现效果如下

    类似 推箱子小游戏 的变种 C/C++版本 BFS最短路径

    黑色代表墙壁 不能越过

    蓝色代表HOME点 灰色代表要找的小箱子

    绿色代表路径  

    最终目标是将灰色的小箱子移动到蓝色的HOME点 

    需要两次搜索 第一次是 出发点到灰色小箱子 

    第二次是灰色小箱子到蓝色HOME点

    BFS 搜索路径之后 找到一条最短路径 

    动画效果用的是JAVA的 一个jar包 

    完整的代码 包含动画效果已经上传点击这里下载

    C语言编译

    gcc box.c graphics.c -o box

    C++编译

    g++ box.cpp graphics.c -o box

    需要安装jdk 17版本  

    如下图 将 安装包里的所有文件放到 如下的jdk bin目录  

    执行如下代码  最后的jar包 用绝对路径就可以 

    ./box | ./java -jar /home/QMCY/robot/drawppp.jar

    代码很乱  临时记录下 

    C语言版本

    1.  
    2. #include 
    3. #include "graphics.h"
    4.  
    5.  
    6.  
    7. #define GRID_SIZE_X 10
    8. #define GRID_SIZE_Y 10
    9.  
    10. #define ROBOT 'R'
    11.  
    12.  
    13.  
    14. #define HOME_X	9
    15. #define HOME_Y	2
    16.  
    17.  
    18. #define MARKER_X	7
    19. #define MARKER_Y	7
    20.  
    21.  
    22. #define	ROBOT_X	6
    23. #define	ROBOT_Y	5
    24. #define MAX_N	105
    25.  
    26. int g_father[MAX_N][MAX_N];	
    27. int dist[MAX_N][MAX_N];
    28. int last_dir[MAX_N][MAX_N];
    29.  
    30. int dir[MAX_N*MAX_N];	
    31.  
    32.  
    33. char g_has_visited[MAX_N][MAX_N];		//vis[x][y]表示xy点是否遍历过
    34.  
    35.  
    36. int Queue[MAX_N*MAX_N];			//用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u  出队是u=Q[front++]
    37.  
    38. int coordinate[MAX_N*MAX_N];
    39.  
    40.  
    41. const int squareSize = 50;
    42.  
    43. const int windowSize = 600;
    44.  
    45. // Define the grid elements
    46. const char EMPTY = ' ';
    47. const char BLOCK = '#';
    48. const char MARKER = '*';
    49. const char HOME = 'H';
    50.  
    51. // Define robot directions
    52. typedef enum   { WEST , EAST, NORTH, SOUTH }Direction;
    53.  
    54. // Define the robot struct
    55. typedef struct  {
    56.     int x;
    57.     int y;
    58.     Direction direction;
    59.     char carryingMarker;
    60. }Robot;
    61.  
    62.  
    63. void drawStep(int homeX, int homeY)
    64. {
    65.     background();
    66.     setColour(pink);
    67.     homeX = homeX*squareSize;
    68.     homeY = homeY*squareSize;
    69.     fillRect(homeX, homeY, squareSize, squareSize);
    70. }
    71.  
    72. void drawMarker(int x,int y)
    73. {
    74.     background();
    75.  
    76.     setColour(gray);
    77.  
    78.     x = x*squareSize;
    79.     y = y*squareSize;
    80.  
    81. 	 
    82.     fillRect(x, y, squareSize, squareSize); 
    83. }
    84.  
    85.  
    86. void drawBlocks(int x,int y)
    87. {
    88.     background();
    89.     setColour(black);
    90.  
    91.     x = x*squareSize;
    92.     y = y*squareSize;
    93.  
    94. 	 
    95.     fillRect(x, y, squareSize, squareSize); 
    96. }
    97.  
    98.  
    99. void drawEmpty(int x,int y)
    100. {
    101.     foreground();
    102.     setColour(white);
    103.  
    104.     x = x*squareSize;
    105.     y = y*squareSize;
    106.  
    107. 	 
    108.     fillRect(x, y, squareSize, squareSize); 
    109. }
    110.  
    111.  
    112. // Function to initialize the grid
    113. void initializeGrid(char grid[][GRID_SIZE_Y]) {
    114.     // Initialize the grid with empty spaces
    115.     for (int i = 0; i < GRID_SIZE_X; ++i) {
    116.         for (int j = 0; j < GRID_SIZE_X; ++j) {
    117.             grid[i][j] = EMPTY;
    118.         }
    119.     }
    120.  
    121.     // Place blocks, markers, and home square
    122. 	grid[8][8] = BLOCK;
    123.  
    124. 	grid[9][5] = BLOCK;
    125. 	grid[8][5] = BLOCK;
    126.  
    127. 	grid[7][5] = BLOCK;
    128.  
    129.  
    130. 	grid[6][7] = BLOCK;
    131. 	grid[8][7] = BLOCK; 
    132. 	grid[7][8] = BLOCK; 
    133. 	grid[7][8] = BLOCK; 
    134.  
    135. 	grid[2][2] = BLOCK; 
    136. 	grid[3][3] = BLOCK; 
    137. 	grid[4][4] = BLOCK; 
    138. 	grid[5][5] = BLOCK; 
    139. 	grid[6][6] = BLOCK; 
    140.  
    141. 	 
    142.     grid[MARKER_X][MARKER_Y] = MARKER;
    143.  
    144.     grid[HOME_X][HOME_Y] = HOME;
    145. }
    146.  
    147. // Function to display the grid
    148. void displayGrid(const char grid[][GRID_SIZE_X]) {
    149.  
    150.  
    151. 	setWindowSize(windowSize, windowSize);
    152. 	background();   // Must draw on the background layer.
    153.  
    154. 	int x;
    155. 	int y;
    156.  
    157. 	for (x=0; x
    158. 	{
    159. 		for (y=0; y
    160. 		{
    161. 		   drawRect(x*squareSize, y*squareSize, 
    162. 		   squareSize, squareSize);
    163. 		}
    164. 	}
    165.  
    166. }
    167.  
    168.  
    169. void draw_north(int x, int y)
    170. {
    171.     int x_coords[] = {x, x+50, x+25};
    172.     int y_coords[] = {y+50, y+50, y};
    173.     fillPolygon(3, x_coords, y_coords);
    174. }
    175.  
    176. void draw_east(int x, int y)
    177. {
    178.     int x_coords[] = {x, x, x+50};
    179.     int y_coords[] = {y, y+50, y+25};
    180.     fillPolygon(3, x_coords, y_coords);
    181. }
    182.  
    183. void draw_south(int x, int y)
    184. {
    185.     int x_coords[] = {x, x+50, x+25};
    186.     int y_coords[] = {y, y, y+50};
    187.     fillPolygon(3, x_coords, y_coords);
    188. }
    189.  
    190. void draw_west(int x, int y)
    191. {
    192.     int x_coords[] = {x+50, x+50, x};
    193.     int y_coords[] = {y, y+50, y+25};
    194.     fillPolygon(3, x_coords, y_coords);
    195. }
    196.  
    197. // Function to drop a marker
    198. void dropMarker(Robot *robot, char grid[][GRID_SIZE_X]) {
    199.     if (!robot->carryingMarker) {
    200.         return; // Robot is not carrying a marker
    201.     }
    202.  
    203.     grid[robot->x][robot->y] = MARKER;
    204.     robot->carryingMarker = 0;
    205. 	//drawRobot(robot.x, robot.y, (int)robot.direction);
    206.  
    207. }
    208.  
    209. void drawRobot(int x, int y, int direction)
    210. {
    211.     foreground();
    212.     clear();
    213.  
    214. 	 
    215.     setColour(green);
    216.     x = x*squareSize;
    217.     y = y*squareSize;
    218.  
    219. 	 switch (direction)
    220.     {
    221.         case NORTH: draw_north(x, y); break;
    222.         case EAST: draw_east(x, y); break;
    223.         case SOUTH: draw_south(x, y); break;
    224.         case WEST: draw_west(x, y); break;
    225.     }
    226.  
    227. 	 
    228. }
    229.  
    230. void drawRobotWithBg(int x, int y, int direction)
    231. {
    232.  
    233. 	foreground();
    234. 	clear();
    235. 	setColour(gray);
    236.  
    237. 	x = x*squareSize;
    238. 	y = y*squareSize;
    239.  
    240.  
    241. 	fillRect(x, y, squareSize, squareSize); 
    242.  
    243. 	setColour(green);
    244.  
    245. 	switch (direction)
    246. 	{
    247. 	  case NORTH: draw_north(x, y); break;
    248. 	  case EAST: draw_east(x, y); break;
    249. 	  case SOUTH: draw_south(x, y); break;
    250. 	  case WEST: draw_west(x, y); break;
    251. 	}
    252.  
    253. }
    254.  
    255.  
    256. void drawHome(int homeX, int homeY)
    257. {
    258.     background();
    259.     setColour(blue);
    260.     homeX = homeX*squareSize;
    261.     homeY = homeY*squareSize;
    262.     fillRect(homeX, homeY, squareSize, squareSize);
    263. }
    264.  
    265.  
    266. void drawShort(int homeX, int homeY)
    267. {
    268.     background();
    269.     setColour(orange);
    270.     homeX = homeX*squareSize;
    271.     homeY = homeY*squareSize;
    272.     fillRect(homeX, homeY, squareSize, squareSize);
    273. }
    274.  
    275.  
    276.  
    277.  
    278. void forward(Robot *robot, char grid[][GRID_SIZE_X]) {
    279.     // Calculate the next position based on the direction
    280.     int nextX = robot->x;
    281.     int nextY = robot->y;
    282.  
    283.     if (robot->direction == NORTH) {
    284.         --nextX;
    285.     } else if (robot->direction == SOUTH) {
    286.         ++nextX;
    287.     } else if (robot->direction == EAST) {
    288.         ++nextY;
    289.     } else if (robot->direction == WEST) {
    290.         --nextY;
    291.     }
    292.  
    293.     // Check if the next position is valid
    294.     if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
    295.         // Move the robot
    296.         grid[robot->x][robot->y] = EMPTY;
    297.         robot->x = nextX;
    298.         robot->y = nextY;
    299.         grid[robot->x][robot->y] = ROBOT;
    300.     }
    301.  
    302. 	drawRobot(robot->x, robot->y, robot->direction);
    303.  
    304. 	 
    305. }
    306.  
    307.  
    308.  
    309. char markersLeft(char grid[][GRID_SIZE_X]) {
    310.     for (int i = 0; i < GRID_SIZE_X; ++i) {
    311.         for (int j = 0; j < GRID_SIZE_X; ++j) {
    312.             if (grid[i][j] == MARKER) {
    313.                 return 1;
    314.             }
    315.         }
    316.     }
    317.     return 0;
    318. }
    319.  
    320.  
    321. void turn_left(Robot *robot) {
    322.     if (robot->direction == NORTH) {
    323.         robot->direction = WEST;
    324.     } else if (robot->direction == SOUTH) {
    325.         robot->direction = EAST;
    326.     } else if (robot->direction == EAST) {
    327.         robot->direction = NORTH;
    328.     } else if (robot->direction == WEST) {
    329.         robot->direction = SOUTH;
    330.     }
    331.  
    332. 	drawRobot(robot->x, robot->y, robot->direction);
    333.  
    334. }
    335.  
    336.  
    337. void turn_right(Robot *robot) {
    338.     if (robot->direction == NORTH) {
    339.         robot->direction = EAST;
    340.     } else if (robot->direction == SOUTH) {
    341.         robot->direction = WEST;
    342.     } else if (robot->direction == EAST) {
    343.         robot->direction = SOUTH;
    344.     } else if (robot->direction == WEST) {
    345.         robot->direction = NORTH;
    346.     }
    347.  
    348. 	drawRobot(robot->x, robot->y, robot->direction);
    349.  
    350. 	 
    351. }
    352.  
    353. // Function to pick up a marker
    354. void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
    355.     if (grid[robot->x][robot->y] == MARKER) {
    356.         robot->carryingMarker = 1;
    357.         grid[robot->x][robot->y] = EMPTY;
    358.     }
    359. }
    360.  
    361.  
    362. void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
    363.     while (markersLeft(grid)) {
    364.         int initialX = robot->x;
    365.         int initialY = robot->y;
    366.         Direction initialDirection = robot->direction;
    367.  
    368.         // Use the "right hand rule" to navigate
    369.         if (robot->direction == NORTH) {
    370.             if (grid[robot->x][robot->y + 1] != BLOCK) {
    371.                 turn_right(robot);
    372.             } else if (grid[robot->x - 1][robot->y] != BLOCK) {
    373.                 forward(robot, grid);
    374.             } else {
    375.                 turn_left(robot);
    376.             }
    377.         } else if (robot->direction == SOUTH) {
    378.             if (grid[robot->x][robot->y - 1] != BLOCK) {
    379.                 turn_right(robot);
    380.             } else if (grid[robot->x + 1][robot->y] != BLOCK) {
    381.                 forward(robot, grid);
    382.             } else {
    383.                 turn_left(robot);
    384.             }
    385.         } else if (robot->direction == EAST) {
    386.             if (grid[robot->x + 1][robot->y] != BLOCK) {
    387.                 turn_right(robot);
    388.             } else if (grid[robot->x][robot->y + 1] != BLOCK) {
    389.                 forward(robot, grid);
    390.             } else {
    391.                 turn_left(robot);
    392.             }
    393.         } else if (robot->direction == WEST) {
    394.             if (grid[robot->x - 1][robot->y] != BLOCK) {
    395.                 turn_right(robot);
    396.             } else if (grid[robot->x][robot->y - 1] != BLOCK) {
    397.                 forward(robot, grid);
    398.             } else {
    399.                 turn_left(robot);
    400.             }
    401.         }
    402.  
    403.         if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
    404.             // Robot is stuck, rotate 180 degrees
    405.             turn_left(robot);
    406.             turn_left(robot);
    407.         }
    408.  
    409.         forward(robot, grid);
    410.         sleep(500); // Adjust sleep duration for animation speed
    411.         pickUpMarker(robot, grid);
    412.     }
    413. }
    414.  
    415.  
    416. int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X]) 
    417. {
    418. 	int nextX = robot->x;
    419. 	int nextY = robot->y;
    420.  
    421.  if (robot->direction == NORTH) {
    422. 		  --nextY;
    423. 	 } else if (robot->direction == SOUTH) {
    424. 		  ++nextY;
    425. 	 } else if (robot->direction == EAST) {
    426. 		  ++nextX;
    427. 	 } else if (robot->direction == WEST) {
    428. 		  --nextX;
    429. 	 }
    430.  
    431.  
    432.     // Check if the next position is valid
    433.     if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
    434.         // Move the robot
    435.         //grid[robot->x][robot->y] = EMPTY;
    436.         //robot->x = nextX;
    437.       	//robot->y = nextY;
    438.         //grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
    439.         return 1;
    440.     }
    441.  
    442. 	 
    443.  
    444. 	 return 0;
    445.  
    446. }
    447.  
    448.  
    449.  
    450.  
    451.  
    452. // Function to turn the robot left (anti-clockwise)
    453. void left(Robot *robot) {
    454.     if (robot->direction == NORTH) {
    455.         robot->direction = WEST;
    456.     } else if (robot->direction ==SOUTH) {
    457.         robot->direction = EAST;
    458.     } else if (robot->direction == EAST) {
    459.         robot->direction =NORTH;
    460.     } else if (robot->direction == WEST) {
    461.         robot->direction = SOUTH;
    462.     }
    463.  
    464. 	 drawRobot(robot->x, robot->y, robot->direction);
    465.  
    466. 	 
    467. }
    468.  
    469. void right(Robot *robot) {
    470.     if (robot->direction == NORTH) {
    471.         robot->direction = EAST;
    472.     } else if (robot->direction ==SOUTH) {
    473.         robot->direction = WEST;
    474.     } else if (robot->direction ==EAST) {
    475.         robot->direction =SOUTH;
    476.     } else if (robot->direction == WEST) {
    477.         robot->direction =NORTH;
    478.     }
    479.  
    480. 	 drawRobot(robot->x, robot->y, robot->direction);
    481.  
    482. 	 
    483. }
    484.  
    485.  
    486.  
    487.  
    488. #if 0
    489. bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
    490.     // Use BFS to find the shortest path
    491.     std::queueint, int>> q; // Queue for BFS
    492.     std::vectorbool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
    493.  
    494.     q.push({robot.x, robot.y});
    495.     visited[robot.x][robot.y] = true;
    496.  
    497. 	int u=robot.x*GRID_SIZE_X+robot.y;
    498.  
    499. 	father[robot.x][robot.y]=u;
    500.  
    501.  
    502. 	sleep(2000);
    503.     while (!q.empty()) {
    504.         int x = q.front().first;
    505.         int y = q.front().second;
    506.         q.pop();
    507. 		  //drawRobot(x, y, (int)robot.direction);
    508.  
    509.         if (grid[x][y] == MARKER) {
    510.             // Found a marker, pick it up
    511.             pickUpMarker(robot, grid);
    512.             //dropMarker(robot, grid);
    513. 		  		//drawMarker(MARKER_X,MARKER_Y);
    514. 		  		//drawRobot(x, y, (int)robot.direction);
    515.  
    516. 			
    517.             return true;
    518.         }
    519.  
    520.         // Explore neighboring cells
    521.         int dx[] = {-1, 1, 0, 0};
    522.         int dy[] = {0, 0, -1, 1};
    523.  
    524.         for (int i = 0; i < 4; ++i) {
    525.  
    526.  
    527. 		 		u=x*GRID_SIZE_X+y;
    528.             int nx = x + dx[i];
    529.             int ny = y + dy[i];
    530.  
    531.             if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
    532.                 q.push({nx, ny});
    533. 					 //drawRobot(nx, ny, (int)robot.direction);
    534. 						//drawStep(nx, ny);
    535. 					
    536. 					 //printf("x=%d y=%d \n",nx,ny);
    537.                 visited[nx][ny] = true;
    538. 					father[nx][ny]=u;
    539. 					//dist[nx][ny]=1+dist[x][y];
    540. 					last_dir[nx][ny]=i;
    541.             }
    542. 				
    543.  
    544. 					// printf("findShortestPath 2222\n");
    545. 				 //sleep(200);
    546.         }
    547.  
    548. 		  //drawRobot(robot.x, robot.y, (int)robot.direction);
    549. 		  //sleep(10);
    550.     }
    551.  
    552.     return false; // No markers found
    553. }
    554.  
    555.  
    556. #else
    557.  
    558. char findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X]) {
    559.    // Use BFS to find the shortest path
    560.     //std::queue> q; // Queue for BFS
    561.     //std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));
    562. 	int front,rear;
    563.  
    564. 	rear=front=0;
    565.  
    566. 	Robot robot;
    567.  
    568. 	
    569. 	robot.x = robot_x;
    570. 	robot.y = robot_y;
    571.     //q.push({robot.x, robot.y});
    572.  
    573.  
    574. 	g_has_visited[robot.x][robot.y] = 1;
    575.  
    576. 	
    577.  
    578.  
    579. 	int u=robot.x*GRID_SIZE_X+robot.y;
    580.  
    581. 	g_father[robot.x][robot.y]=u;
    582. 	Queue[rear++]=u;
    583.  
    584.  
    585. 	sleep(1000);
    586.     //while (!q.empty()) 
    587. 	 	while(rear>front)
    588. 	 	{
    589. 	 		u=Queue[front++];
    590. 			
    591.         //int x = q.front().first;
    592.         //int y = q.front().second;
    593.         //q.pop();
    594. 		  //drawRobot(x, y, (int)robot.direction);
    595. 		  int x=u/GRID_SIZE_X;
    596. 		  int y=u%GRID_SIZE_X;
    597.  
    598.         if (grid[x][y] == MARKER) {
    599.             // Found a marker, pick it up
    600.             pickUpMarker(&robot, grid);
    601.             //dropMarker(robot, grid);
    602. 		  		//drawMarker(MARKER_X,MARKER_Y);
    603. 		  		//drawRobot(x, y, (int)robot.direction);
    604.  
    605. 			
    606.             return 1;
    607.         }
    608.  
    609.         // Explore neighboring cells
    610.         int dx[] = {-1, 1, 0, 0};
    611.         int dy[] = {0, 0, -1, 1};
    612.  
    613.         for (int i = 0; i < 4; ++i) {
    614.  
    615.  
    616. 		 		//u=x*GRID_SIZE_X+y;
    617.             int nx = x + dx[i];
    618.             int ny = y + dy[i];
    619.  
    620.             if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !g_has_visited[nx][ny] && grid[nx][ny] != BLOCK) {
    621.  
    622. 					 #if 0
    623. 					 q.push({nx, ny});
    624. 					 //drawRobot(nx, ny, (int)robot.direction);
    625. 						//drawStep(nx, ny);
    626. 					
    627. 					 //printf("x=%d y=%d \n",nx,ny);
    628.                 visited[nx][ny] = true;
    629. 					father[nx][ny]=u;
    630. 					//dist[nx][ny]=1+dist[x][y];
    631. 					last_dir[nx][ny]=i;
    632.  
    633. 					#else
    634. 						int v=nx*GRID_SIZE_X+ny;
    635. 						Queue[rear++]=v;
    636. 						g_has_visited[nx][ny]=1;
    637. 						g_father[nx][ny]=u;
    638. 						dist[nx][ny]=1+dist[x][y];
    639. 						last_dir[nx][ny]=i;
    640. 						if (grid[nx][ny] == MARKER)
    641. 						{
    642. 							return 1;
    643. 						}
    644. 					#endif
    645. 					
    646.             }
    647. 				
    648.  
    649. 					// printf("findShortestPath 2222\n");
    650. 				 //sleep(200);
    651.         }
    652.  
    653. 		  //drawRobot(robot.x, robot.y, (int)robot.direction);
    654. 		  //sleep(10);
    655.     }
    656.  
    657.     return 0; // No markers found
    658. }
    659.  
    660. #endif
    661.  
    662. void print_path(int x,int y,char has_marker)
    663. {
    664. 	int steps=0;
    665. 	int pos_x,pos_y,direction;
    666. 	while(1)
    667. 	{
    668. 		int u=g_father[x][y];
    669. 		int fx=u/GRID_SIZE_X;
    670. 		int fy=u%GRID_SIZE_X;
    671. 		if (fx==x && fy==y)
    672. 		{
    673. 			break;
    674. 		}
    675. 		dir[steps]=last_dir[x][y];
    676. 		coordinate[steps++] = x*GRID_SIZE_X+y;
    677. 			
    678. 		x=fx;
    679. 		y=fy;
    680. 	}
    681. 	while(steps--)
    682. 	{
    683. 		pos_x = coordinate[steps]/GRID_SIZE_X;
    684. 		pos_y = coordinate[steps]%GRID_SIZE_X;
    685. 		direction = dir[steps];
    686.  
    687. 		if(has_marker)
    688. 		{
    689. 			drawRobotWithBg(pos_x,pos_y,direction);
    690. 		}
    691. 		else
    692. 		{
    693. 			drawRobot(pos_x, pos_y,direction);
    694. 		}
    695.  
    696.  
    697. 		sleep(300);
    698. 		
    699. 	}
    700.  
    701. }
    702.  
    703.  
    704. int main(int argc, char **argv) {
    705.  
    706.     char grid[GRID_SIZE_X][GRID_SIZE_X];
    707. 	 
    708.     initializeGrid(grid);
    709.  
    710.     Robot robot;
    711.     robot.x = ROBOT_X; // Initial X position
    712.     robot.y = ROBOT_Y; // Initial Y position
    713.     robot.direction = EAST;
    714.     robot.carryingMarker = 0;
    715.  
    716.     // Display the initial grid
    717.  
    718. 	displayGrid(grid);
    719. 	drawHome(HOME_X, HOME_Y);
    720. 	drawMarker(MARKER_X,MARKER_Y);
    721.  
    722. 	drawBlocks(8,8);	
    723. 	drawBlocks(9,5);
    724. 	drawBlocks(8,5);
    725. 	drawBlocks(7,5);
    726.  
    727. 	drawBlocks(6,7);
    728. 	drawBlocks(8,7);
    729. 	drawBlocks(7,8);
    730. 	drawBlocks(2,2);
    731.  
    732. 	drawBlocks(3,3);
    733. 	drawBlocks(4,4);
    734. 	drawBlocks(5,5);
    735. 	drawBlocks(6,6);
    736.  
    737.  
    738.  
    739. 	 #if 0
    740. 	 findAndCollectMarkers(&robot, grid);
    741. 	 #elif 0
    742. 	 
    743.     while (!robot.carryingMarker) {
    744.  
    745. 		if(canMoveForward(&robot, grid))
    746. 		{
    747. 			forward(robot, grid);
    748. 		}
    749. 		else
    750. 		{
    751. 			left(robot);
    752. 		}
    753.  
    754.  
    755. 		
    756. 		sleep(500);
    757. 		  //printf("robot.x = %d  y = %d dir = %d\n",robot.x,robot.y,robot.direction);
    758.     }
    759. #else
    760. 	
    761. 	while (!findShortestPath(robot.x,robot.y, grid)) {
    762. 		  forward(&robot, grid);
    763. 		  sleep(500); // Adjust sleep duration for animation speed
    764. 	 }
    765.  
    766. 	print_path(MARKER_X,MARKER_Y,0);
    767.  
    768.  
    769.  
    770. 	robot.x = MARKER_X;
    771. 	robot.y = MARKER_Y;
    772.  
    773. 	grid[MARKER_X][MARKER_Y] = EMPTY;
    774. 	grid[HOME_X][HOME_Y] = MARKER;
    775.  
    776.  
    777. 	memset(g_has_visited,0,sizeof(g_has_visited));
    778.  
    779. 	while (!findShortestPath(robot.x,robot.y, grid)) {
    780. 		  forward(&robot, grid);
    781. 		  sleep(500); // Adjust sleep duration for animation speed
    782. 	 }
    783.  
    784. 	print_path(HOME_X,HOME_Y,1);
    785.  
    786.  
    787. #endif
    788.  
    789.  
    790.     return 0;
    791. }

    C++版本

    1.  
    2.  
    3. #include  // For sleep function
    4.  
    5. #include "graphics.h"
    6.  
    7.  
    8. #define GRID_SIZE_X 10
    9. #define GRID_SIZE_Y 10
    10.  
    11. #define ROBOT 'R'
    12.  
    13. const int maxn=105;
    14.  
    15.  
    16. #define HOME_X	0
    17. #define HOME_Y	0
    18.  
    19.  
    20. #define MARKER_X	7
    21. #define MARKER_Y	7
    22.  
    23.  
    24. #define	ROBOT_X	9
    25. #define	ROBOT_Y	0
    26.  
    27.  
    28. int father[maxn][maxn];	
    29. int dist[maxn][maxn];
    30. int last_dir[maxn][maxn];
    31.  
    32. int dir[maxn*maxn];	
    33.  
    34.  
    35. bool visit_first[maxn][maxn];		//vis[x][y]表示xy点是否遍历过
    36. bool visit_second[maxn][maxn];		//vis[x][y]表示xy点是否遍历过
    37.  
    38. int Queue[maxn*maxn];			//用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u  出队是u=Q[front++]
    39.  
    40. int coordinate[maxn*maxn];
    41.  
    42.  
    43. const int squareSize = 50;
    44.  
    45. const int windowSize = 600;
    46.  
    47. // Define the grid elements
    48. const char EMPTY = ' ';
    49. const char BLOCK = '#';
    50. const char MARKER = '*';
    51. const char HOME = 'H';
    52.  
    53. // Define robot directions
    54. enum  Direction { WEST , EAST, NORTH, SOUTH };
    55.  
    56. // Define the robot struct
    57. struct Robot {
    58.     int x;
    59.     int y;
    60.     Direction direction;
    61.     bool carryingMarker;
    62. };
    63.  
    64.  
    65. void drawStep(int homeX, int homeY)
    66. {
    67.     background();
    68.     setColour(pink);
    69.     homeX = homeX*squareSize;
    70.     homeY = homeY*squareSize;
    71.     fillRect(homeX, homeY, squareSize, squareSize);
    72. }
    73.  
    74. void drawMarker(int x,int y)
    75. {
    76.     background();
    77.  
    78.     setColour(gray);
    79.  
    80.     x = x*squareSize;
    81.     y = y*squareSize;
    82.  
    83. 	 
    84.     fillRect(x, y, squareSize, squareSize); 
    85. }
    86.  
    87.  
    88. void drawBlocks(int x,int y)
    89. {
    90.     background();
    91.     setColour(black);
    92.  
    93.     x = x*squareSize;
    94.     y = y*squareSize;
    95.  
    96. 	 
    97.     fillRect(x, y, squareSize, squareSize); 
    98. }
    99.  
    100.  
    101. void drawEmpty(int x,int y)
    102. {
    103.     foreground();
    104.     setColour(white);
    105.  
    106.     x = x*squareSize;
    107.     y = y*squareSize;
    108.  
    109. 	 
    110.     fillRect(x, y, squareSize, squareSize); 
    111. }
    112.  
    113.  
    114. // Function to initialize the grid
    115. void initializeGrid(char grid[][GRID_SIZE_Y]) {
    116.     // Initialize the grid with empty spaces
    117.     for (int i = 0; i < GRID_SIZE_X; ++i) {
    118.         for (int j = 0; j < GRID_SIZE_X; ++j) {
    119.             grid[i][j] = EMPTY;
    120.         }
    121.     }
    122.  
    123.     // Place blocks, markers, and home square
    124. 	grid[8][8] = BLOCK;
    125.  
    126. 	grid[9][5] = BLOCK;
    127. 	grid[8][5] = BLOCK;
    128.  
    129. 	grid[7][5] = BLOCK;
    130.  
    131.  
    132. 	grid[6][7] = BLOCK;
    133. 	grid[8][7] = BLOCK; 
    134. 	grid[7][8] = BLOCK; 
    135. 	grid[7][8] = BLOCK; 
    136.  
    137. 	grid[2][2] = BLOCK; 
    138. 	grid[3][3] = BLOCK; 
    139. 	grid[4][4] = BLOCK; 
    140. 	grid[5][5] = BLOCK; 
    141. 	grid[6][6] = BLOCK; 
    142.  
    143. 	 
    144.     grid[MARKER_X][MARKER_Y] = MARKER;
    145.  
    146.     grid[HOME_X][HOME_Y] = HOME;
    147. }
    148.  
    149. // Function to display the grid
    150. void displayGrid(const char grid[][GRID_SIZE_X]) {
    151.  
    152.  
    153. 	setWindowSize(windowSize, windowSize);
    154. 	background();   // Must draw on the background layer.
    155.  
    156. 	int x;
    157. 	int y;
    158.  
    159. 	for (x=0; x
    160. 	{
    161. 		for (y=0; y
    162. 		{
    163. 		   drawRect(x*squareSize, y*squareSize, 
    164. 		   squareSize, squareSize);
    165. 		}
    166. 	}
    167.  
    168. }
    169.  
    170.  
    171. void draw_north(int x, int y)
    172. {
    173.     int x_coords[] = {x, x+50, x+25};
    174.     int y_coords[] = {y+50, y+50, y};
    175.     fillPolygon(3, x_coords, y_coords);
    176. }
    177.  
    178. void draw_east(int x, int y)
    179. {
    180.     int x_coords[] = {x, x, x+50};
    181.     int y_coords[] = {y, y+50, y+25};
    182.     fillPolygon(3, x_coords, y_coords);
    183. }
    184.  
    185. void draw_south(int x, int y)
    186. {
    187.     int x_coords[] = {x, x+50, x+25};
    188.     int y_coords[] = {y, y, y+50};
    189.     fillPolygon(3, x_coords, y_coords);
    190. }
    191.  
    192. void draw_west(int x, int y)
    193. {
    194.     int x_coords[] = {x+50, x+50, x};
    195.     int y_coords[] = {y, y+50, y+25};
    196.     fillPolygon(3, x_coords, y_coords);
    197. }
    198.  
    199. // Function to drop a marker
    200. void dropMarker(Robot &robot, char grid[][GRID_SIZE_X]) {
    201.     if (!robot.carryingMarker) {
    202.         return; // Robot is not carrying a marker
    203.     }
    204.  
    205.     grid[robot.x][robot.y] = MARKER;
    206.     robot.carryingMarker = false;
    207. 	//drawRobot(robot.x, robot.y, (int)robot.direction);
    208.  
    209. }
    210.  
    211. void drawRobot(int x, int y, int direction)
    212. {
    213.     foreground();
    214.     clear();
    215.  
    216. 	 
    217.     setColour(green);
    218.     x = x*squareSize;
    219.     y = y*squareSize;
    220.  
    221. 	 switch (direction)
    222.     {
    223.         case Direction::NORTH: draw_north(x, y); break;
    224.         case Direction::EAST: draw_east(x, y); break;
    225.         case Direction::SOUTH: draw_south(x, y); break;
    226.         case Direction::WEST: draw_west(x, y); break;
    227.     }
    228.  
    229. 	 
    230. }
    231.  
    232. void drawRobotWithBg(int x, int y, int direction)
    233. {
    234.  
    235. 	foreground();
    236. 	clear();
    237. 	setColour(gray);
    238.  
    239. 	x = x*squareSize;
    240. 	y = y*squareSize;
    241.  
    242.  
    243. 	fillRect(x, y, squareSize, squareSize); 
    244.  
    245. 	setColour(green);
    246.  
    247. 	switch (direction)
    248. 	{
    249. 	  case Direction::NORTH: draw_north(x, y); break;
    250. 	  case Direction::EAST: draw_east(x, y); break;
    251. 	  case Direction::SOUTH: draw_south(x, y); break;
    252. 	  case Direction::WEST: draw_west(x, y); break;
    253. 	}
    254.  
    255. }
    256.  
    257.  
    258. void drawHome(int homeX, int homeY)
    259. {
    260.     background();
    261.     setColour(blue);
    262.     homeX = homeX*squareSize;
    263.     homeY = homeY*squareSize;
    264.     fillRect(homeX, homeY, squareSize, squareSize);
    265. }
    266.  
    267.  
    268. void drawShort(int homeX, int homeY)
    269. {
    270.     background();
    271.     setColour(orange);
    272.     homeX = homeX*squareSize;
    273.     homeY = homeY*squareSize;
    274.     fillRect(homeX, homeY, squareSize, squareSize);
    275. }
    276.  
    277.  
    278.  
    279.  
    280. void forward(Robot *robot, char grid[][GRID_SIZE_X]) {
    281.     // Calculate the next position based on the direction
    282.     int nextX = robot->x;
    283.     int nextY = robot->y;
    284.  
    285.     if (robot->direction == NORTH) {
    286.         --nextX;
    287.     } else if (robot->direction == SOUTH) {
    288.         ++nextX;
    289.     } else if (robot->direction == EAST) {
    290.         ++nextY;
    291.     } else if (robot->direction == WEST) {
    292.         --nextY;
    293.     }
    294.  
    295.     // Check if the next position is valid
    296.     if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
    297.         // Move the robot
    298.         grid[robot->x][robot->y] = EMPTY;
    299.         robot->x = nextX;
    300.         robot->y = nextY;
    301.         grid[robot->x][robot->y] = ROBOT;
    302.     }
    303.  
    304. 	drawRobot(robot->x, robot->y, robot->direction);
    305.  
    306. 	 
    307. }
    308.  
    309.  
    310.  
    311. bool markersLeft(char grid[][GRID_SIZE_X]) {
    312.     for (int i = 0; i < GRID_SIZE_X; ++i) {
    313.         for (int j = 0; j < GRID_SIZE_X; ++j) {
    314.             if (grid[i][j] == MARKER) {
    315.                 return true;
    316.             }
    317.         }
    318.     }
    319.     return false;
    320. }
    321.  
    322.  
    323. void turn_left(Robot *robot) {
    324.     if (robot->direction == NORTH) {
    325.         robot->direction = WEST;
    326.     } else if (robot->direction == SOUTH) {
    327.         robot->direction = EAST;
    328.     } else if (robot->direction == EAST) {
    329.         robot->direction = NORTH;
    330.     } else if (robot->direction == WEST) {
    331.         robot->direction = SOUTH;
    332.     }
    333.  
    334. 	drawRobot(robot->x, robot->y, robot->direction);
    335.  
    336. }
    337.  
    338.  
    339. void turn_right(Robot *robot) {
    340.     if (robot->direction == NORTH) {
    341.         robot->direction = EAST;
    342.     } else if (robot->direction == SOUTH) {
    343.         robot->direction = WEST;
    344.     } else if (robot->direction == EAST) {
    345.         robot->direction = SOUTH;
    346.     } else if (robot->direction == WEST) {
    347.         robot->direction = NORTH;
    348.     }
    349.  
    350. 	drawRobot(robot->x, robot->y, robot->direction);
    351.  
    352. 	 
    353. }
    354.  
    355. // Function to pick up a marker
    356. void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
    357.     if (grid[robot->x][robot->y] == MARKER) {
    358.         robot->carryingMarker = true;
    359.         grid[robot->x][robot->y] = EMPTY;
    360.     }
    361. }
    362.  
    363.  
    364. void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
    365.     while (markersLeft(grid)) {
    366.         int initialX = robot->x;
    367.         int initialY = robot->y;
    368.         Direction initialDirection = robot->direction;
    369.  
    370.         // Use the "right hand rule" to navigate
    371.         if (robot->direction == NORTH) {
    372.             if (grid[robot->x][robot->y + 1] != BLOCK) {
    373.                 turn_right(robot);
    374.             } else if (grid[robot->x - 1][robot->y] != BLOCK) {
    375.                 forward(robot, grid);
    376.             } else {
    377.                 turn_left(robot);
    378.             }
    379.         } else if (robot->direction == SOUTH) {
    380.             if (grid[robot->x][robot->y - 1] != BLOCK) {
    381.                 turn_right(robot);
    382.             } else if (grid[robot->x + 1][robot->y] != BLOCK) {
    383.                 forward(robot, grid);
    384.             } else {
    385.                 turn_left(robot);
    386.             }
    387.         } else if (robot->direction == EAST) {
    388.             if (grid[robot->x + 1][robot->y] != BLOCK) {
    389.                 turn_right(robot);
    390.             } else if (grid[robot->x][robot->y + 1] != BLOCK) {
    391.                 forward(robot, grid);
    392.             } else {
    393.                 turn_left(robot);
    394.             }
    395.         } else if (robot->direction == WEST) {
    396.             if (grid[robot->x - 1][robot->y] != BLOCK) {
    397.                 turn_right(robot);
    398.             } else if (grid[robot->x][robot->y - 1] != BLOCK) {
    399.                 forward(robot, grid);
    400.             } else {
    401.                 turn_left(robot);
    402.             }
    403.         }
    404.  
    405.         if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
    406.             // Robot is stuck, rotate 180 degrees
    407.             turn_left(robot);
    408.             turn_left(robot);
    409.         }
    410.  
    411.         forward(robot, grid);
    412.         sleep(500); // Adjust sleep duration for animation speed
    413.         pickUpMarker(robot, grid);
    414.     }
    415. }
    416.  
    417.  
    418. int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X]) 
    419. {
    420. 	int nextX = robot->x;
    421. 	int nextY = robot->y;
    422.  
    423.  if (robot->direction == Direction::NORTH) {
    424. 		  --nextY;
    425. 	 } else if (robot->direction == Direction::SOUTH) {
    426. 		  ++nextY;
    427. 	 } else if (robot->direction == Direction::EAST) {
    428. 		  ++nextX;
    429. 	 } else if (robot->direction == Direction::WEST) {
    430. 		  --nextX;
    431. 	 }
    432.  
    433.  
    434.     // Check if the next position is valid
    435.     if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
    436.         // Move the robot
    437.         //grid[robot->x][robot->y] = EMPTY;
    438.         //robot->x = nextX;
    439.       	//robot->y = nextY;
    440.         //grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
    441.         return 1;
    442.     }
    443.  
    444. 	 
    445.  
    446. 	 return 0;
    447.  
    448. }
    449.  
    450.  
    451.  
    452.  
    453.  
    454. // Function to turn the robot left (anti-clockwise)
    455. void left(Robot *robot) {
    456.     if (robot->direction == Direction::NORTH) {
    457.         robot->direction = Direction::WEST;
    458.     } else if (robot->direction == Direction::SOUTH) {
    459.         robot->direction = Direction::EAST;
    460.     } else if (robot->direction == Direction::EAST) {
    461.         robot->direction = Direction::NORTH;
    462.     } else if (robot->direction == Direction::WEST) {
    463.         robot->direction = Direction::SOUTH;
    464.     }
    465.  
    466. 	 drawRobot(robot->x, robot->y, robot->direction);
    467.  
    468. 	 
    469. }
    470.  
    471. void right(Robot *robot) {
    472.     if (robot->direction == Direction::NORTH) {
    473.         robot->direction = Direction::EAST;
    474.     } else if (robot->direction == Direction::SOUTH) {
    475.         robot->direction = Direction::WEST;
    476.     } else if (robot->direction == Direction::EAST) {
    477.         robot->direction = Direction::SOUTH;
    478.     } else if (robot->direction == Direction::WEST) {
    479.         robot->direction = Direction::NORTH;
    480.     }
    481.  
    482. 	 drawRobot(robot->x, robot->y, robot->direction);
    483.  
    484. 	 
    485. }
    486.  
    487. // Function to pick up a marker
    488. void pickUpMarker(Robot &robot, char grid[][GRID_SIZE_X]) {
    489.     if (grid[robot.x][robot.y] == MARKER) {
    490.         robot.carryingMarker = true;
    491.         grid[robot.x][robot.y] = EMPTY;
    492.  
    493. 		//drawRobot(robot.x, robot.y, (int)robot.direction);
    494.     }
    495. }
    496.  
    497.  
    498. #if 0
    499. bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
    500.     // Use BFS to find the shortest path
    501.     std::queueint, int>> q; // Queue for BFS
    502.     std::vectorbool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
    503.  
    504.     q.push({robot.x, robot.y});
    505.     visited[robot.x][robot.y] = true;
    506.  
    507. 	int u=robot.x*GRID_SIZE_X+robot.y;
    508.  
    509. 	father[robot.x][robot.y]=u;
    510.  
    511.  
    512. 	sleep(2000);
    513.     while (!q.empty()) {
    514.         int x = q.front().first;
    515.         int y = q.front().second;
    516.         q.pop();
    517. 		  //drawRobot(x, y, (int)robot.direction);
    518.  
    519.         if (grid[x][y] == MARKER) {
    520.             // Found a marker, pick it up
    521.             pickUpMarker(robot, grid);
    522.             //dropMarker(robot, grid);
    523. 		  		//drawMarker(MARKER_X,MARKER_Y);
    524. 		  		//drawRobot(x, y, (int)robot.direction);
    525.  
    526. 			
    527.             return true;
    528.         }
    529.  
    530.         // Explore neighboring cells
    531.         int dx[] = {-1, 1, 0, 0};
    532.         int dy[] = {0, 0, -1, 1};
    533.  
    534.         for (int i = 0; i < 4; ++i) {
    535.  
    536.  
    537. 		 		u=x*GRID_SIZE_X+y;
    538.             int nx = x + dx[i];
    539.             int ny = y + dy[i];
    540.  
    541.             if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
    542.                 q.push({nx, ny});
    543. 					 //drawRobot(nx, ny, (int)robot.direction);
    544. 						//drawStep(nx, ny);
    545. 					
    546. 					 //printf("x=%d y=%d \n",nx,ny);
    547.                 visited[nx][ny] = true;
    548. 					father[nx][ny]=u;
    549. 					//dist[nx][ny]=1+dist[x][y];
    550. 					last_dir[nx][ny]=i;
    551.             }
    552. 				
    553.  
    554. 					// printf("findShortestPath 2222\n");
    555. 				 //sleep(200);
    556.         }
    557.  
    558. 		  //drawRobot(robot.x, robot.y, (int)robot.direction);
    559. 		  //sleep(10);
    560.     }
    561.  
    562.     return false; // No markers found
    563. }
    564.  
    565.  
    566. #else
    567.  
    568. bool findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X],bool visit[][maxn]) {
    569.    // Use BFS to find the shortest path
    570.     //std::queue> q; // Queue for BFS
    571.     //std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));
    572. 	int front,rear;
    573.  
    574. 	rear=front=0;
    575.  
    576. 	Robot robot;
    577.  
    578. 	
    579. 	robot.x = robot_x;
    580. 	robot.y = robot_y;
    581.     //q.push({robot.x, robot.y});
    582. 	bool **visited = NULL;
    583.  
    584. 	visit[robot.x][robot.y] = true;
    585.  
    586. 	
    587.  
    588.  
    589. 	int u=robot.x*GRID_SIZE_X+robot.y;
    590.  
    591. 	father[robot.x][robot.y]=u;
    592. 	Queue[rear++]=u;
    593.  
    594.  
    595. 	sleep(1000);
    596.     //while (!q.empty()) 
    597. 	 	while(rear>front)
    598. 	 	{
    599. 	 		u=Queue[front++];
    600. 			
    601.         //int x = q.front().first;
    602.         //int y = q.front().second;
    603.         //q.pop();
    604. 		  //drawRobot(x, y, (int)robot.direction);
    605. 		  int x=u/GRID_SIZE_X;
    606. 		  int y=u%GRID_SIZE_X;
    607.  
    608.         if (grid[x][y] == MARKER) {
    609.             // Found a marker, pick it up
    610.             pickUpMarker(robot, grid);
    611.             //dropMarker(robot, grid);
    612. 		  		//drawMarker(MARKER_X,MARKER_Y);
    613. 		  		//drawRobot(x, y, (int)robot.direction);
    614.  
    615. 			
    616.             return true;
    617.         }
    618.  
    619.         // Explore neighboring cells
    620.         int dx[] = {-1, 1, 0, 0};
    621.         int dy[] = {0, 0, -1, 1};
    622.  
    623.         for (int i = 0; i < 4; ++i) {
    624.  
    625.  
    626. 		 		//u=x*GRID_SIZE_X+y;
    627.             int nx = x + dx[i];
    628.             int ny = y + dy[i];
    629.  
    630.             if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visit[nx][ny] && grid[nx][ny] != BLOCK) {
    631.  
    632. 					 #if 0
    633. 					 q.push({nx, ny});
    634. 					 //drawRobot(nx, ny, (int)robot.direction);
    635. 						//drawStep(nx, ny);
    636. 					
    637. 					 //printf("x=%d y=%d \n",nx,ny);
    638.                 visited[nx][ny] = true;
    639. 					father[nx][ny]=u;
    640. 					//dist[nx][ny]=1+dist[x][y];
    641. 					last_dir[nx][ny]=i;
    642.  
    643. 					#else
    644. 						int v=nx*GRID_SIZE_X+ny;
    645. 						Queue[rear++]=v;
    646. 						visit[nx][ny]=true;
    647. 						father[nx][ny]=u;
    648. 						dist[nx][ny]=1+dist[x][y];
    649. 						last_dir[nx][ny]=i;
    650. 						if (grid[nx][ny] == MARKER)
    651. 						{
    652. 							return true;
    653. 						}
    654. 					#endif
    655. 					
    656.             }
    657. 				
    658.  
    659. 					// printf("findShortestPath 2222\n");
    660. 				 //sleep(200);
    661.         }
    662.  
    663. 		  //drawRobot(robot.x, robot.y, (int)robot.direction);
    664. 		  //sleep(10);
    665.     }
    666.  
    667.     return false; // No markers found
    668. }
    669.  
    670. #endif
    671.  
    672. void print_path(int x,int y,bool has_marker)
    673. {
    674. 	int steps=0;
    675. 	int pos_x,pos_y,direction;
    676. 	while(true)
    677. 	{
    678. 		int u=father[x][y];
    679. 		int fx=u/GRID_SIZE_X;
    680. 		int fy=u%GRID_SIZE_X;
    681. 		if (fx==x && fy==y)
    682. 		{
    683. 			break;
    684. 		}
    685. 		dir[steps]=last_dir[x][y];
    686. 		coordinate[steps++] = x*GRID_SIZE_X+y;
    687. 			
    688. 		x=fx;
    689. 		y=fy;
    690. 	}
    691. 	while(steps--)
    692. 	{
    693. 		pos_x = coordinate[steps]/GRID_SIZE_X;
    694. 		pos_y = coordinate[steps]%GRID_SIZE_X;
    695. 		direction = dir[steps];
    696.  
    697. 		if(has_marker)
    698. 		{
    699. 			drawRobotWithBg(pos_x,pos_y,direction);
    700. 		}
    701. 		else
    702. 		{
    703. 			drawRobot(pos_x, pos_y,direction);
    704. 		}
    705.  
    706.  
    707. 		sleep(300);
    708. 		
    709. 	}
    710.  
    711. }
    712.  
    713.  
    714. int main(int argc, char **argv) {
    715.  
    716.     char grid[GRID_SIZE_X][GRID_SIZE_X];
    717. 	 
    718.     initializeGrid(grid);
    719.  
    720.     Robot robot;
    721.     robot.x = ROBOT_X; // Initial X position
    722.     robot.y = ROBOT_Y; // Initial Y position
    723.     robot.direction = Direction::EAST;
    724.     robot.carryingMarker = false;
    725.  
    726.     // Display the initial grid
    727.  
    728. 	displayGrid(grid);
    729. 	drawHome(HOME_X, HOME_Y);
    730. 	drawMarker(MARKER_X,MARKER_Y);
    731.  
    732. 	drawBlocks(8,8);	
    733. 	drawBlocks(9,5);
    734. 	drawBlocks(8,5);
    735. 	drawBlocks(7,5);
    736.  
    737. 	drawBlocks(6,7);
    738. 	drawBlocks(8,7);
    739. 	drawBlocks(7,8);
    740. 	drawBlocks(2,2);
    741.  
    742. 	drawBlocks(3,3);
    743. 	drawBlocks(4,4);
    744. 	drawBlocks(5,5);
    745. 	drawBlocks(6,6);
    746.  
    747.  
    748.  
    749. 	 #if 0
    750. 	 findAndCollectMarkers(&robot, grid);
    751. 	 #elif 0
    752. 	 
    753.     while (!robot.carryingMarker) {
    754.  
    755. 		if(canMoveForward(&robot, grid))
    756. 		{
    757. 			forward(robot, grid);
    758. 		}
    759. 		else
    760. 		{
    761. 			left(robot);
    762. 		}
    763.  
    764.  
    765. 		
    766. 		sleep(500);
    767. 		  //printf("robot.x = %d  y = %d dir = %d\n",robot.x,robot.y,robot.direction);
    768.     }
    769. #else
    770. 	
    771. 	while (!findShortestPath(robot.x,robot.y, grid,visit_first)) {
    772. 		  forward(&robot, grid);
    773. 		  sleep(500); // Adjust sleep duration for animation speed
    774. 	 }
    775.  
    776. 	print_path(MARKER_X,MARKER_Y,false);
    777.  
    778.  
    779.  
    780. 	robot.x = MARKER_X;
    781. 	robot.y = MARKER_Y;
    782.  
    783. 	grid[MARKER_X][MARKER_Y] = EMPTY;
    784. 	grid[HOME_X][HOME_Y] = MARKER;
    785.  
    786.  
    787.  
    788. 	while (!findShortestPath(robot.x,robot.y, grid,visit_second)) {
    789. 		  forward(&robot, grid);
    790. 		  sleep(500); // Adjust sleep duration for animation speed
    791. 	 }
    792.  
    793. 	print_path(HOME_X,HOME_Y,true);
    794.  
    795.  
    796. #endif
    797.  
    798.  
    799.     return 0;
    800. }

  • 相关阅读:
    开源的远程桌面软件RustDesk
    线性同余方程（ 数学知识 + 同余 + 扩展欧几里得算法 ）
    入门ROS其实也没有那么难！
    确定Mac\Linux系统的架构类型是 x86-64（amd64），还是 arm64 架构
    20230909java面经整理
    win10 安装openssl并使用openssl创建自签名证书
    【题解】盛最多水的容器
    在pandas中使用query替代loc进行高效简洁的条件筛选
    sem_post
    Linux用户与权限管理命令
  • 原文地址：https://blog.csdn.net/baoecit/article/details/134426734