full_support_area.py 文件源码

def compute_polygon_shadow(n, light_poly, ground_poly):
    """Polygons are described by their convex hulls.

    n -- normal vector used in ZMP computations
    light_poly -- polygon acting as light source
    ground_poly -- polygon whose shadow is projected under the light one

    """
    t = [n[2] - n[1], n[0] - n[2], n[1] - n[0]]
    n = array(n) / pymanoid.toolbox.norm(n)
    t = array(t) / pymanoid.toolbox.norm(t)
    b = cross(n, t)

    light_proj = [array([dot(t, p), dot(b, p)]) for p in light_poly]
    light_hull = ConvexHull(light_proj)
    ground_proj = [array([dot(t, p), dot(b, p)]) for p in ground_poly]
    ground_hull = ConvexHull(ground_proj)
    vertex2poly = {i: j for (i, j) in enumerate(ground_hull.vertices)}
    light_vertices = [light_proj[i] for i in light_hull.vertices]
    ground_vertices = [ground_proj[i] for i in ground_hull.vertices]
    mink_diff = [gv - lv for gv in ground_vertices for lv in light_vertices]

    try:
        u_low, u_high = pymanoid.draw.pick_2d_extreme_rays(mink_diff)
    except pymanoid.exceptions.UnboundedPolyhedron:
        big_dist = 1000  # [m]
        vertices = [
            array([-big_dist, -big_dist, light_poly[0][2]]),
            array([-big_dist, +big_dist, light_poly[0][2]]),
            array([+big_dist, -big_dist, light_poly[0][2]]),
            array([+big_dist, +big_dist, light_poly[0][2]])]
        return vertices, []

    nb_vertices = len(ground_vertices)
    vertex_indices = range(len(ground_vertices))

    def f_low(i):
        return cross(u_low, ground_vertices[i])

    def f_high(i):
        return cross(u_high, ground_vertices[i])

    v_low = min(vertex_indices, key=f_low)
    v_high = max(vertex_indices, key=f_high)
    vertices = [ground_poly[vertex2poly[vertex_index % nb_vertices]]
                for vertex_index in xrange(v_high, (v_low + nb_vertices + 1))]
    rays = [u_low[0] * t + u_low[1] * b,
            u_high[0] * t + u_high[1] * b]
    return vertices, rays