tools.py 文件源码

def quat2euler(q0, q1, q2, q3):
    """
    Converts quaterion to (pitch, yaw, roll).
    Values are in -PI to PI range.
    """
    qq0, qq1, qq2, qq3 = q0**2, q1**2, q2**2, q3**2
    xa = qq0 - qq1 - qq2 + qq3
    xb = 2 * (q0 * q1 + q2 * q3)
    xn = 2 * (q0 * q2 - q1 * q3)
    yn = 2 * (q1 * q2 + q0 * q3)
    zn = qq3 + qq2 - qq0 - qq1

    pitch = atan2(xb , xa)
    yaw   = atan2(xn , sqrt(1 - xn**2))
    roll  = atan2(yn , zn)
    return pitch, yaw, roll