import pygame import heapq from config import * from queue import PriorityQueue class Astar: def __init__(self, game): self.game = game self.open_queue = [] self.close_queue = [] self.wall_cells = [] self.enemy_cells = [] self.little_rocks_cells = [] self.grass_cells = [] def Astar(self): start_cell = self.get_cell_number(self.game.agent.x,self.game.agent.y) goal_cell = self.get_cell_number(self.game.flower.x,self.game.flower.y) f_value = {start_cell: self.heuristic_value(self.game.agent.x, self.game.agent.y, self.game.flower)} g_values = {start_cell: 0} f2_value = {start_cell: 0} parents = {start_cell: None} new_child_node = {int, int} priority_queue = [] find_path = False open_queue = PriorityQueue() open_queue.put((0,start_cell)) path = [] processing = True while processing: for event in pygame.event.get(): if event.type == pygame.QUIT: exit() if not open_queue.empty() : current_node_cell = open_queue.get() if(current_node_cell in self.close_queue): continue if (current_node_cell[1] == goal_cell): self.close_queue.append(current_node_cell[1]) found_goal_cell = current_node_cell[1] print("Znaleziono cel, szukanie drogi z odwiedzonych węzłów, kolejka odwiedzonych:", self.close_queue) processing = False find_path = True self.game.clock.tick(2) else: self.close_queue.append(current_node_cell) child_node_cells = self.get_child_nodes(current_node_cell[1]) for child_node in child_node_cells: if child_node not in [item[1] for item in open_queue.queue] and child_node not in [item[1] for item in self.close_queue]: x, y = self.get_coordinates(child_node) g_values[child_node] = g_values[current_node_cell[1]] f_value[child_node] = g_values[child_node] + self.heuristic_value(x,y, self.game.flower) + self.move_cost(current_node_cell[1], child_node) open_queue.put((f_value[child_node], child_node)) parents[child_node] = current_node_cell[1] elif child_node in [item[1] for item in open_queue.queue]: f2_value[child_node] = g_values[child_node] + self.heuristic_value(x,y, self.game.flower) + self.move_cost(current_node_cell[1], child_node) print("CURRENT NODE:", current_node_cell) print("CURRENT CHILD:", child_node) print("F1:", f_value[child_node]) print("F2:", f2_value[child_node]) if f2_value[child_node] < f_value[child_node]: parents[child_node] = current_node_cell[1] open_queue.put((f2_value[child_node], child_node)) else: print("Brak nowych węzłów, kolejka: ",self. open_queue) print("Odwiedzone : ", self.close_queue) return self.close_queue dead_end_nodes = [] resultList = [t[1] for t in self.close_queue if isinstance(t, tuple)] resultList.append(goal_cell) print("F_VALUES:", f_value) print("RESULT LIST", resultList) print("PARENTS: ", parents) while find_path: path.append(resultList[0]) for i in range(len(resultList) -1): from_cell = path[-1] to_cell = resultList[i+1] if to_cell in dead_end_nodes: continue if self.verify_move(from_cell, to_cell): path.append(to_cell) if path[-1] == found_goal_cell: find_path = False else: dead_end_nodes.append(path[-1]) path = [] print("Droga: ", path) self.move_agent(path) def heuristic_value(self, x,y, goal_cell): return abs(x - goal_cell.x) + abs(y - goal_cell.y) def move_cost(self, node1, node2): if node2 in self.little_rocks_cells: return 50 if node2 in self.grass_cells: return 5 return 1 def get_cell_number(self,x, y): #zamienia koordynaty na numer kratki cell_number = None cell_number =(x // TILE_SIZE) + (NUM_ROWS * (( y// TILE_SIZE))) return cell_number def get_coordinates(self,cell_to_move): #zamienia numer kratki na koordynaty cell_row_number = cell_to_move // NUM_ROWS cell_column_number = cell_to_move % NUM_ROWS y = cell_row_number * TILE_SIZE x = cell_column_number * TILE_SIZE return x, y def get_child_nodes(self,cell_number): children = [] up = self.get_up_cell(cell_number) if up is not None and up not in self.wall_cells and up not in self.enemy_cells: children.append(up) right = self.get_right_cell(cell_number) if right is not None and right not in self.wall_cells and up not in self.enemy_cells: children.append(right) down = self.get_down_cell(cell_number) if down is not None and down not in self.wall_cells and up not in self.enemy_cells: children.append(down) left = self.get_left_cell(cell_number) if left is not None and left not in self.wall_cells and up not in self.enemy_cells: children.append(left) return children def get_up_cell(self,cell_number): cell_row_number = cell_number // NUM_ROWS if (cell_row_number - 1 < 0): return None else: return (cell_number - NUM_ROWS) def get_right_cell(self,cell_number): cell_column_number = cell_number % NUM_ROWS if (cell_column_number + 1 >= NUM_ROWS): return None else: return (cell_number + 1) def get_down_cell(self,cell_number): cell_row_number = cell_number // NUM_ROWS if (cell_row_number + 1 >= NUM_ROWS): return None else: return (cell_number + NUM_ROWS) def get_left_cell(self,cell_number): cell_column_number = cell_number % NUM_ROWS if (cell_column_number - 1 < 0): return None else: return (cell_number - 1) def verify_move(self,from_cell, to_cell): #sprawdzenie czy ruch jest poprawny czyt. czy następna kratka to przeszkoda lub mob if (to_cell in self.wall_cells or to_cell in self.enemy_cells): return False if(from_cell + 1 == to_cell): return True if(from_cell - 1 == to_cell): return True if(from_cell - NUM_ROWS == to_cell): return True if(from_cell + NUM_ROWS == to_cell): return True return False def move_agent(self,path): for cell_to_move in path: x, y = self.get_coordinates(cell_to_move) print("Ruch do kratki : ", cell_to_move, " z x: ", x, ", y: ", y, ", agent.x: ", self.game.agent.rect.x, ", agent.y: ", self.game.agent.rect.y) if(self.get_cell_number(self.game.agent.x,self.game.agent.y)!=cell_to_move): if x > self.game.agent.rect.x: self.game.agent.direction = 0 elif y > self.game.agent.rect.y: self.game.agent.direction = 1 elif x < self.game.agent.rect.x: self.game.agent.direction = 2 elif y < self.game.agent.rect.y: self.game.agent.direction = 3 if self.game.agent.direction==0: print("DIRECTION: "+self.game.agent.AGENT_IMAGES[self.game.agent.direction]) self.game.agent.x_change += TILE_SIZE elif self.game.agent.direction==1: print("DIRECTION: "+self.game.agent.AGENT_IMAGES[self.game.agent.direction]) self.game.agent.y_change += TILE_SIZE elif self.game.agent.direction==2: print("DIRECTION: "+self.game.agent.AGENT_IMAGES[self.game.agent.direction]) self.game.agent.x_change -= TILE_SIZE elif self.game.agent.direction==3: print("DIRECTION: "+self.game.agent.AGENT_IMAGES[self.game.agent.direction]) self.game.agent.y_change -= TILE_SIZE self.game.agent.rotate() self.game.update() self.game.map() print("Położenie agenta: agent.x: ", self.game.agent.rect.x, ", agent.y: ", self.game.agent.rect.y) self.game.clock.tick(2)