AIprojekt-wozek/Astar.py

161 lines
6.7 KiB
Python
Raw Normal View History

import math
2022-05-12 21:01:31 +02:00
from operator import ne
import pygame
from Global_variables import Global_variables as G_var
2022-05-12 21:01:31 +02:00
from Min_heap import Min_heap
from Node import Node, State
from Package import Package
from Shelf import Shelf
2022-04-28 17:44:04 +02:00
class Pathfinding:
def __init__(self, enviroment_2d):
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)]
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
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)
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
################## 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]
2022-05-12 21:01:31 +02:00
fringe = Min_heap()
explored = []
is_target_node_walkable = True
if not target_node.walkable:
target_node.walkable = True
is_target_node_walkable = False
2022-05-12 21:01:31 +02:00
fringe.insert(start_node)
while fringe.count() > 0:
# current_node = fringe[0]
current_node = fringe.extract()
#################################################### 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)
if current_node.state == target_node.state:
path = self.retrace_path(start_node,target_node)
self.path = path
2022-05-12 22:53:12 +02:00
target_node.walkable = is_target_node_walkable
2022-05-12 21:01:31 +02:00
return
for neighbour in self.succ(current_node):
2022-05-12 22:53:12 +02:00
neighbour_in_explored = [e for e in explored if e.state == neighbour.state]
if not neighbour.walkable or len(neighbour_in_explored) > 0:
continue
new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour)
2022-05-12 21:01:31 +02:00
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
2022-05-12 21:01:31 +02:00
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)
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
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)