def cli(point, circle, sympy, filename):
"""Comparing the `sympy.geometry` and `matplotlib.path` libraries"""
figure = plt.figure()
x, y = point
plt.scatter([x], [y], c='r', marker='X')
cx, cy, r = circle
theta = np.linspace(0, 2 * np.pi, 500)
plt.scatter([cx], [cy], c='b', marker='x')
plt.plot(r * np.cos(theta) + cx, r * np.sin(theta) + cy, 'b--')
plt.title(f'Comparing the .{sympy} and .contains_point methods')
plt.grid()
plt.xlim((-(r + 1) + cx, +(r + 1) + cx))
plt.ylim((-(r + 1) + cy, +(r + 1) + cy))
plt.gca().set_aspect('equal', adjustable='box')
plt.tight_layout()
c1 = ['red', 'blue'][Path.circle((cx, cy), r).contains_point(point)]
c2 = ['red', 'blue'][getattr(Circle((cx, cy), r), sympy)(Point(x, y))]
p1 = mpatches.Patch(color=c1, label='.contains_point')
p2 = mpatches.Patch(color=c2, label=f'.{sympy}')
plt.legend(handles=[p1, p2], loc='upper left')
if filename is None:
plt.show()
else:
figure.savefig(filename, format="png")
def nearest_point(self, lon, lat, lons, lats, length=0.06): #0.3/5==0.06
'''Find the nearest point to (lon,lat) from (lons,lats),
return the nearest-point (lon,lat)
author: Bingwei'''
p = Path.circle((lon,lat),radius=length)
#numpy.vstack(tup):Stack arrays in sequence vertically
points = np.vstack((lons.flatten(),lats.flatten())).T
insidep = []
#collect the points included in Path.
for i in xrange(len(points)):
if p.contains_point(points[i]):# .contains_point return 0 or 1
insidep.append(points[i])
# if insidep is null, there is no point in the path.
if not insidep:
#print 'This point out of the model area or hits the land.'
raise Exception()
#calculate the distance of every points in insidep to (lon,lat)
distancelist = []
for i in insidep:
ss=math.sqrt((lon-i[0])**2+(lat-i[1])**2)
distancelist.append(ss)
# find index of the min-distance
mindex = np.argmin(distancelist)
# location the point
lonp = insidep[mindex][0]; latp = insidep[mindex][1]
return lonp,latp
def circumpolar_axis(ax):
"""
Draw a circumpolar grid of longitudes around a map at latitude 62 degrees N
License
-------
GNU-GPLv3, (C) A. R.
(https://github.com/poplarShift/python-data-science-utils)
"""
circle = Path.circle(radius=3e6)
proj = ax.projection
ax.set_boundary(circle, transform=proj)
# collections are only clipped when added after this point, so re-add them
for c in ax.collections:
c.remove()
ax.add_collection(c)
lat = 62
def rotation(lon):
if abs(lon) <= 90:
return lon
else:
return lon - 180
for lon in np.arange(-180, 180, 60):
lon_text = '{:3d}$^\circ${}'.format(abs(lon), {
True: 'E',
False: 'W'
}[lon >= 0])
textopts = dict(va='center', ha='center', rotation=rotation(lon))
ax.text(*proj.transform_point(lon, lat, ccrs.PlateCarree()), lon_text,
**textopts)
def nearest_point(self, lon, lat, lons, lats, length): #0.3/5==0.06
'''Find the nearest point to (lon,lat) from (lons,lats),
return the nearest-point (lon,lat)
author: Bingwei'''
p = Path.circle((lon, lat), radius=length)
#numpy.vstack(tup):Stack arrays in sequence vertically
points = np.vstack((lons.flatten(), lats.flatten())).T
insidep = []
#collect the points included in Path.
for i in xrange(len(points)):
if p.contains_point(points[i]): # .contains_point return 0 or 1
insidep.append(points[i])
# if insidep is null, there is no point in the path.
if not insidep:
print 'There is no model-point near the given-point.'
raise Exception()
#calculate the distance of every points in insidep to (lon,lat)
distancelist = []
for i in insidep:
ss = math.sqrt((lon - i[0])**2 + (lat - i[1])**2)
distancelist.append(ss)
# find index of the min-distance
mindex = np.argmin(distancelist)
# location the point
lonp = insidep[mindex][0]
latp = insidep[mindex][1]
return lonp, latp
def transmute(self, x0, y0, width, height, mutation_size):
# def __call__(self, x0, y0, width, height, mutation_size):
'''
x0 and y0 are the lower left corner of original bbox.
They are set automatically by matplotlib.
'''
xc, yc = x0 + width / 2, y0 + height / 2 # Center of the circle
pad = mutation_size * self.pad
radius = max(self.width / 2, width / 2 + pad)
return Path.circle((xc, yc),
radius) # Return a Path object as the new bbox
def gpf(infile, masked_image, aperture):
ts = time()
mask = fits.open(masked_image) # mask image
w = WCS(mask_image) # wcs
imdata = mask[0].data # mask data
mask.close()
xscale, yscale = abs(mask[0].header['CD1_1']), abs(
mask[0].header['CD2_2']) # 2.3979657626886E-4 degrees/pixel
xpixarcmin, ypixarcmin = (1 / (60 * xscale)), (
1 / (60 * yscale)) # pixels/arcmin along the X(Y) axis
pix_arcmin = (xpixarcmin + ypixarcmin) / 2.0 # <pixels>/arcmin
aperture = aperture * pix_arcmin # in pixels
pxcrd = w.wcs_world2pix(infile['ra'], infile['dec'],
0) # source table in pixel coords
pix_cat = np.column_stack([pxcrd[0], pxcrd[1]])
pix_dict = {key: [] for key in ['tpix', 'gpix', 'gpf']}
for rn in range(len(pix_cat)):
if rn % 5000 == 0:
print(rn)
# boundaries: larger box than the box which fits the circle with a given aperture radius
cmin, cmax = int(round(pix_cat[rn, 0] - 1.1 * aperture)), int(
round(pix_cat[rn, 0] + 1.1 * aperture))
rmin, rmax = int(round(pix_cat[rn, 1] - 1.1 * aperture)), int(
round(pix_cat[rn, 1] + 1.1 * aperture))
pts_inside_box = [
k for k in product(range(cmin, cmax +
1, 1), range(rmin, rmax + 1, 1))
] # points inside box
circ = Path.circle(center=(pix_cat[rn][0], pix_cat[rn][1]),
radius=aperture) # circle with the given aperture
truth_array = np.where(circ.contains_points(pts_inside_box))[
0] # point indices that are within the circle
pts_inside_circ = [pts_inside_box[inx] for inx in truth_array
] # points that are within the circle
good_pixels = sum([
1 for pt in pts_inside_circ if imdata[pt[1], pt[0]] == 0
]) # good pixels in the circle
gp_frac = round(good_pixels / float(len(pts_inside_circ)),
3) # good pixel fraction
pix_dict['tpix'].append(len(pts_inside_circ)) # of total pixels
pix_dict['gpix'].append(good_pixels) # good pixels
pix_dict['gpf'].append(gp_frac) # gpf
print(time() - ts)
return pix_dict
def _circle(xy, radius, facecolor, edgecolor, config):
main_path = Path.circle(xy, radius)
inner_path = Path.circle(xy, radius - config["goalless_edge_thickness"])
main_path_data = list(zip(main_path.codes, main_path.vertices))
inner_path_data = list(zip(inner_path.codes, inner_path.vertices))
main_path_data += ([inner_path_data[0]] + inner_path_data[1:-2][::-1] +
inner_path_data[-2:])
codes, verts = zip(*main_path_data)
main_path = Path(verts, codes)
outer_patch = PathPatch(main_path, facecolor=edgecolor, linewidth=0)
codes, verts = zip(*inner_path_data)
inner_path = Path(verts, codes)
inner_patch = PathPatch(inner_path,
facecolor=facecolor,
edgecolor=facecolor,
linewidth=2)
return [outer_patch, inner_patch]
def __init__(self, shape, xy, radius, **kwargs):
self.shape = shape
self.xy = xy
self.radius = radius
if shape == 'o': # circle
self.numVertices = None
self.orientation = 0
elif shape == '^': # triangle up
self.numVertices = 3
self.orientation = 0
elif shape == '<': # triangle left
self.numVertices = 3
self.orientation = np.pi * 0.5
elif shape == 'v': # triangle down
self.numVertices = 3
self.orientation = np.pi
elif shape == '>': # triangle right
self.numVertices = 3
self.orientation = np.pi * 1.5
elif shape == 's': # square
self.numVertices = 4
self.orientation = np.pi * 0.25
elif shape == 'd': # diamond
self.numVertices = 4
self.orientation = np.pi * 0.5
elif shape == 'p': # pentagon
self.numVertices = 5
self.orientation = 0
elif shape == 'h': # hexagon
self.numVertices = 6
self.orientation = 0
elif shape == '8': # octagon
self.numVertices = 8
self.orientation = 0
else:
raise ValueError(
"Node shape should be one of: 'so^>v<dph8'. Current shape:{}".
format(shape))
if self.shape == 'o': # circle
self.linewidth_correction = 2
self._path = Path.circle()
else: # regular polygon
self.linewidth_correction = 2 * np.sin(
np.pi / self.numVertices
) # derives from the ratio between a side and the radius in a regular polygon.
self._path = Path.unit_regular_polygon(self.numVertices)
self._patch_transform = transforms.Affine2D()
super().__init__(path=self._path, **kwargs)
def shrink_data(self,lon,lat,lons,lats):
# ind = argwhere((lonc >= size[0]) & (lonc <= size[1]) & (latc >= size[2]) & (latc <= size[3]))
lont = []; latt = []
p = Path.circle((lon,lat),radius=0.6)
pints = np.vstack((lons.flatten(),lats.flatten())).T
for i in range(len(pints)):
if p.contains_point(pints[i]):
lont.append(pints[i][0])
latt.append(pints[i][1])
lonl=np.array(lont); latl=np.array(latt)#'''
if not lont:
print 'point position error! shrink_data'
sys.exit()
return lonl,latl
def draw_lineage_graph(graph: DiGraph) -> None:
try:
import matplotlib.pyplot as plt
from matplotlib.colors import colorConverter
from matplotlib.patches import FancyArrowPatch
from matplotlib.path import Path
except ImportError as e:
raise ImportError("Matplotlib required for draw()") from e
except RuntimeError:
logger.error("Matplotlib unable to open display")
raise
try:
import pygraphviz # noqa
except ImportError as e:
raise ImportError("requires pygraphviz") from e
pos = nx.nx_agraph.graphviz_layout(graph,
prog="dot",
args="-Grankdir='LR'")
edge_color, node_color, font_color = "#9ab5c7", "#3499d9", "#35393e"
arrowsize = font_size = radius = 10
node_size = 30
ha, va = "left", "bottom"
nx.draw(
graph,
pos=pos,
with_labels=True,
edge_color=edge_color,
arrowsize=arrowsize,
node_color=node_color,
node_size=node_size,
font_color=font_color,
font_size=font_size,
horizontalalignment=ha,
verticalalignment=va,
)
# selfloop edges
for edge in nx.selfloop_edges(graph):
x, y = pos[edge[0]]
arrow = FancyArrowPatch(
path=Path.circle((x, y + radius), radius),
arrowstyle="-|>",
color=colorConverter.to_rgba_array(edge_color)[0],
mutation_scale=arrowsize,
linewidth=1.0,
zorder=1,
)
plt.gca().add_patch(arrow)
plt.show()
def nearest_point(lon, lat, lons, lats, length,x): #0.3/5==0.06
'''Find the nearest point to (lon,lat) from (lons,lats),
return the nearest-point (lon,lat)
author: Bingwei'''
#FN='necscoast_worldvec.dat'
#CL=np.genfromtxt(FN,names=['lon','lat'])
p = Path.circle((lon,lat),radius=length)
#plt.figure(figsize=(8,7))
#numpy.vstack(tup):Stack arrays in sequence vertically
points = np.vstack((lons.flatten(),lats.flatten())).T
insidep = []
#collect the points included in Path.
for i in np.arange(len(points)):
if p.contains_point(points[i]):# .contains_point return 0 or 1
insidep.append(points[i])
# if insidep is null, there is no point in the path.
if len(insidep)<2:
print ('There is no model-point near the given-point.')
raise Exception()
#calculate the distance of every points in insidep to (lon,lat)
distancelist = []
for i in insidep:
#print (lon,i[0],'########',lat,i[1])
ss=haversine(lon, lat, i[0], i[1])
#plt.scatter(i[0],i[1],color='blue',s=1)
#print (ss)
distancelist.append(ss)
mindex = np.argmin(distancelist)
distancelist[mindex]=100000000
mindex1 = np.argmin(distancelist)
#plt.scatter(lon,lat,color='green',s=1)
#plt.scatter(insidep[mindex1][0],insidep[mindex1][1],color='red',s=1)
#plt.axis([-70.75,-70,41.72,42.07])
#plt.plot(CL['lon'],CL['lat'],color='black',linewidth=1)
#plt.title('cape cod')
#plt.savefig('cape cod %s'%(x))
#plt.show()
return distancelist[mindex1]
def add_path(self,
path,
facecolor='k',
edgecolor='k',
alpha=None,
lw=1,
clipping_radius=0,
clipping_pt=(0, 0)):
stroke_patch = patches.PathPatch(path,
facecolor=facecolor,
edgecolor=edgecolor,
alpha=alpha,
lw=lw,
antialiased=True)
if clipping_radius > 0:
circle = Path.circle(clipping_pt, clipping_radius)
patch = patches.PathPatch(circle, visible=False)
self._axes.add_patch(patch)
self._axes.add_patch(stroke_patch).set_clip_path(patch)
else:
self._axes.add_patch(stroke_patch)
self._patches.append(stroke_patch)
def plotObstacles(ax):
Source1_location = [150, 120]
Source2_location = [366, 76]
Source3_location = [620, 120]
Cask1_location = [200, 100]
Cask2_location = [400, 100]
Cask3_location = [600, 100]
Cask_rad = 20
def makeRect(xcoord, ycoord, length, width):
x = np.linspace((xcoord - length / 2), (xcoord + length / 2),
length + 1,
dtype=int)
y = np.linspace((ycoord + width / 2), (ycoord - width / 2),
width + 1,
dtype=int)
x_1, y_1 = np.meshgrid(x, y, indexing='xy')
points = np.array(list(zip(x_1.flatten(), y_1.flatten())))
vertices = [((xcoord - length / 2), (ycoord - width / 2)),
((xcoord - length / 2), (ycoord + width / 2)),
((xcoord + length / 2), (ycoord + width / 2)),
((xcoord + length / 2), (ycoord - width / 2)), (0, 0)]
return points, vertices
#################### ROBOT COORDINATE
robot_x_coord = 30 #LOCATION OF THE EQUIPMENT ENTRY DOOR WHERE ROBOT WILL START
robot_y_coord = 30 #LOCATION OF THE EQUIPMENT ENTRY DOOR WHERE ROBOT WILL START
robot_height = 6
robot_breadth = 6
Robot_points, rvertices = makeRect(robot_x_coord, robot_y_coord,
robot_breadth, robot_height)
########### PLOTTING THE ROBOT ################
rcodes = [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
robot = Path(rvertices, rcodes)
robotpatch = PathPatch(robot, facecolor='green', edgecolor='green')
#################### PLOTTING SOURCE 1
circle = Path.circle(Source1_location, radius=1)
source1patch = PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING SOURCE 2
circle = Path.circle(Source2_location, radius=1)
source2patch = PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING SOURCE 3
circle = Path.circle(Source3_location, radius=1)
source3patch = PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING DRY STORAGE CASK 1
circle = Path.circle(Cask1_location, radius=Cask_rad)
sourcedrycast1 = PathPatch(circle, facecolor='white', edgecolor='black')
#################### PLOTTING DRY STORAGE CASK 2
circle = Path.circle(Cask2_location, radius=Cask_rad)
sourcedrycast2 = PathPatch(circle, facecolor='white', edgecolor='black')
#################### PLOTTING DRY STORAGE CASK 3
circle = Path.circle(Cask3_location, radius=Cask_rad)
sourcedrycast3 = PathPatch(circle, facecolor='white', edgecolor='black')
#################### TOOL BOX COORDINATE
TB_x_coord = 400 # LOCATION OF TOOL BOX CENTER
TB_y_coord = 40 # LOCATION OF TOOL BOX CENTER
TB_length = 200
TB_height = 10
TB_points, TBvertices = makeRect(TB_x_coord, TB_y_coord, TB_length,
TB_height)
TB = Path(TBvertices, rcodes)
TBpatch = PathPatch(TB, facecolor='purple', edgecolor='purple')
ax.add_patch(robotpatch)
ax.add_patch(TBpatch)
ax.add_patch(source1patch)
ax.add_patch(source2patch)
ax.add_patch(source3patch)
ax.add_patch(sourcedrycast1)
ax.add_patch(sourcedrycast2)
ax.add_patch(sourcedrycast3)
return ax
x_1,y_1=np.meshgrid(x,y, indexing='xy')
points = np.array(list(zip(x_1.flatten(),y_1.flatten())))
vertices = [((xcoord-length/2), (ycoord-width/2)), ((xcoord-length/2), (ycoord+width/2)), ((xcoord+length/2), (ycoord+width/2)), ((xcoord+length/2), (ycoord-width/2)), (0, 0)]
return points, vertices
#################### ROBOT COORDINATE
robot_x_coord=30 #LOCATION OF THE EQUIPMENT ENTRY DOOR WHERE ROBOT WILL START
robot_y_coord=30 #LOCATION OF THE EQUIPMENT ENTRY DOOR WHERE ROBOT WILL START
robot_height=6
robot_breadth=6
Robot_points,rvertices=makeRect(robot_x_coord,robot_y_coord,robot_breadth,robot_height)
########### PLOTTING THE ROBOT ################
rcodes = [Path.MOVETO] + [Path.LINETO]*3 + [Path.CLOSEPOLY]
robot = Path(rvertices,rcodes)
robotpatch = PathPatch(robot, facecolor='green', edgecolor='green')
#################### PLOTTING SOURCE 1
circle=Path.circle(Source1_location, radius=1)
source1patch=PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING SOURCE 2
circle=Path.circle(Source2_location, radius=1)
source2patch=PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING SOURCE 3
circle=Path.circle(Source3_location, radius=1)
source3patch=PathPatch(circle, facecolor='red', edgecolor='red')
#################### PLOTTING DRY STORAGE CASK 1
circle=Path.circle(Cask1_location, radius=Cask_rad)
sourcedrycast1=PathPatch(circle, facecolor='white', edgecolor='none')
#################### PLOTTING DRY STORAGE CASK 2
circle=Path.circle(Cask2_location, radius=Cask_rad)
sourcedrycast2=PathPatch(circle, facecolor='white', edgecolor='none')
#################### PLOTTING DRY STORAGE CASK 3
#print model_points['lon'][a][j],model_points['lat'][a][j],dmlon[a][j],dmlat[a][j],d
#print 'd',d
d = haversine(dmlon[a + 1], dmlat[a + 1], dmlon[a], dmlat[a])
dd = dd + d
#print 'dd',dd
return dd
FN = 'necscoast_worldvec.dat'
CL = np.genfromtxt(FN, names=['lon', 'lat'])
length = 60 * 0.009009009
latc = np.linspace(44.65, 45.02, 10)
lonc = np.linspace(-66.6, -65.93, 10)
p1 = Path.circle(((lonc[5] + lonc[4]) / 2, (latc[5] + latc[4]) / 2),
radius=length)
fig, axes = plt.subplots(1, 1, figsize=(10, 10)) #figure()
cl = plt.Circle(((lonc[5] + lonc[4]) / 2, (latc[5] + latc[4]) / 2),
length,
alpha=0.6,
color='yellow')
axes.add_patch(cl)
latc = np.linspace(44.65, 45.02, 10)
lonc = np.linspace(-66.6, -65.93, 10)
lon1 = []
latc1 = np.linspace(44.65, 45.02, 6)
#lat1=[]
for a in np.arange(len(lonc) - 2):
lon1.append((lonc[a] + lonc[a + 1]) / 2)
def circle(self, offset, radius, linewidth, edgecolor, facecolor):
path = Path.circle(offset, radius)
self.path(path, linewidth, edgecolor, facecolor)
########### PLOTTING THE MAP SPACE #############
vertices = []
codes = []
######## POLYGON OBSTACLES ###################
codes = [Path.MOVETO] + [Path.LINETO] * 5 + [Path.CLOSEPOLY]
vertices = [(20, 120), (25, 185), (75, 185), (100, 150), (75, 120), (50, 150),
(0, 0)]
codes += [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
vertices += [(30, 67.54603), (40, 84.88341025), (105, 47.32472), (95, 35),
(0, 0)]
codes += [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
vertices += [(200, 25), (225, 40), (250, 25), (225, 10), (0, 0)]
vertices = np.array(vertices, float)
path = Path(vertices, codes)
####### CIRCLE OBSTACLE #####################
circle = Path.circle((225, 150), radius=25, readonly=False)
####### ELIPSE OBSTACLE ####################
elipse = Ellipse((150, 100), 80, 40, 0, facecolor='None', edgecolor='green')
###### PATCHING THEM TOGETHER ###############
pathpatch = PathPatch(path, facecolor='None', edgecolor='green')
pathpatch2 = PathPatch(circle, facecolor='None', edgecolor='green')
pathpatch3 = Ellipse((150, 100),
80,
40,
0,
facecolor='None',
edgecolor='green')
####### CHECKING TO SEE IF ROBOT IS IN OBSTACLE ################
inside_polygons = (path.contains_points(points, radius=1e-9)
) #true if it is inside the polygon,otherwise false
def eline_path(self,lon,lat):
'''
When drifter close to boundary(less than 0.1),find one nearest point to drifter from boundary points,
then find two nearest boundary points to previous boundary point, create a boundary path using that
three boundary points.
'''
def boundary_location(locindex,pointt,wl):
'''
Return the index of boundary points nearest to 'locindex'.
'''
loca = []
dx = pointt[locindex]; #print 'func',dx
for i in dx: # i is a number.
#print i
if i ==0 :
continue
dx1 = pointt[i-1]; #print dx1
if 0 in dx1:
loca.append(i-1)
else:
for j in dx1:
if j != locindex+1:
if wl[j-1] == 1:
loca.append(j-1)
return loca
p = Path.circle((lon,lat),radius=0.02) #0.06
dis = []; bps = []; pa = []
tlons = []; tlats = []; loca = []
for i in self.b_points:
if p.contains_point(i):
bps.append((i[0],i[1]))
d = math.sqrt((lon-i[0])**2+(lat-i[1])**2)
dis.append(d)
bps = np.array(bps)
if not dis:
return None
else:
#print "Close to boundary."
dnp = np.array(dis)
dmin = np.argmin(dnp)
lonp = bps[dmin][0]; latp = bps[dmin][1]
index1 = np.where(self.lonc==lonp)
index2 = np.where(self.latc==latp)
elementindex = np.intersect1d(index1,index2)[0] # location 753'''
#print index1,index2,elementindex
loc1 = boundary_location(elementindex,self.pointt,self.wl) ; #print 'loc1',loc1
loca.extend(loc1)
loca.insert(1,elementindex)
for i in range(len(loc1)):
loc2 = boundary_location(loc1[i],self.pointt,self.wl); #print 'loc2',loc2
if len(loc2)==1:
continue
for j in loc2:
if j != elementindex:
if i ==0:
loca.insert(0,j)
else:
loca.append(j)
for i in loca:
tlons.append(self.lonc[i]); tlats.append(self.latc[i])
for i in xrange(len(tlons)):
pa.append((tlons[i],tlats[i]))
path = Path(pa)#,codes
return path
rotcodes = [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
rotrobot = Path(rotvertices, rotcodes)
rotrobotpatch = PathPatch(rotrobot, facecolor='blue', edgecolor='blue')
########### PLOTTING THE GOAL - change to circle? ################
gcodes = [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
gvertices = [
((goal_x_coord - robot_breadth / 2), (goal_y_coord - robot_height / 2)),
((goal_x_coord - robot_breadth / 2), (goal_y_coord + robot_height / 2)),
((goal_x_coord + robot_breadth / 2), (goal_y_coord + robot_height / 2)),
((goal_x_coord + robot_breadth / 2), (goal_y_coord - robot_height / 2)),
(0, 0)
]
goal = Path(gvertices, gcodes)
goalpatch = PathPatch(goal, facecolor='red', edgecolor='red')
######### CIRCLE OBSTACLES #####################
circle1 = Path.circle((5, 5), radius=1, readonly=False)
circle2 = Path.circle((3, 2), radius=1, readonly=False)
circle3 = Path.circle((7.2, 2), radius=1, readonly=False)
circle4 = Path.circle((7.2, 8), radius=1, readonly=False)
pathpatch1 = PathPatch(circle1, facecolor='None', edgecolor='blue')
pathpatch2 = PathPatch(circle2, facecolor='None', edgecolor='blue')
pathpatch3 = PathPatch(circle3, facecolor='None', edgecolor='blue')
pathpatch4 = PathPatch(circle4, facecolor='None', edgecolor='blue')
############# RECTANGULAR OBSTACLES #############
vertices = []
codes = []
########## POLYGON OBSTACLES ###################
codes = [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
vertices = [(0.25, 5.25), (1.75, 5.25), (1.75, 3.75), (0.25, 3.75), (0, 0)]
codes += [Path.MOVETO] + [Path.LINETO] * 3 + [Path.CLOSEPOLY]
vertices += [(2.25, 8.75), (3.75, 8.75), (3.75, 7.25), (2.25, 7.25), (0, 0)]
CL = np.genfromtxt(FNCL, names=['lon', 'lat'])
cl = dict(lon=[], lat=[])
for a in np.arange(len(CL['lon'])):
if CL['lon'][a] > lon - size and CL['lon'][a] < lon + size and CL['lat'][
a] < lat + size and CL['lat'][a] > lat - size:
cl['lon'].append(CL['lon'][a])
cl['lat'].append(CL['lat'][a])
fig = plt.figure(figsize=(5, 5))
ax = fig.add_subplot(111)
ax.plot(cl['lon'], cl['lat'], color='black')
ax.scatter(lon, lat, color='red')
ax.axis([lon - size, lon + size, lat - size, lat + size])
cir1 = Circle(xy=(lon, lat), radius=r, alpha=0.5)
ax.add_patch(cir1)
p = Path.circle((lon, lat), radius=r)
points = np.vstack(
(np.array(cl['lon']).flatten(), np.array(cl['lat']).flatten())).T
insidep = []
#collect the points included in Path.
for i in np.arange(len(points)):
if p.contains_point(points[i]): # .contains_point return 0 or 1
insidep.append(points[i])
lon1 = insidep[0][0]
lat1 = insidep[0][1]
lon2 = insidep[-1][0]
lat2 = insidep[-1][1]
ax.scatter(lon1, lat1)
ax.scatter(lon2, lat2)
def compress_points(points):
bi, bj, bx, by = get_boundary_intersections(points)
f = bi == bj
alone_points = points[bi[f]]
alone_paths = [Path.circle(xy, 0.5) for xy in alone_points]
edge_lists = [[] for i in range(len(points))]
n = 0
for i, j, x, y in zip(bi, bj, bx, by):
if i != j:
edge_lists[j].append((i, x, y))
n += 1
print("%s points in total: %s edges, %s alone points" %
(len(points), n, len(alone_points)))
def patan2(dy, dx):
"""
Return pseudo-arctangent of dy/dx such that
patan2(y1, x1) < patan2(y2, x2)
if and only if
atan2(y1, x1) < atan2(y2, x2)
"""
if dy > 0 and dx > 0:
return (0, dy - dx)
elif dy > 0 and dx <= 0:
return (1, -dy - dx)
elif dy <= 0 and dx > 0:
return (2, dx - dy)
else:
return (3, dx + dy)
def shift(u, v):
if v < u:
return (v[0] + 4, v[1])
else:
return v
def pop_next(i, ox, oy):
def local_patan2(y, x):
return patan2(y - points[i, 1], x - points[i, 0])
u = local_patan2(oy, ox)
j = min(range(len(edge_lists[i])),
key=lambda j: shift(u, local_patan2(edge_lists[i][j][2],
edge_lists[i][j][1])))
return edge_lists[i].pop(j)
paths = []
# print("<path fill=\"black\" fillrule=\"wind\">")
while n > 0:
assert sum(len(e) for e in edge_lists) == n
i = 0
while not edge_lists[i]:
i += 1
start = i
j, ox, oy = edge_lists[i].pop(0)
startx, starty = ox, oy
ux, uy = ox, oy
# path = ['%s %s m' % (startx, starty)]
path_vert_lists = [[[startx, starty]]]
path_code_lists = [[Path.MOVETO]]
n -= 1
while j != start:
i = j
j, vx, vy = pop_next(i, ux, uy)
n -= 1
# path.append(
# '%s 0 0 %s %s %s %s %s a' %
# (R, R, points[i, 0], points[i, 1], ox, oy))
ox, oy = points[i]
theta1 = np.arctan2(uy - oy, ux - ox)
theta2 = np.arctan2(vy - oy, vx - ox)
a = Path.arc(theta1 * 180 / np.pi, theta2 * 180 / np.pi)
a = a.transformed(Affine2D().scale(0.5).translate(ox, oy))
path_vert_lists.append(a._vertices[1:])
path_code_lists.append(a._codes[1:])
ux, uy = vx, vy
# path.append(
# '%s 0 0 %s %s %s %s %s a' %
# (R, R, points[j, 0], points[j, 1], startx, starty))
ox, oy = points[j]
theta1 = np.arctan2(uy - oy, ux - ox)
theta2 = np.arctan2(starty - oy, startx - ox)
a = Path.arc(theta1 * 180 / np.pi, theta2 * 180 / np.pi)
a = a.transformed(Affine2D().scale(0.5).translate(ox, oy))
path_vert_lists.append(a._vertices[1:])
path_code_lists.append(a._codes[1:])
# print('\n'.join(path))
paths.append(
Path(np.concatenate(path_vert_lists),
np.concatenate(path_code_lists).astype(Path.code_type)))
# print("</path>")
return Path.make_compound_path(*(alone_paths + paths))
def eline_path(self, lon, lat):
'''
When drifter close to boundary(less than 0.1),find one nearest point to drifter from boundary points,
then find two nearest boundary points to previous boundary point, create a boundary path using that
three boundary points.
'''
def boundary_location(locindex, pointt, wl):
'''
Return the index of boundary points nearest to 'locindex'.
'''
loca = []
dx = pointt[locindex]
#print 'func',dx
for i in dx: # i is a number.
#print i
if i == 0:
continue
dx1 = pointt[i - 1]
#print dx1
if 0 in dx1:
loca.append(i - 1)
else:
for j in dx1:
if j != locindex + 1:
if wl[j - 1] == 1:
loca.append(j - 1)
return loca
p = Path.circle((lon, lat), radius=0.02) #0.06
dis = []
bps = []
pa = []
tlons = []
tlats = []
loca = []
for i in self.b_points:
if p.contains_point(i):
bps.append((i[0], i[1]))
d = math.sqrt((lon - i[0])**2 + (lat - i[1])**2)
dis.append(d)
bps = np.array(bps)
if not dis:
return None
else:
print "Close to boundary."
dnp = np.array(dis)
dmin = np.argmin(dnp)
lonp = bps[dmin][0]
latp = bps[dmin][1]
index1 = np.where(self.lonc == lonp)
index2 = np.where(self.latc == latp)
elementindex = np.intersect1d(index1, index2)[0] # location 753'''
#print index1,index2,elementindex
loc1 = boundary_location(elementindex, self.pointt, self.wl)
#print 'loc1',loc1
loca.extend(loc1)
loca.insert(1, elementindex)
for i in range(len(loc1)):
loc2 = boundary_location(loc1[i], self.pointt, self.wl)
#print 'loc2',loc2
if len(loc2) == 1:
continue
for j in loc2:
if j != elementindex:
if i == 0:
loca.insert(0, j)
else:
loca.append(j)
for i in loca:
tlons.append(self.lonc[i])
tlats.append(self.latc[i])
for i in xrange(len(tlons)):
pa.append((tlons[i], tlats[i]))
#path = Path(pa)#,codes
return pa
def get_points(x_coord,y_coord):
x=np.linspace((robot_x_coord-robot_breadth//2), (robot_x_coord+robot_breadth//2), robot_breadth+1, dtype=int)
y=np.linspace((robot_y_coord+ robot_height//2), (robot_y_coord- robot_height//2), robot_height+1, dtype=int)
x_1,y_1=np.meshgrid(x,y, indexing='xy')
return np.array(list(zip(x_1.flatten(),y_1.flatten())))
robot_points = get_points(robot_x_coord,robot_y_coord)
goal_points = get_points( goal_x_coord, goal_y_coord)
#print(robot_points==goal_points)
############# PLOTTING THE ROBOT - changed to circle, can see rotation from plotted curves ################
#rcodes = [Path.MOVETO] + [Path.LINETO]*3 + [Path.CLOSEPOLY]
#rvertices = [((robot_x_coord-robot_breadth/2), (robot_y_coord-robot_height/2)), ((robot_x_coord-robot_breadth/2), (robot_y_coord+robot_height/2)), ((robot_x_coord+robot_breadth/2), (robot_y_coord+robot_height/2)), ((robot_x_coord+robot_breadth/2), (robot_y_coord-robot_height/2)), (0, 0)]
#robot = Path(rvertices,rcodes)
robot = Path.circle((robot_x_coord,robot_y_coord), radius=rob_radius)
robotpatch = PathPatch(robot, facecolor='green', edgecolor='green')
#print("vertices ",rvertices,"shape ", np.shape(rvertices))
#rotvertices=[]
#for i in np.array(rvertices):
# i=np.array(i)
# t=i-rvertices[0]
# #print("t",t)
# roti=drotmatrix(t,theta_s)
# rotvertices.append((roti+rvertices[0]))
###print("new vertices ",rotvertices,"shape ", np.shape(rotvertices))
#rotcodes = [Path.MOVETO] + [Path.LINETO]*3 + [Path.CLOSEPOLY]
#rotrobot = Path(rotvertices,rotcodes)
#rotrobotpatch = PathPatch(rotrobot, facecolor='blue', edgecolor='blue')
########### PLOTTING THE GOAL - changed to circle ################