实现效果如下
类似 推箱子小游戏 的变种 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语言版本
#include <string.h>
#include "graphics.h"
#define GRID_SIZE_X 10
#define GRID_SIZE_Y 10
#define ROBOT 'R'
#define HOME_X 9
#define HOME_Y 2
#define MARKER_X 7
#define MARKER_Y 7
#define ROBOT_X 6
#define ROBOT_Y 5
#define MAX_N 105
int g_father[MAX_N][MAX_N];
int dist[MAX_N][MAX_N];
int last_dir[MAX_N][MAX_N];
int dir[MAX_N*MAX_N];
char g_has_visited[MAX_N][MAX_N]; //vis[x][y]表示xy点是否遍历过
int Queue[MAX_N*MAX_N]; //用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u 出队是u=Q[front++]
int coordinate[MAX_N*MAX_N];
const int squareSize = 50;
const int windowSize = 600;
// Define the grid elements
const char EMPTY = ' ';
const char BLOCK = '#';
const char MARKER = '*';
const char HOME = 'H';
// Define robot directions
typedef enum { WEST , EAST, NORTH, SOUTH }Direction;
// Define the robot struct
typedef struct {
int x;
int y;
Direction direction;
char carryingMarker;
}Robot;
void drawStep(int homeX, int homeY)
{
background();
setColour(pink);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void drawMarker(int x,int y)
{
background();
setColour(gray);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
void drawBlocks(int x,int y)
{
background();
setColour(black);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
void drawEmpty(int x,int y)
{
foreground();
setColour(white);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
// Function to initialize the grid
void initializeGrid(char grid[][GRID_SIZE_Y]) {
// Initialize the grid with empty spaces
for (int i = 0; i < GRID_SIZE_X; ++i) {
for (int j = 0; j < GRID_SIZE_X; ++j) {
grid[i][j] = EMPTY;
}
}
// Place blocks, markers, and home square
grid[8][8] = BLOCK;
grid[9][5] = BLOCK;
grid[8][5] = BLOCK;
grid[7][5] = BLOCK;
grid[6][7] = BLOCK;
grid[8][7] = BLOCK;
grid[7][8] = BLOCK;
grid[7][8] = BLOCK;
grid[2][2] = BLOCK;
grid[3][3] = BLOCK;
grid[4][4] = BLOCK;
grid[5][5] = BLOCK;
grid[6][6] = BLOCK;
grid[MARKER_X][MARKER_Y] = MARKER;
grid[HOME_X][HOME_Y] = HOME;
}
// Function to display the grid
void displayGrid(const char grid[][GRID_SIZE_X]) {
setWindowSize(windowSize, windowSize);
background(); // Must draw on the background layer.
int x;
int y;
for (x=0; x<GRID_SIZE_X; x++)
{
for (y=0; y<GRID_SIZE_X; y++)
{
drawRect(x*squareSize, y*squareSize,
squareSize, squareSize);
}
}
}
void draw_north(int x, int y)
{
int x_coords[] = {x, x+50, x+25};
int y_coords[] = {y+50, y+50, y};
fillPolygon(3, x_coords, y_coords);
}
void draw_east(int x, int y)
{
int x_coords[] = {x, x, x+50};
int y_coords[] = {y, y+50, y+25};
fillPolygon(3, x_coords, y_coords);
}
void draw_south(int x, int y)
{
int x_coords[] = {x, x+50, x+25};
int y_coords[] = {y, y, y+50};
fillPolygon(3, x_coords, y_coords);
}
void draw_west(int x, int y)
{
int x_coords[] = {x+50, x+50, x};
int y_coords[] = {y, y+50, y+25};
fillPolygon(3, x_coords, y_coords);
}
// Function to drop a marker
void dropMarker(Robot *robot, char grid[][GRID_SIZE_X]) {
if (!robot->carryingMarker) {
return; // Robot is not carrying a marker
}
grid[robot->x][robot->y] = MARKER;
robot->carryingMarker = 0;
//drawRobot(robot.x, robot.y, (int)robot.direction);
}
void drawRobot(int x, int y, int direction)
{
foreground();
clear();
setColour(green);
x = x*squareSize;
y = y*squareSize;
switch (direction)
{
case NORTH: draw_north(x, y); break;
case EAST: draw_east(x, y); break;
case SOUTH: draw_south(x, y); break;
case WEST: draw_west(x, y); break;
}
}
void drawRobotWithBg(int x, int y, int direction)
{
foreground();
clear();
setColour(gray);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
setColour(green);
switch (direction)
{
case NORTH: draw_north(x, y); break;
case EAST: draw_east(x, y); break;
case SOUTH: draw_south(x, y); break;
case WEST: draw_west(x, y); break;
}
}
void drawHome(int homeX, int homeY)
{
background();
setColour(blue);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void drawShort(int homeX, int homeY)
{
background();
setColour(orange);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void forward(Robot *robot, char grid[][GRID_SIZE_X]) {
// Calculate the next position based on the direction
int nextX = robot->x;
int nextY = robot->y;
if (robot->direction == NORTH) {
--nextX;
} else if (robot->direction == SOUTH) {
++nextX;
} else if (robot->direction == EAST) {
++nextY;
} else if (robot->direction == WEST) {
--nextY;
}
// Check if the next position is valid
if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
// Move the robot
grid[robot->x][robot->y] = EMPTY;
robot->x = nextX;
robot->y = nextY;
grid[robot->x][robot->y] = ROBOT;
}
drawRobot(robot->x, robot->y, robot->direction);
}
char markersLeft(char grid[][GRID_SIZE_X]) {
for (int i = 0; i < GRID_SIZE_X; ++i) {
for (int j = 0; j < GRID_SIZE_X; ++j) {
if (grid[i][j] == MARKER) {
return 1;
}
}
}
return 0;
}
void turn_left(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = WEST;
} else if (robot->direction == SOUTH) {
robot->direction = EAST;
} else if (robot->direction == EAST) {
robot->direction = NORTH;
} else if (robot->direction == WEST) {
robot->direction = SOUTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
void turn_right(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = EAST;
} else if (robot->direction == SOUTH) {
robot->direction = WEST;
} else if (robot->direction == EAST) {
robot->direction = SOUTH;
} else if (robot->direction == WEST) {
robot->direction = NORTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
// Function to pick up a marker
void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
if (grid[robot->x][robot->y] == MARKER) {
robot->carryingMarker = 1;
grid[robot->x][robot->y] = EMPTY;
}
}
void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
while (markersLeft(grid)) {
int initialX = robot->x;
int initialY = robot->y;
Direction initialDirection = robot->direction;
// Use the "right hand rule" to navigate
if (robot->direction == NORTH) {
if (grid[robot->x][robot->y + 1] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x - 1][robot->y] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == SOUTH) {
if (grid[robot->x][robot->y - 1] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x + 1][robot->y] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == EAST) {
if (grid[robot->x + 1][robot->y] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x][robot->y + 1] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == WEST) {
if (grid[robot->x - 1][robot->y] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x][robot->y - 1] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
}
if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
// Robot is stuck, rotate 180 degrees
turn_left(robot);
turn_left(robot);
}
forward(robot, grid);
sleep(500); // Adjust sleep duration for animation speed
pickUpMarker(robot, grid);
}
}
int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X])
{
int nextX = robot->x;
int nextY = robot->y;
if (robot->direction == NORTH) {
--nextY;
} else if (robot->direction == SOUTH) {
++nextY;
} else if (robot->direction == EAST) {
++nextX;
} else if (robot->direction == WEST) {
--nextX;
}
// Check if the next position is valid
if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
// Move the robot
//grid[robot->x][robot->y] = EMPTY;
//robot->x = nextX;
//robot->y = nextY;
//grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
return 1;
}
return 0;
}
// Function to turn the robot left (anti-clockwise)
void left(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = WEST;
} else if (robot->direction ==SOUTH) {
robot->direction = EAST;
} else if (robot->direction == EAST) {
robot->direction =NORTH;
} else if (robot->direction == WEST) {
robot->direction = SOUTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
void right(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = EAST;
} else if (robot->direction ==SOUTH) {
robot->direction = WEST;
} else if (robot->direction ==EAST) {
robot->direction =SOUTH;
} else if (robot->direction == WEST) {
robot->direction =NORTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
#if 0
bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
// Use BFS to find the shortest path
std::queue<std::pair<int, int>> q; // Queue for BFS
std::vector<std::vector<bool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
q.push({robot.x, robot.y});
visited[robot.x][robot.y] = true;
int u=robot.x*GRID_SIZE_X+robot.y;
father[robot.x][robot.y]=u;
sleep(2000);
while (!q.empty()) {
int x = q.front().first;
int y = q.front().second;
q.pop();
//drawRobot(x, y, (int)robot.direction);
if (grid[x][y] == MARKER) {
// Found a marker, pick it up
pickUpMarker(robot, grid);
//dropMarker(robot, grid);
//drawMarker(MARKER_X,MARKER_Y);
//drawRobot(x, y, (int)robot.direction);
return true;
}
// Explore neighboring cells
int dx[] = {-1, 1, 0, 0};
int dy[] = {0, 0, -1, 1};
for (int i = 0; i < 4; ++i) {
u=x*GRID_SIZE_X+y;
int nx = x + dx[i];
int ny = y + dy[i];
if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
q.push({nx, ny});
//drawRobot(nx, ny, (int)robot.direction);
//drawStep(nx, ny);
//printf("x=%d y=%d \n",nx,ny);
visited[nx][ny] = true;
father[nx][ny]=u;
//dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
}
// printf("findShortestPath 2222\n");
//sleep(200);
}
//drawRobot(robot.x, robot.y, (int)robot.direction);
//sleep(10);
}
return false; // No markers found
}
#else
char findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X]) {
// Use BFS to find the shortest path
//std::queue<std::pair<int, int>> q; // Queue for BFS
//std::vector<std::vector<bool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
int front,rear;
rear=front=0;
Robot robot;
robot.x = robot_x;
robot.y = robot_y;
//q.push({robot.x, robot.y});
g_has_visited[robot.x][robot.y] = 1;
int u=robot.x*GRID_SIZE_X+robot.y;
g_father[robot.x][robot.y]=u;
Queue[rear++]=u;
sleep(1000);
//while (!q.empty())
while(rear>front)
{
u=Queue[front++];
//int x = q.front().first;
//int y = q.front().second;
//q.pop();
//drawRobot(x, y, (int)robot.direction);
int x=u/GRID_SIZE_X;
int y=u%GRID_SIZE_X;
if (grid[x][y] == MARKER) {
// Found a marker, pick it up
pickUpMarker(&robot, grid);
//dropMarker(robot, grid);
//drawMarker(MARKER_X,MARKER_Y);
//drawRobot(x, y, (int)robot.direction);
return 1;
}
// Explore neighboring cells
int dx[] = {-1, 1, 0, 0};
int dy[] = {0, 0, -1, 1};
for (int i = 0; i < 4; ++i) {
//u=x*GRID_SIZE_X+y;
int nx = x + dx[i];
int ny = y + dy[i];
if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !g_has_visited[nx][ny] && grid[nx][ny] != BLOCK) {
#if 0
q.push({nx, ny});
//drawRobot(nx, ny, (int)robot.direction);
//drawStep(nx, ny);
//printf("x=%d y=%d \n",nx,ny);
visited[nx][ny] = true;
father[nx][ny]=u;
//dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
#else
int v=nx*GRID_SIZE_X+ny;
Queue[rear++]=v;
g_has_visited[nx][ny]=1;
g_father[nx][ny]=u;
dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
if (grid[nx][ny] == MARKER)
{
return 1;
}
#endif
}
// printf("findShortestPath 2222\n");
//sleep(200);
}
//drawRobot(robot.x, robot.y, (int)robot.direction);
//sleep(10);
}
return 0; // No markers found
}
#endif
void print_path(int x,int y,char has_marker)
{
int steps=0;
int pos_x,pos_y,direction;
while(1)
{
int u=g_father[x][y];
int fx=u/GRID_SIZE_X;
int fy=u%GRID_SIZE_X;
if (fx==x && fy==y)
{
break;
}
dir[steps]=last_dir[x][y];
coordinate[steps++] = x*GRID_SIZE_X+y;
x=fx;
y=fy;
}
while(steps--)
{
pos_x = coordinate[steps]/GRID_SIZE_X;
pos_y = coordinate[steps]%GRID_SIZE_X;
direction = dir[steps];
if(has_marker)
{
drawRobotWithBg(pos_x,pos_y,direction);
}
else
{
drawRobot(pos_x, pos_y,direction);
}
sleep(300);
}
}
int main(int argc, char **argv) {
char grid[GRID_SIZE_X][GRID_SIZE_X];
initializeGrid(grid);
Robot robot;
robot.x = ROBOT_X; // Initial X position
robot.y = ROBOT_Y; // Initial Y position
robot.direction = EAST;
robot.carryingMarker = 0;
// Display the initial grid
displayGrid(grid);
drawHome(HOME_X, HOME_Y);
drawMarker(MARKER_X,MARKER_Y);
drawBlocks(8,8);
drawBlocks(9,5);
drawBlocks(8,5);
drawBlocks(7,5);
drawBlocks(6,7);
drawBlocks(8,7);
drawBlocks(7,8);
drawBlocks(2,2);
drawBlocks(3,3);
drawBlocks(4,4);
drawBlocks(5,5);
drawBlocks(6,6);
#if 0
findAndCollectMarkers(&robot, grid);
#elif 0
while (!robot.carryingMarker) {
if(canMoveForward(&robot, grid))
{
forward(robot, grid);
}
else
{
left(robot);
}
sleep(500);
//printf("robot.x = %d y = %d dir = %d\n",robot.x,robot.y,robot.direction);
}
#else
while (!findShortestPath(robot.x,robot.y, grid)) {
forward(&robot, grid);
sleep(500); // Adjust sleep duration for animation speed
}
print_path(MARKER_X,MARKER_Y,0);
robot.x = MARKER_X;
robot.y = MARKER_Y;
grid[MARKER_X][MARKER_Y] = EMPTY;
grid[HOME_X][HOME_Y] = MARKER;
memset(g_has_visited,0,sizeof(g_has_visited));
while (!findShortestPath(robot.x,robot.y, grid)) {
forward(&robot, grid);
sleep(500); // Adjust sleep duration for animation speed
}
print_path(HOME_X,HOME_Y,1);
#endif
return 0;
}
C++版本
#include <unistd.h> // For sleep function
#include "graphics.h"
#define GRID_SIZE_X 10
#define GRID_SIZE_Y 10
#define ROBOT 'R'
const int maxn=105;
#define HOME_X 0
#define HOME_Y 0
#define MARKER_X 7
#define MARKER_Y 7
#define ROBOT_X 9
#define ROBOT_Y 0
int father[maxn][maxn];
int dist[maxn][maxn];
int last_dir[maxn][maxn];
int dir[maxn*maxn];
bool visit_first[maxn][maxn]; //vis[x][y]表示xy点是否遍历过
bool visit_second[maxn][maxn]; //vis[x][y]表示xy点是否遍历过
int Queue[maxn*maxn]; //用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u 出队是u=Q[front++]
int coordinate[maxn*maxn];
const int squareSize = 50;
const int windowSize = 600;
// Define the grid elements
const char EMPTY = ' ';
const char BLOCK = '#';
const char MARKER = '*';
const char HOME = 'H';
// Define robot directions
enum Direction { WEST , EAST, NORTH, SOUTH };
// Define the robot struct
struct Robot {
int x;
int y;
Direction direction;
bool carryingMarker;
};
void drawStep(int homeX, int homeY)
{
background();
setColour(pink);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void drawMarker(int x,int y)
{
background();
setColour(gray);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
void drawBlocks(int x,int y)
{
background();
setColour(black);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
void drawEmpty(int x,int y)
{
foreground();
setColour(white);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
}
// Function to initialize the grid
void initializeGrid(char grid[][GRID_SIZE_Y]) {
// Initialize the grid with empty spaces
for (int i = 0; i < GRID_SIZE_X; ++i) {
for (int j = 0; j < GRID_SIZE_X; ++j) {
grid[i][j] = EMPTY;
}
}
// Place blocks, markers, and home square
grid[8][8] = BLOCK;
grid[9][5] = BLOCK;
grid[8][5] = BLOCK;
grid[7][5] = BLOCK;
grid[6][7] = BLOCK;
grid[8][7] = BLOCK;
grid[7][8] = BLOCK;
grid[7][8] = BLOCK;
grid[2][2] = BLOCK;
grid[3][3] = BLOCK;
grid[4][4] = BLOCK;
grid[5][5] = BLOCK;
grid[6][6] = BLOCK;
grid[MARKER_X][MARKER_Y] = MARKER;
grid[HOME_X][HOME_Y] = HOME;
}
// Function to display the grid
void displayGrid(const char grid[][GRID_SIZE_X]) {
setWindowSize(windowSize, windowSize);
background(); // Must draw on the background layer.
int x;
int y;
for (x=0; x<GRID_SIZE_X; x++)
{
for (y=0; y<GRID_SIZE_X; y++)
{
drawRect(x*squareSize, y*squareSize,
squareSize, squareSize);
}
}
}
void draw_north(int x, int y)
{
int x_coords[] = {x, x+50, x+25};
int y_coords[] = {y+50, y+50, y};
fillPolygon(3, x_coords, y_coords);
}
void draw_east(int x, int y)
{
int x_coords[] = {x, x, x+50};
int y_coords[] = {y, y+50, y+25};
fillPolygon(3, x_coords, y_coords);
}
void draw_south(int x, int y)
{
int x_coords[] = {x, x+50, x+25};
int y_coords[] = {y, y, y+50};
fillPolygon(3, x_coords, y_coords);
}
void draw_west(int x, int y)
{
int x_coords[] = {x+50, x+50, x};
int y_coords[] = {y, y+50, y+25};
fillPolygon(3, x_coords, y_coords);
}
// Function to drop a marker
void dropMarker(Robot &robot, char grid[][GRID_SIZE_X]) {
if (!robot.carryingMarker) {
return; // Robot is not carrying a marker
}
grid[robot.x][robot.y] = MARKER;
robot.carryingMarker = false;
//drawRobot(robot.x, robot.y, (int)robot.direction);
}
void drawRobot(int x, int y, int direction)
{
foreground();
clear();
setColour(green);
x = x*squareSize;
y = y*squareSize;
switch (direction)
{
case Direction::NORTH: draw_north(x, y); break;
case Direction::EAST: draw_east(x, y); break;
case Direction::SOUTH: draw_south(x, y); break;
case Direction::WEST: draw_west(x, y); break;
}
}
void drawRobotWithBg(int x, int y, int direction)
{
foreground();
clear();
setColour(gray);
x = x*squareSize;
y = y*squareSize;
fillRect(x, y, squareSize, squareSize);
setColour(green);
switch (direction)
{
case Direction::NORTH: draw_north(x, y); break;
case Direction::EAST: draw_east(x, y); break;
case Direction::SOUTH: draw_south(x, y); break;
case Direction::WEST: draw_west(x, y); break;
}
}
void drawHome(int homeX, int homeY)
{
background();
setColour(blue);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void drawShort(int homeX, int homeY)
{
background();
setColour(orange);
homeX = homeX*squareSize;
homeY = homeY*squareSize;
fillRect(homeX, homeY, squareSize, squareSize);
}
void forward(Robot *robot, char grid[][GRID_SIZE_X]) {
// Calculate the next position based on the direction
int nextX = robot->x;
int nextY = robot->y;
if (robot->direction == NORTH) {
--nextX;
} else if (robot->direction == SOUTH) {
++nextX;
} else if (robot->direction == EAST) {
++nextY;
} else if (robot->direction == WEST) {
--nextY;
}
// Check if the next position is valid
if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
// Move the robot
grid[robot->x][robot->y] = EMPTY;
robot->x = nextX;
robot->y = nextY;
grid[robot->x][robot->y] = ROBOT;
}
drawRobot(robot->x, robot->y, robot->direction);
}
bool markersLeft(char grid[][GRID_SIZE_X]) {
for (int i = 0; i < GRID_SIZE_X; ++i) {
for (int j = 0; j < GRID_SIZE_X; ++j) {
if (grid[i][j] == MARKER) {
return true;
}
}
}
return false;
}
void turn_left(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = WEST;
} else if (robot->direction == SOUTH) {
robot->direction = EAST;
} else if (robot->direction == EAST) {
robot->direction = NORTH;
} else if (robot->direction == WEST) {
robot->direction = SOUTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
void turn_right(Robot *robot) {
if (robot->direction == NORTH) {
robot->direction = EAST;
} else if (robot->direction == SOUTH) {
robot->direction = WEST;
} else if (robot->direction == EAST) {
robot->direction = SOUTH;
} else if (robot->direction == WEST) {
robot->direction = NORTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
// Function to pick up a marker
void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
if (grid[robot->x][robot->y] == MARKER) {
robot->carryingMarker = true;
grid[robot->x][robot->y] = EMPTY;
}
}
void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
while (markersLeft(grid)) {
int initialX = robot->x;
int initialY = robot->y;
Direction initialDirection = robot->direction;
// Use the "right hand rule" to navigate
if (robot->direction == NORTH) {
if (grid[robot->x][robot->y + 1] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x - 1][robot->y] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == SOUTH) {
if (grid[robot->x][robot->y - 1] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x + 1][robot->y] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == EAST) {
if (grid[robot->x + 1][robot->y] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x][robot->y + 1] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
} else if (robot->direction == WEST) {
if (grid[robot->x - 1][robot->y] != BLOCK) {
turn_right(robot);
} else if (grid[robot->x][robot->y - 1] != BLOCK) {
forward(robot, grid);
} else {
turn_left(robot);
}
}
if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
// Robot is stuck, rotate 180 degrees
turn_left(robot);
turn_left(robot);
}
forward(robot, grid);
sleep(500); // Adjust sleep duration for animation speed
pickUpMarker(robot, grid);
}
}
int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X])
{
int nextX = robot->x;
int nextY = robot->y;
if (robot->direction == Direction::NORTH) {
--nextY;
} else if (robot->direction == Direction::SOUTH) {
++nextY;
} else if (robot->direction == Direction::EAST) {
++nextX;
} else if (robot->direction == Direction::WEST) {
--nextX;
}
// Check if the next position is valid
if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
// Move the robot
//grid[robot->x][robot->y] = EMPTY;
//robot->x = nextX;
//robot->y = nextY;
//grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
return 1;
}
return 0;
}
// Function to turn the robot left (anti-clockwise)
void left(Robot *robot) {
if (robot->direction == Direction::NORTH) {
robot->direction = Direction::WEST;
} else if (robot->direction == Direction::SOUTH) {
robot->direction = Direction::EAST;
} else if (robot->direction == Direction::EAST) {
robot->direction = Direction::NORTH;
} else if (robot->direction == Direction::WEST) {
robot->direction = Direction::SOUTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
void right(Robot *robot) {
if (robot->direction == Direction::NORTH) {
robot->direction = Direction::EAST;
} else if (robot->direction == Direction::SOUTH) {
robot->direction = Direction::WEST;
} else if (robot->direction == Direction::EAST) {
robot->direction = Direction::SOUTH;
} else if (robot->direction == Direction::WEST) {
robot->direction = Direction::NORTH;
}
drawRobot(robot->x, robot->y, robot->direction);
}
// Function to pick up a marker
void pickUpMarker(Robot &robot, char grid[][GRID_SIZE_X]) {
if (grid[robot.x][robot.y] == MARKER) {
robot.carryingMarker = true;
grid[robot.x][robot.y] = EMPTY;
//drawRobot(robot.x, robot.y, (int)robot.direction);
}
}
#if 0
bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
// Use BFS to find the shortest path
std::queue<std::pair<int, int>> q; // Queue for BFS
std::vector<std::vector<bool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
q.push({robot.x, robot.y});
visited[robot.x][robot.y] = true;
int u=robot.x*GRID_SIZE_X+robot.y;
father[robot.x][robot.y]=u;
sleep(2000);
while (!q.empty()) {
int x = q.front().first;
int y = q.front().second;
q.pop();
//drawRobot(x, y, (int)robot.direction);
if (grid[x][y] == MARKER) {
// Found a marker, pick it up
pickUpMarker(robot, grid);
//dropMarker(robot, grid);
//drawMarker(MARKER_X,MARKER_Y);
//drawRobot(x, y, (int)robot.direction);
return true;
}
// Explore neighboring cells
int dx[] = {-1, 1, 0, 0};
int dy[] = {0, 0, -1, 1};
for (int i = 0; i < 4; ++i) {
u=x*GRID_SIZE_X+y;
int nx = x + dx[i];
int ny = y + dy[i];
if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
q.push({nx, ny});
//drawRobot(nx, ny, (int)robot.direction);
//drawStep(nx, ny);
//printf("x=%d y=%d \n",nx,ny);
visited[nx][ny] = true;
father[nx][ny]=u;
//dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
}
// printf("findShortestPath 2222\n");
//sleep(200);
}
//drawRobot(robot.x, robot.y, (int)robot.direction);
//sleep(10);
}
return false; // No markers found
}
#else
bool findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X],bool visit[][maxn]) {
// Use BFS to find the shortest path
//std::queue<std::pair<int, int>> q; // Queue for BFS
//std::vector<std::vector<bool>> visited(GRID_SIZE_X, std::vector<bool>(GRID_SIZE_X, false));
int front,rear;
rear=front=0;
Robot robot;
robot.x = robot_x;
robot.y = robot_y;
//q.push({robot.x, robot.y});
bool **visited = NULL;
visit[robot.x][robot.y] = true;
int u=robot.x*GRID_SIZE_X+robot.y;
father[robot.x][robot.y]=u;
Queue[rear++]=u;
sleep(1000);
//while (!q.empty())
while(rear>front)
{
u=Queue[front++];
//int x = q.front().first;
//int y = q.front().second;
//q.pop();
//drawRobot(x, y, (int)robot.direction);
int x=u/GRID_SIZE_X;
int y=u%GRID_SIZE_X;
if (grid[x][y] == MARKER) {
// Found a marker, pick it up
pickUpMarker(robot, grid);
//dropMarker(robot, grid);
//drawMarker(MARKER_X,MARKER_Y);
//drawRobot(x, y, (int)robot.direction);
return true;
}
// Explore neighboring cells
int dx[] = {-1, 1, 0, 0};
int dy[] = {0, 0, -1, 1};
for (int i = 0; i < 4; ++i) {
//u=x*GRID_SIZE_X+y;
int nx = x + dx[i];
int ny = y + dy[i];
if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visit[nx][ny] && grid[nx][ny] != BLOCK) {
#if 0
q.push({nx, ny});
//drawRobot(nx, ny, (int)robot.direction);
//drawStep(nx, ny);
//printf("x=%d y=%d \n",nx,ny);
visited[nx][ny] = true;
father[nx][ny]=u;
//dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
#else
int v=nx*GRID_SIZE_X+ny;
Queue[rear++]=v;
visit[nx][ny]=true;
father[nx][ny]=u;
dist[nx][ny]=1+dist[x][y];
last_dir[nx][ny]=i;
if (grid[nx][ny] == MARKER)
{
return true;
}
#endif
}
// printf("findShortestPath 2222\n");
//sleep(200);
}
//drawRobot(robot.x, robot.y, (int)robot.direction);
//sleep(10);
}
return false; // No markers found
}
#endif
void print_path(int x,int y,bool has_marker)
{
int steps=0;
int pos_x,pos_y,direction;
while(true)
{
int u=father[x][y];
int fx=u/GRID_SIZE_X;
int fy=u%GRID_SIZE_X;
if (fx==x && fy==y)
{
break;
}
dir[steps]=last_dir[x][y];
coordinate[steps++] = x*GRID_SIZE_X+y;
x=fx;
y=fy;
}
while(steps--)
{
pos_x = coordinate[steps]/GRID_SIZE_X;
pos_y = coordinate[steps]%GRID_SIZE_X;
direction = dir[steps];
if(has_marker)
{
drawRobotWithBg(pos_x,pos_y,direction);
}
else
{
drawRobot(pos_x, pos_y,direction);
}
sleep(300);
}
}
int main(int argc, char **argv) {
char grid[GRID_SIZE_X][GRID_SIZE_X];
initializeGrid(grid);
Robot robot;
robot.x = ROBOT_X; // Initial X position
robot.y = ROBOT_Y; // Initial Y position
robot.direction = Direction::EAST;
robot.carryingMarker = false;
// Display the initial grid
displayGrid(grid);
drawHome(HOME_X, HOME_Y);
drawMarker(MARKER_X,MARKER_Y);
drawBlocks(8,8);
drawBlocks(9,5);
drawBlocks(8,5);
drawBlocks(7,5);
drawBlocks(6,7);
drawBlocks(8,7);
drawBlocks(7,8);
drawBlocks(2,2);
drawBlocks(3,3);
drawBlocks(4,4);
drawBlocks(5,5);
drawBlocks(6,6);
#if 0
findAndCollectMarkers(&robot, grid);
#elif 0
while (!robot.carryingMarker) {
if(canMoveForward(&robot, grid))
{
forward(robot, grid);
}
else
{
left(robot);
}
sleep(500);
//printf("robot.x = %d y = %d dir = %d\n",robot.x,robot.y,robot.direction);
}
#else
while (!findShortestPath(robot.x,robot.y, grid,visit_first)) {
forward(&robot, grid);
sleep(500); // Adjust sleep duration for animation speed
}
print_path(MARKER_X,MARKER_Y,false);
robot.x = MARKER_X;
robot.y = MARKER_Y;
grid[MARKER_X][MARKER_Y] = EMPTY;
grid[HOME_X][HOME_Y] = MARKER;
while (!findShortestPath(robot.x,robot.y, grid,visit_second)) {
forward(&robot, grid);
sleep(500); // Adjust sleep duration for animation speed
}
print_path(HOME_X,HOME_Y,true);
#endif
return 0;
}