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