-
Notifications
You must be signed in to change notification settings - Fork 2
/
pathFind.py
157 lines (129 loc) · 4.05 KB
/
pathFind.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#!/usr/bin/python
# -*- coding: utf8 -*-
from pandac.PandaModules import *
#-----------------------------------------------------------------------
# pathfinding
#-----------------------------------------------------------------------
def heurisDist(a,b):
return abs(a[0]-b[0]) + abs(a[1]-b[1])
class PathfindNode:
def __init__(self,prev,pos,dest):
self.prev,self.pos,self.dest = prev,pos,dest
if self.prev == None: self.g = 0
else: self.g = self.prev.g + 1
self.h = heurisDist(pos,dest)
self.f = self.g+self.h
def astar(start,end,layer):
"""
start = (x1, y1), end = (x2, y2), layer = CollisionGrid.data
0 = empty, 1 = wall
taken from Phil's pygame utilities (check PGU on pygame.org)
"""
#print "calling astar for start : %s and end : %s" % (start, end)
if len(start)<=1:return []
if len(end)<=1:return []
w,h = len(layer[0]),len(layer)
if start[0] < 0 or start[1] < 0 or start[0] >= w or start[1] >= h: return []
if end[0] < 0 or end[1] < 0 or end[0] >= w or end[1] >= h: return []
if layer[start[1]][start[0]]: return []
if layer[end[1]][end[0]]: return []
opens = []
open = {}
closed = {}
cur = PathfindNode(None,start,end)
open[cur.pos] = cur
opens.append(cur)
while len(open):
cur = opens.pop(0)
if cur.pos not in open: continue
del open[cur.pos]
closed[cur.pos] = cur
if cur.pos == end: break
for dx,dy in [(0,-1),(1,0),(0,1),(-1,0),(-1,-1),(1,-1),(-1,1),(1,1)]:
x,y = pos = cur.pos[0]+dx,cur.pos[1]+dy
if not (0<=y<len(layer)):continue
if not (0<=x<len(layer[0])):continue
if layer[y][x]>0: continue
#check for blocks of diagonals
if layer[cur.pos[1]+dy][cur.pos[0]]>0: continue
if layer[cur.pos[1]][cur.pos[0]+dx]>0: continue
new = PathfindNode(cur,pos,end)
if pos in open and new.f >= open[pos].f: continue
if pos in closed and new.f >= closed[pos].f: continue
if pos in open: del open[pos]
if pos in closed: del closed[pos]
open[pos] = new
lo = 0
hi = len(opens)
while lo < hi:
mid = (lo+hi)/2
if new.f < opens[mid].f: hi = mid
else: lo = mid + 1
opens.insert(lo,new)
if cur.pos != end:
return []
path = []
while cur.prev != None:
path.append(cur.pos)
cur = cur.prev
#path.append(start)
path.reverse()
return path
class Drawer:
def __init__(self):
self.node = GeomNode("lines")
self.np = NodePath(self.node)
self.np.reparentTo(render)
self.np.setShaderOff()
self.np.setLightOff()
self.path = []
self.start = (0, 0)
self.end = (0, 0)
self.h = 0.2
'''
self.startModel = loader.loadModel("frowney")
self.endModel = loader.loadModel("smiley")
self.startModel.reparentTo(render)
self.endModel.reparentTo(render)
'''
def setStart(self, start):
self.start = start
#self.startModel.setPos(start[0]+0.5, start[1]+0.5, 0)
def setEnd(self, end):
self.end = end
#self.endModel.setPos(end[0]+0.5, end[1]+0.5, 0)
def clear(self):
self.np.remove()
self.node = GeomNode("lines")
self.np = NodePath(self.node)
self.np.reparentTo(render)
self.np.setShaderOff()
self.np.setLightOff()
def destroy(self):
self.np.detachNode()
del self.np
def drawLine(self, start, end):
#print "Draw line : %s, %s" % (str(start), str(end))
self.node.addGeom(self.line(start, end))
def line (self, start, end):
# since we're doing line segments, just vertices in our geom
format = GeomVertexFormat.getV3()
# build our data structure and get a handle to the vertex column
vdata = GeomVertexData ('', format, Geom.UHStatic)
vertices = GeomVertexWriter (vdata, 'vertex')
# build a linestrip vertex buffer
lines = GeomLinestrips (Geom.UHStatic)
vertices.addData3f (start[0]+0.5, start[1]+0.5, self.h)
vertices.addData3f (end[0]+0.5, end[1]+0.5, self.h)
lines.addVertices (0, 1)
lines.closePrimitive()
geom = Geom (vdata)
geom.addPrimitive (lines)
# Add our primitive to the geomnode
#self.gnode.addGeom (geom)
return geom
def setPath(self, path):
self.path = path
self.clear()
for pos in range(len(self.path)-1):
self.drawLine(self.path[pos], self.path[pos+1])