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)
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 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()
