SHARE
TWEET

temp A* pathfinding, with wiki as example

TankorSmash Oct 6th, 2011 168 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #used http://en.wikipedia.org/wiki/A*#Example as example
  2.  
  3. import pygame
  4. import os.path
  5. import math
  6. BLACK = (0,0,0)
  7. WHITE = (255,255,255)
  8.  
  9. UP = (0, -1)
  10. DOWN = (0, 1)
  11. LEFT = (-1, 0)
  12. RIGHT = (1, 0)
  13.  
  14. pygame.init()
  15.  
  16. screen = pygame.display.set_mode((250,250))
  17. screen.set_colorkey((WHITE))
  18. pygame.display.set_caption('test')
  19. screen.fill(WHITE)
  20. clock = pygame.time.Clock()
  21. basicFont = pygame.font.SysFont(None, 48)
  22. pygame.display.flip()
  23.  
  24.  
  25. #image = pygame.image.load(r'C:\Python26\projects\squares\art\man\1.png')
  26. #image.set_colorkey((WHITE))
  27.  
  28.  
  29. started = False
  30.  
  31. pygame.display.flip()
  32. running = 1
  33.  
  34.  
  35. class algo():
  36.     def __init__(self):
  37.         self.closedset = []
  38.         self.openset = [] #put the first click here
  39.         self.came_from = {} #where the algo has been, with all the node per pass looking to be eval
  40.  
  41.         self.g_score = {}
  42.         self.h_score = {}
  43.         self.f_score = {}
  44.        
  45.         self.path = []
  46.      
  47.     def dist_between(self, first, last):
  48.        
  49.         xs = last[0] - first[0]
  50.         ys = last[1] - first[1]
  51.        
  52.         xs **= 2
  53.         ys **= 2
  54.        
  55.         z = xs + ys
  56.        
  57.         return int(math.sqrt(z))
  58.    
  59.     def get_g(self, point, pointbefore):
  60.         self.g_score[point] = self.g_score[pointbefore] + self.dist_between(pointbefore,point)
  61.         return self.g_score[point]
  62.     def get_h(self, point):
  63.         self.h_score[point] = self.dist_between(point,self.END)
  64.         return self.h_score[point]
  65.     def get_f(self,point,pointbefore):
  66.        
  67.         self.f_score[point] = self.get_g(point,pointbefore) + self.get_h(point)
  68.         return self.f_score[point]
  69.     def first(self):
  70.              
  71.         self.g_score[self.START] = 0 #cost from start along best current path
  72.         self.h_score[self.START] = self.dist_between(self.START, self.END) #distance straight line from A to B
  73.         self.f_score[self.START] = self.g_score[self.START] + self.h_score[self.START] #cost of following current path from A to B
  74.         #self.openset.append(self.START)
  75.         #self.openset.sort()
  76.         pass
  77.    
  78.    
  79.     def start(self, START):
  80.         self.START = START
  81.         self.openset.append(self.START)
  82.        
  83.     def end(self, END):
  84.         self.END = END
  85.     def calc(self):
  86.         while len(self.openset)is not 0:
  87.             self.fscores = []
  88.            
  89.             for node in self.openset:
  90.                 self.fscores.append((self.f_score[node], node))
  91.             self.fscores.sort()
  92.             #print self.fscores
  93.             #self.openset.sort()
  94.             x = self.fscores[0][1]
  95.             print 'lowest f_score in openset: ', x,'with f of ' ,self.fscores[0][0],', not ', self.fscores[-1][1],'with f of ' ,self.fscores[-1][0]
  96.             print 'also, lowest f in open is {0}, while the highest is {1}'.format(min(self.fscores[0]),max(self.fscores[0]))
  97.             print '*'* 10
  98.             if x == self.END:
  99.                 self.path = self.reconstruct_path(self.came_from, self.came_from[self.END])
  100.                 for i in self.path:
  101.                     screen.set_at(i,BLACK)
  102.                 pygame.display.flip()
  103.                 print 'OVER!!!',
  104.                 break
  105.             self.openset.remove(x)
  106.             self.closedset.append(x)
  107.             nearby = self.neighborNodes(x)
  108.             for y in nearby:
  109.                 if y in self.closedset:
  110.                     #print 'got rid of ', y
  111.                     continue
  112.                 self.tentative_g_score = self.g_score[x]+ self.dist_between(x,y)
  113.                 #print 'dist', self.dist_between(x,y)
  114.                 #self.tentative_is_better = False
  115.                 if y not in self.openset:
  116.                     self.openset.append(y)
  117.                     self.tentative_is_better = True
  118.                 elif self.tentative_g_score < self.g_score[y]:
  119.                     self.tentative_is_better= True
  120.                 else:
  121.                     self.tentative_is_better = False
  122.                     #print 'tent is better'
  123.                 if self.tentative_is_better:
  124.                     #screen.set_at(x, (255,255,0))
  125.                     self.came_from[y] = x
  126.                     self.g_score[y] = self.tentative_g_score
  127.                     self.h_score[y] = self.dist_between(y, self.END)
  128.                     self.f_score[y] = self.g_score[y] + self.h_score[y]
  129.                     print 'g {0}, h {1}, f {2}, from point {3}'.format(self.g_score[y],self.h_score[y],self.f_score[y], y)
  130.         print 'returned path'
  131.         #print self.came_from
  132.         return self.came_from
  133.     def neighborNodes(self,cur):
  134.        
  135.         #cur = list(cur[1])
  136.  
  137.         left = tuple(sum(x) for x in zip(cur, LEFT))
  138.         up = tuple(map(sum,zip(cur,UP)))
  139.         right = tuple(map(sum,zip(cur,RIGHT)))
  140.         down = tuple(map(sum,zip(cur,DOWN)))
  141.         nodes = (left,up,right,down)
  142.         unused = []
  143.         for node in nodes:
  144.             if node not in self.came_from:
  145.                 unused.append(node)
  146.         return unused
  147.     def reconstruct_path(self, came_from, current_node):
  148.         #if self.came_from[current_node] is not None:
  149.         if current_node in came_from.keys():
  150.             p = self.reconstruct_path(came_from, came_from[current_node])
  151.             return p + [current_node]
  152.        
  153.         else : return [current_node]
  154.    
  155. a= algo()      
  156. while running:
  157.    
  158.     screen.blit(image,(0,0))
  159.  
  160.     pygame.display.flip()
  161.    
  162.     for event in pygame.event.get():
  163.         if event.type== pygame.QUIT:
  164.             running = 0
  165.            
  166.         if event.type== pygame.MOUSEBUTTONUP:
  167.             if not started:
  168.                 started = 1
  169.                
  170.                 a.start(event.pos)
  171.                 print 'start point: ', event.pos
  172.             elif started:
  173.                 started = 0
  174.                 a.end(event.pos)
  175.                 a.first()
  176.                 print 'end point: ', event.pos
  177.         if event.type == pygame.KEYDOWN:
  178.             if event.key == pygame.K_c:
  179.                 print 'calc that shit'
  180.                 done = a.calc()
  181.                
  182.                 print done
  183.                
  184.             elif event.key == pygame.K_SPACE:
  185.                 print 'skipping, shortcutting'
  186.                 a.start((10,10))
  187.                 a.end((20,20))
  188.                 a.first()
  189.                 done = a.calc()
  190.                 #print done
  191.                 #for point in done.keys():
  192.                     #screen.set_at((point),(BLACK))
  193.                 pygame.display.flip()
  194.                 print 'done drawing'
  195.                
  196.            
  197.     clock.tick(30)
  198.  
RAW Paste Data
Top