def _filter_block(vertices, block_origin, block_shape):
"""
Keyword arguments:
vertices --
block_origin_list --
block_shape --
Return values
-- True/False vertices contain block
"""
# check arguments
# create path object
path = Path(vertices, closed=True)
# create path for the block
x0 = block_origin[0]
y0 = block_origin[1]
w = block_shape[0]
h = block_shape[1]
box_vertices = np.asarray([[x0, y0], [x0 + w, y0], [x0 + w, y0 + h], [x0, y0 + h]])
box = Path(box_vertices, closed=True)
# determine if points inside the specified path
# Note: there appears to be an error in contains_points(). For
# example, reversing the direction of the vertices requires raidus=-0.01
# to be specified. See the e-mails in the link below
# http://matplotlib.1069221.n5.nabble.com/How-to-properly-use-path-Path-contains-point-tc40718.html#none
return path.contains_path(box)
def check_paths(path):
for other_path in a:
res = 'no cross'
chck = Path(other_path)
if chck.contains_path(path) == 1:
res = 'cross'
break
return res
def check_paths(path):
for other_path in a:
res = 'no cross'
chck = Path(other_path)
if chck.contains_path(path) == 1:
res = 'cross'
break
return res
def block_in_polygon(block_vertices, polygon_vertices):
"""
Keyword arguments:
vertices -- [[x0, y0], [x1, y1], ...]
polygon_vertices -- [[x0, y0], [x1, y1], ...]
Return values
val -- True/False block is in polygon
"""
try:
from matplotlib.path import Path
#print('\nMatplotlib is installed.')
except:
print('\nMatplotlib is not installed. block_in_polygon() returns None')
return None
# verify polygon vertices
if len(polygon_vertices) < 3:
raise ValueError(
'[Error] Encountered input error for polygon_vertices. ' +
'Three vertices needed to specify polygon.')
if [len(v) for v in polygon_vertices] != [
2,
] * len(polygon_vertices):
raise ValueError(
'[Error] Encountered input error for polygon_vertices. ' +
'Vertices do not have two coordinates.')
# repeat first polygon vertex at end of list
if not all(
[a == b for a, b in zip(polygon_vertices[0], polygon_vertices[-1])]):
polygon_vertices = polygon_vertices + [polygon_vertices[0]]
# create path object
# Path wants [x, y], while vertices are [y, x]. It's okay though because
# polygon_path and block_path have the same discrepancy
polygon_path = Path(polygon_vertices, closed=True)
# repeat first block vertex at end of list
if block_vertices[0] != block_vertices[-1]:
block_vertices = block_vertices + [block_vertices[0]]
block_path = Path(block_vertices, closed=True)
# Note: there appears to be an error in contains_points(). For
# example, reversing the direction of the vertices requires raidus=-0.01
# to be specified. See the e-mails in the link below
# http://matplotlib.1069221.n5.nabble.com/How-to-properly-use-path-Path-contains-point-tc40718.html#none
return bool(polygon_path.contains_path(block_path))
def isCollisionFree(robot, point, obstacles):
# Your code goes here.
#define robot path
verts_b = [(0, 0), (0, 10), (10, 10), (10, 0), (0, 0)]
codes_b = [
Path.MOVETO, Path.LINETO, Path.LINETO, Path.LINETO, Path.CLOSEPOLY
]
boundary_path = Path(verts_b, codes_b)
verts_r = []
codes_r = []
#codes_r.append(Path.MOVETO)
for i in range(len(robot)):
x = robot[i][0] + point[0]
y = robot[i][1] + point[1]
r_adjusted = (x, y)
verts_r.append(r_adjusted)
if i == 0:
codes_r.append(Path.MOVETO)
else:
codes_r.append(Path.LINETO)
verts_r.append(verts_r[0])
codes_r.append(Path.CLOSEPOLY)
robot_path = Path(verts_r, codes_r)
if not boundary_path.contains_path(robot_path):
return False
obstacle_paths = []
for j in range(len(obstacles)):
verts_o = []
codes_o = []
for i in range(len(obstacles[j])):
verts_o.append(obstacles[j][i])
if i == 0:
codes_o.append(Path.MOVETO)
else:
codes_o.append(Path.LINETO)
verts_o.append(verts_o[0])
codes_o.append(Path.CLOSEPOLY)
obstacle_paths.append(Path(verts_o, codes_o))
for i in range(len(obstacle_paths)):
if robot_path.intersects_path(obstacle_paths[i], filled=True):
return False
else:
continue
return True
