A* search

astar.py

@@ -0,0 +1,155 @@
+import pygame
+from board import Board
+from constant import width, height, rows, cols
+from tractor import Tractor
+import heapq
+import math
+
+fps = 2
+WIN = pygame.display.set_mode((width, height))
+pygame.display.set_caption('Inteligenty Traktor')
+
+class Node:
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.f = 0
+        self.g = 0
+        self.h = 0
+        self.cost = 1
+        self.visited = False
+        self.closed = False
+        self.parent = None
+
+    def __lt__(self, other):
+        return self.f < other.f
+
+    def neighbors(self, grid):
+        ret = []
+        x, y = self.x, self.y
+        if x > 0 and grid[x - 1][y]:
+            ret.append(grid[x - 1][y])
+        if x < len(grid) - 1 and grid[x + 1][y]:
+            ret.append(grid[x + 1][y])
+        if y > 0 and grid[x][y - 1]:
+            ret.append(grid[x][y - 1])
+        if y < len(grid[0]) - 1 and grid[x][y + 1]:
+            ret.append(grid[x][y + 1])
+        return ret
+
+def init(grid):
+    for x in range(len(grid)):
+        for y in range(len(grid[x])):
+            node = grid[x][y]
+            node.f = 0
+            node.g = 0
+            node.h = 0
+            node.cost = 1
+            node.visited = False
+            node.closed = False
+            node.parent = None
+
+def heap():
+    return []
+
+def search(grid, start, end, board, heuristic=None):
+    init(grid)
+    if heuristic is None:
+        heuristic = manhattan
+    open_heap = heap()
+    heapq.heappush(open_heap, start)
+
+    while open_heap:
+        current_node = heapq.heappop(open_heap)
+        if (current_node.x, current_node.y) == (end.x, end.y):
+
+            ret = []
+            while current_node.parent:
+                ret.append(current_node)
+                current_node = current_node.parent
+            ret.append(start)    
+            ret_path = ret[::-1]
+            for node in ret_path:
+                print(f"({node.x}, {node.y}): {node.g}")
+            print("Znaleziono ścieżkę [(x,y)jako(kolumna,wiersz)] o koszcie:", ret_path[-1].g)
+            return ret_path, start 
+        current_node.closed = True
+        for neighbor in current_node.neighbors(grid):
+            if neighbor.closed:  
+                continue
+            g_score = current_node.g + board.get_cost(neighbor.x, neighbor.y)  
+            been_visited = neighbor.visited
+            if not been_visited or g_score < neighbor.g:
+                neighbor.visited = True
+                neighbor.parent = current_node
+                neighbor.h = neighbor.h or heuristic((neighbor.x, neighbor.y), (end.x, end.y))
+                neighbor.g = g_score
+                neighbor.f = neighbor.g + neighbor.h
+                if not been_visited:
+                    heapq.heappush(open_heap, neighbor)
+    
+    print("Nie znaleziono ścieżki.")
+    return None
+
+
+def manhattan(pos0, pos1):
+    d1 = abs(pos1[0] - pos0[0])
+    d2 = abs(pos1[1] - pos0[1])
+    return d1 + d2
+
+
+def main():
+    rotation  = ["left", "up", "right", "down"]  
+    run = True
+    clock = pygame.time.Clock()
+    board = Board()
+    board.load_images()
+    
+    start_row, start_col = 1, 1
+    end_row, end_col = 5, 0
+    tractor = Tractor(start_row, start_col)  
+    board.set_grass(start_row, start_col)
+    board.set_grass(end_row, end_col)
+   
+    grid = [[Node(x, y) for y in range(rows)] for x in range(cols)]
+    
+    
+    start = grid[start_row][start_col]
+    end = grid[end_row][end_col]
+    
+    path, start_node = search(grid, start, end, board)
+    
+
+    while run:
+        
+        
+        clock.tick(fps)
+   
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                run = False
+
+        if not path:
+            run = False
+            continue
+
+        next_node = path.pop(0) if path else start_node
+        tractor.row, tractor.col = next_node.y, next_node.x
+        
+        
+        #rotation
+        
+
+
+
+        board.draw_cubes(WIN)
+        tractor.draw(WIN)
+        pygame.display.update()
+
+    while True:
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                pygame.quit()
+                return
+
+main()

board.py

@@ -8,6 +8,7 @@ class Board:
         self.board = []
         self.load_images()
         self.generate_board()
+        self.load_costs()
 
 
     def load_images(self):
@@ -16,6 +17,7 @@ class Board:
         self.rock= pygame.image.load("board/rock.png")
         self.weeds =  pygame.image.load("board/weeds.png")
         self.soil = pygame.image.load("board/zyzna.png")
+        self.carrot = pygame.image.load("board/carrot.png")
 
     def generate_board(self):
         self.board = [[random.choice([0,1,2,3,4,5,6,7,8,9]) for _ in range(rows)] for _ in range(cols)]
@@ -41,9 +43,35 @@ class Board:
                     win.blit(self.grass,  cube_rect)
                 elif cube == 10:
                     win.blit(self.soil, cube_rect)
+                elif cube == 11:
+                    carrot_scale = pygame.transform.scale(self.carrot, (size,size))
+                    win.blit(self.carrot, cube_rect) 
+                    win.blit(carrot_scale, cube_rect)
+                       
                 else:
                     win.blit(self.dirt,  cube_rect)
                     
+                    
+    def load_costs(self):
+            self.costs = {
+                0: 100,  #kamien
+                1: 2,  #chwasty
+                2: 1,   #po trawie
+                3: 1,   #po trawie
+                4: 1,   #po trawie
+                5: 1,   #po trawie
+                6: 3,   #ziemia
+                7: 3,   #ziemia
+                8: 3,   #ziemia
+                9: 3,   #ziemia    
+                10: 4,  #zyzna
+                11: 10  #marchewka
+            }
+
+    def get_cost(self, row, col):
+        tile_type = self.board[row][col]
+        return self.costs.get(tile_type, 1)  
+            
     def is_rock(self, row, col):
         return self.board[row][col] == 0
 
@@ -56,5 +84,11 @@ class Board:
     def is_dirt(self,row,col):
         return self.board[row][col] in (6,7,8,9)
         
+    def is_soil(self, row, col):
+        return self.board[row][col] == 10
+    
     def set_soil(self, row, col):
         self.board[row][col] = 10
+        
+    def set_carrot(self, row, col):
+        self.board[row][col] = 11