################################################################################
# pocket.py: utility methods for dealing with pockets
from hexsim import *
################################################################################
def findPocketCells(obstacleCells,goalCells,start):
"""
Given a cell array of contigious cells, usually of obstacle cells,
and a starting perimeter cell, return all pockets cells in a
new cell array.
A pocket cell falls along a straight
line that goes through the center of two cells in the
list of cells, where the line is normal to the sides
of the the hexagons it passes through. For example,
in the below figure, if O are the obstacle cells
listed in 'cells', all the cells labeled P would
be pocket cells.
__
__/O \
__/P \__/
__/P \__/
__/P \__/
__/P \__/
/O \__/
\__/
"""
pocketCells = newCellArray()
# move along the perimeter of the cells, finding pocket cells
curPerim = None
while curPerim != start:
if curPerim == None: curPerim = start
# move outward in all 6 directions
for direction in rotation():
curCell = curPerim
isPath=0
isObs=0
#isPerim=0
if curCell.nearby(opposite(direction)).path:
isPath = 1
if curCell.nearby(opposite(direction)).obstacle:
isObs = 1
#if curCell.nearby(opposite(direction)).perim:
# isPerim = 1
# do not move in directions that do not have
# an obstacle on the opposite side
if isPath ==0 and isObs ==0: #and isPerim ==0:
continue
# continue looking out in this direction until we hit the
# bounds of this cell array, or we know the cells we've
# crossed are pocket cells.
possiblePocketCells = []
while curCell.x >= obstacleCells.minX and \
curCell.x <= obstacleCells.maxX and \
curCell.y >= obstacleCells.minY and \
curCell.y <= obstacleCells.maxY:
# if we find another cell in the array at the other end
# we mark all the cells in between as pockets
if obstacleCells.at(curCell.x,curCell.y) != None:
for cell in possiblePocketCells:
if classifyCell(cell) != BLOCKED:
pocketCells.add(cell)
break
# add this cell as a possible pocket cell and
# move to the next cell in this direction
possiblePocketCells.append(curCell)
curCell = curCell.nearby(direction)
# move to the next cell in the perimeter
curPerim = obstacleCells.findNextPerimeter(CW,curPerim)
if curPerim == None:
print "ERROR: curPerim was set to None"
break
# define the pocket flag
#defineCellAttribute("pocket",(0.5,0.7,0.0))
for cell in pocketCells: cell.pocket = 1
# return all the pocket cells
return pocketCells
################################################################################
def seperatePockets(pocketCells):
"""
Given all the pocket cells, divide them into several contiguous
pocket groups.
PARAMS:
pocketCells - a cell array of all pocket cells. The flag pocket is
assumed to be set to 1 for all such cells, and all cells with the
flag pocket set to 1 are assumed to be in the cell array.
RESULTS:
Returns a list of several HSCellArray objects, each a contiguous
collection of pocket cells. The 'pocket' flag for
all pocket cells are set to 1 + index where index is the index
of the HSCellArray in the returned list that contains that pocket cell.
"""
# do a breadth first search to find all cells
# in all contiguous blocks
pockets = []
while len(pocketCells) > 0:
firstCell = pocketCells.first
pocketCells.remove(firstCell)
pocketQueue = [firstCell]
newPocket = newCellArray()
pockets.append(newPocket)
while len(pocketQueue) > 0:
curCell = pocketQueue.pop(0)
for neighbor in curCell.neighbors():
if neighbor.pocket and pocketCells.at(neighbor.x,neighbor.y):
pocketQueue.append(neighbor)
pocketCells.remove(neighbor)
newPocket.add(curCell)
curCell.pocket = len(pockets)
return pockets
################################################################################
def orderPocket(firstPocketCell,pocketDir,pocketCells,obstacleCells):
"""
Orders the cells of a pocket
PARAMS:
firstPocketCell - a valid perimeter pocket cell of this pocket
where modules will enter from.
pocketDir - the direction module will rotate into this cell
pocketCells - the pocket cells of this pocket
obstacleCells - the obstacle cells that make this pocket.
RESULT:
The ordered pocket as a list of HSCell objects.
"""
##print "inside pocket.orderPocket, pocket cells are "+str(pocketCells)
# create the pocket perimeter
pocketPerim = []
curPerim = firstPocketCell
##print "first pocket cell = "+str(firstPocketCell)
perimeterIndex = 1
for cell in pocketCells: cell.perimeter = 0
while curPerim.pocket:
pocketPerim.append(curPerim)
curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)
curPerim.perimeter = perimeterIndex
perimeterIndex += 1
##print "current perimeter is "+str(curPerim)
##print "perimeter length is "+str(perimeterIndex)
##print "perimeter is "+str(pocketPerim)
# calculate the free sides for all pocket cells
for cell in pocketCells:
cell.freeSides = 6
for neighbor in cell.neighbors():
if obstacleCells.at(neighbor.x,neighbor.y): cell.freeSides -= 1
##for cell in pocketPerim:
##print "Cell "+str(cell)+" has "+str(cell.freeSides)+" free sides"
# while there is still a perimeter
pocketOrder = []
while len(pocketPerim) > 0:
# find the cell to fill and add it to the pocket order
cellIndex = findNextPocketIndex(pocketPerim)
cellToAdd = pocketPerim[cellIndex]
##print "Inside pocket.orderPocket, just adding cell "+str(cellToAdd)
##print "added cell has perimeter # "+str(cellToAdd.perimeter)
cellToAdd.perimeter = 0
pocketOrder.append(cellToAdd)
obstacleCells.add(cellToAdd)
# remove the cell from the perimeter
del pocketPerim[cellIndex]
# renumber the perimeter
if len(pocketPerim) > 0:
curPerim = pocketPerim[0]
pocketPerim = []
perimeterIndex = 1
if not curPerim == None:
while not curPerim == None and curPerim.pocket:
if curPerim in pocketPerim:
break
pocketPerim.append(curPerim)
curPerim.perimeter = perimeterIndex
##print "Current perimeter cell is "+str(curPerim)+ \
##" with index "+str(perimeterIndex)
curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)
perimeterIndex = perimeterIndex + 1
##print "Cells on pocket perimeter are "+str(pocketPerim)
# reset the number of freesides for all neighbors of cellToAdd to 6
for cell in cellToAdd.neighbors():
if not cell.freeSides:
cell.freeSides = 6
# then remove a free side for each of the neighbors of the cell we added
for cell in cellToAdd.neighbors():
if cell.pocket:
cell.freeSides -= 1
### END OF WHILE len(pocket) > 0
# remove pocket cells from the obstacleCells
for cell in pocketOrder: obstacleCells.remove(cell)
for cell in pocketCells:
if not cell in pocketOrder:
pocketOrder.append(cell)
##print "The pocket order is: " + str(pocketOrder) + "."
# return the pocket order
return pocketOrder
def openC(cell):
return not cell.obstacle
################################################################################
def findNextPocketIndex(pocketPerim):
minFreeSides = 6
minFreeSideIndex = 0
startIndex, endIndex = findNPPRange(pocketPerim)
print "Range of indicies is: " + str(startIndex) + " - " + \
str(endIndex) + "."
for index in range(startIndex,endIndex+1):
if pocketPerim[index].freeSides <= minFreeSides:
minFreeSides = pocketPerim[index].freeSides
minFreeSideIndex = index
return minFreeSideIndex
################################################################################
def findNPPRange(pocketPerim):
startIndex = index = 0
endIndex = len(pocketPerim)-1
nppEnd = 0
while index < endIndex:
# for each pocket cell on the perimeter,
# check for neighbors with nonconsecutive perimeter values
lowest = endIndex
for cell in pocketPerim[index].neighbors():
if cell.pocket and cell.perimeter and \
abs(pocketPerim[index].perimeter - cell.perimeter) > 1 and \
pocketPerim[index].perimeter < cell.perimeter:
# The next line was crucial for correctness in icra09 paper
if cell.perimeter < lowest:
nppEnd = cell.perimeter
lowest = nppEnd
if nppEnd > 0 and nppEnd <= endIndex:
startIndex = index
endIndex = nppEnd
index += 1
nppEnd = 0
if startIndex == 0 and endIndex == len(pocketPerim)-1:
return startIndex, endIndex
else:
return startIndex+1, endIndex-1
################################################################################
# given a valid range of perimeter cells, this method will iterate through the
# perimeter cells, returning an array of maximum range NPP pairs. If no NPPs
# are found in the pocket, it will return an empty array.
def findMaxRangeNPPs(pocketPerim):
startIndex = index = 0
endIndex = len(pocketPerim)-1
nppEnd = 0
npps = []
while index < len(pocketPerim)-1:
# for each pocket cell on the perimeter,
# check for neighbors with nonconsecutive perimeter values
for cell in pocketPerim[index].neighbors():
if cell.pocket and cell.perimeter and \
abs(pocketPerim[index].perimeter - cell.perimeter) > 2 and \
pocketPerim[index].perimeter < cell.perimeter:
startIndex = index
nppEnd = cell.perimeter
break;
if nppEnd > 0 and nppEnd <= len(pocketPerim)-1:
startIndex = index
endIndex = nppEnd-1
npp = []
npp.append(pocketPerim[startIndex])
npp.append(pocketPerim[endIndex])
npps.append(npp)
index = endIndex+1
else:
index = index + 1
nppEnd = 0
return npps
###############################################################################
def findNPPDelay(firstPocketCell,pocketCells,obstacleCells,pocketDir):
curPerim = firstPocketCell
pocketPerim = []
while curPerim.pocket:
pocketPerim.append(curPerim)
curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)
print "current perimeter is "+str(pocketPerim)
startIndex, endIndex = findNPPRange(pocketPerim)
return endIndex - startIndex
###############################################################################
# As appears in the Multiple Pocket paper, count pockets
# separates pocket cells into discrete pockets.
def countPockets(pocketCells):
pc = []
for i in pocketCells:
pc.append(i)
i = 0
q = []
pockets = []
while len(pc) > 0:
x = pc[0]
q.append(x)
pc.remove(x)
pockets.append([])
while len(q) > 0:
#print q
u = q.pop(0)
contacts = []
for cell in u.neighbors():
#print cell
if cell in pc:
contacts.append(cell)
for c in contacts:
q.append(c)
pc.remove(c)
pockets[i].append(u)
i += 1
return pockets
############################################################################
def countObstacles(obstacleCells):
ob = []
for i in obstacleCells:
ob.append(i)
i = 0
q = []
obstacles = []
while len(ob) > 0:
x = ob[0]
q.append(x)
ob.remove(x)
obstacles.append([])
while len(q) > 0:
u = q.pop(0)
contacts = []
for cell in u.neighbors():
if cell in ob:
contacts.append(cell)
for c in contacts:
q.append(c)
ob.remove(c)
obstacles[i].append(u)
i += 1
return obstacles
#############################################################################
def orderObstacles(obstacles):
northeastarray = [];
i = 0
for obs in obstacles:
northeastarray.append((findNortheast(obs), i))
i += 1
sortedNE = sortNEarray(northeastarray)
orderedObstacles = []
for ob in sortedNE:
orderedObstacles.append(obstacles[ob[1]])
return orderedObstacles
def sortNEarray(array):
if len(array) < 2:
return array
middle = len(array)/2
end = len(array)
first = []
second = []
for i in range(0, middle):
first.append(array[i])
for i in range(middle, end):
second.append(array[i])
return merge(sortNEarray(first), sortNEarray(second))
def merge(a1, a2):
counter1 = 0
counter2 = 0
newarray = []
while counter1 < len(a1) and counter2 < len(a2):
if a1[counter1][0][0] < a2[counter2][0][0]:
newarray.append(a1[counter1])
counter1 = counter1 + 1
elif a1[counter1][0][0] == a2[counter2][0][0] and a1[counter1][0][1] > a2[counter2][0][1]:
newarray.append(a1[counter1])
counter1 = counter1 + 1
else:
newarray.append(a2[counter2])
counter2 = counter2 + 1
while counter1 < len(a1):
newarray.append(a1[counter1])
counter1 = counter1 + 1
while counter2 < len(a2):
newarray.append(a2[counter2])
counter2 = counter2 + 1
return newarray
def findNortheast(obstacle):
if len(obstacle) < 1:
print "no obstacle given to findNortheast"
northmost = obstacle[0].y
eastmost = obstacle[0].x
for obs in obstacle:
if obs.x < eastmost:
northmost = obs.y
eastmost = obs.x
elif obs.x == eastmost and obs.y > northmost:
northmost = obs.y
return (eastmost,northmost)
###############################################################################
def pocketMember(cell, pockets):
for pocket in pockets:
for pCell in pocket:
if cell.x == pCell.x and cell.y == pCell.y:
return pocket
BLOCKED = 0
FREE = 1
PARTITIONING = 2
######################################################################
def classifyCell(cell):
"""
Classifies a cell as BLOCKED, PARTITIONING, or FREE
"""
########################################
# Count the number and length of open regions.
# An open region is defined as a set of consecutively
# located cell neighbors of a cell (to the N,NE,SE,etc.) that are open.
openRegionLength = 0 # length of the current open region
maxOpenRegionLength = 0 # length of the longest open region
openRegionCount = 0 # number of open regions
openRegionFlag = 0 # whether we're currently looking at an open region
neighborhood = cell.neighbors() # find the cell's neighborhood
# look at each cell in the neighborhood consecutively
for cell in neighborhood:
# check for an open region
if openC(cell):
# if we have a new open region, flag this, and start counting
# its length
if not openRegionFlag:
openRegionLength = 1
openRegionCount += 1
openRegionFlag = 1
# if we have a continuing open region, continue
# counting its length
else: openRegionLength += 1
# check for closed (i.e. not open) regions
else:
# if we have a new closed region, stop counting
# the open region, and note its length
if openRegionFlag:
maxOpenRegionLength = \
max(maxOpenRegionLength,openRegionLength)
openRegionFlag = 0
maxOpenRegionLength = max(maxOpenRegionLength,openRegionLength)
# below we handle open regions that are divided by the ending and beginning
# of the loop above, as in this visual example:
#
# Open(Start)
# _____
# Open / \ Open
# / \
# \ /
#Closed \_____/ Closed
# Closed
#
# If we start the loop at the top neighbor, as indicated, and look
# at each side in clockwise order, the loop above would find
# TWO open regions instead of one. The following code
# fixes this problem
if openC(neighborhood[0]) and openC(neighborhood[len(neighborhood)-1]):
openRegionCount -= 1
maxOpenRegionLength += 1
# note that we may increase the length by too little when
# handling this exception, but for purposes of the code below
# this doesn't matter. (This is because we don't care if a region is
# greater than two in length, only if it's greater than one).
# blocked modules have no open regions with a length greater than one
if maxOpenRegionLength < 2: return BLOCKED
# partitioning modules have more than one open region
elif openRegionLength > 1: return PARTITIONING
# free modules have only one open region, at least two in length
else: return FREE