import torch
import math


def q_to_euler(q):
    """Converts a Quaternion vector to an Euler angle vector.

    Arguments:
        q (B,4) - Quaternion vectors

    Returns:
        euler (B,3) - Euler angle vectors.
    """
    sinr_cosp = 2.0 * (q[:, 0] * q[:, 1] + q[:, 2] * q[:, 3])
    cosr_cosp = 1.0 - 2.0 * (q[:, 1] * q[:, 1] + q[:, 2] * q[:, 2])
    x = torch.atan2(sinr_cosp, cosr_cosp)

    sinp = 2.0 * (q[:, 0] * q[:, 2] - q[:, 3] * q[:, 1])
    idx = (sinp.abs() >= 1).nonzero()
    y = torch.asin(sinp)
    y[idx] = math.pi / 2.0

    siny_cosp = 2.0 * (q[:, 0] * q[:, 3] + q[:, 1] * q[:, 2])
    cosy_cosp = 1.0 - 2.0 * (q[:, 2] * q[:, 2] + q[:, 3] * q[:, 3])
    z = torch.atan2(siny_cosp, cosy_cosp)

    return torch.stack((x, y, z)).transpose(0, 1)