AIprojekt-wozek/Astar.py
2022-05-12 21:01:31 +02:00

119 lines
4.7 KiB
Python

import math
from operator import ne
import pygame
from Global_variables import Global_variables as G_var
from Min_heap import Min_heap
from Node import Node, State
from Package import Package
from Shelf import Shelf
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 = []
fringe = Min_heap()
explored = []
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)
explored.append(current_node)
if current_node.state == target_node.state:
path = self.retrace_path(start_node,target_node)
self.path = path
return
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:
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
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)