189 lines
6.2 KiB
Python
189 lines
6.2 KiB
Python
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.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 = []
|
|
|
|
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)
|
|
|
|
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
|
|
while node.parent is not None: # FIX!!!!!!!!!!
|
|
cost = cost + 1 + 1
|
|
node = node.parent
|
|
return cost
|
|
#
|
|
|
|
|
|
def f(goal, node): # funkcja zwracająca sumę funkcji kosztu oraz heurestyki
|
|
return cost(node) + heuristic(goal, node)
|
|
|
|
|
|
def heuristic(goal, node): # funkcja heurestyki : oszacowuje koszt osiągnięcia stanu końcowego (droga)
|
|
return abs(node.x - goal[0]) + abs(node.y - goal[1])
|
|
|
|
|
|
def print_moves(elem): # zwraca listę ruchów jakie należy wykonać by dotrzeć do punktu docelowego
|
|
moves_list = []
|
|
while elem.parent is not None:
|
|
moves_list.append(elem.action)
|
|
elem = elem.parent
|
|
moves_list.reverse()
|
|
return moves_list
|
|
|
|
|
|
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
|