def light_veh_pattern5(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('Pattern5')
# Before veh get out of the intersection
while new_position[0] < destination[0]:
# Check if all parts of veh have been in intersection
if new_position[1] - speed >= origin[0] + 2:
if not self.intersec_grid[time][(270, 270)]:
print('firstgrid')
return False
else:
new_position = (new_position[0] + speed, new_position[1])
check_first = True
else:
# All parts of veh have been in intersection
# Calculate rotation angle
if ((new_position[1] - 270 + speed) / 60) * 90 > 15:
self.r[veh_num] = ((new_position[1] - 270 + 3) / 60) * 90
else:
self.r[veh_num] = 0
if ((new_position[1] - 270 + speed) / 60) * 90 > 90:
self.r[veh_num] = 90
# Calculate trajectory by using Bezier Curve
x = pow(1 - (self.beze_t[veh_num] / 60), 2) * 270 + 2 * (self.beze_t[veh_num] / 60) * (
1 - self.beze_t[veh_num] / 60) * 330 + pow(
self.beze_t[veh_num] / 60, 2) * 330
y = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
1 - self.beze_t[veh_num] / 60) * 273 + pow(
self.beze_t[veh_num] / 60, 2) * 330
self.beze_t[veh_num] += 2
# if x >= 328:
# new_position = (x, y + speed)
new_position = (x, y)
# print('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
# print(new_position)
# Calculate the big Square's coordinate
self.up_left_x[veh_num] = rec_funcs.W2S_up_left_x(new_position[0], self.r[veh_num])
self.up_left_y[veh_num] = rec_funcs.W2S_up_left_y(new_position[1])
self.down_left_x[veh_num] = rec_funcs.W2S_down_left_x(new_position[0], self.r[veh_num])
self.down_left_y[veh_num] = rec_funcs.W2S_down_left_y(new_position[1], self.r[veh_num])
self.up_right_x[veh_num] = rec_funcs.W2S_up_right_x(new_position[0], self.r[veh_num])
self.up_right_y[veh_num] = rec_funcs.W2S_up_right_y(new_position[1])
self.down_right_x[veh_num] = rec_funcs.W2S_down_right_x(new_position[0], self.r[veh_num])
self.down_right_y[veh_num] = rec_funcs.W2S_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, pattern1")
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, pattern1")
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, pattern1")
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, pattern1")
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][(270, 270)] = False
check_first = False
# print("check p1 current_time", current_time)
# print('time', time)
# print(self.beze_t[veh_num])
# print('new_position', new_position, 'r', self.r)
# print(self.up_left_x[veh_num], self.up_left_y[veh_num])
# print(self.up_right_x[veh_num], self.up_right_y[veh_num])
# print(self.down_left_x[veh_num], self.down_left_y[veh_num])
# print(self.down_right_x[veh_num], self.down_right_y[veh_num])
# print(self.intersec_grid[time])
time += 1
# Initiate beze_t
self.beze_t[veh_num] = 2
return True
def light_veh_pattern1(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('Pattern1')
for i in range(veh_num):
self.r[i] = 0
# Before veh get out of the intersection
while new_position[1] < destination[1]:
# Check if all parts of veh have been in intersection
if new_position[0] + speed <= origin[0] + 2:
if not self.intersec_grid[time][(270, 270)]:
print('firstgrid')
return False
else:
new_position = (new_position[0] + speed, new_position[1])
check_first = True
else:
# All parts of veh have been in intersection
# Calculate rotation angle
if ((new_position[1] - 270 + speed) / 60) * 90 > 15:
self.r[veh_num] = ((new_position[1] - 270 + 3) / 60) * 90
else:
self.r[veh_num] = 0
if ((new_position[1] - 270 + speed) / 60) * 90 > 90:
self.r[veh_num] = 90
# Calculate trajectory by using Bezier Curve
x = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
1 - self.beze_t[veh_num] / 60) * 330 + pow(
self.beze_t[veh_num] / 60, 2) * 330
y = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
1 - self.beze_t[veh_num] / 60) * 273 + pow(
self.beze_t[veh_num] / 60, 2) * 330
self.beze_t[veh_num] += 2
new_position = (x, y)
# Calculate the big Square's coordinate
self.up_left_x[veh_num] = rec_funcs.W2S_up_left_x(new_position[0], self.r[veh_num])
self.up_left_y[veh_num] = rec_funcs.W2S_up_left_y(new_position[1])
self.down_left_x[veh_num] = rec_funcs.W2S_down_left_x(new_position[0], self.r[veh_num])
self.down_left_y[veh_num] = rec_funcs.W2S_down_left_y(new_position[1], self.r[veh_num])
self.up_right_x[veh_num] = rec_funcs.W2S_up_right_x(new_position[0], self.r[veh_num])
self.up_right_y[veh_num] = rec_funcs.W2S_up_right_y(new_position[1])
self.down_right_x[veh_num] = rec_funcs.W2S_down_right_x(new_position[0], self.r[veh_num])
self.down_right_y[veh_num] = rec_funcs.W2S_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][(270, 270)] = False
check_first = False
print("check p1 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.up_left_x[veh_num], self.up_left_y[veh_num])
print(self.up_right_x[veh_num], self.up_right_y[veh_num])
print(self.down_left_x[veh_num], self.down_left_y[veh_num])
print(self.down_right_x[veh_num], self.down_right_y[veh_num])
print(self.intersec_grid[time])
time += 1
# Initiate beze_t
self.beze_t[veh_num] = 2
return True