From b771ac0f00ea4b2bbf5edbb7c86159dc3555d23a Mon Sep 17 00:00:00 2001 From: tonywesoly Date: Thu, 12 May 2022 21:01:31 +0200 Subject: [PATCH] =?UTF-8?q?To=20jeszcze=20nie=20dzia=C5=82a.=20Testowy=20c?= =?UTF-8?q?ommit?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Astar.py | 81 ++++++++++++++--------------------------------------- Min_heap.py | 76 +++++++++++++++++++++++++++++++++++++++++++++++++ Node.py | 64 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 161 insertions(+), 60 deletions(-) create mode 100644 Min_heap.py create mode 100644 Node.py diff --git a/Astar.py b/Astar.py index 57223cf..491b32f 100644 --- a/Astar.py +++ b/Astar.py @@ -1,60 +1,14 @@ 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 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 = [] @@ -89,39 +43,46 @@ class Pathfinding: start_node = self.grid[starting_state.x][starting_state.y] target_node = self.grid[target_state.x][target_state.y] - fringe = [] + # 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.append(start_node) + fringe.insert(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] + # 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) + # 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 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: - fringe.append(neighbour) + # if not neighbour in fringe: + if not fringe.contains(neighbour): + fringe.insert(neighbour) target_node.walkable = is_target_node_walkable diff --git a/Min_heap.py b/Min_heap.py new file mode 100644 index 0000000..633bc11 --- /dev/null +++ b/Min_heap.py @@ -0,0 +1,76 @@ +from cgitb import small +from heapq import heapify +import math +from multiprocessing.dummy import Array + +from Node import Node, State + + + +class Min_heap: + def __init__(self): + self.items = [] + + def parent(self,i): + return (i - 1) >> 1 + + def left(self,i): + return (i << 1) + 1 + + def right(self,i): + return (i << 1) + 2 + + def heapify(self, i): + l = self.left(i) + r = self.right(i) + if l < len(self.items) - 1 and self.items[l] < self.items[i]: + smallest = l + else: + smallest = i + if r < len(self.items) - 1 and self.items[r] < self.items[smallest]: + smallest = r + if smallest != i: + self.items[i], self.items[smallest] = self.items[smallest], self.items[i] + self.items[i].heap_index, self.items[smallest].heap_index = self.items[smallest].heap_index, self.items[i].heap_index + self.heapify(smallest) + + def extract(self): + if len(self.items) < 1: + 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) + return min + + def decrese_key(self, index, item): + if item > self.items[index]: + print("Nowy klucz wiekszy od klucza aktualnego!") + return + self.items[index] = item + while index > 0 and self.items[self.parent(index)] > self.items[index]: + self.items[index], self.items[self.parent( + index)] = self.items[self.parent(index)], self.items[index] + self.items[index].heap_index, self.items[self.parent( + index)].heap_index = self.items[self.parent(index)].heap_index, self.items[index].heap_index + index = self.parent(index) + + def insert(self, item): + temp_node = Node(State(0,0,0),False) + temp_node.h_cost = math.inf + self.items.append(temp_node) + item.heap_index = len(self.items) - 1 + self.decrese_key(len(self.items) - 1, item) + + def count(self): + return len(self.items) + + def contains(self, item): + in_range = len(self.items) > item.heap_index + contains = False + if in_range: + contains = self.items[item.heap_index] == item + return in_range and contains \ No newline at end of file diff --git a/Node.py b/Node.py new file mode 100644 index 0000000..f3e1cdf --- /dev/null +++ b/Node.py @@ -0,0 +1,64 @@ + +import math + + +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 + self.heap_index = 0 + + 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 + # 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): + def __lt__(self, other): + if self.f_cost() == other.f_cost(): + return self.h_cost < other.h_cost + return self.f_cost() < other.f_cost() + def __gt__(self,other): + if self.f_cost() == other.f_cost(): + return self.h_cost > other.h_cost + return self.f_cost() > other.f_cost() + def __eq__(self,other): + return self.f_cost() == other.f_cost() and self.h_cost == other.h_cost +