wheel.py 文件源码

def recordCoordinate(item, p):
    x, y, z = p

    # precalculate angle from wheel center (0, -20, 13.9)
    dy = y + 20
    dz = z - 14.75
    theta = atan2(dy, dz)
    if theta < 0:
        theta = 2 * pi + theta

    r = sqrt(dy * dy + dz * dz)
    zr = cos(theta)
    yr = sin(theta)

    item['x'] = x
    item['y'] = y
    item['z'] = z
    item['theta'] = theta
    item['r'] = r

    # for backwards compatibility, remove in future?
    item['coord'] = (x, y, z, theta, r, zr, yr)