diff --git a/Environment.py b/Environment.py index 2fe54dd..8107490 100644 --- a/Environment.py +++ b/Environment.py @@ -10,6 +10,8 @@ from Truck import Truck from Global_variables import Global_variables as G_var from pygame.constants import * +from astar import Pathfinding, State + class Environment: def __init__(self, window): @@ -25,13 +27,30 @@ class Environment: self.truck = new_truck self.moving_truck = Moving_truck( self.window, self.enviroment_2d, self.truck) + self.astar = Pathfinding(self.enviroment_2d) def draw_all_elements(self): for row in self.enviroment_2d: for field in row: field.draw() self.grid.draw_grid() + self.use_astar() # w przyszlosci trzeba przeniesc funkcje w jakies logiczniejsze miejsce np funkcje update() + self.astar.draw_path(self.window) pygame.display.flip() + + def use_astar(self): + start_state = State(1,self.truck.x,self.truck.y) + package = self.find_packate() + end_state = State(1,package.x, package.y) + self.astar.find_path(start_state,end_state) + + def find_packate(self): #ta funkcja została zrobiona na szybko, może nie być potrzebna w przyszłości kiedy + #ktoś wpadnie na lepsze rozwiązanie + for row in self.enviroment_2d: + for field in row: + if isinstance(field,Package): + return field + return None def update_truck(self, event): if event.type == KEYDOWN: diff --git a/astar.py b/astar.py index e7460c9..599bb55 100644 --- a/astar.py +++ b/astar.py @@ -1,8 +1,15 @@ +from ast import walk +import math + +import pygame +from Global_variables import Global_variables as G_var +from Shelf import Shelf + class State: def __init__(self, direction, x, y): - self.direction = direction # kierunek w ktorym "patrzy wozek" + self.direction = direction # kierunek w ktorym "patrzy wozek" self.x = x self.y = y @@ -23,11 +30,13 @@ class State: class Node: - def __init__(self, action, state, parent): + def __init__(self, state, walkable): self.state = state - self.action = action # akcja jaką ma wykonać (jedz prawo, lewo, przod) self.direction = state.direction - self.parent = parent # ojciec wierzchołka + self.walkable = walkable + self.g_cost = 0 + self.h_cost = 0 + self.parent = None def get_action(self): return self.action @@ -38,6 +47,110 @@ class Node: def get_parent(self): return self.parent + def f_cost(self): + if self.walkable: + return self.g_cost + self.h_cost + else: + 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 = [] + + 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: + 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) + + 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) + + + def cost(node): # funkcja kosztu : ile kosztuje przejechanie przez dane pole cost = 0 @@ -45,6 +158,7 @@ def cost(node): # funkcja kosztu : ile kosztuje przejechanie przez dane pole cost = cost + 1 + 1 node = node.parent return cost +# def f(goal, node): # funkcja zwracająca sumę funkcji kosztu oraz heurestyki @@ -67,6 +181,6 @@ def print_moves(elem): # zwraca listę ruchów jakie należy wykonać by dotrze def succ(elem): # funkcja następnika, przypisuje jakie akcje są możliwe do wykonania na danym polu oraz jaki będzie stan (kierunek, położenie) po wykonaniu tej akcji pass + def graphsearch(explored, fringe, goaltest, istate): # przeszukiwanie grafu wszerz pass -