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.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