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.grid = []
        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
                if isinstance(enviroment_2d[x][y], Shelf):
                    is_walkable = False
                self.grid[x][y] = Node(State(1, x, y), is_walkable)
        
        self.path = []

    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
                

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

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

        # while len(fringe) > 0:
        while fringe.count() > 0:
            # current_node = fringe[0]
            current_node = fringe.extract()
            # for i in range(1, len(fringe)):
            #     if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
            #         current_node = fringe[i]
            
            # fringe.remove(current_node)
            explored.append(current_node)
            # explored.add(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
                # if neighbour in explored:
                    # 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 neighbour in fringe:
                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 neighbour in fringe:
                    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)

        if dist_x > dist_y:
            return 10 * (dist_x - dist_y)
        return 10 * (dist_y - dist_x)
    
    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)