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)
