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
#!/usr/bin/python import random from BinHeap import BinHeap left = BinHeap() right = BinHeap() numbers = [] median = 0 incoming = [1,9,1,2,5,10,1] def mean2(a,b): 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)
from BinHeap import BinHeap bh = BinHeap() bh.buildHeap([9, 5, 6, 2, 3]) print(bh.delMin()) print(bh.delMin()) print(bh.delMin()) print(bh.delMin()) print(bh.delMin())
def testBinHeap(): test = [1,2,3,4,5,6,7,8,9] #+----------------------------------------------------- Test Insert ---+# random.shuffle(test) minheap = BinHeap(type='min') maxheap = BinHeap(type='max') for n in test: minheap.push(n) maxheap.push(n) # END for if minheap.heap[1] != 1: print "Min Heap Failed Insert Test" print minheap.heap return -1 if maxheap.heap[1] != 9: print "Max Heap Failed Insert Test" print maxheap.heap return -1 # END if #+---------------------------------------------------- Test Pop -----+# v = minheap.pop() if v != 1: print "Min Heap Extracted Wrong Value" print v print minheap return -1 if minheap.heap[1] != 2: print "Min Heap Failed to Bubble Up" print minheap return -1 # END if minheap.push(1) v = maxheap.pop() if v != 9: print "Max Heap Extracted Wrong Value" print v print maxheap return -1 if maxheap.heap[1] != 8: print "Max Heap Failed to Bubble Up" print maxheap return -1 # END if maxheap.push(9) #+---------------------------------------------------- Test PopPush ----+# v = minheap.pop_push(0) if v != 1: print "Min Heap Extracted Wrong Value (poppush)" print v print minheap return -1 if minheap.heap[1] != 0: print "Min Heap Didn't Bubble Correctly (poppush 0)" print minheap return -1 v = minheap.pop_push(10) if minheap.heap[1] != 2: print "Min Heap Didn't Bubble Correctly (poppush 10)" print minheap return -1 # END if v = maxheap.pop_push(10) if v != 9: print "Max Heap Extracted Wrong Value (poppush)" print v print maxheap return -1 if maxheap.heap[1] != 10: print "Max Heap Didn't Bubble Correctly (poppush 10)" print maxheap return -1 v = maxheap.pop_push(0) if maxheap.heap[1] != 8: print "Max Heap Didn't Bubble Correctly (poppush 0)" print maxheap return -1 # END if #+---------------------------------------------------- Test Heapify ----+# random.shuffle(test) minheap = BinHeap(type='min') minheap.heapify(test) for k in range(1,10): v = minheap.pop() if v != k: print "Min Heap Didn't Output Sorted List" print "Got {0}, Expected {1}".format(v, k) print minheap return -1 # END if # END for random.shuffle(test) maxheap = BinHeap(type='max') maxheap.heapify(test) for k in range(9,0,-1): v = maxheap.pop() if v != k: print "Max Heap Didn't Output Sorted List" print "Got {0}, expected {1}".format(v, k) print maxheap return -1 # END if # END for print "All Tests Pass" pass
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)
def main(): heap = BinHeap([10, 5, 2, 9, 3, 6]) heap.print() min = heap.del_min() print("delete min=", min) heap.print() min = heap.del_min() print("delete min=", min) heap.print() min = heap.del_min() print("delete min=", min) heap.print()