-
Notifications
You must be signed in to change notification settings - Fork 2
/
do_step.m
95 lines (82 loc) · 3.19 KB
/
do_step.m
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
function [x, y] = do_step(current_position_x, current_position_y, step_x, step_y)
global map;
global map_size;
global goal_position_x;
global goal_position_y;
if (step_x > 0)
true_step_x = step_x;
for i = 1:1:step_x
if (current_position_x + i > map_size)
true_step_x = i - 1;
break;
elseif current_position_y == goal_position_y && current_position_x + i == goal_position_x
true_step_x = i;
break;
elseif map(current_position_y,current_position_x + i) == 0
true_step_x = i - 1;
break;
end
end
x = current_position_x + true_step_x;
y = current_position_y;
if (true_step_x > 0)
map(current_position_y,current_position_x + 1:current_position_x + true_step_x) = 0.5;
end
elseif (step_x < 0)
true_step_x = step_x;
for i = -1:-1:step_x
if (current_position_x + i <= 0)
true_step_x = i + 1;
break;
elseif current_position_y == goal_position_y && current_position_x + i == goal_position_x
true_step_x = i;
break;
elseif map(current_position_y,current_position_x + i) == 0
true_step_x = i + 1;
break;
end
end
x = current_position_x + true_step_x;
y = current_position_y;
if (true_step_x < 0)
map(current_position_y,current_position_x + true_step_x:current_position_x - 1) = 0.5;
end
elseif (step_y > 0)
true_step_y = step_y;
for i = 1:1:step_y
if (current_position_y + i > map_size)
true_step_y = i - 1;
break;
elseif current_position_x == goal_position_x && current_position_y + i == goal_position_y
true_step_y = i;
break;
elseif map(current_position_y + i,current_position_x) == 0
true_step_y = i - 1;
break;
end
end
x = current_position_x;
y = current_position_y + true_step_y;
if (true_step_y > 0)
map(current_position_y + 1:current_position_y + true_step_y,current_position_x) = 0.5;
end
elseif (step_y < 0)
true_step_y = step_y;
for i = -1:-1:step_y
if (current_position_y + i <= 0)
true_step_y = i + 1;
break;
elseif current_position_x == goal_position_x && current_position_y + i == goal_position_y
true_step_y = i;
break;
elseif map(current_position_y + i,current_position_x) == 0
true_step_y = i + 1;
break;
end
end
x = current_position_x;
y = current_position_y + true_step_y;
if (true_step_y < 0)
map(current_position_y + true_step_y:current_position_y - 1,current_position_x) = 0.5;
end
end