Gra-SI/bfs.py

import pygame
from config import *
class Bfs():

    def __init__(self,game):

        self.game = game
        
        self.open_queue = []
        self.close_queue = []
        self.wall_cells = []
        self.enemy_cells = []
    def heuristic(a,b):
        return abs(a[0]-b[0])+abs(a[1]-b[1])
    def bfs(self,goal_cell):
        print("x: ", self.game.agent.x, "y: ", self.game.agent.y)
        cost_road=0
        self.open_queue.append(self.get_cell_number(self.game.agent.x,self.game.agent.y))
        # tutaj dodaje się cel agenta
        #goal_cell = self.get_cell_number(self.game.flower.x,self.game.flower.y)

        path = []
        processing = True
        find_path = False
        while processing: # główna pętla
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    exit()
    
            if len(self.open_queue) > 0 :
                current_node_cell = self.open_queue.pop(0)
            
                if(current_node_cell in self.close_queue):
                    continue

                print("Aktualna kratka: ", current_node_cell)
                print("Cel znajduje sie na kratce: ", goal_cell)

                if (current_node_cell == goal_cell):
                    self.close_queue.append(current_node_cell)
                    found_goal_cell = current_node_cell
                    print("Znaleziono cel, szukanie drogi z odwiedzonych wezlow, kolejka odwiedzonych:", self.close_queue)
                    processing = False
                    find_path = True
                    self.game.clock.tick(2)
                else:
                    child_node_cells = self.get_child_nodes(current_node_cell)
                    self.close_queue.append(current_node_cell) 
                    print("Sasiedzi: ", child_node_cells)
                    for child_node in child_node_cells:
                        if child_node not in self.open_queue and child_node not in self.close_queue:
                            self.open_queue.append(child_node)
                    print("Kolejka: ", self.open_queue, "\n")            
            else:
                print("Brak nowych wezlow, kolejka: ",self. open_queue)
                print("Odwiedzone : ", self.close_queue)
                return self.close_queue
    
        dead_end_nodes = []
        while find_path:
            path.append(self.close_queue[0])

            for i in range(len(self.close_queue) -1):
                from_cell = path[-1]
                to_cell = self.close_queue[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 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_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):
        print("PATH:::::",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("Polozenie agenta: agent.x: ", self.game.agent.rect.x, ", agent.y: ", self.game.agent.rect.y)
            self.game.clock.tick(2)

    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