Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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)