def reRouteTotal(state, start, end, n, rz=15): state.mode = "TotalReroute" V = SampleFreeN(n, state.obstacles, state.size) state.vis.target = (int(end[0]), int(end[1])) print("Total ReRoute") #V = sampleRadius(40,(0,0),state) VOpen = BinHeap() state.vis.nodes = V z = Node(start[0], start[1]) state.root = z state.vis.root = state.root V.append(z) z.visited = True VOpen.insert(z) r_z = rz N_z = MemoNear(V, z, r_z, state) count = 0 while (distToPoint(z, end) > state.acc): count += 1 #need to write get Closed vOpenNew = [] xNear = getVisited(N_z, visited=False) for x in xNear: N_x = MemoNear(V, x, r_z, state) yNear = getOpen(VOpen, N_x) #yMin = min(yNear,key = lambda y:Cost(y)+CostOfEdge(y,x)) #arg min line #add check for empty to break if (len(yNear) == 0): break yMin = min(yNear, key=lambda y: y.cost + CostLI(y, x, state) ) #arg min line#need to change this if collisionFree(yMin, x, state.obstacles): yMin.addChild(x) x.setCost(CostLI(yMin, x, state)) # update cost here vOpenNew.append(x) x.visited = True state.vis.update() #pygame.event.get() state.CheckEvents() setOpen(VOpen, vOpenNew) z.open = False z.closed = True #relook at closed stuff if (VOpen.currentSize == 0): print("failed Total") return [] z = VOpen.delMin() N_z = MemoNear(V, z, r_z, state) #vis.update() #state.mode = "hold" return getPathToGoal(z)
def _find_local_maxima(ordered_RD, k=1): """ Find local maxima to the k neighbor of left and k neighbors of right.\ It equivalent to a moving windows of size 2*k+1. RD is an 1D numpy array.\ Since we are looking for max, a max heap would be most beneficial.\ For the sake of simplicity, we just take the negtive of RD, and use \ a min heap already implemented a while ago. """ local_maxima = heapdict() moving_window = BinHeap() window_size = 2 * k + 1 poi_idx = -k # index for point of interest start from -k from convinience. RD_size = len(ordered_RD) assert window_size < RD_size, 'Invalid k or RD, window size can not be smaller than RD size' while poi_idx < RD_size: w_s_idx = poi_idx - k # window start idx w_e_idx = poi_idx + k # window end idx if w_e_idx < RD_size: moving_window.insert( (-ordered_RD[w_e_idx], w_e_idx) ) # negtive is to accomadate use of min heap, since RD is always non-negtive. if (moving_window.heap_size > window_size) or (w_e_idx >= RD_size): heap_el_idx = moving_window.pos[w_s_idx - 1] moving_window.extract_element( heap_el_idx) # delete elements outside of the window. if poi_idx >= 0: current_RD = -ordered_RD[poi_idx] if current_RD <= moving_window.heap[0][0]: local_maxima[ poi_idx] = current_RD # a negative RD is also used for local maxima since heapdict is a min priority queue poi_idx += 1 return local_maxima
return (a.returnMax() + b.returnMin())/2.0 for n in incoming: # i = random.randint(0,10) i = n numbers.append(i) print "I: ", i print "L size: ", left.currentSize print "R size: ", right.currentSize print "L max: ", left.returnMax() print "R min: ", right.returnMin() if len(numbers) == 1: median = i left.insert(i) elif len(numbers) == 2: if i > left.returnMax(): right.insert(i) else: right.insert(left.returnMax()) left.delMax() left.insert(i) median = mean2(left,right) else: if left.currentSize < right.currentSize: if i <= left.returnMax(): left.insert(i) else: # i > left.returnMax(): right.insert(i) rightMin = right.returnMin()
def reRoute(state, robot, instantMesh): #find the closest node with cheepest cost state.mode = "reRouting" goalPoint = robot.pathToGo[-1] successful = True #finding the best node to traverse to for point in robot.pathToGo: print(instantMesh.getCost(point)) print(state.getPointCost(point)) if (instantMesh.getCost(point) == state.getPointCost(point)): goalPoint = point break print(robot.position) print(robot.pathToGo[-1]) print(goalPoint) V = sampleRadius(distToPoint(point, robot.position, p2p=True), robot.position, state) VOpen = BinHeap() state.vis.nodes = V z = Node(robot.position[0], robot.position[1]) state.root = z V.append(z) z.visited = True VOpen.insert(z) r_z = 20 N_z = MemoNear(V, z, r_z, state) count = 0 while (distToPoint(z, goalPoint) > 4): count += 1 #need to write get Closed vOpenNew = [] xNear = getVisited(N_z, visited=False) for x in xNear: N_x = MemoNear(V, x, r_z, state) yNear = getOpen(VOpen, N_x) #yMin = min(yNear,key = lambda y:Cost(y)+CostOfEdge(y,x)) #arg min line #add check for empty to break if (len(yNear) == 0): break yMin = min(yNear, key=lambda y: y.cost + CostLI(y, x, state) ) #arg min line#need to change this if collisionFree(yMin, x, state.obstacles): yMin.addChild(x) x.setCost(CostLI(yMin, x, state)) # update cost here vOpenNew.append(x) x.visited = True state.vis.update() #pygame.event.get() state.CheckEvents() setOpen(VOpen, vOpenNew) z.open = False z.closed = True #relook at closed stuff if (VOpen.currentSize == 0): print("failed") totalReroute = reRouteTotal(state, (robot.position[0], robot.position[1]), state.target, 1500, rz=20) return (totalReroute, state.target, False) z = VOpen.delMin() N_z = MemoNear(V, z, r_z, state) #vis.update() state.mode = "hold" return (getPathToGoal(z), goalPoint, True)