import math


def quat_to_euler(q):
    """returns yaw, pitch, roll

    at singularities (north and south pole), assumes roll = 0
    """

    # See http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/

    eps=1e-14
    qw = q.w; qx = q.x; qy = q.z; qz = -q.y
    pitch_y = 2*qx*qy + 2*qz*qw
    if pitch_y > (1.0-eps): # north pole]
        yaw = 2*math.atan2( qx, qw)
        pitch = math.asin(1.0)
        roll = 0.0
    elif pitch_y < -(1.0-eps): # south pole
        yaw = -2*math.atan2( qx, qw)
        pitch = math.asin(-1.0)
        roll = 0.0
    else:
        yaw = math.atan2(2*qy*qw-2*qx*qz , 1 - 2*qy**2 - 2*qz**2)
        pitch = math.asin(pitch_y)
        roll = math.atan2(2*qx*qw-2*qy*qz , 1 - 2*qx**2 - 2*qz**2)
    return yaw, pitch, roll