def Q_op()

in fairmotion/ops/quaternion.py [0:0]

• 16 lines of code
• 8 McCabe index (conditional complexity)

def Q_op(Q, op, xyzw_in=True):
    """
    Perform operations on quaternion. The operations currently supported are
    "change_order", "normalize" and "halfspace".

    `change_order` changes order of quaternion to xyzw if it's in wxyz and
    vice-versa
    `normalize` divides the quaternion by its norm
    `half-space` negates the quaternion if w < 0

    Args:
        Q: Numpy array of shape (..., 4)
        op: String; The operation to be performed on the quaternion. `op` can
            take values "change_order", "normalize" and "halfspace"
        xyzw_in: Set to True if input order is "xyzw". Otherwise, the order
            "wxyz" is assumed.
    """

    def q2q(q):
        result = q.copy()
        if "normalize" in op:
            norm = np.linalg.norm(result)
            if norm < constants.EPSILON:
                raise Exception("Invalid input with zero length")
            result /= norm
        if "halfspace" in op:
            w_idx = 3 if xyzw_in else 0
            if result[w_idx] < 0.0:
                result *= -1.0
        if "change_order" in op:
            result = result[[3, 0, 1, 2]] if xyzw_in else result[[1, 2, 3, 0]]
        return result

    return utils._apply_fn_agnostic_to_vec_mat(Q, q2q)