diff --git a/route1.py b/route1.py new file mode 100644 index 0000000..7f16546 --- /dev/null +++ b/route1.py @@ -0,0 +1,240 @@ +import pygame +import numpy as np +import math + +# Colors: +# Define some colors +BLACK = (0, 0, 0) +WHITE = (255, 255, 255) +GREEN = (0, 255, 0) +RED = (255, 0, 0) +BLUE = (0, 0, 240) + +#Width and Height of each square: +WIDTH = 20 +HEIGHT = 20 + +#Margin: +MARGIN = 5 +grid = [[0 for x in range(16)] for y in range(16)] + +def change_value(i, j, width, n): + for r in range (i, i+width): + for c in range (j, j+width): + grid[r][c] = n + +class Table: + def __init__(self, coordinate_i, coordinate_j): + self.coordinate_i = coordinate_i + self.coordinate_j = coordinate_j + change_value(coordinate_i, coordinate_j, 2, 1) + def get_destination_coor(self): + return [self.coordinate_i, self.coordinate_j-1] + +class Kitchen: + def __init__(self, coordinate_i, coordinate_j): + self.coordinate_i = coordinate_i + self.coordinate_j = coordinate_j + change_value(coordinate_i, coordinate_j, 3, 2) + +class Agent: + def __init__(self,orig_coordinate_i, orig_coordinate_j): + self.orig_coordinate_i = orig_coordinate_i + self.orig_coordinate_j = orig_coordinate_j + self.state = np.array([1,2]) + change_value(orig_coordinate_j, orig_coordinate_j, 1, 3) + self.state_update(orig_coordinate_i, orig_coordinate_j) + self.previous_grid = np.array([-1, -1]) + + def state_update(self, c1, c2): + self.state[0] = c1 + self.state[1] = c2 + + def leave(self): + change_value(self.state[0], self.state[1], 1, 0) + + def previous_grid_update(self): + self.previous_grid[0] = self.state[0] + self.previous_grid[1] = self.state[1] + + def move_to(self, nextPos): + self.previous_grid_update() + self.leave() + self.state_update(x + nextPos[0], y + nextPos[1]) + change_value(self.state[0], self.state[1], 1, 3) + + # check the next grid is not the previous grid to prevent the loop + def next_is_previous(self, x, y): + return np.array_equal(self.previous_grid, np.array([x, y])) + +def check_done(): + for event in pygame.event.get(): # Checking for the event + if event.type == pygame.QUIT: # If the program is closed: + return True # To exit the loop + +def draw_grid(): + for row in range(16): # Drawing the grid + for column in range(16): + color = WHITE + if grid[row][column] == 1: + color = GREEN + if grid[row][column] == 2: + color = RED + if grid[row][column] == 3: + color = BLUE + pygame.draw.rect(screen, + color, + [(MARGIN + WIDTH) * column + MARGIN, + (MARGIN + HEIGHT) * row + MARGIN, + WIDTH, + HEIGHT]) + +# calculate the distance between two points +def distance(point1, point2): + return math.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) + +## default positions of the agent: +x = 12 +y = 12 +agent = Agent(x, y) + +table1 = Table(2, 2) +table2 = Table (2,7) +table3 = Table(2, 12) +table4 = Table(7, 2) +table5 = Table(7, 7) +table6 = Table(7, 12) +table7 = Table(12, 2) +table8 = Table(12, 7) + +#class Kitchen: +kitchen = Kitchen(13, 13) + +# destination array +destination_tables = [] + +destination_tables.append(table1.get_destination_coor()) # append [2,1] +destination_tables.append(table2.get_destination_coor()) # append [2,6] +destination_tables.append(table3.get_destination_coor()) # append [2,11] +destination_tables.append(table4.get_destination_coor()) # append [7,1] +destination_tables.append(table5.get_destination_coor()) # append [7,6] +destination_tables.append(table6.get_destination_coor()) # append [7,11] +destination_tables.append(table7.get_destination_coor()) # append [12,1] +destination_tables.append(table8.get_destination_coor()) # append [12,6] + +pygame.init() +WINDOW_SIZE = [405, 405] +screen = pygame.display.set_mode(WINDOW_SIZE) + +pygame.display.set_caption("Waiter_Grid3") + +done = False + +clock = pygame.time.Clock() + +# -------- Main Program Loop ----------- +while not done: + screen.fill(BLACK) # Background color + draw_grid() + done = check_done() + while len(destination_tables) != 0: + + # set the first element(table) in array as currDestination + currDestination = destination_tables[0] + # from kitchen to table + while agent.state[0] != currDestination[0] or agent.state[1] != currDestination[1]: + + #/////////////////////////////////////// + x = agent.state[0] + y = agent.state[1] + + # set a huge default number + minDis = 9999 + nextPos = [] + # check whether the agent goes left + if y-1 >= 0 and grid[x][y-1] != 1 and not agent.next_is_previous(x, y-1): + minDis = distance([x, y-1], currDestination) + nextPos = [0, -1] # means go left + + # check whether the agent goes right + if y+1 <= 15 and grid[x][y+1] != 1 and grid[x][y+1] != 2 and not agent.next_is_previous(x, y+1): + d = distance([x, y+1], currDestination) + if d < minDis: + minDis = d + nextPos = [0, 1] # means go right + + # check whether the agent goes up + if x-1 >= 0 and grid[x-1][y] != 1 and not agent.next_is_previous(x-1, y): + d = distance([x-1, y], currDestination) + if d < minDis: + minDis = d + nextPos = [-1, 0] # means go up + + # check whether the agent goes down + if x+1 <= 15 and grid[x+1][y] != 1 and grid[x+1][y] != 2 and not agent.next_is_previous(x+1, y): + d = distance([x+1, y], currDestination) + if d < minDis: + minDis = d + nextPos = [1, 0] # means go down + + print(agent.previous_grid) + agent.move_to(nextPos) + #//////////////////////////////////////////////// + + pygame.time.delay(100) + screen.fill(BLACK) # Background color + draw_grid() # Drawing the grid + clock.tick(60) # Limit to 60 frames per second + pygame.display.flip() # Updating the screen + + # set the kitchen as currDestination + currDestination = [13, 12] + # from table to kitchen + while agent.state[0] != currDestination[0] or agent.state[1] != currDestination[1]: + + #/////////////////////////////////////// + x = agent.state[0] + y = agent.state[1] + + # set a huge default number + minDis = 9999 + nextPos = [] + # check whether the agent goes left + if y-1 >= 0 and grid[x][y-1] != 1 and not agent.next_is_previous(x, y-1): + minDis = distance([x, y-1], currDestination) + nextPos = [0, -1] # means go left + + # check whether the agent goes right + if y+1 <= 15 and grid[x][y+1] != 1 and grid[x][y+1] != 2 and not agent.next_is_previous(x, y+1): + d = distance([x, y+1], currDestination) + if d < minDis: + minDis = d + nextPos = [0, 1] # means go right + + # check whether the agent goes up + if x-1 >= 0 and grid[x-1][y] != 1 and grid[x-1][y] != 2 and not agent.next_is_previous(x-1, y): + d = distance([x-1, y], currDestination) + if d < minDis: + minDis = d + nextPos = [-1, 0] # means go up + + # check whether the agent goes down + if x+1 <= 15 and grid[x+1][y] != 1 and grid[x+1][y] != 2 and not agent.next_is_previous(x+1, y): + d = distance([x+1, y], currDestination) + if d < minDis: + minDis = d + nextPos = [1, 0] # means go down + + agent.move_to(nextPos) + #//////////////////////////////////////////////// + + pygame.time.delay(100) + screen.fill(BLACK) # Background color + draw_grid() # Drawing the grid + clock.tick(60) # Limit to 60 frames per second + pygame.display.flip() # Updating the screen + + + destination_tables = destination_tables[1:] # remove the first element in the list + +pygame.quit() \ No newline at end of file