Fixing astar and BIG performance improvments
This commit is contained in:
tonywesoly
parent
2c258fb6d5
commit
71db6fb6b9
83
Astar.py
83
Astar.py
@ -11,7 +11,11 @@ from Shelf import Shelf
|
||||
|
||||
class Pathfinding:
|
||||
def __init__(self, enviroment_2d):
|
||||
# self.grid = []
|
||||
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)]
|
||||
@ -20,12 +24,13 @@ class Pathfinding:
|
||||
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):
|
||||
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)
|
||||
|
||||
self.path = []
|
||||
|
||||
def succ(self,node): #funckja zwraca sąsiadów noda w argumencie
|
||||
node_x = node.state.x
|
||||
node_y = node.state.y
|
||||
@ -39,33 +44,73 @@ class Pathfinding:
|
||||
return neighbours
|
||||
|
||||
|
||||
def find_path(self, starting_state, target_state): # algorytm wyszukiwania trasy
|
||||
################## 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 = []
|
||||
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)
|
||||
#################################################### 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)
|
||||
# explored.add(current_node)
|
||||
|
||||
if current_node.state == target_node.state:
|
||||
path = self.retrace_path(start_node,target_node)
|
||||
@ -77,15 +122,11 @@ class Pathfinding:
|
||||
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
|
||||
@ -94,9 +135,7 @@ class Pathfinding:
|
||||
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)
|
||||
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
|
||||
|
||||
@ -28,9 +28,9 @@ class Environment:
|
||||
new_truck = Truck(window, 14, 7)
|
||||
self.enviroment_2d[14][7] = new_truck
|
||||
self.truck = new_truck
|
||||
self.moving_truck = Moving_truck(
|
||||
self.window, self.enviroment_2d, self.truck, self.package_spawner)
|
||||
self.astar = Pathfinding(self.enviroment_2d)
|
||||
self.moving_truck = Moving_truck(
|
||||
self.window, self.enviroment_2d, self.truck, self.package_spawner, self.astar)
|
||||
self.finding_fields = Finding_fields(self.enviroment_2d)
|
||||
|
||||
def draw_all_elements(self):
|
||||
@ -38,13 +38,13 @@ class Environment:
|
||||
for field in row:
|
||||
field.draw()
|
||||
self.grid.draw_grid()
|
||||
self.astar.draw_path(self.window)
|
||||
# self.astar.draw_path(self.window)
|
||||
pygame.display.flip()
|
||||
|
||||
def update_all_elements(self):
|
||||
self.use_astar() # wywyoływanie za każdym razem astar jest bardzo zasobożerne. Lepiej raz na przejście
|
||||
self.update_truck()
|
||||
time.sleep(0.5)
|
||||
# time.sleep(0.5)
|
||||
|
||||
|
||||
def use_astar(self):
|
||||
@ -54,7 +54,7 @@ class Environment:
|
||||
else:
|
||||
end_position = self.finding_fields.find_package()
|
||||
end_state = State(1,end_position.x, end_position.y)
|
||||
self.astar.find_path(start_state,end_state)
|
||||
self.astar.find_path(start_state,end_state,self.window)
|
||||
|
||||
def update_truck(self):
|
||||
next_field_to_move = self.astar.path[0].state
|
||||
|
||||
@ -10,11 +10,6 @@ class Global_variables(object):
|
||||
RECT_COLOR = (70, 77, 87)
|
||||
SHELF_COLOR = (143, 68, 33)
|
||||
|
||||
def __init__(self) -> None:
|
||||
dim_x = 28
|
||||
dim_y = 15
|
||||
self.GRID = [["empty" for i in range(dim_x)] for j in range(dim_y)]
|
||||
|
||||
def __new__(cls):
|
||||
if cls._instance is None:
|
||||
cls._instance = super(Global_variables, cls).__new__(cls)
|
||||
|
||||
@ -39,8 +39,6 @@ class Min_heap:
|
||||
print("STOS PUSTY!")
|
||||
return
|
||||
min = self.items[0]
|
||||
# self.items[0] = self.items[last_item_index]
|
||||
# self.items[0] = self.items.pop()
|
||||
self.items[0] = self.items[len(self.items) - 1]
|
||||
self.items.pop()
|
||||
self.heapify(0)
|
||||
@ -64,7 +62,6 @@ class Min_heap:
|
||||
temp_node.h_cost = math.inf
|
||||
temp_node.heap_index = len(self.items) - 1
|
||||
self.items.append(temp_node)
|
||||
# item.heap_index = len(self.items) - 1
|
||||
self.decrese_key(len(self.items) - 1, item)
|
||||
|
||||
def count(self):
|
||||
@ -74,5 +71,5 @@ class Min_heap:
|
||||
in_range = len(self.items) > item.heap_index
|
||||
contains = False
|
||||
if in_range:
|
||||
contains = self.items[item.heap_index] == item
|
||||
contains = self.items[item.heap_index] is item
|
||||
return in_range and contains
|
||||
@ -2,13 +2,14 @@ from Empty import Empty
|
||||
from Package import Package
|
||||
from Shelf import Shelf
|
||||
|
||||
|
||||
# TODO: DODAC OBSERWER ZAMIAST PRZEKAZYWANIE WSZYSTKICH BZDET
|
||||
class Moving_truck:
|
||||
def __init__(self, window, enviroment_2d, truck, package_spawner):
|
||||
def __init__(self, window, enviroment_2d, truck, package_spawner, a_star):
|
||||
self.enviroment_2d = enviroment_2d
|
||||
self.truck = truck
|
||||
self.window = window
|
||||
self.package_spawner = package_spawner
|
||||
self.astar = a_star
|
||||
|
||||
def move(self, x, y):
|
||||
truck_x = self.truck.x
|
||||
@ -29,6 +30,7 @@ class Moving_truck:
|
||||
package = self.enviroment_2d[package_x][package_y]
|
||||
self.truck.has_package = True
|
||||
self.truck.package_type = package.type
|
||||
self.astar.reset_grid()
|
||||
self.move_without_swapping(truck_x, truck_y, package_x, package_y)
|
||||
|
||||
|
||||
@ -41,6 +43,7 @@ class Moving_truck:
|
||||
y].is_placed = True
|
||||
self.truck.has_package = False
|
||||
self.package_spawner.spawn_package()
|
||||
self.astar.reset_grid()
|
||||
|
||||
def swap_fields(self, x1, y1, x2, y2):
|
||||
self.enviroment_2d[x1][y1], self.enviroment_2d[x2][y2] = self.enviroment_2d[x2][y2], self.enviroment_2d[x1][y1]
|
||||
|
||||
@ -17,5 +17,5 @@ class Program:
|
||||
for event in pygame.event.get(): # integrating with keyboard
|
||||
if event.type == QUIT:
|
||||
running = False
|
||||
self.environment.update_all_elements()
|
||||
self.environment.draw_all_elements()
|
||||
self.environment.update_all_elements()
|
||||
self.environment.draw_all_elements()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user