Convert Euler angle to quaternion.

    Parameters
    ----------
    yaw: number
    pitch: number
    roll: number

    Returns
    -------
    np.array
189467
def qeuler(yaw, pitch, roll):
    
    yaw = np.radians(yaw)
    pitch = np.radians(pitch)
    roll = np.radians(roll)

    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)

    q = np.array((
        cy * cr * cp + sy * sr * sp, cy * sr * cp - sy * cr * sp,
        cy * cr * sp + sy * sr * cp, sy * cr * cp - cy * sr * sp
    ))
    return q
1341267
def rotate(self, vec):
    

    qvec = self * Quaternion(vec.x, vec.y, vec.z, 0) * ~self
    return type(vec)(qvec.x, qvec.y, qvec.z)
1146273
def _is_yaw_flip(lat, delta=10):
        """"""
        logger.debug('Computing yaw flip flag')
        # In case of yaw-flip the data and coordinates in the netCDF files are
        # also flipped. Just check whether the latitude increases or decrases
        # with the line number.
        crow, ccol = np.array(lat.shape) // 2
        return (lat[crow+delta, ccol] - lat[crow, ccol]).values > 0
483074
def rotation_matrix_to_quaternion(rotation_matrix):
    """"""
    invert = (np.linalg.det(rotation_matrix) < 0)
    if invert:
        factor = -1
    else:
        factor = 1
    c2 = 0.25*(factor*np.trace(rotation_matrix) + 1)
    if c2 < 0:
        #print c2
        c2 = 0.0
    c = np.sqrt(c2)
    r2 = 0.5*(1 + factor*np.diagonal(rotation_matrix)) - c2
    #print "check", r2.sum()+c2
    r = np.zeros(3, float)
    for index, r2_comp in enumerate(r2):
        if r2_comp < 0:
            continue
        else:
            row, col = off_diagonals[index]
            if (rotation_matrix[row, col] - rotation_matrix[col, row] < 0):
                r[index] = -np.sqrt(r2_comp)
            else:
                r[index] = +np.sqrt(r2_comp)
    return factor, np.array([c, r[0], r[1], r[2]], float)
1138928
def matrix_to_euler(rotmat):
    ''''''
    if not isinstance(rotmat, np.matrixlib.defmatrix.matrix):
        # As this calculation relies on np.matrix algebra - convert array to
        # matrix
        rotmat = np.matrix(rotmat)

    def cvec(x, y, z):
        return np.matrix([[x, y, z]]).T
    ex = cvec(1., 0., 0.)
    ez = cvec(0., 0., 1.)
    exs = rotmat.T * ex
    ezs = rotmat.T * ez
    enodes = np.cross(ez.T, ezs.T).T
    if np.linalg.norm(enodes) < 1e-10:
        enodes = exs
    enodess = rotmat * enodes
    cos_alpha = float((ez.T * ezs))
    if cos_alpha > 1.:
        cos_alpha = 1.
    if cos_alpha < -1.:
        cos_alpha = -1.
    alpha = acos(cos_alpha)
    beta = np.mod(atan2(enodes[1, 0], enodes[0, 0]), pi * 2.)
    gamma = np.mod(-atan2(enodess[1, 0], enodess[0, 0]), pi * 2.)
    return unique_euler(alpha, beta, gamma)
1134400
def to_euler(self):
        ''''''
        if self.c.x >= 1.0:
            pitch = pi
        elif self.c.x <= -1.0:
            pitch = -pi
        else:
            pitch = -asin(self.c.x)
        roll = atan2(self.c.y, self.c.z)
        yaw  = atan2(self.b.x, self.a.x)
        return (roll, pitch, yaw)
797710
def create_rot2d(angle):
    """"""
    ca = math.cos(angle)
    sa = math.sin(angle)
    return np.array([[ca, -sa], [sa, ca]])
901502
def qrotate(vector, axis, angle):
    
    n_axis = axis / vnorm(axis)
    sin_angle = np.expand_dims(np.sin(angle / 2), 0)
    if np.ndim(n_axis) == 1:
        n_axis = np.expand_dims(n_axis, 1)
        p__ = np.dot(n_axis, sin_angle)[:, np.newaxis]
    else:
        p__ = n_axis * sin_angle

    q__ = Quaternion(np.cos(angle / 2), p__)
    shape = vector.shape
    return np.einsum("kj, ikj->ij",
                     vector.reshape((3, -1)),
                     q__.rotation_matrix()[:3, :3]).reshape(shape)
905046
def earth_rates(ATTITUDE):
    ''''''
    from math import sin, cos, tan, fabs

    p     = ATTITUDE.rollspeed
    q     = ATTITUDE.pitchspeed
    r     = ATTITUDE.yawspeed
    phi   = ATTITUDE.roll
    theta = ATTITUDE.pitch
    psi   = ATTITUDE.yaw

    phiDot   = p + tan(theta)*(q*sin(phi) + r*cos(phi))
    thetaDot = q*cos(phi) - r*sin(phi)
    if fabs(cos(theta)) < 1.0e-20:
        theta += 1.0e-10
    psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta)
    return (phiDot, thetaDot, psiDot)
1073120
def quat_to_rotation_matrix(q):
    
    q = q.flatten()
    assert q.size == 4
    assert_almost_equal(np.linalg.norm(q), 1.0, err_msg="Not a unit quaternion!")
    qq = q ** 2
    R = np.array([[qq[0] + qq[1] - qq[2] - qq[3], 2*q[1]*q[2] -
2*q[0]*q[3], 2*q[1]*q[3] + 2*q[0]*q[2]],
                [2*q[1]*q[2] + 2*q[0]*q[3], qq[0] - qq[1] + qq[2] -
qq[3], 2*q[2]*q[3] - 2*q[0]*q[1]],
                [2*q[1]*q[3] - 2*q[0]*q[2], 2*q[2]*q[3] + 2*q[0]*q[1],
qq[0] - qq[1] - qq[2] + qq[3]]])
    return R
911154
def rotation_df(ATT):
    ''''''
    r = Matrix3()
    r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw))
    return r
454372
def quaternion_inverse(quaternion):
    
    q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
    numpy.negative(q[1:], q[1:])
    return q / numpy.dot(q, q)
134763
def quaternion_about_axis(angle, axis):
    
    q = np.array([0.0, axis[0], axis[1], axis[2]])
    qlen = vector_norm(q)
    if qlen > _EPS:
        q *= math.sin(angle / 2.0) / qlen
    q[0] = math.cos(angle / 2.0)
    return q
304976
def quaternion(self):
        
        q_xyzw = transformations.quaternion_from_matrix(self.matrix)
        q_wxyz = np.roll(q_xyzw, 1)
        return q_wxyz
1123188
public Quaterniond rotationX(double angle) {
        double sin = Math.sin(angle * 0.5);
        double cos = Math.cosFromSin(sin, angle * 0.5);
        w = cos;
        x = sin;
        y = 0.0;
        z = 0.0;
        return this;
    }
262638
def euler_angles(self):
        
        q_wxyz = self.quaternion
        q_xyzw = np.roll(q_wxyz, -1)
        return transformations.euler_from_quaternion(q_xyzw)
971000
def vectors(self, pos, vel, roll=0.0, pitch=0.0, yaw=0.0):
        
        # TODO: yaw steering mode !

        # Fake nadir: This is the intersection point between the satellite
        # looking down at the centre of the ellipsoid and the surface of the
        # ellipsoid. Nadir on the other hand is the point which vertical goes
        # through the satellite...
        # nadir = -pos / vnorm(pos)

        nadir = subpoint(-pos)
        nadir /= vnorm(nadir)

        # x is along track (roll)
        x = vel / vnorm(vel)

        # y is cross track (pitch)
        y = np.cross(nadir, vel, 0, 0, 0)
        y /= vnorm(y)

        # rotate first around x
        x_rotated = qrotate(nadir, x, self.fovs[0] + roll)
        # then around y
        xy_rotated = qrotate(x_rotated, y, self.fovs[1] + pitch)
        # then around z
        return qrotate(xy_rotated, nadir, yaw)