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