def player1(points,p1_stones_placed):
x = int(raw_input("Please enter the x-coordinate for your move"))
y = int(raw_input("Please enter the y-coordinate for your move"))
vor = Voronoi(points,incremental=True) #needed or you can't add points
move = np.array([[x,y]])
vor.add_points(move)
print "You're new move looks like this:"
voronoi_plot_2d(vor)
plt.show()
p1_stones_placed += 1
return move, p1_stones_placed
class VoronoiData:
def __init__(self, hydrants):
self.vor = Voronoi(hydrants, incremental=True)
self.poly = Polygon([(float(x.split(', ')[0]), float(x.split(', ')[1])) for x in open('coords.txt').read().split('\n')[:-1]])
def polygons(self):
l={}
for point_index, region_index in enumerate(self.vor.point_region):
region = self.vor.regions[region_index]
if region == [] or -1 in region:
continue
points = self.vor.vertices[region]
l[tuple(self.vor.points[point_index])] = points
return l
def valid(self, candidate):
#lat, long = candidate
#return (lat <= 41.861571 and lat >= 41.772414) and (long <= -71.369694 and long >= -71.472667)
return self.poly.contains(Point(*candidate))
def most_vulnerable_point(self):
l = self.polygons()
candidates = []
for center, points in l.items():
farthest = None
farthestDistance = 0.0
for p in points:
dist = (p[0] - center[0])**2 + (p[1] - center[1])**2
if dist > farthestDistance:
farthestDistance = dist
farthest = p
candidates.append((farthestDistance, farthest))
candidates = sorted(candidates, reverse=True)
for candidate in candidates:
if self.valid(candidate[1]):
return candidate
raise Exception("vulnerable point not found")
def add_hydrant(self, p):
self.vor.add_points([p[1]])
[-0.0321, 0.3261], \
[-0.0471, 0.2770], \
[-0.0513, 0.2565], \
[-0.0497, 0.2213], \
[-0.0798, 0.1677], \
[-0.0878, 0.1246], \
[-0.0948, 0.0768], \
[-0.042, -0.0005]]))
# Transform points
x__ = ar[:, 0] * cosphi - ar[:, 1] * sinphi
y__ = ar[:, 1] * cosphi + ar[:, 0] * sinphi
ar[:, 0] = x__
ar[:, 1] = y__
vor.add_points(ar)
vpts2 = vor.points
# A method for finding areas/volumes of the Voronoi regions
def voronoi_volumes(v):
vol = np.zeros(v.npoints)
for i, reg_num in enumerate(v.point_region):
indices = v.regions[reg_num]
if -1 in indices: # some regions can be opened
vol[i] = np.inf
else:
vol[i] = ConvexHull(v.vertices[indices]).volume
return vol
def Astar(initCord, clrn, exp=0, scale=2):
actionSet = []
fact = 1
hc = 250
wc = -250
clrr = np.array(clrn, dtype='f')
cart = np.array(initCord, dtype='f')
print(clrr)
if (clrr[0] >= 0):
clr = clrr[0]
else:
print('Clearnace cannot be negative.....\nTerminating')
return actionSet, -1
# clr = float(int(clr*100)/100.)
rad = 17.7 # 35,4/2 cm
tot = int(math.ceil((rad + clr)) * fact)
map1 = FinalMap(500, 500, tot)
map1.circ(100, 200, 225)
map1.circ(50, 400, 100)
map1.circ(40, 375, 375)
obsChk = Obs(500, 500, tot)
cv2.imwrite('grid_init.jpg', map1.grid)
map2 = cv2.imread('grid_init.jpg')
map2 = cv2.cvtColor(map2, cv2.COLOR_BGR2RGB)
plt.grid()
plt.ion()
plt.imshow(map2)
print('Generating voronoi diagram.....')
density = 0.8 # in percentage of total pixels
samples = np.random.randint(tot,
501 - tot,
size=(int(500 * 500 * density), 2))
vor = Voronoi(samples, incremental=True)
print('Voronoi generated....')
plt.show()
init = np.array([hc - cart[1], cart[0] - wc, cart[2]], dtype='f')
if (not obsChk.notObs(init[:-1])):
print('Start position cannot be in the obstacle.....\nTerminating')
return actionSet, -1
checkInp = True
while checkInp:
print('Total clearance set at: ', tot)
print(
'Enter the step "d" in integer (1<=d<total clearance (value as above)): '
)
step_d = int(input())
if (step_d < 1 or step_d >= tot):
print('Wrong step size, try again.....')
else:
checkInp = False
# checkInp= True
# while checkInp:
# print('Enter the initial starting coordinates in y as (0,500) & x as (0,500) and angle; ')
# print('With origin at top left, Enter in the order of y, x, theta [separated by commas]: ')
# cart= np.array(input(), dtype='f')
# if(len(cart)!=3):
# print 'Wrong input....Try again \nNote: Only 3 numbers needed inside the map boundaries'
# else:
# init= np.array([cart[0], cart[1], cart[2]], dtype='f')
# if(not obsChk.notObs(init[:-1])):
# print 'Start position cannot be in the obstacle.....Try again'
# else:
# checkInp = False
checkInp = True
while checkInp:
print(
'Enter the goal coordinates with origin at the top left as y, x [separated by commas]: '
)
fs = np.array(input(), dtype='f')
if (len(fs) != 2):
print 'Wrong input....Try again \nNote: Only 2 numbers needed inside the map boundaries'
else:
finalState = np.array([fs[0], fs[1], 0], dtype='f')
if (not obsChk.notObs(finalState[:-1])):
print 'Goal position cannot be in the obstacle.....Try again'
else:
checkInp = False
height = 500
width = 500
parentState = init
parentNode = threshold_state(parentState)
finalNode = threshold_state(finalState)
samples2 = np.array([
parentState[:-1], finalState[:-1],
[finalState[0] - 2, finalState[1] - 2],
[finalState[0] + 2, finalState[1] - 2],
[finalState[0] - 2, finalState[1] + 2],
[finalState[0] + 2, finalState[1] + 2]
])
Voronoi.add_points(vor, samples2)
Voronoi.close(vor)
A = vor.vertices.copy() #(x,y) with origin at bottom left
A[:, 1] = 500 - A[:, 1]
isOKvert = obsChk.notObs1(A)
allOKvert = A * isOKvert
allOKvert = allOKvert[np.unique(np.nonzero(allOKvert)[0])]
allOKvertThres = thresh(allOKvert)
graph = AllNodes(height, width, 12)
parentState = init
parentNode = threshold_state(parentState)
finalNode = threshold_state(finalState)
parentCost = 0
graph.updateCost2Come(parentNode, parentCost, 0, -1, parentState,
finalState, scale)
parent_ownId = graph.visit(parentNode, parentState)
reached = goalReached(parentNode, finalNode)
found = False
if (reached):
found = True
print('Input position is within the goal region')
for i in allOKvertThres:
graph.visited[i[1], i[0]] = 1
fourcc = cv2.VideoWriter_fourcc(*'XVID')
if exp:
vw = cv2.VideoWriter('maskVideo.avi', fourcc, 10.0, (501, 501))
vw2 = cv2.VideoWriter('visitVideo.avi', fourcc, 10.0, (501, 501))
tempImg = np.zeros([501, 501, 3], dtype='uint8')
tempVisit = np.zeros([501, 501, 3], dtype='uint8')
tempVisit[:, :, 1] = graph.visited * 120
tempVisit[:, :, 0] = graph.visited * 120
itr = 0
flag = 0
count = 0
step_d = 3
Rmin = tot
print('Processing...Please wait')
start_time = time.time()
while (found != True):
itr += 1
mask = FinalMap(500, 500, -1)
mask.circ(Rmin, parentNode[0], parentNode[1])
finalMask = (
(mask.grid[:, :, 0] / 255) - 1
) / 255 #dtype uint8 makes -1 value as 255 as [0,255] is only allowed
explore = ((graph.visited * finalMask) == 1) * 1
if exp:
tempImg[:, :, 0] = explore * 255
vw.write(tempImg)
for angle in range(0, 360,
30): # Iterating for all possible angles
chk = True
for sd in range(1, step_d + 1):
step = action(sd, angle + parentState[2])
tempState = parentState + step
tempState[2] = tempState[2] - parentState[2]
if (not obsChk.notObs(tempState[:-1])):
chk = False
break
if (chk):
actId = angle
tempNode = threshold_state(tempState)
if (explore[tempNode[0], tempNode[1]] == 1):
tempCost2Come = parentCost + step_d
graph.updateCost2Come(tempNode, tempCost2Come,
parent_ownId, actId, tempState,
finalState, scale)
status, minCost, new_parentState, org_parentState = graph.minCostIdx(
step_d, explore)
if (status):
parentState = new_parentState
parentNode = threshold_state(parentState)
map1.grid[parentNode[0], parentNode[1], 0] = 255
map1.grid[parentNode[0], parentNode[1], 1] = 0
map1.grid[parentNode[0], parentNode[1], 2] = 0
parentCost = graph.cost2come[parentNode[0], parentNode[1]]
parent_ownId = graph.visit(parentNode, parentState)
# cv2.imwrite('gridExplored.jpg',map1.grid)
if exp:
tempVisit[:, :, 2] = graph.visited * 120
vw2.write(tempVisit)
reached = goalReached(parentState, finalState)
if (reached):
found = True
print('Solved')
break
else:
lenRemain = graph.removeLastState()
if lenRemain:
parentState = graph.getStates(-1)
parentNode = threshold_state(parentState)
parentCost = graph.cost2come[parentNode[0], parentNode[1]]
parent_ownId = graph.getOwnId(parentNode)
else:
print('No solution exist, terminating....')
count = 1
return actionSet, -1
if exp:
vw.release()
vw2.release()
cv2.imwrite('gridExplored.jpg', map1.grid)
plt.imshow(map1.grid)
plt.show()
plt.pause(0.0001)
print("Time explored = %2.3f seconds " % (time.time() - start_time))
map2 = cv2.imread('gridExplored.jpg')
map3 = cv2.imread('grid_init.jpg')
if (not count):
reached_state = graph.getStates(int(len(graph.allStates)) - 1)
reached_node = threshold_state(reached_state)
ans = graph.getOwnId(reached_node)
print '\nYellow area shows all the obstacles and White area is the free space'
print 'Blue color show all the explored Nodes (area)'
print 'Red line shows optimal path (traced from start node to final node)'
allNodes = []
nextState = graph.getStates(ans)
nextNode = threshold_state(nextState)
g_actId = graph.actDone[nextNode[0], nextNode[1]]
allNodes.append(nextNode)
actionSet.append(g_actId)
while (ans != 0 and count == 0):
startState = nextState
startNode = nextNode
ans = graph.getParentId(startNode)
nextState = graph.getStates(ans)
nextNode = threshold_state(nextState)
g_actId = graph.actDone[nextNode[0], nextNode[1]]
allNodes.append(nextNode)
actionSet.append(g_actId)
idx = len(allNodes) - 1
vw1 = cv2.VideoWriter('Vid_backTrack_on_explored.avi', fourcc, 10.0,
(501, 501))
vw2 = cv2.VideoWriter('Vid_backTrack.avi', fourcc, 10.0, (501, 501))
while idx > 0:
startNode = allNodes[idx]
nextNode = allNodes[idx - 1]
idx -= 1
cv2.line(map2, (startNode[1], startNode[0]),
(nextNode[1], nextNode[0]), (0, 0, 255), 1)
cv2.line(map3, (startNode[1], startNode[0]),
(nextNode[1], nextNode[0]), (0, 0, 255), 1)
vw1.write(map2)
vw2.write(map3)
vw1.release()
vw2.release()
plt.imshow(map2)
plt.show()
plt.pause(0.0001)
cv2.imwrite('back_tracking_explored.jpg', map2)
cv2.imwrite('back_tracking.jpg', map3)
if (count == 0):
actionSet.reverse()
input('Path computed: ')
plt.ioff()
return actionSet, step_d
def uniform_meshing_test(self,
domain='square',
plot=True,
interior_nodes=True,
add_cnode=False):
if domain == 'square':
vertices = np.array([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0),
(0.0, 1.0)],
dtype=np.float)
facets = np.array([(0, 1), (1, 2), (2, 3), (3, 0)], dtype=np.int)
subdomain = np.array([(1, 0), (1, 0), (1, 0), (1, 0)],
dtype=np.int)
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=2)
bnode = uniform_mesh.bnode
elif domain == 'LShape':
vertices = np.array([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0),
(0.0, 1.0), (-1.0, 1.0), (-1.0, 0.0),
(-1.0, -1.0), (0.0, -1.0)],
dtype=np.float)
fixed = np.array([0, 0, 0, 1, 0, 1, 0, 0], dtype=np.bool)
facets = np.array([(0, 1), (1, 2), (2, 3), (3, 4), (4, 5), (5, 6),
(6, 7), (7, 0)],
dtype=np.int)
subdomain = np.array([(1, 0), (1, 0), (1, 0), (1, 0), (1, 0),
(1, 0), (1, 0), (1, 0)],
dtype=np.int)
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain,
fixed)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=2)
bnode = uniform_mesh.bnode
elif domain == 'circle':
n = 20
h = 2 * np.pi / n
theta = np.arange(0, 2 * np.pi, h)
vertices = np.zeros((n, 2), dtype=np.float)
vertices[:, 0] = np.cos(theta)
vertices[:, 1] = np.sin(theta)
fixed = np.zeros(n, dtype=np.bool)
facets = np.zeros((n, 2), dtype=np.int)
facets[:, 0] = range(0, n)
facets[:-1, 1] = range(1, n)
subdomain = np.zeros((n, 2), dtype=np.int)
subdomain[:, 0] = 1
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain,
fixed)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=0)
bnode = uniform_mesh.bnode
elif domain == 'partition1':
vertices = np.array([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0),
(0.0, 1.0), (0.5, 0.5)],
dtype=np.float)
facets = np.array([(0, 1), (1, 2), (2, 3), (3, 0), (0, 4), (4, 3),
(4, 1), (4, 2)],
dtype=np.int)
subdomain = np.array([(1, 0), (2, 0), (3, 0), (4, 0), (4, 1),
(4, 3), (2, 1), (3, 2)],
dtype=np.int)
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=3)
bnode = uniform_mesh.bnode
elif domain == 'partition2':
vertices = np.array([(0.0, 0.0), (0.5, 0.0), (1.0, 0.0),
(1.0, 0.5), (1.0, 1.0), (0.5, 1.0),
(0.0, 1.0), (0.0, 0.5), (0.5, 0.5)],
dtype=np.float)
facets = np.array([(0, 1), (1, 2), (2, 3), (3, 4), (4, 5), (5, 6),
(6, 7), (7, 0), (1, 8), (8, 7), (8, 3), (8, 5)],
dtype=np.int)
subdomain = np.array([(1, 0), (2, 0), (2, 0), (3, 0), (3, 0),
(4, 0), (4, 0), (1, 0), (1, 2), (1, 4),
(3, 2), (4, 3)],
dtype=np.int)
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=2)
bnode = uniform_mesh.bnode
elif domain == 'hole1':
vertices = np.array([(0.0, 0.0), (0.5, 0.0), (1.0, 0.0),
(1.0, 0.5), (1.0, 1.0), (0.5, 1.0),
(0.0, 1.0), (0.0, 0.5), (0.4, 0.4),
(0.7, 0.4), (0.7, 0.7), (0.4, 0.7)],
dtype=np.float)
facets = np.array([(0, 1), (1, 2), (2, 3), (3, 4), (4, 5), (5, 6),
(6, 7), (7, 0), (8, 9), (9, 10), (10, 11),
(11, 8)],
dtype=np.int)
subdomain = np.array([(1, 0), (1, 0), (1, 0), (1, 0), (1, 0),
(1, 0), (1, 0), (1, 0), (1, -1), (1, -1),
(1, -1), (1, -1)],
dtype=np.int)
"""
vertices = np.array([
( 0.0, 0.0),( 1.0, 0.0),( 1.0, 1.0),( 0.0, 1.0),
( 0.4, 0.4),( 0.7, 0.4),( 0.7, 0.7),( 0.4, 0.7)],dtype=np.float)
facets = np.array([
(0, 1),(1, 2),( 2, 3),( 3, 0),
(4, 5),(5, 6),( 6, 7),( 7, 4)], dtype=np.int)
subdomain = np.array([
(1, 0),(1, 0),(1, 0),(1, 0),
(1,-1),(1,-1),(1,-1),(1,-1)], dtype=np.int)
"""
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=1)
bnode = uniform_mesh.bnode
elif domain == 'hole2':
"""
vertices = np.array([
( 0.0, 0.0),( 0.5, 1.0),( 1.0, 0.0),( 1.5, 0.0),
( 2.0, 0.0),( 2.0, 0.5),( 2.0, 1.0),( 2.0, 1.5),
( 2.0, 2.0),( 1.5, 2.0),( 1.0, 2.0),( 0.5, 2.0),
( 0.0, 2.0),( 0.0, 1.5),( 0.0, 1.0),( 0.0, 0.5),
( 0.4, 0.4),( 0.4, 0.7),( 0.7, 0.7),( 0.7, 0.4),
( 1.2, 1.2),( 1.2, 1.5),( 1.5, 1.5),( 1.5, 1.2)],dtype=np.float)
facets = np.array([
( 0, 1),( 1, 2),( 2, 3),( 3, 4),
( 4, 5),( 5, 6),( 6, 7),( 7, 8),
( 8, 9),( 9,10),(10,11),(11,12),
(12,13),(13,14),(14,15),(15, 0),
(16,17),(17,18),(18,19),(19,16),
(20,21),(21,22),(22,23),(23,20)], dtype=np.int)
subdomain = np.array([
(1, 0),(1, 0),(1, 0),(1, 0),
(1, 0),(1, 0),(1, 0),(1, 0),
(1, 0),(1, 0),(1, 0),(1, 0),
(1, 0),(1, 0),(1, 0),(1, 0),
(1,-1),(1,-1),(1,-1),(1,-1),
(1,-2),(1,-2),(1,-2),(1,-2)], dtype=np.int)
"""
#"""
vertices = np.array(
[(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (2.0, 1.0), (2.0, 2.0),
(1.0, 2.0), (0.0, 2.0), (0.0, 1.0), (0.4, 0.4), (0.4, 0.7),
(0.7, 0.7), (0.7, 0.4), (1.2, 1.2), (1.2, 1.5), (1.5, 1.5),
(1.5, 1.2)],
dtype=np.float)
facets = np.array([
(0, 1),
(1, 2),
(2, 3),
(3, 4),
(4, 5),
(5, 6),
(6, 7),
(7, 0),
(8, 9),
(9, 10),
(10, 11),
(11, 8),
(12, 13),
(13, 14),
(14, 15),
(15, 12),
],
dtype=np.int)
subdomain = np.array([(1, 0), (1, 0), (1, 0), (1, 0), (1, 0),
(1, 0), (1, 0), (1, 0), (1, -1), (1, -1),
(1, -1), (1, -1), (1, -2), (1, -2), (1, -2),
(1, -2)],
dtype=np.int)
#"""
mesh = HalfEdgeMesh2d.from_edges(vertices, facets, subdomain)
uniform_mesh = CVTPMesher(mesh)
uniform_mesh.uniform_boundary_meshing(n=1)
bnode = uniform_mesh.bnode
if add_cnode == True:
cnode = uniform_mesh.cnode
bnode = np.append(bnode, cnode, axis=0)
vor = Voronoi(bnode, incremental=True)
if interior_nodes:
uniform_mesh.uniform_init_interior_nodes()
newnode = uniform_mesh.inode
for k in newnode:
vor.add_points(newnode[k])
if plot:
fig = plt.figure()
axes = fig.gca()
mesh.add_plot(axes)
mesh.find_node(axes, color='k', showindex=True)
mesh.find_node(axes, node=bnode, showindex=True)
mesh.print()
voronoi_plot_2d(vor, ax=axes)
plt.show()
