transforms.py 文件源码

def angvec2r(theta, v):
    """
    ANGVEC2R(THETA, V) is an orthonormal rotation matrix (3x3)
    equivalent to a rotation of THETA about the vector V.

    :param theta: rotation in radians
    :param v: vector
    :return: rotation matrix

    Notes::
    - If THETA == 0 then return identity matrix.
    - If THETA ~= 0 then V must have a finite length.
    """
    if np.isscalar(theta) is False or common.isvec(v) is False:
        raise AttributeError("Arguments must be theta and vector")
    # TODO implement ISA
    elif np.linalg.norm(v) < 10 * np.spacing([1])[0]:
        if False:
            raise AttributeError("Bad arguments")
        else:
            return np.eye(3)
    sk = skew(np.matrix(unitize(v)))
    m = np.eye(3) + np.sin(theta) * sk + (1 - np.cos(theta)) * sk * sk
    return m


# ---------------------------------------------------------------------------------------#