def getPathWithPathDistanceMap(start, end, mapData, finishWhenFoundPath = True):
sq2 = math.sqrt(2)
w = len(mapData)
h = len(mapData[0])
distance =[ [(0) for y in range(h)] for x in range(w)]
parent =[ [None for y in range(h)] for x in range(w)]
queue = PriorityQueue()
xstart = int(start.x)
ystart = int(start.y)
xend = int(end.x)
yend = int(end.y)
def heuristic(x0,y0, x1,y1):
return math.sqrt((x0-x1)*(x0-x1)+(y0-y1)*(y0-y1))
def checkPos(d, x, y, dx ,dy, delta):
x1 =x+dx
y1 = y+dy
result = False
if (x1<0 or x1>=w or y1<0 or y1>=h):
return False
fullDistance = d+delta*(mapData[x][y])
if (mapData[x1][y1]!=-1) and (parent[x1][y1]==None or distance[x1][y1]>fullDistance):
parent[x1][y1] = (x,y)
distance[x1][y1] = fullDistance
heursticD = fullDistance+heuristic(x,y,x1,y1)
queue._put((heursticD, (x1,y1)))
if ((x1==xend) and (y1==yend)):
result = True
return result
pathFounded = False
if checkPos(0, xstart,ystart, 0,0, 0) and finishWhenFoundPath:
return [Vector2(x+0.5,y+0.5)], distance, parent
while queue._qsize()>0:
prior,pos = queue._get()
x,y=pos
d = distance[x][y]
if (checkPos(d, x,y, 1,0, 1) or checkPos(d, x,y, -1,0, 1) or checkPos(d, x,y, 0,1, 1) or checkPos(d, x,y, 0,-1, 1) or
checkPos(d, x,y, 1,1, sq2) or checkPos(d, x,y, -1,1, sq2) or checkPos(d, x,y, 1,-1, sq2) or checkPos(d, x,y, -1,-1, sq2)):
pathFounded = True
if (finishWhenFoundPath):
break
result = []
if pathFounded:
x = xend
y = yend
while (parent[x][y]!=(x,y)):
result.append(Vector2(x+0.5,y+0.5))
x,y = parent[x][y]
result.reverse()
savePath('path',result, mapData)
return result, distance, parent
def _get(self, heappop=heapq.heappop):
item = PriorityQueue._get(self, heappop)
self.values.remove(item[1])
return item
def _get(self, heappop=heapq.heappop):
_, _, item = PriorityQueue._get(self, heappop)
return item
def _get(self, heappop=heapq.heappop):
item = PriorityQueue._get(self, heappop)
self.values.remove(item[1])
return item
def _get(self, heappop=heapq.heappop):
item = PriorityQueue._get(self, heappop)
self.itemsInQueue.pop(self.locations.index(item[1]))
self.locations.remove(item[1])
return item
def _get(self, heappop=heapq.heappop):
item = PriorityQueue._get(self, heappop)
self.itemsInQueue.pop( self.locations.index(item[1]) )
self.locations.remove(item[1])
return item
