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

news2024/9/20 20:29:39

实现效果如下

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

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

python爬取网站数据,作为后端数据

一. 内容简介 python爬取网站数据&#xff0c;作为后端数据 二. 软件环境 2.1vsCode 2.2Anaconda version: conda 22.9.0 2.3代码 链接&#xff1a; 三.主要流程 3.1 通过urllib请求网站 里面用的所有的包 ! pip install lxml ! pip install selenium ! pip install…

100G.的DDoS高防够用吗?

很多人以为100G的DDoS防御已经足够了&#xff0c;但殊不知DDoS攻击大小也是需要分行业类型的&#xff0c;比如游戏、金融、影视、电商甚至ZF或者行业龙头等等行业类型&#xff0c;都是大型DDoS攻击的重灾区&#xff0c;别说100G防御&#xff0c;就算300G防御服务器也不一定够用…

开启核磁数据处理新篇章-MestReNova(MNOVA14)助您轻松解读科学界密码

在科学研究领域&#xff0c;核磁共振&#xff08;NMR&#xff09;技术被广泛应用于分析和解读化学物质的结构和性质。而MestReNova&#xff08;MNOVA14&#xff09;作为一款专业的核磁数据处理软件&#xff0c;凭借其强大的功能和易用性&#xff0c;已成为众多科研人员的首选工…

用这个平台制作电子杂志,,还能实时分享,太方便啦!

在我们看电子杂志的时候&#xff0c;总会觉得图文效果有点枯燥&#xff0c;如果能做出翻页书效果的电子杂志&#xff0c;还给人一种身临其境的真实翻书感就好了。 其实制作这种翻页电子杂志很简单&#xff0c;不需要下载安装任何软件&#xff0c;只需登录FLBOOK这个平台 &…

SpringCloud微服务:Nacos的下载和配置

目录 Nacos的下载 Nacos的配置 Nacos的下载 nacos压缩包&#xff0c;点击下载百度网盘为您提供文件的网络备份、同步和分享服务。空间大、速度快、安全稳固&#xff0c;支持教育网加速&#xff0c;支持手机端。注册使用百度网盘即可享受免费存储空间https://pan.baidu.com/…

NVIDIA安装

电脑显卡类型 两种方法&#xff1a; 选择对应的版本 产品系列下载Notebooks&#xff0c;这样产品才会出现Laptop的GPU&#xff08;Laptop是代表笔记本&#xff09;。 下载完之后双击安装&#xff0c;更改下载路径后&#xff0c;选择默认的下载即可。 卸载 如果之后要卸载…

上位机模块之halcon绘制ROI与获取ROI,在hsmartwindow实现

在上位机中通常需要使用到绘制ROI模块或者获取已经绘制好的ROI区域的参数&#xff0c;在这里通过使用hsmartwindow窗体控件进行对ROI的绘制和获取。 先上代码&#xff1a; /// <summary>/// 创建ROI/// </summary>/// <param name"Win">传入HSmar…

2023鸿蒙预定未来,环境搭建学习

鸿蒙开发基础知识 鸿蒙的基本概念和特点 鸿蒙&#xff08;HarmonyOS&#xff09;是华为公司开发的一款全场景分布式操作系统。它的设计目标是为各种设备提供统一的、无缝的用户体验。鸿蒙的核心特点包括以下几个方面&#xff1a; 分布式架构&#xff1a;鸿蒙采用分布式架构&…

EDA实验------数控分频器设计(QuartusII)

目录 一、实验目的 二、实验原理 三、实验内容 四、实验步骤 五、注意事项 六、思考题 七、实验过程 分频器的基本原理 什么是分频器&#xff1f; 如何去分频&#xff1f; 1.创建新项目 2.创建Verilog文件&#xff0c;写入代码 3.连接电路 ​编辑 锁相环的创建 4…

9.基于SpringBoot3+MybatisPlus定制化代码生成器类

我们在3.基于SpringBoot3集成MybatisPlus中讲到自定义代码生成器&#xff0c;但是往往遗留代码生成的类格式或者命名不符合要求&#xff0c;需要手工修改&#xff0c;但是当表很多时就比较头痛,所以我们自定义模板在进行代码生成 1. 新建MyTemplateEngine.java类 里面大多实现…

企业大文件传输的四大误区:你还在用传统的FTP和网盘吗?

在当前数字化时代&#xff0c;数据已经成为企业的核心资产&#xff0c;而文件传输则是数据流动的重要方式。企业需要高效、安全、稳定地传输各种类型和规模的文件&#xff0c;无论是内部协作还是外部交付。然而&#xff0c;很多企业在文件传输方面存在一些误区&#xff0c;导致…

“大数据分析师”来了,提高职业含金量,欢迎来领

大数据分析师是指在不同行业中&#xff0c;专门从事相关数据的收集、整理、分析&#xff0c;并依据数据通过科学算法模型进行行业研究、评估和预测等工作的专项人才。应用行业涉及互联网信息技术企业、科研院校、金融行业、制造业、物流、生物医疗、农业等大数据相关行业。 常…

Unity中Shader矩阵的逆矩阵

文章目录 前言一、逆矩阵的表示二、逆矩阵的作用四、逆矩阵的计算五、顺序的重要性六、矩阵的逆总结1、求矩阵的逆前&#xff0c;这个矩阵必须得是个方阵2、只有 A x A ^-1^ A^-1^ x A 1时&#xff0c;A的逆才是A^-1^3、求2x2矩阵的逆&#xff1a;交换 a 和 b 的位置&#xf…

线程安全问题及其解决

文章目录 一. 线程安全问题1.1 线程不安全的例子1.2 线程不安全的原因1.2.1 随即调度, 抢占式执行1.2.2 修改共享数据1.2.3 修改操作非原子性1.2.4 内存可见性1.2.5 指令重排序1.2.6 总结 二. 线程安全问题的解决2.1 synchronized(解决前三个问题)2.1.1 synchronized 的锁是什么…

flutter开发web应用支持浏览器跨域设置

开发web应用难免会遇到跨域问题&#xff0c;所以flutter设置允许web跨域的设置是要在你的flutter安装路径下面 flutter\bin\cache 找到flutter_tools.stamp文件&#xff0c;然后删除掉&#xff1a;这个文件是临时缓存文件 然后找到 flutter\packages\flutter_tools\lib\src\web…

上位机模块之圆形测量,基于halcon的二次开发

夸克网盘可自取。链接&#xff1a;https://pan.quark.cn/s/ac192950e051 //在此之前可以先浏览2篇博客&#xff0c;分别是序列化与反序列化和ROI绘制获取 https://blog.csdn.net/m0_51559565/article/details/134422834 //序列化与反序列化 https://blog.csdn.net/m0_51559565…

【文件读取/包含】任意文件读取漏洞 afr_2

1.1漏洞描述 漏洞名称任意文件读取漏洞 afr_2漏洞类型文件读取漏洞等级⭐⭐漏洞环境dockers攻击方式 1.2漏洞等级 高危 1.3影响版本 暂无 1.4漏洞复现 1.4.1.基础环境 靶场dockers工具BurpSuite 1.4.2.环境搭建 1.kali创建docker-compose.yml文件 touch docker-compose.ym…

msvcp140_CODECVT_IDS.dll丢失怎么办,分享两个有效的方法

在计算机使用的过程中&#xff0c;我们经常会遇到一些错误提示&#xff0c;其中最常见的就是“缺少xxx.dll文件”。这些文件是动态链接库&#xff08;DLL&#xff09;文件&#xff0c;它们包含了程序运行所需的函数和资源。而msvcp140_CODECVT_IDS.dll就是其中之一。那么&#…

PY32F002B从压缩包到实现串口printf输出

最近学习使用芯领的PY32F002B开发板&#xff0c;记录学习历程供有同样需求的人参考。 本文主要讲述利用开发板实现printf语句串口输出。 开发环境的初步搭建 官方提供了一个压缩文件&#xff0c;文件名py32f002B_231026.zip&#xff0c; 链接&#xff1a;https://pan.baidu.c…