dcm.py 文件源码

def from_quat(quat):
    """Create a direction cosine matrix from a quaternion.

    First 3 elements of the quaternion form its vector part.

    Parameters
    ----------
    quat : array_like, shape (4,) or (n, 4)
        Quaternions.

    Returns
    -------
    dcm : ndarray, shape (3, 3) or (n, 3, 3)
        Direction cosine matrices.
    """
    q = np.asarray(quat)
    if q.ndim == 1:
        rho = q[:3]
        q4 = q[3]
        rho_skew = _skew_matrix_single(rho)
        dcm = 2 * (np.outer(rho, rho) + q4 * rho_skew)
        dcm[np.diag_indices_from(dcm)] += q4**2 - np.dot(rho, rho)
    else:
        rho = q[:, :3]
        q4 = q[:, 3]
        rho_skew = _skew_matrix_array(rho)
        dcm = 2 * (rho[:, None, :] * rho[:, :, None] +
                   q4[:, None, None] * rho_skew)
        diag = q4**2 - np.sum(rho**2, axis=1)
        dcm[:, np.arange(3), np.arange(3)] += diag[:, None]

    return dcm