Advertisement
cookertron

Python A* Demo using Pygame

Feb 22nd, 2020
537
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 7.93 KB | None | 0 0
  1. import math, random, sys
  2. import lzma, base64
  3. import pygame
  4. from pygame.locals import *
  5.  
  6. # exit the program
  7. def events():
  8.     for event in pygame.event.get():
  9.         if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
  10.             pygame.quit()
  11.             sys.exit()
  12.  
  13. # define display surface           
  14. W, H = 10 * 100, 10 * 100
  15. HW, HH = W / 2, H / 2
  16. AREA = W * H
  17.  
  18. # initialise display
  19. pygame.init()
  20. pygame.font.init()
  21. CLOCK = pygame.time.Clock()
  22. FONT_SMALL = pygame.font.Font(None, 18)
  23. FONT_LARGE = pygame.font.Font(None, 24)
  24. DS = pygame.display.set_mode((W, H))
  25. pygame.display.set_caption("A* Demo")
  26. FPS = 1
  27.  
  28. # define some colors
  29. BLACK = (0, 0, 0, 255)
  30. WHITE = (255, 255, 255, 255)
  31. RED = (255, 0, 0, 255)
  32. GREEN = (0, 128, 0, 255)
  33. BLUE = (0, 0, 255, 255)
  34. PURPLE = (255, 0, 255, 255)
  35.  
  36. # define node class
  37. class node:
  38.     def __init__(self, x, y, obstacle):
  39.         self.x = x
  40.         self.y = y
  41.         self.pos = (x, y)
  42.         self.h = 0
  43.         self.g = 0
  44.         self.f = 0
  45.         self.obstacle = obstacle
  46.         self.other = None
  47.         self.parent = None
  48.    
  49.     def neighbourPos(self, offset):
  50.         return (self.x + offset[0], self.y + offset[1])
  51.    
  52.     def draw(self, size, color = None, id = None, surface = None):
  53.         global text, FONT_SMALL, FONT_LARGE
  54.         if not surface: surface = pygame.display.get_surface()
  55.         pos = (self.x * size[0], self.y * size[1])
  56.         if not color:
  57.             if not self.obstacle:
  58.                 if not self.other: pygame.draw.rect(surface, BLACK, pos + size, 0)
  59.                 else: pygame.draw.rect(surface, BLUE, pos + size, 0)
  60.             else:
  61.                 pygame.draw.rect(surface, WHITE, pos + size, 0)
  62.         else:
  63.             pygame.draw.rect(surface, color, pos + size, 0)
  64.         pygame.draw.rect(surface, WHITE, pos + size, 1)
  65.         if self.f:
  66.             text(FONT_SMALL, "G:{0}".format(self.g), pos[0] + 5, pos[1] + 5, 0, 0, surface)
  67.             text(FONT_SMALL, "H:{0}".format(self.h), pos[0] + size[0] - 5, pos[1] + 5, 1, 0, surface)
  68.             text(FONT_LARGE, "F:{0}".format(self.f),  pos[0] + size[0] / 2, pos[1] + size[1] / 2 , 2, 2, surface)
  69.             if not id == None:
  70.                 text(FONT_SMALL, "{0}".format(id), pos[0] + 5, pos[1] + size[1] - 5, 0, 1, surface)
  71.                
  72.            
  73. def drawNodes(n, ms, cs):
  74.     for x in range(ms[0]):
  75.         for y in range(ms[1]):
  76.             n[x][y].draw(cs)
  77.  
  78. def drawNodeList(node_list, cs, color):
  79.     id = 0
  80.     for n in node_list:
  81.         n.draw(cs, color, id)
  82.         id += 1
  83.        
  84. def heuristics(pos1, pos2):
  85.     return int(math.hypot(pos1[0] - pos2[0], pos1[1] - pos2[1]) * 10)
  86.  
  87. def text(font, string, x, y, xJustify = None, yJustify = None, surface = None):
  88.     global WHITE
  89.     if not surface: surface = pygame.display.get_surface()
  90.     textSurface = font.render(string, 1, WHITE)
  91.     textRect = textSurface.get_rect()
  92.     if xJustify == 1:
  93.         x -= textRect.width
  94.     elif xJustify == 2:
  95.         x -= textRect.center[0]
  96.     if yJustify == 1:
  97.         y -= textRect.height
  98.     elif yJustify == 2:
  99.         y -= textRect.center[1]
  100.     surface.blit(textSurface, (x, y))
  101.  
  102. def imgUnzip(lz64):
  103.     data = lzma.decompress(base64.b64decode(lz64))
  104.  
  105.     imageW = int.from_bytes(data[0:2], "big")
  106.     imageH = int.from_bytes(data[2:4], "big")
  107.     paletteLength = int.from_bytes(data[4:5], "big")
  108.     palette = [int.from_bytes(data[5 + index * 3:8 + index * 3], "big") for index in range(paletteLength)]
  109.     pixelDataStart = 5 + paletteLength * 3    
  110.  
  111.     surface = pygame.Surface((imageW, imageH))    
  112.     pixelArray = pygame.PixelArray(surface)
  113.    
  114.     for index in range(0, len(data) - pixelDataStart):
  115.         pixelArray[index % imageW][int(index / imageW)] = palette[data[pixelDataStart + index]]
  116.  
  117.     pixelArray.close()
  118.     del pixelArray
  119.     return surface
  120.  
  121. MAZE_DATA = b'/Td6WFoAAATm1rRGAgAhARYAAAB0L+Wj4AB0AChdAAADBGeLvBgf6WPhiDZ7E9c4Z5zvAdoLLoCxQgwVuFIwedPgadQpDAAAW\
  122. bRfbpSNdjcAAUR1mbr8lB+2830BAAAAAARZWg=='
  123.  
  124. maze = imgUnzip(MAZE_DATA)
  125. maze_size = maze_width, maze_height = maze.get_size()
  126. cell_size = (W / maze_width, H / maze_height)
  127.  
  128. #create list of nodes
  129. nodes = list([])
  130. for x in range(maze_width):
  131.     nodes.append(list([]))
  132.     for y in range(maze_height):
  133.         color = maze.get_at((x, y))
  134.         if color != WHITE:
  135.             nodes[x].append(node(x, y, False))
  136.             if color == BLUE:
  137.                 start = nodes[x][y]
  138.                 start.other = True
  139.             elif color == RED:
  140.                 end = nodes[x][y]
  141.                 end.other = True
  142.         else:
  143.             nodes[x].append(node(x, y, True))
  144.  
  145.  
  146. # This list contains relative x & y positions to reference a node's neighbour
  147. NEIGHBOURS = list([(-1, -1), (0, -1), (1, -1), (1, 0), (1, 1), (0, 1), (-1, 1), (-1, 0)])          
  148.  
  149. # the closed list contains all the nodes that have been considered economical viable.
  150. # By that I mean a node that has been closer to the end node than any other in the open list at one time
  151. closed = list([])
  152.  
  153. # The open list contains all the closed list's neighbours that haven't been identified as being economically sound node yet
  154. open = list([])
  155. open.append(start) # add the start node so that we can then add it's neighbours
  156.  
  157. # if the algorithm finds the end node then pathFound will be true otherwise it's false.
  158. # Once it becomes true there's no more calculations to do so the path finding script will be skipped over
  159. pathFound = False
  160. completedPath = list([]) #
  161.  
  162. # main loop
  163. while True:
  164.     DS.fill(BLACK) 
  165.     drawNodes(nodes, maze_size, cell_size)
  166.     drawNodeList(open, cell_size, GREEN)
  167.     drawNodeList(closed, cell_size, RED)
  168.     if pathFound: drawNodeList(completedPath, cell_size, PURPLE)
  169.     pygame.display.update()
  170.    
  171.     # wait for user to press mouse button
  172.     while not pygame.mouse.get_pressed()[0]:
  173.         events()
  174.     while pygame.mouse.get_pressed()[0]:
  175.         events()
  176.    
  177.     # if we've found the quickest path from start node to end node then just draw, no need continue path finding
  178.     if pathFound: continue
  179.     if not open: continue
  180.    
  181.    
  182.     # get lowest f from the open list, the node with the lowest f is the most economical in terms of the path towards the end node
  183.     openNodeWithlowestF = open[0]
  184.     for o in open:
  185.         if  o.f < openNodeWithlowestF.f: openNodeWithlowestF = o
  186.  
  187.     mostEconomicalNodeSoFar = openNodeWithlowestF # let's make this more readable! Economical means the best path to the end given the choices but not definitive.
  188.    
  189.     # remove the mostEconomicalNodeSoFar from the open list
  190.     open.remove(mostEconomicalNodeSoFar)
  191.     # add mostEconomicalNodeSoFar to the closed list
  192.     closed.append(mostEconomicalNodeSoFar)
  193.    
  194.     # if the mostEconomicalNodeSoFar is equal to the end node then we've reach our target
  195.     if mostEconomicalNodeSoFar == end:
  196.         temp = end
  197.         while temp.parent:
  198.             completedPath.append(temp)
  199.             temp = temp.parent
  200.         completedPath.append(start)
  201.         pathFound = True
  202.         # get the path etc
  203.        
  204.     # iterate through the list of neighbours belonging to the mostEconomicalNodeSoFar. Why?
  205.     for neighbourOffset in NEIGHBOURS:
  206.         nx, ny = mostEconomicalNodeSoFar.neighbourPos(neighbourOffset)
  207.  
  208.         if nx < 0 or nx >= maze_width or ny < 0 or ny >= maze_height: continue
  209.         neighbour = nodes[nx][ny] # create a variable to represent the mostEconomicalNodeSoFar's neighbour
  210.         if neighbour.obstacle: continue # if the mostEconomicalNodeSoFar's neighbouring node is an obstacle then we can't ...?
  211.         if neighbour in closed: continue # if the mostEconomicalNodeSoFar's neighbouring node is in the closed list then we can't ...?
  212.  
  213.         # now we need to see if the mostEconomicalNodeSoFar's neighbour is more economical ...?
  214.         hypotheticalFScore = mostEconomicalNodeSoFar.g + heuristics(neighbour.pos, mostEconomicalNodeSoFar.pos)
  215.         NeighbourIsBetterThanMostEconomicalNodeSoFar = False # Yes it's a long variable name but it describes what it is so all is good!
  216.        
  217.         # is this neighbour already in open list? if it is then we don't want to be adding it again. to chec
  218.         if not neighbour in open:
  219.             NeighbourIsBetterThanMostEconomicalNodeSoFar = True
  220.             neighbour.h = heuristics(neighbour.pos, end.pos)
  221.             open.append(neighbour)
  222.         elif hypotheticalFScore < neighbour.g:
  223.             NeighbourIsBetterThanMostEconomicalNodeSoFar = True
  224.  
  225.         if NeighbourIsBetterThanMostEconomicalNodeSoFar:
  226.             neighbour.parent = mostEconomicalNodeSoFar
  227.             neighbour.g = hypotheticalFScore
  228.             neighbour.f = neighbour.g + neighbour.h
  229.     #sys.exit()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement