use crate::{V3, V3X};
use itertools::zip;
use na::{convert, RealField};

/// Component-wise cartesian to spherical conversion.
pub fn cart_to_sph<T>(matrix: &V3X<T>) -> V3X<T>
where
    T: RealField,
{
    let mut sphericals = V3X::zeros(matrix.ncols());
    for (mut spherical, cartesian) in zip(sphericals.column_iter_mut(), matrix.column_iter()) {
        if cartesian.norm() == convert(0.) {
            spherical.copy_from(&V3::<T>::zeros());
        } else {
            spherical.copy_from_slice(&[
                cartesian[1].atan2(cartesian[0]),
                (cartesian[2] / cartesian.norm()).asin(),
                cartesian.norm(),
            ]);
        }
    }
    sphericals
}
