libcruft-util/cruft/util/vector.hpp

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * Copyright 2011-2019 Danny Robson <danny@nerdcruft.net>
 */

#ifndef CRUFT_UTIL_VECTOR_HPP
#define CRUFT_UTIL_VECTOR_HPP

#include "coord/fwd.hpp"
#include "coord/ops.hpp"
#include "coord.hpp"

#include "debug/assert.hpp"
#include "maths.hpp"

#include <cstddef>
#include <cmath>


///////////////////////////////////////////////////////////////////////////////
namespace cruft {
    template <size_t S, typename T>
    struct vector : public coord::base<S,T,vector<S,T>>
    {
        using coord::base<S,T,vector<S,T>>::base;

        // use a forwarding assignment operator so that we can let the base
        // take care of the many different types of parameters. otherwise we
        // have to deal with scalar, vector, initializer_list, ad nauseum.
        template <typename Arg>
        vector&
        operator= (Arg&&arg)
        {
            coord::base<S,T,vector<S,T>>::operator=(std::forward<Arg> (arg));
            return *this;
        }

        // representations
        vector<S+1,T>
        homog (void) const
        {
            return (*this).template redim<S+1> (0.f);
        }

        // constants
        static constexpr vector<S,T> ones  (void) { return vector<S,T> {1}; }
        static constexpr vector<S,T> zeros (void) { return vector<S,T> {0}; }
    };

    template <typename T>
    constexpr vector<3,T>
    cross (vector<3,T> a, vector<3,T> b)
    {
        return {
            a.y * b.z - a.z * b.y,
            a.z * b.x - a.x * b.z,
            a.x * b.y - a.y * b.x
        };
    }

    template <typename T>
    constexpr
    T
    cross (vector<2,T> a, vector<2,T> b)
    {
        return a[0] * b[1] - a[1] * b[0];
    }


    //-------------------------------------------------------------------------
    // given a vector find two vectors which produce an orthonormal basis.
    //
    template <typename T>
    std::pair<
        cruft::vector<3,T>,
        cruft::vector<3,T>
    >
    make_basis (const cruft::vector<3,T> n)
    {
#if 0
        // frisvad's method avoids explicit normalisation. a good alternative
        // is hughes-moeller, but the paper is hard to find.
        CHECK (is_normalised (n));

        // avoid a singularity
        if (n.z < -T(0.9999999)) {
            return {
                {  0, -1, 0 },
                { -1, -1, 0 }
            };
        }

        const T a = 1 / (1 + n.z);
        const T b = -n.x * n.y * a;

        const cruft::vector<3,T> v0 { 1 - n.x * n.x * a, b, -n.x };
        const cruft::vector<3,T> v1 { b, 1 - n.y * n.y * a, -n.y };

        CHECK (is_normalised (v0));
        CHECK (is_normalised (v1));

        return { v0, v1 };
#else
        // huges-moeller isn't as fast, but is more accurate
        if(cruft::abs (n.x) > cruft::abs (n.z))
        {
            // Normalization factor for b2
            auto const a = rsqrt (n.x * n.x + n.y * n.y);
            cruft::vector<3,T> b1 { -n.y * a, n.x * a, 0 };

             // Cross product using that b2 has a zero component
            cruft::vector<3,T> b0 { b1.y * n.z, -b1.x * n.z, b1.x * n.y - b1.y * n.x };

            return { b0, b1 };
        }
        else
        {
            // Normalization factor for b2
            auto const a = rsqrt (n.y * n.y + n.z * n.z);
            cruft::vector<3,T> b1 { 0.0f, -n.z * a, n.y * a };
            // Cross product using that b2 has a zero component
            cruft::vector<3,T> b0 { b1.y * n.z - b1.z * n.y, b1.z * n.x, -b1.y * n.x };

            return { b0, b1 };
        }
#endif
    }


    // polar/cartesian conversions; assumes (mag, angle) form.
    //
    // The angle is specified in radians.
    template <typename T> vector<2,T> polar_to_cartesian (vector<2,T>);
    template <typename T> vector<2,T> cartesian_to_polar (vector<2,T>);

    // convert vector in spherical coordinates (r,theta,phi) with theta
    // inclination and phi azimuth to cartesian coordinates (x,y,z)
    template <typename T>
    constexpr vector<3,T>
    spherical_to_cartesian (const vector<3,T> s)
    {
        return {
            s.x * std::sin (s.y) * std::cos (s.z),
            s.x * std::sin (s.y) * std::sin (s.z),
            s.x * std::cos (s.y)
        };
    }

    // convert vector in cartesian coordinates (x,y,z) to spherical
    // coordinates (using ISO convention: r,inclination,azimuth) with theta
    // inclination and phi azimuth.
    template <typename T>
    constexpr vector<3,T>
    cartesian_to_spherical (vector<3,T> c)
    {
        auto r = norm (c);
        return {
            r,
            std::acos (c.z / r),
            std::atan2 (c.y, c.x)
        };
    }

    template <typename T>
    constexpr vector<3,T>
    canonical_spherical (vector<3,T> s)
    {
        if (s.x < 0) {
            s.x  = -s.x;
            s.y += cruft::pi<T>;
        }

        if (s.y < 0) {
            s.y = -s.y;
            s.z += cruft::pi<T>;
        }

        s.y = std::fmod (s.y, cruft::pi<T>);
        s.z = std::fmod (s.z, cruft::pi<T>);

        return s;
    }

    template <typename T> vector<2,T> to_euler   (vector<3,T>);
    template <typename T> vector<3,T> from_euler (vector<2,T>);

    template <typename T> using vector1 = vector<1,T>;
    template <typename T> using vector2 = vector<2,T>;
    template <typename T> using vector3 = vector<3,T>;
    template <typename T> using vector4 = vector<4,T>;

    template <size_t S> using vectoru = vector<S,unsigned>;
    template <size_t S> using vectori = vector<S,int>;
    template <size_t S> using vectorf = vector<S,float>;
    template <std::size_t S> using vectorb = vector<S,bool>;

    using vector2u = vector2<unsigned>;
    using vector3u = vector3<unsigned>;
    using vector4u = vector4<unsigned>;

    using vector2i = vector2<int>;
    using vector3i = vector3<int>;
    using vector4i = vector4<int>;

    using vector1f = vector1<float>;
    using vector2f = vector2<float>;
    using vector3f = vector3<float>;
    using vector4f = vector4<float>;

    using vector2d = vector2<double>;
    using vector3d = vector3<double>;
    using vector4d = vector4<double>;

    using vector2b = vector2<bool>;
    using vector3b = vector3<bool>;
    using vector4b = vector4<bool>;
}

#endif