SPICE: Frame transform function

[subagonsouth]

Algorithm Description:

Implement a function for transforming vectors from one frame to another.

From EMUS:

def get_vec_pxform():
    """
    Return a vectorized form of spice.pxform function.

    "Return the matrix that transforms position vectors from one specified frame to another at a specified epoch."
    https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/pxform_c.html
    """
    return np.vectorize(spice.pxform,
                        excluded=['fromstr', 'tostr'],
                        signature='(),(),()->(3,3)',
                        otypes=[np.float64])


@ensure_spice
def frame_transform(from_frame: SpiceFrame, to_frame: SpiceFrame, et: float or np.ndarray, position: np.ndarray,
                    normalize: bool = False) -> np.ndarray:
    """
    Transform a position <x, y, z> vector between reference frames, optionally normalizing the result.

    :param from_frame: SpiceFrame, required, Reference frame of position
    :param to_frame: SpiceFrame, required, Reference frame of output
    :param et: np.float64 or np.ndarray with dtype np.float64, required, Ephemeris time(s) corresponding to position(s)
    :param position: np.ndarray, required, <x, y, z> vector or array of vectors in reference frame `from_frame`
    :param normalize: bool, optional, Optionally normalize the output vector
    :return: np.ndarray, 3d position vector(s) in reference frame `to_frame`
    """
    if position.ndim == 1:
        assert len(position) == 3
        assert isinstance(et, float)
    elif position.ndim == 2:
        assert len(position) == len(et)
        assert len(position[0]) == 3

    vec_pxform = get_vec_pxform()
    rotate = vec_pxform(from_frame.value.strid, to_frame.value.strid, et)

    if hasattr(rotate[0][0], '__len__'):
        result = np.array([np.matmul(rotate, pos).astype(np.float64) for rotate, pos in zip(rotate, position)])
        if normalize:
            result = normalize_vectors(result)
    else:
        result = np.matmul(rotate, position).astype(np.float64)
        if normalize:
            result = result / np.linalg.norm(result)

    return result