AIprojekt-wozek/Astar.py

import math
from operator import ne

import pygame
from Global_variables import Global_variables as G_var
from Min_heap import Min_heap
from Node import Node, State
from Package import Package
from Shelf import Shelf


class Pathfinding:
    def __init__(self, enviroment_2d):
        self.enviroment_2d = enviroment_2d
        self.reset_grid()
        self.path = []

    def reset_grid(self):
        self.grid = [[      # tworze pustej tablicy o wymiarach naszej kraty
            None 
            for y in range(G_var().DIMENSION_Y)]
            for x in range(G_var().DIMENSION_X)
        ]
        for x in range(G_var().DIMENSION_X):       # zapełnianie tablicy obiektami Node
            for y in range(G_var().DIMENSION_Y):
                is_walkable = True
                to_check_type = self.enviroment_2d[x][y]
                if isinstance(to_check_type, Shelf):
                    is_walkable = False
                elif isinstance(to_check_type, Package) and to_check_type.is_placed:
                    is_walkable = False
                self.grid[x][y] = Node(State(1, x, y), is_walkable)
        
    def succ(self,node):    #funckja zwraca sąsiadów noda w argumencie
        node_x = node.state.x
        node_y = node.state.y
        neighbours = []
        neighbours_cords = [[1,0],[-1,0],[0,-1],[0,1]]
        for cord in neighbours_cords:
            neighbour_x = node_x + cord[0]
            neighbour_y = node_y + cord[1]
            if(neighbour_x >= 0 and neighbour_x < G_var().DIMENSION_X and neighbour_y >= 0 and neighbour_y < G_var().DIMENSION_Y):
                neighbours.append(self.grid[neighbour_x][neighbour_y])
        return neighbours

################## TO REMOVE 
    def set_text(self, string, coordx, coordy, fontSize): #Function to set text

        font = pygame.font.Font('freesansbold.ttf', fontSize) 
        #(0, 0, 0) is black, to make black text
        string = str(string)
        text = font.render(string, True, (0, 0, 0)) 
        textRect = text.get_rect()
        textRect.center = (coordx, coordy) 
        return (text, textRect)

    def draw_node(self,node, window, color):
        node_x = node.state.x
        node_y = node.state.y
        #######################SET TEXT
        f_cost_text = self.set_text(node.f_cost(), node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/2), node_y *
            G_var().RECT_SIZE + (G_var().RECT_SIZE/2), 10)
        g_cost_text = self.set_text(node.g_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4), node_y *
            G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)
        h_cost_text = self.set_text(node.h_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4*3), node_y *
            G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)

        ###############################
        node_x = node.state.x
        node_y = node.state.y
        block = pygame.Rect(
            node_x * G_var().RECT_SIZE, node_y *
            G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE
        )
        pygame.draw.rect(window,
                         color,
                         block)
        window.blit(f_cost_text[0], f_cost_text[1])
        window.blit(g_cost_text[0], g_cost_text[1])
        window.blit(h_cost_text[0], h_cost_text[1])
        
###############################

    def find_path(self, starting_state, target_state, window):   # algorytm wyszukiwania trasy
        start_node = self.grid[starting_state.x][starting_state.y]
        target_node = self.grid[target_state.x][target_state.y]

        fringe = Min_heap()
        explored = []
        
        is_target_node_walkable = True
        if not target_node.walkable:
            target_node.walkable = True
            is_target_node_walkable = False
        fringe.insert(start_node)

        while fringe.count() > 0:
            # current_node = fringe[0]
            current_node = fringe.extract()
            #################################################### TEST
            # current_node_color = (213, 55, 221)
            # to_check_color = (55, 213, 55)
            # current_color = (233,55,55)
            # # self.draw_node(current_node,window,current_node_color)
            # for node_to_check in explored:
            #     self.draw_node(node_to_check,window, current_node_color)
            # for node_to_check in fringe.items:
            #     self.draw_node(node_to_check,window, to_check_color)
            # self.draw_node(current_node,window,current_color)
            # pygame.display.flip()
            ###############################################################
            explored.append(current_node)

            if current_node.state == target_node.state:
                path = self.retrace_path(start_node,target_node)
                self.path = path
                target_node.walkable = is_target_node_walkable
                return
            
            for neighbour in self.succ(current_node):
                neighbour_in_explored = [e for e in explored if e.state == neighbour.state]
                if not neighbour.walkable or len(neighbour_in_explored) > 0:
                    continue
                new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour)
                if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour):
                    neighbour.g_cost = new_movement_cost_to_neighbour
                    neighbour.h_cost = self.get_distance(neighbour,target_node)
                    neighbour.parent = current_node
                    if not fringe.contains(neighbour):
                        fringe.insert(neighbour)
        target_node.walkable = is_target_node_walkable
    
    def get_distance(self, node_a, node_b): # funckja liczy dystans dla odległości między dwoma nodami
        dist_x = abs(node_a.state.x - node_b.state.x)
        dist_y = abs(node_a.state.y - node_b.state.y)

        return (dist_x + dist_y) * 10
    
    def retrace_path(self, start_node, end_node): # funkcja zwraca tablice która ma w sobie wartosci pola parent
        # od end_node do start_node
        path = []
        current_node = end_node
        
        while current_node != start_node:
            path.append(current_node)
            current_node = current_node.parent
        path.reverse()
        return path
    
    def draw_path(self, window): # rysuję ścieżkę na ekranie
        color = (213, 55, 221)
        for node in self.path:
            node_x = node.state.x
            node_y = node.state.y
            block = pygame.Rect(
                node_x * G_var().RECT_SIZE, node_y *
                G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE
            )
            pygame.draw.rect(window,
                             color,
                             block)