To jeszcze nie działa. Testowy commit

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:
+        # while len(fringe) > 0:
-            current_node = fringe[0]
+        while fringe.count() > 0:
-            for i in range(1, len(fringe)):
+            # current_node = fringe[0]
-                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.extract()
-                    current_node = fringe[i]
+            # 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:
+                    # if not neighbour in fringe:
-                        fringe.append(neighbour)
+                    if not fringe.contains(neighbour):
+                        fringe.insert(neighbour)
 
         target_node.walkable = is_target_node_walkable
     

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

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
+