-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgenerate_random_path.m
79 lines (64 loc) · 1.92 KB
/
generate_random_path.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
function path = generate_random_path(preferred_length)
field = get_field();
start_intersection = 1;
path = cell(preferred_length, 1);
intersection_num = start_intersection;
rng("shuffle");
for i=1:preferred_length
current_intersection = field(intersection_num, :);
numNonZeros = countNumNonZeros(current_intersection);
nextIdx = randi(numNonZeros);
for k=1:length(current_intersection)
if k > nextIdx
break
end
if current_intersection(k) == 0
nextIdx = nextIdx + 1;
end
end
if nextIdx == 1
path{i} = 'Straight';
elseif nextIdx == 2
path{i} = 'Left';
else
path{i} = 'Right';
end
intersection_num = current_intersection(nextIdx);
end
[~, back_to_start_path_route] = dijkstra(generate_adjacency_matrix(field), intersection_num, start_intersection);
back_to_start_path_route = flip(back_to_start_path_route);
back_to_start_path = cell(length(back_to_start_path_route)-1, 1);
for i=2:length(back_to_start_path_route)
current_intersection = field(intersection_num, :);
if field(intersection_num, 1) == back_to_start_path_route(i)
back_to_start_path{i-1} = 'Straight';
nextIdx = 1;
elseif field(intersection_num, 2) == back_to_start_path_route(i)
back_to_start_path{i-1} = 'Left';
nextIdx = 2;
else
back_to_start_path{i-1} = 'Right';
nextIdx = 3;
end
intersection_num = current_intersection(nextIdx);
end
path = [path; back_to_start_path];
end
function count = countNumNonZeros(array)
count = 0;
for i=1:length(array)
if array(i) ~= 0
count = count + 1;
end
end
end
function adjancency_matrix = generate_adjacency_matrix(field)
adjancency_matrix = zeros(length(field), length(field));
for i=1:length(field)
for k=1:length(field(1, :))
if field(i, k) ~= 0
adjancency_matrix(i, field(i, k)) = 1;
end
end
end
end