Example #1
0
def orientation_numpy(normals, weights):

    # Project the normals against the plane
    dx, dy, dz = np.rollaxis(normals, 2)

    # Use the quadruple angle formula to push everything around the
    # circle 4 times faster, like doing mod(x,pi/2)
    qz = 4 * dz * dx * dx * dx - 4 * dz * dz * dz * dx
    qx = dx * dx * dx * dx - 6 * dx * dx * dz * dz + dz * dz * dz * dz

    # Build the weights using a threshold, finding the normals lying on
    # the XZ plane
    d = 0.3
    global cx, qqx, qqz
    cx = np.max((1.0 - dy * dy / (d * d), 0 * dy), 0)
    w = weights * cx

    qqx = np.nansum(w * qx) / w.sum()
    qqz = np.nansum(w * qz) / w.sum()
    angle = np.arctan2(qqz, qqx) / 4

    q0 = np.array([np.cos(angle), 0, np.sin(angle)])
    q0 /= np.sqrt(np.dot(q0, q0))
    q2 = np.cross(q0, np.array([0, 1, 0]))

    # Build an output matrix out of the components
    mat = np.vstack((q0, np.array([0, 1, 0]), q2))
    axes = expmap.rot2axis(mat)

    return axes