AIprojekt-wozek/Astar.py

import math

import pygame
from Global_variables import Global_variables as G_var
from Package import Package
from Shelf import Shelf


class State:

    def __init__(self, direction, x, y):
        self.direction = direction  # kierunek w ktorym "patrzy wozek"
        self.x = x
        self.y = y

    def get_direction(self):
        return self.direction

    def get_x(self):
        return self.x

    def get_y(self):
        return self.y

    def goal_test(self, goal):    # sprawdza czy osiagnelismy cel
        if self.x == goal[0] and self.y == goal[1]:
            return True
        else:
            return False


class Node:
    def __init__(self, state, walkable):
        self.state = state
        self.direction = state.direction
        self.walkable = walkable
        self.g_cost = 0
        self.h_cost = 0
        self.parent = None

    def get_action(self):
        return self.action

    def get_direction(self):
        return self.direction

    def get_parent(self):
        return self.parent

    def f_cost(self):
        if self.walkable:
            return self.g_cost + self.h_cost
        else:
            # return 0
            return math.inf


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 = []
        explored = []
        
        is_target_node_walkable = True
        if not target_node.walkable:
            target_node.walkable = True
            is_target_node_walkable = False
        fringe.append(start_node)

        while len(fringe) > 0:
            current_node = fringe[0]
            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)

            if current_node.state == target_node.state:
                path = self.retrace_path(start_node,target_node)
                self.path = path
            
            for neighbour in self.succ(current_node):
                if not neighbour.walkable or neighbour in explored:
                # 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:
                    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:
                        fringe.append(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)