def light_veh_pattern4(self, veh_num, current, origin, destination, speed, current_time):
new_position = current
time = 0
# To light up grid(270, 270)
check_first = False
# Initiate intersection grid
if self.time_step == 0:
self.init_intersec_grid(self.t_ahead)
if current_time > self.time_step:
self.update_intersec_grid(current_time, self.time_step, veh_num)
print('Pattern4')
# Before veh get out of the intersection
while new_position[0] > destination[0]:
print('***************************************************')
# Check if all parts of veh have been in intersection
if origin[1] - (new_position[1] - speed) <= 10:
if not self.intersec_grid[time][(290, 320)]:
print('firstgrid')
return False
else:
new_position = (new_position[0], new_position[1] - speed)
check_first = True
else:
# All parts of veh have been in intersection
# Calculate rotation angle
if ((330 - new_position[1] - speed) / 22) * 90 > 15:
self.r[veh_num] = ((330 - new_position[1] - speed) / 22) * 90
else:
self.r[veh_num] = 0
if ((330 - new_position[1] - speed) / 22) * 90 > 90:
self.r[veh_num] = 90
# Calculate trajectory by using Bezier Curve
x = pow(1 - (self.beze_t[veh_num] / 22), 2) * 293 + 2 * (self.beze_t[veh_num] / 22) * (
1 - self.beze_t[veh_num] / 22) * 293 + pow(
self.beze_t[veh_num] / 22, 2) * 270
y = pow(1 - (self.beze_t[veh_num] / 22), 2) * 320 + 2 * (self.beze_t[veh_num] / 22) * (
1 - self.beze_t[veh_num] / 22) * 306 + pow(
self.beze_t[veh_num] / 22, 2) * 306
self.beze_t[veh_num] += 2
new_position = (x, y)
print('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
print(new_position)
# Calculate the big Square's coordinate
self.up_left_x[veh_num] = rec_funcs.S2W_up_left_x(new_position[0])
self.up_left_y[veh_num] = rec_funcs.S2W_up_left_y(new_position[1], self.r[veh_num])
self.down_left_x[veh_num] = rec_funcs.S2W_down_left_x(new_position[0])
self.down_left_y[veh_num] = rec_funcs.S2W_down_left_y(new_position[1], self.r[veh_num])
self.up_right_x[veh_num] = rec_funcs.S2W_up_right_x(new_position[0], self.r[veh_num])
self.up_right_y[veh_num] = rec_funcs.S2W_up_right_y(new_position[1], self.r[veh_num])
self.down_right_x[veh_num] = rec_funcs.S2W_down_right_x(new_position[0], self.r[veh_num])
self.down_right_y[veh_num] = rec_funcs.S2W_down_right_y(new_position[1], self.r[veh_num])
# Up left
if not self.collision(veh_num, self.up_left_x[veh_num], self.up_left_y[veh_num], time):
print("upleft, pattern4")
return False
# Up right
if not self.collision(veh_num, self.up_right_x[veh_num], self.up_right_y[veh_num], time):
print("upright, pattern4")
return False
# Down left
if not self.collision(veh_num, self.down_left_x[veh_num], self.down_left_y[veh_num], time):
print("downleft, pattern4")
return False
# Down right
if ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10) in \
self.intersec_grid[time]:
if self.intersec_grid[time][
((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] == False:
print('downright')
self.beze_t[veh_num] = 2
return False
else:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
self.intersec_grid[time][
((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
self.intersec_grid[time][
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
self.intersec_grid[time][
((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] = False
# Situation that down_right is out of the region of inter_sec grid
else:
if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[
time]:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
# situation that middle grid exists
# x coordinate is the reason
if abs(self.up_left_x[veh_num] - self.up_right_x[veh_num]) > 10:
if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.down_left_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_left_x[veh_num] + 10) // 10 * 10,
self.down_left_y[veh_num] // 10 * 10)] = False
# y coordinate is the reason
if abs(self.up_left_y[veh_num] - self.down_left_y[veh_num]) > 10:
if (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
if (self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(
self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
if check_first:
self.intersec_grid[time][(290, 320)] = False
check_first = False
# print("check p4 current_time", current_time)
# print('time', time)
# print(self.beze_t[veh_num])
# print('new_position', new_position, 'r', self.r)
# print(self.intersec_grid[time])
time += 1
# Initiate beze_t
self.beze_t[veh_num] = 2
return True
def light_veh_pattern4(self, veh_num, current, origin, destination, speed, current_time):
new_position = current
time = 0
# Avoid situation that grid is lighted up by same vehicle
check_grid = []
# To light up grid(270, 270)
check_first = False
# Initiate intersection grid
if self.time_step == 0:
for k in range(self.t_ahead):
for i in range(270, 330, 10):
for j in range(270, 330, 10):
self.intersec_grid[k][(i, j)] = True
if current_time > self.time_step:
sa = current_time - self.time_step
print('sa', sa)
self.time_step = current_time
# Update intersec_grid for right times
for i in range(sa):
self.intersec_grid.append(copy.deepcopy(self.grid))
del self.intersec_grid[0]
print('Pattern4')
for i in range(veh_num):
self.r[i] = 0
# Before veh get out of the intersection
while new_position[0] > destination[0]:
print('***************************************************')
# Check if all parts of veh have been in intersection
if origin[1] - (new_position[1] - speed) < 10:
if not self.intersec_grid[time][(290, 320)]:
print('firstgrid')
return False
else:
new_position = (new_position[0], new_position[1] - speed)
check_first = True
else:
# All parts of veh have been in intersection
# Calculate rotation angle
if ((330 - new_position[1] - speed) / 22) * 90 > 15:
self.r[veh_num] = ((330 - new_position[1] - speed) / 22) * 90
else:
self.r[veh_num] = 0
if ((330 - new_position[1] - speed) / 22) * 90 > 90:
self.r[veh_num] = 90
# Calculate trajectory by using Bezier Curve
x = pow(1 - (self.beze_t[veh_num] / 22), 2) * 293 + 2 * (self.beze_t[veh_num] / 22) * (
1 - self.beze_t[veh_num] / 22) * 293 + pow(
self.beze_t[veh_num] / 22, 2) * 270
y = pow(1 - (self.beze_t[veh_num] / 22), 2) * 320 + 2 * (self.beze_t[veh_num] / 22) * (
1 - self.beze_t[veh_num] / 22) * 306 + pow(
self.beze_t[veh_num] / 22, 2) * 306
self.beze_t[veh_num] += 2
new_position = (x, y)
# Calculate the big Square's coordinate
self.up_left_x[veh_num] = rec_funcs.S2W_up_left_x(new_position[0])
self.up_left_y[veh_num] = rec_funcs.S2W_up_left_y(new_position[1], self.r[veh_num])
self.down_left_x[veh_num] = rec_funcs.S2W_down_left_x(new_position[0])
self.down_left_y[veh_num] = rec_funcs.S2W_down_left_y(new_position[1], self.r[veh_num])
self.up_right_x[veh_num] = rec_funcs.S2W_up_right_x(new_position[0], self.r[veh_num])
self.up_right_y[veh_num] = rec_funcs.S2W_up_right_y(new_position[1], self.r[veh_num])
self.down_right_x[veh_num] = rec_funcs.S2W_down_right_x(new_position[0], self.r[veh_num])
self.down_right_y[veh_num] = rec_funcs.S2W_down_right_y(new_position[1], self.r[veh_num])
# Up left
# print(time)
if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[time]:
if self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] == False:
print('upleft')
self.beze_t[veh_num] = 2
return False
else:
check_grid.append((self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10))
# Up right
if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in self.intersec_grid[
time]:
if self.intersec_grid[time][
((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] == False:
if ((self.up_right_x[veh_num]) // 10 * 10,
self.up_right_y[veh_num] // 10 * 10) not in check_grid:
print('upright')
self.beze_t[veh_num] = 2
return False
else:
check_grid.append(((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10))
# Down left
if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in self.intersec_grid[
time]:
if self.intersec_grid[time][
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] == False:
if (self.down_left_x[veh_num] // 10 * 10,
(self.down_left_y[veh_num]) // 10 * 10) not in check_grid:
print('Downleft')
self.beze_t[veh_num] = 2
return False
else:
check_grid.append(
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10))
# Down right
if ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10) in \
self.intersec_grid[time]:
if self.intersec_grid[time][
((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] == False:
if ((self.down_right_x[veh_num]) // 10 * 10,
(self.down_right_y[veh_num]) // 10 * 10) not in check_grid:
print('downright')
self.beze_t[veh_num] = 2
return False
else:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
self.intersec_grid[time][
((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
self.intersec_grid[time][
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
self.intersec_grid[time][
((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] = False
# Situation that down_right is out of the region of inter_sec grid
else:
if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[
time]:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
# situation that middle grid exists
# x coordinate is the reason
if abs(self.up_left_x[veh_num] - self.up_right_x[veh_num]) > 10:
if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.down_left_y[veh_num] // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
((self.up_left_x[veh_num] + 10) // 10 * 10,
self.down_left_y[veh_num] // 10 * 10)] = False
# y coordinate is the reason
if abs(self.up_left_y[veh_num] - self.down_left_y[veh_num]) > 10:
if (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
if (self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
self.intersec_grid[time]:
self.intersec_grid[time][
(
self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
if check_first:
self.intersec_grid[time][(290, 320)] = False
check_first = False
print("check p4 current_time", current_time)
check_grid = []
# print('time', time)
# print(self.beze_t[veh_num])
# print('new_position', new_position, 'r', self.r)
# print(self.intersec_grid[time])
time += 1
# Initiate beze_t
self.beze_t[veh_num] = 2
return True