-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathrobot_bfs.cpp
More file actions
128 lines (113 loc) · 5.23 KB
/
Copy pathrobot_bfs.cpp
File metadata and controls
128 lines (113 loc) · 5.23 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include <cstdio>
#include <cstring>
#include <queue>
#include <vector>
using namespace std;
#define MAP_SIZE 24
int step_move[4][2] = {{ 1, 0},
{-1, 0},
{0 , 1},
{0 , -1}};
double gps_move[4][2] = {{-0.125, 0},
{0.125 , 0},
{0 , 0.125},
{0 , -0.125}};
struct pos
{
double x, y;
}robot;
struct step
{
int x, y;
int t;
int way;
}step_now, step_temp;
struct solution
{
struct step pos;
vector<step> record;
}now, temp;
char map[MAP_SIZE][MAP_SIZE] =
{
{'0', '0', '1', '1', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '1', '0', '0', '0', '0', 'E'},
{'0', '0', '0', '1', '1', '0', '1', '1', '0', '0', '0', '0', '0', '1', '1', '0', '1', '1', '1', '0', '0', '0', '0', '0'},
{'0', '0', '0', '1', '1', '0', '0', '0', '1', '1', '1', '0', '1', '1', '1', '0', '1', '1', '1', '1', '0', '0', '1', '1'},
{'0', '0', '1', '1', '0', '0', '0', '0', '1', '1', '1', '0', '1', '1', '0', '0', '0', '0', '1', '1', '0', '0', '1', '1'},
{'0', '0', '1', '1', '0', '1', '1', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'},
{'0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '1', '1', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0'},
{'1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '1', '1', '0', '0', '0'},
{'1', '1', '0', '0', '1', '0', '0', '0', '0', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '1', '1', '0'},
{'0', '0', '1', '1', '1', '0', '0', '0', '0', '1', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '1', '1', '0'},
{'0', '0', '1', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '1', '1', '0'},
{'0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '1', '0'},
{'1', '1', '0', '0', '0', '1', '1', '1', '0', '1', '1', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0'},
{'1', '1', '0', '0', '0', '1', '1', '1', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'},
{'1', '1', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '1', '0', '0', '0', '0', '1', '0', '0', '1', '0', '0'},
{'0', '0', '1', '1', '1', '1', '1', '0', '0', '0', '0', '0', '0', '1', '0', '0', '1', '1', '1', '0', '0', '1', '0', '0'},
{'0', '0', '1', '1', '1', '1', '1', '0', '0', '1', '1', '0', '0', '0', '0', '0', '1', '0', '0', '0', '0', '1', '0', '0'},
{'0', '0', '1', '1', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'},
{'0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '1', '0', '0', '0', '1', '1', '0', '0', '0', '0'},
{'1', '1', '1', '0', '0', '0', '1', '1', '0', '0', '0', '0', '0', '1', '1', '1', '0', '0', '1', '1', '0', '0', '0', '0'},
{'1', '1', '1', '0', '0', '0', '1', '1', '0', '0', '0', '0', '1', '1', '1', '1', '0', '0', '1', '1', '0', '0', '0', '0'},
{'0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '0', '1', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'},
{'0', '1', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '1', '1', '0', '0', '1', '1', '0', '0'},
{'0', '1', '1', '1', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0', '1', '1', '0', '0'},
{'S', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '0', '0'},
};
bool is_visited[MAP_SIZE][MAP_SIZE];
char route_type[4] = {'D', 'U', 'R', 'L'};
vector<pos> target;
solution bfs()
{
queue<solution> q;
q.push(now);
while(!q.empty())
{
temp = q.front();
q.pop();
if(map[temp.pos.x][temp.pos.y] == 'E')
return temp;
for(int i = 0;i < 4;i++)
{
struct solution new_leaf;
struct step new_step;
int dx = temp.pos.x + step_move[i][0];
int dy = temp.pos.y + step_move[i][1];
if(dx >= 0 && dx < MAP_SIZE && dy >= 0 && dy < MAP_SIZE && map[dx][dy] != '1' && !is_visited[dx][dy])
{
new_step.x = dx;
new_step.y = dy;
new_step.t = temp.pos.t + 1;
new_step.way = i;
is_visited[new_step.x][new_step.y] = true;
new_leaf.pos = new_step;
new_leaf.record = temp.record;
new_leaf.record.push_back(new_step);
q.push(new_leaf);
}
}
}
}
int main()
{
for(int i = 0;i < MAP_SIZE;i++)
memset(is_visited[i], false, sizeof(is_visited[i]));
step_now.x = 23;
step_now.y = 0;
step_now.t = 0;
step_now.way = -1;
now.pos = step_now;
now.record.push_back(step_now);
struct solution result = bfs();
printf("step = %d\n", result.pos.t);
robot.x = -1.4375;
robot.y = -1.4375;
target.push_back(robot);
for(int i = 1;i < result.record.size();i++)
{
robot.x = robot.x + gps_move[result.record[i].way][0];
robot.y = robot.y + gps_move[result.record[i].way][1];
target.push_back(robot);
printf("{%f, %f},\n", robot.x, robot.y);
}
}