-
Notifications
You must be signed in to change notification settings - Fork 1
/
add_car.m
67 lines (60 loc) · 1.91 KB
/
add_car.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
%% Note:
% If all lanes are not available, then enter no car
%%
function tmp_lane = add_car(in_car)
global plaza; % all car on this cell
global vmax; % speed limit
global sd_lane; % self_driving lanes chosen
global prob_self_car; % proportion of self-driving car
global t; % time
if nargin < 1
W = size(plaza,1);
success = 0;
visited = zeros(1,W); %!!
while success == 0 && (size(find(visited == 1),2) ~= W)
tmp_lane = floor(rand*W)+1;
if visited(tmp_lane) == 1
continue;
end
visited(tmp_lane) = 1;
cars = cell2mat(plaza(tmp_lane)); % eg. 5 cars should be 5*3 matrix
num = size(cars,1);
tmp_ty = (rand < prob_self_car); % 1Łşnormal, 0Łşself-driving
tmp_v = rand * vmax;
% distance to the last car in chosen lane should be larger than
% safe distance
if num ~= 0
if cars(num,2) >= safe_distance(tmp_v, cars(num,1), tmp_ty)
success = 1;
% if a normal car initiated at self-driving lane
if tmp_ty == 1 && size(find(sd_lane == tmp_lane),2) ~= 0
success = 0;
else
plaza(tmp_lane) = mat2cell([cars; [tmp_v 0 tmp_ty t]], num+1, 4);
end
end
else
plaza(tmp_lane) = mat2cell([tmp_v 0 tmp_ty t], 1, 4);
success = 1;
end
end
if size(find(visited == 1),2) == W
tmp_lane = 0;
end
else
tmp_lane = in_car(5);
cars = cell2mat(plaza(tmp_lane));
num = size(cars,1);
type = in_car(3);
v = in_car(1);
in_car(2) = 0;
in_car(4) = t;
in_car = in_car(1:4);
if size(cars,1) == 0 || ...
(cars(num,2) >= safe_distance(v, cars(num,1), type))
plaza(tmp_lane) = mat2cell([cars; in_car], num+1, 4);
else
tmp_lanes = 0;
end
end
end