Update AStar algorithm

UI/Apath.py

@@ -27,14 +27,16 @@ def APath(table, start, end):
     # Add the start node
     open_list.append(start_node)
 
-    # Loop until you find the end
-    while len(open_list) > 0:
 
-        # Get the current node
+    # Loop until you find the end
+    i = 0
+    while len(open_list) > 0:
+        print(i)
+        i=i+1
         current_node = open_list[0]
         current_index = 0
         for index, item in enumerate(open_list):
-            if item.f < current_node.f:
+            if(item.f < current_node.f):
                 current_node = item
                 current_index = index
 
@@ -75,20 +77,39 @@ def APath(table, start, end):
         # Loop through children
         for child in children:
 
+
+            def InClosedlist(child: AStarNode):
+                for closed_child in closed_list:
+                    if child.position == closed_child.position:
+                        return True
+                    
+
+            def InOpenlist(child: AStarNode):
+                for open_node in open_list:
+                    if child == open_node: # and child.g > open_node.g:
+                        return True
+                    
+
+           
             # Child is on the closed list
-            for closed_child in closed_list:
-                if child == closed_child:
-                    continue
+            if InClosedlist(child)==True:
+                continue
 
             # Create the f, g, and h values
             child.g = current_node.g + 1
-            child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2)
+            child.h = max(abs(child.position[0] - end_node.position[0]), abs(child.position[1] - end_node.position[1]))
             child.f = child.g + child.h
 
             # Child is already in the open list
-            for open_node in open_list:
-                if child == open_node and child.g > open_node.g:
-                    continue
+            if InOpenlist(child)==True:
+                continue
 
             # Add the child to the open list
             open_list.append(child)
+
+        
+
+           
+
+        
+

UI/window.py

@@ -7,12 +7,15 @@ from UI.Apath import APath
 
 
 class Window():
-    def __init__(self, grid: Grid):
+    def __init__(self, grid: Grid,start: (int,int),end: (int,int)):
         pg.init()   # pylint: disable=no-member
         # setup window
         pg.display.set_caption('Inteligentna śmieciarka')
-
+        
+        self.start = start
+        self.end = end
         self.grid = grid
+
         # assign to variables for brevity
         cols = self.grid.cols
         rows = self.grid.rows
@@ -20,6 +23,7 @@ class Window():
         height = Node.r_height
         margin = Node.r_margin
 
+
         screen_width = cols * (width + margin) + 2 * margin
         screen_height = rows * (height + margin) + 2 * margin
 
@@ -28,21 +32,32 @@ class Window():
         self.end = False
 
         self.clock = pg.time.Clock()
-        grid.change_field(0, 0, 1)
-        grid.change_field(19, 19, 2)
+        
 
-        #random obsticle
-        for x in range(200):
-            grid.change_field(random.randint(1,18),random.randint(1,18),3)
+        def Obstacles(self, grid: Grid,option: int):
+            if option == 1:
+                for x in range(len(grid.table)*5):
+                    grid.change_field(random.randint(1,len(grid.table)-1),random.randint(1,len(grid.table)-1),3)
+            elif option ==2:
+                for x in range (13):
+                    grid.change_field(x,14,3)
+                for x in range (8):
+                    grid.change_field(12,x+6,3)
+                
 
-        #path
-        path = [(i, i) for i in range(1, 20, 1)]
+
+        Obstacles(self,grid,2)
+        grid.change_field(start[0], start[1], 1)
+        grid.change_field(end[0], end[1], 2)
+
+        #draw starting map
         self.grid.draw_map(self.screen)
 
         #copy table
         array = [[self.grid.table[col][row] for row in range(cols)] for col in range(rows)]
-        path = APath(array,(0,0),(19,19))
-
+        path = APath(array,(start[0],start[1]),(end[0],end[1]))
+        print("Path:",path)
+        
         #draw movement of garbage truck
         for index, t  in enumerate(path):
             x,y =t
@@ -56,4 +71,6 @@ class Window():
 
     
 
+    
+
 

main.py

@@ -6,7 +6,7 @@ def main():
     # initialize grid
     grid = Grid(20, 20)
     # initialize window
-    Window(grid)
+    Window(grid,(0,0),(19,19))
 
 
 if __name__ == "__main__":