algorytm astar v1

astar.py

@@ -0,0 +1,48 @@
+import heapq
+
+
+def heurystyka(a, b):
+    return abs((b[0] - a[0])) + abs((b[1] - a[1]))
+
+
+def astar(start, cel):
+    import game
+
+    obiekty = game.utworzObiekty()
+    sasiedzi = [(0, 1), (0, -1), (1, 0), (-1, 0)]
+    close_set = set()
+    came_from = {}
+    gscore = {start: 0}
+    fscore = {start: heurystyka(start, cel)}
+    oheap = []
+    heapq.heappush(oheap, (fscore[start], start))
+    while oheap:
+        current = heapq.heappop(oheap)[1]
+        if current == cel:
+            data = []
+            while current in came_from:
+                data.append(current)
+                current = came_from[current]
+            return data[::-1]
+        close_set.add(current)
+        for i, j in sasiedzi:
+            sasiad = current[0] + i, current[1] + j
+            tentative_g_score = gscore[current] + heurystyka(current, sasiad)
+            if 14 < sasiad[0] or sasiad[0] < 0:
+                continue
+            elif 14 < sasiad[1] or sasiad[1] < 0:
+                continue
+            elif 6 <= sasiad[0] <= 7 and 10 <= sasiad[1] <= 11:
+                continue
+            elif 4 < sasiad[0] and 4 < sasiad[1]:
+                continue
+            elif 4 > sasiad[0] and 4 > sasiad[1]:
+                continue
+            if sasiad in close_set and tentative_g_score >= gscore.get(sasiad, 0):
+                continue
+            if tentative_g_score < gscore.get(sasiad, 0) or sasiad not in [i[1] for i in oheap]:
+                came_from[sasiad] = current
+                gscore[sasiad] = tentative_g_score
+                fscore[sasiad] = tentative_g_score + heurystyka(sasiad, cel)
+                heapq.heappush(oheap, (fscore[sasiad], sasiad))
+    return False

game.py

@@ -4,6 +4,7 @@ import numpy as np
 import random
 import os
 import shutil
+import astar
 
 pygame.init()
 
@@ -67,8 +68,13 @@ def game():
                 if event.key == pygame.K_DOWN:
                     obiekty["smieciarka"].w_dol()
 
+
         obiekty["smieciarka"].rand_move()
         clock.tick(7)
+    #start = obiekty["plansza"][0, 14]
+    #koniec = obiekty["plansza"][14, 0]
+    print(astar.astar((0, 14), (14, 0)))
+    #print(len(astar.astar(start, koniec)))
     pygame.quit()
 
 

modele.py

@@ -190,21 +190,40 @@ class Kratka(pygame.sprite.Sprite):
     def __init__(self, poz_x, poz_y):
         self.pozX = poz_x
         self.pozY = poz_y
+        self.pozycja = (self.pozX, self.pozY)
+        self.rodzic = None
         self.jestDomem = False
         self.jestKontenerem = False
         self.jestWysypiskiem = False
         self.jestPrzeszkoda = False
         self.kolor = GREY
         self.obiekt = None
+        self.g = 0  # Distance to start node
+        self.h = 0  # Distance to goal node
+        self.f = 0  # Total cost
         pygame.sprite.Sprite.__init__(self)
         self.image = pygame.image.__class__
         self.rect = pygame.Rect(self.pozX * WIDTH + MARGIN * self.pozX + MARGIN,
                                 self.pozY * HEIGHT + MARGIN * self.pozY + MARGIN,
                                 WIDTH, HEIGHT)
 
+    # Sort nodes
+    def __lt__(self, other):
+        return self.f < other.f
+
+    def __repr__(self):
+        return ('{0}'.format(self.pozycja))
+
+    # Compare nodes
+    def __eq__(self, other):
+        return True if self.pozycja == other.pozycja else False
+
     def setImage(self, image):
         self.image = image
 
+    def setRodzic(self, rodzic):
+        self.rodzic = rodzic
+
     def setObiekt(self, obiekt):
         self.obiekt = obiekt