research/transmission_spectroscopy/simulator/pgu-0.18/build/lib/pgu/algo.py

   1 """Some handy algorithms for use in games, etc.
   2
   3 Please note that this file is alpha, and is subject to modification in
   4 future versions of pgu!
   5 """
   6
   7 # The manhattan distance metric
   8 def manhattan_dist(a,b):
   9     return abs(a[0]-b[0]) + abs(a[1]-b[1])
  10
  11 class node:
  12     def __init__(self, prev, pos, dest, dist):
  13         self.prev,self.pos,self.dest = prev,pos,dest
  14         if self.prev == None: self.g = 0
  15         else: self.g = self.prev.g + 1
  16         self.h = dist(pos,dest)
  17         self.f = self.g+self.h
  18
  19
  20 def astar(start,end,layer,dist=manhattan_dist):
  21     """Uses the a* algorithm to find a path, and returns a list of positions
  22     from start to end.
  23
  24     Arguments:
  25         start -- start position
  26         end -- end position
  27         layer -- a grid where zero cells are open and non-zero cells are walls
  28         dist -- a distance function dist(a,b) - manhattan distance is used
  29             by default
  30
  31     """
  32
  33     w,h = len(layer[0]),len(layer)
  34     if start[0] < 0 or start[1] < 0 or start[0] >= w or start[1] >= h:
  35         return [] #start outside of layer
  36     if end[0] < 0 or end[1] < 0 or end[0] >= w or end[1] >= h:
  37         return [] #end outside of layer
  38
  39     if layer[start[1]][start[0]]:
  40         return [] #start is blocked
  41     if layer[end[1]][end[0]]:
  42         return [] #end is blocked
  43
  44     opens = []
  45     open = {}
  46     closed = {}
  47     cur = node(None, start, end, dist)
  48     open[cur.pos] = cur
  49     opens.append(cur)
  50     while len(open):
  51         cur = opens.pop(0)
  52         if cur.pos not in open: continue
  53         del open[cur.pos]
  54         closed[cur.pos] = cur
  55         if cur.pos == end: break
  56         for dx,dy in [(0,-1),(1,0),(0,1),(-1,0)]:#(-1,-1),(1,-1),(-1,1),(1,1)]:
  57             pos = cur.pos[0]+dx,cur.pos[1]+dy
  58             # Check if the point lies in the grid
  59             if (pos[0] < 0 or pos[1] < 0 or
  60                 pos[0] >= w or pos[1] >= h or
  61                 layer[pos[0]][pos[1]]):
  62                 continue
  63             #check for blocks of diagonals
  64             if layer[cur.pos[1]+dy][cur.pos[0]]: continue
  65             if layer[cur.pos[1]][cur.pos[0]+dx]: continue
  66             new = node(cur, pos, end, dist)
  67             if pos in open and new.f >= open[pos].f: continue
  68             if pos in closed and new.f >= closed[pos].f: continue
  69             if pos in open: del open[pos]
  70             if pos in closed: del closed[pos]
  71             open[pos] = new
  72             lo = 0
  73             hi = len(opens)
  74             while lo < hi:
  75                 mid = (lo+hi)/2
  76                 if new.f < opens[mid].f: hi = mid
  77                 else: lo = mid + 1
  78             opens.insert(lo,new)
  79
  80     if cur.pos != end:
  81         return []
  82
  83     path = []
  84     while cur.prev != None:
  85         path.append(cur.pos)
  86         cur = cur.prev
  87     path.reverse()
  88     return path
  89
  90
  91 def getline(a,b):
  92     """Returns a path of points from a to b
  93
  94     Arguments:
  95         a -- starting point
  96         b -- ending point
  97
  98     """
  99
 100     path = []
 101
 102     x1,y1 = a
 103     x2,y2 = b
 104     dx,dy = abs(x2-x1),abs(y2-y1)
 105
 106     if x2 >= x1: xi1,xi2 = 1,1
 107     else: xi1,xi2 = -1,-1
 108
 109     if y2 >= y1: yi1,yi2 = 1,1
 110     else: yi1,yi2 = -1,-1
 111
 112     if dx >= dy:
 113         xi1,yi2 = 0,0
 114         d = dx
 115         n = dx/2
 116         a = dy
 117         p = dx
 118     else:
 119         xi2,yi1 = 0,0
 120         d = dy
 121         n = dy/2
 122         a = dx
 123         p = dy
 124
 125     x,y = x1,y1
 126     c = 0
 127     while c <= p:
 128         path.append((x,y))
 129         n += a
 130         if n > d:
 131             n -= d
 132             x += xi1
 133             y += yi1
 134         x += xi2
 135         y += yi2
 136         c += 1
 137     return path
 138