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

实现效果如下

类似 推箱子小游戏 的变种 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;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/155493.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

IntelliJ IDE 插件开发 |(一)快速入门

前言 IntelliJ IDEA 作为 Java 开发的首选 IDE&#xff0c;其强大、方便之处不必多说。不过&#xff0c;由于个人或者团队的个性化需求&#xff0c;我们或多或少会想对其功能进行拓展&#xff0c;这时就需要开发插件&#xff08;在 IntelliJ 平台下的所有 IDE 均可运行&#x…

QtCreator13源码windows编译

1.下载QtCreator13源码: https://download.qt.io/snapshots/qtcreator/13.0/13.0.0-beta1/installer_source/latest/qt-creator-opensource-src-13.0.0-beta1.zip 2.下载并安装llvm Release LLVM 17.0.5 llvm/llvm-project GitHub 3.系统 要求&#xff1a; Windows 10 (64…

设计模式-代理模式-笔记

动机&#xff08;Motivation&#xff09; 在面向对象系统中&#xff0c;有些对象由于某种原因&#xff08;比如对象创建的开销很大&#xff0c;或者某些操作需要安全控制&#xff0c;或者需要远程外的访问等&#xff09;&#xff0c;直接访问会给使用者、或者系统结构带来很多…

Python ... takes 0 positional arguments but 1 was given

最近&#xff0c;博主在学习python时遇到这么个报错&#xff0c; 系统&#xff1a;windows10 开发环境&#xff1a;VS Code Python版本&#xff1a;3.12 错误重现&#xff1a; class Dog:def __init__(self):passdef eatSomething(self):self.eatBone()def eatBone():prin…

python趣味编程-5分钟实现一个Flappy Bird游戏(含源码、步骤讲解)

Python 中的 Flappy Bird 游戏可以免费下载开源代码,它是为想要学习 Python 的初学者创建的。 该项目系统使用了 Pygame 和 Random 模块。 Pygame 是一组跨平台的 Python 模块,专为编写视频游戏而设计。 Python 中的 Flappy Bird 代码 – 项目信息 项目名称:Python 中的 Fl…

pipeline jenkins流水线

Pipeline 是 Jenkins 中一种灵活且强大的工作流机制&#xff0c;它允许您以代码的形式来定义和管理持续集成和持续交付的流程。 Pipeline 的作用主要体现在以下几个方面&#xff1a; 可编排的构建流程&#xff1a;使用 Pipeline&#xff0c;您可以将一个或多个阶段&#xff08…

腾讯云服务器租用价格,腾讯云服务器租用价格多少钱一年?

腾讯云服务器租用价格&#xff0c;腾讯云服务器租用价格多少钱一年&#xff1f;腾讯云服务器有优惠活动&#xff0c;现在租用只需要88元/年&#xff01;腾讯云服务器优惠购买入口&#xff1a;https://1111.mian100.cn 随着互联网的发展&#xff0c;越来越多的人开始选择将自己…

解决:Error: Missing binding xxxxx\node_modules\node-sass\vendor\win32-x64-83\

一、具体报错 二、报错原因 这个错误是由于缺少 node-sass 模块的绑定文件引起的。 三、导致原因 3.1、环境发生了变化 3.2、安装过程出现问题 四、解决方法步骤&#xff1a; 4.1、重新构建 node-sass 模块 npm rebuild node-sass 4.2、清除缓存并重新安装依赖 npm c…

【数据结构】【版本1.4】【线性时代】——公平队列

目录 引言 队列的概念与结构 队列的实现 定义 初始化 销毁 入队 判断队列是否为空 出队 获取队头元素 获取队尾元素 检测队列中有效元素个数 元素访问 源代码 queue.h queue.c test.c 个人专栏&#xff1a;《数据结构世界》 引言 数据结构世界遇到栈后&a…

【观察】OpenHarmony:技术先进“创新局”,持续创新“谋新篇”

毫无疑问&#xff0c;开源作为今天整个软件产业的创新“原动力”&#xff0c;目前在软件产业发展中的重要性愈加凸显。根据Linux基金会的统计&#xff0c;现在全球软件产业中有70%以上的代码来源于开源软件。 从这个角度来看&#xff0c;开源技术已逐渐成为推动企业数字化转型和…

【Gitpod】云部署Stable Diffusion并且可以本地访问

文章目录 前言项目部署 项目启动参考文献 前言 本文介绍如何使用 Gitpod 部署 Stable Diffusion web UI。Gitpod 是一个基于云的开发环境&#xff0c;通过与 GitHub 集成&#xff0c;可以在浏览器中轻松进行代码开发和部署&#xff1b;Stable Diffusion 是 GitHub 上面的开源 …

【电路笔记】-脉冲宽度调制(PWM)与电机转速控制

脉冲宽度调制&#xff08;PWM&#xff09;与电机转速控制 文章目录 脉冲宽度调制&#xff08;PWM&#xff09;与电机转速控制1、概述2、电机转速控制3、PWM产生 有许多不同的方法来控制直流电机的速度&#xff0c;但一种非常简单且容易的方法是使用脉冲宽度调制&#xff08;PWM…

【AI视野·今日Robot 机器人论文速览 第六十二期】Wed, 25 Oct 2023

AI视野今日CS.Robotics 机器人学论文速览 Wed, 25 Oct 2023 Totally 25 papers &#x1f449;上期速览✈更多精彩请移步主页 Daily Robotics Papers EquivAct: SIM(3)-Equivariant Visuomotor Policies beyond Rigid Object Manipulation Authors Jingyun Yang, Congyue Deng,…

ES Kibana 安装

ES & Kibana 本文基于Docker安装部署使用 Kibana的版本和ElasticSearch的版本&#xff0c;以及IK分词器的版本一一对应 Kibana 安装 安装Kibana # 创建网络 [rootiZ2zeg7mctvft5renx1qvbZ ~]# docker network create --driver bridge --subnet 192.168.0.0/16 --gateway …

Alibaba Nacos注册中心实战

为什么需要注册中心 思考&#xff1a;网络请求&#xff0c;如果服务提供者发生变动&#xff0c;服务调用者如何感知服务提供者的ip和端口变化&#xff1f; // 微服务之间通过RestTemplate调用&#xff0c;ip:port写死&#xff0c;如果ip或者port变化呢&#xff1f; String ur…

[PHP]写个简单的分页静态接口用宝塔部署到Nginx

使用get方式传入page和pageSize参数&#xff0c;接口根据参数进行分页处理。 1.创建一个 PHP 文件 例如 city.php&#xff0c;用于定义接口和返回 JSON 数据。 2.在 city.php 文件中编写接口 <?php// 设置响应内容为 JSON 格式 header(Content-Type: application/json);…

EasyCVR视频监控+AI智能分析网关如何助力木材厂安全生产?

旭帆科技有很多工厂的视频监管方案&#xff0c;小编也经常分享出来供大家参考。近期&#xff0c;又有伙伴后台私信我们想要关于木材厂的方案。针对木材厂的生产过程与特性以及安全风险等&#xff0c;我们来分享一下相关的监管方案&#xff1a; 1&#xff09;温湿度监测&#xf…

Skywalking流程分析_9(JDK类库中增强流程)

前言 之前的文章详细介绍了关于非JDK类库的静态方法、构造方法、实例方法的增强拦截流程&#xff0c;本文会详细分析JDK类库中的类是如何被增强拦截的 回到最开始的SkyWalkingAgent#premain try {/** 里面有个重点逻辑 把一些类注入到Boostrap类加载器中 为了解决Bootstrap类…

springboot-RedisTemplate

pom.xml: <?xml version"1.0" encoding"UTF-8"?> <project xmlns"http://maven.apache.org/POM/4.0.0"xmlns:xsi"http://www.w3.org/2001/XMLSchema-instance"xsi:schemaLocation"http://maven.apache.org/POM/4.0.…

汉诺塔移动次数

描述 汉诺塔&#xff08;又称河内塔&#xff09;问题是源于印度一个古老传说的益智玩具。大梵天创造世界的时候做了三根金刚石柱子&#xff0c;在一根柱子上从下往上按照大小顺序摞着64片黄金圆盘。大梵天命令婆罗门把圆盘从下面开始按大小顺序重新摆放在另一根柱子上。并且规…