def __convert_cone2d_to_vertices(vertices, rays, BIG_DIST=1000):
if not rays:
return vertices
r0, r1 = __pick_extreme_rays([r[:2] for r in rays])
r0 = array([r0[0], r0[1], 0.])
r1 = array([r1[0], r1[1], 0.])
r0 = r0 / norm(r0)
r1 = r1 / norm(r1)
conv_vertices = [v for v in vertices]
conv_vertices += [v + r0 * BIG_DIST for v in vertices]
conv_vertices += [v + r1 * BIG_DIST for v in vertices]
return conv_vertices
def draw_polygon(env, points, n=None, color=None, plot_type=3, linewidth=1.,
pointsize=0.02):
"""
Draw a polygon defined as the convex hull of a set of points. The normal
vector n of the plane containing the polygon must also be supplied.
env -- openravepy environment
points -- list of 3D points
n -- plane normal vector
color -- RGBA vector
plot_type -- bitmask with 1 for edges, 2 for surfaces and 4 for summits
linewidth -- openravepy format
pointsize -- openravepy format
"""
assert n is not None, "Please provide the plane normal as well"
t1 = array([n[2] - n[1], n[0] - n[2], n[1] - n[0]], dtype=float)
t1 /= norm(t1)
t2 = cross(n, t1)
points2d = [[dot(t1, x), dot(t2, x)] for x in points]
hull = ConvexHull(points2d)
return draw_polyhedron(env, points, color, plot_type, hull, linewidth,
pointsize)
