AngleFunctions.h

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants 
Licensee a limited, non-exclusive, non-transferable, royalty-free license 
(without the right to sublicense) to use the API solely for the purpose of 
Licensee's internal development efforts to develop applications for which 
the API was provided.
 
The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Base
File: AngleFunctions
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include "DLLInfo.h"
#include <NDEVR/Angle.h>
#include <NDEVR/Vector.h>
namespace NDEVR
{
    extern NDEVR_BASE_API const fltp08* const s_index_sin; 
    template<class t_type_a, class t_type_b>
    constexpr static bool operator>(const Angle<t_type_a>& angle_a, const Angle<t_type_b>& angle_b)
    {
        return angle_a.template internal<false>() > cast<t_type_a>(angle_b.template internal<false>());
    }

    template<class t_type_a, class t_type_b>
    constexpr static bool operator<(const Angle<t_type_a>& angle_a, const Angle<t_type_b>& angle_b)
    {
        return angle_a.template internal<false>() < cast<t_type_a>(angle_b.template internal<false>());
    }

    template<class t_type_a, class t_type_b>
    constexpr static bool operator>=(const Angle<t_type_a>& angle_a, const Angle<t_type_b>& angle_b)
    {
        return angle_a.template internal<false>() >= cast<t_type_a>(angle_b.template internal<false>());
    }

    template<class t_type_a, class t_type_b>
    constexpr static bool operator<=(const Angle<t_type_a>& angle_a, const Angle<t_type_b>& angle_b)
    {
        return angle_a.template internal<false>() <= cast<t_type_a>(angle_b.template internal<false>());
    }

    template<class t_type>
    typename std::enable_if<!ObjectInfo<t_type>::Float, fltp08>::type sin(const Angle<t_type>& angle)
    {
 
        const uint02 internal_angle = cast<uint02>(angle.template internal<true>());
        if (internal_angle <= Angle<t_type>::INDEX_PI)
        {
            if (internal_angle > (Angle<t_type>::INDEX_PI / 2))
                return s_index_sin[Angle<t_type>::INDEX_PI - internal_angle];
            else
                return s_index_sin[internal_angle];
        }
        else
        {
            if (internal_angle >= Angle<t_type>::INDEX_PI + (Angle<t_type>::INDEX_PI / 2))
                return -s_index_sin[2 * Angle<t_type>::INDEX_PI - internal_angle];
            else
                return -s_index_sin[internal_angle - Angle<t_type>::INDEX_PI];
        }
    }

    template<class t_type>
    typename std::enable_if<ObjectInfo<t_type>::Float, fltp08>::type sin(const Angle<t_type>& angle)
    {
        return ::sin(angle.template as<RADIANS>());
    }

    template<class t_type>
    t_type sin(const t_type& angle)
    {
        return ::sin(angle);
    }

    template<class t_type>
    typename std::enable_if<!ObjectInfo<t_type>::Float, fltp08>::type cos(const Angle<t_type>& angle)
    {
        const Angle<t_type> internal_angle = Angle<t_type>(cast<uint02>((angle.template internal<true>() + (Angle<t_type>::INDEX_PI / 2)) % (2 * Angle<t_type>::INDEX_PI)));
        return sin(internal_angle);
    }

    template<class t_type>
    typename std::enable_if<ObjectInfo<t_type>::Float, fltp08>::type cos(const Angle<t_type>& angle)
    {
        return ::cos(angle.template as<RADIANS>());
    }

    template<class t_type>
    t_type cos(const t_type& angle)
    {
        return ::cos(angle);
    }

    template<class t_type>
    typename std::enable_if<!ObjectInfo<t_type>::Float, fltp08>::type tan(const Angle<t_type>& angle)
    {
        return sin(angle) / cos(angle);
    }

    template<class t_type>
    typename std::enable_if<ObjectInfo<t_type>::Float, fltp08>::type tan(const Angle<t_type>& angle)
    {
        return ::tan(angle.template as<RADIANS>());
    }

    template<class t_type>
    t_type tan(const t_type& angle)
    {
        return ::tan(angle);
    }

    template<class t_type>
    constexpr static Angle<t_type> operator-(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        return Angle<t_type>(cast<t_type>(angle_a.template internal<false>() - angle_b.template internal<false>()));
    }

    template<class t_type>
    constexpr static Angle<t_type> operator+(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        return Angle<t_type>(cast<t_type>(angle_a.template internal<false>() + angle_b.template internal<false>()));
    }

    template<class t_type>
    constexpr static Angle<t_type> difference(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        Angle<t_type> angle = (angle_a - angle_b).normalized();
        if (angle <= Angle<t_type>(DEGREES, 180.0f))
            return angle;
        else
            return Angle<t_type>(DEGREES, -360.0f) + angle;
    }

    template<uint01 t_dims, class t_type>
    constexpr static Vector<t_dims, Angle<t_type>> difference(const Vector<t_dims, Angle<t_type>>& angle_a, const Vector<t_dims, Angle<t_type>>& angle_b)
    {
        Vector<t_dims, Angle<t_type>> angle;
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            angle[dim] = difference(angle_a[dim], angle_b[dim]);
        }
        return angle;
    }

    template<class t_type>
    constexpr static Angle<t_type> Average(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        Angle<t_type> angle = difference(angle_a, angle_b) / 2.0;
        angle += angle_b;
        return angle;
    }

    template<uint01 t_dims, class t_type>
    constexpr static Vector<t_dims, Angle<t_type>> Average(const Vector<t_dims, Angle<t_type>>& angle_a, const Vector<t_dims, Angle<t_type>>& angle_b)
    {
        Vector<t_dims, Angle<t_type>> angle;
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            angle[dim] = Average(angle_a[dim], angle_b[dim]);
        }
        return angle;
    }
    
    template<class t_type>
    constexpr static Angle<t_type> operator*(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        return Angle<t_type>(cast<t_type>(angle_a.template internal<false>() * angle_b.template internal<false>()));
    }

    template<class t_type>
    constexpr static fltp08 operator/(const Angle<t_type>& angle_a, const Angle<t_type>& angle_b)
    {
        return cast<fltp08>(angle_a.template internal<false>()) / cast<fltp08>(angle_b.template internal<false>());
    }

    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type> operator*(const Angle<t_angle_type>& angle, t_type mult)
    {
        return Angle<t_angle_type>(cast<t_angle_type>(cast<t_type>(angle.template internal<false>()) * mult));
    }

    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type> operator*(t_type mult, const Angle<t_angle_type>& angle)
    {
        return Angle<t_angle_type>(cast<t_angle_type>(cast<t_type>(angle.template internal<false>()) * mult));
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
    operator*(const t_vector_type& angle, const t_type& mult)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = angle[dim] * mult;
        return product;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
    operator*(const t_vector_type& angle, const t_type& mult)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = angle[dim] * mult;
        return product;
    }

    template<uint01 t_dims, class t_type, class t_vector_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<fltp08>>, t_vector_type>::value, t_vector_type>::type
    operator*(const t_vector_type& angle, const Vector<t_dims, t_type>& mult)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            product[dim] = angle[dim] * mult[dim];
        return product;
    }

    template<uint01 t_dims, class t_type, class t_vector_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<sint04>>, t_vector_type>::value, t_vector_type>::type
    operator*(const t_vector_type& angle, const Vector<t_dims, t_type>& mult)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            product[dim] = angle[dim] * mult[dim];
        return product;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
    operator*(const t_type& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult * angle[dim];
        return product;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
    operator*(const t_type& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult * angle[dim];
        return product;
    }

    template<uint01 t_dims, class t_type, class t_vector_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<fltp08>>, t_vector_type>::value, t_vector_type>::type
    operator*(const Vector<t_dims, t_type>& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult[dim] * angle[dim];
        return product;
    }

    template<uint01 t_dims, class t_type, class t_vector_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<sint04>>, t_vector_type>::value, t_vector_type>::type
    operator*(const Vector<t_dims, t_type>& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult[dim] * angle[dim];
        return product;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
    operator*(const t_vector_type& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult[dim] * angle[dim];
        return product;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
    operator*(const t_vector_type& mult, const t_vector_type& angle)
    {
        t_vector_type product;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            product[dim] = mult[dim] * angle[dim];
        return product;
    }

    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type> operator/(const Angle<t_angle_type>& num, t_type den)
    {
        return Angle<t_angle_type>(cast<t_angle_type>(cast<t_type>(num.template internal<false>()) / den));
    }

    template<class t_type, class t_vector_type, class t_angle_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<t_angle_type>>::value, t_vector_type>::type
    operator/(const t_vector_type& angle, const t_type& den)
    {
        t_vector_type div;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            div[dim] = angle[dim] / den;
        return div;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
    operator/(const t_vector_type& angle, const t_type& den)
    {
        t_vector_type div;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            div[dim] = angle[dim] / den;
        return div;
    }

    template<class t_type, class t_vector_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
    operator/(const t_vector_type& angle, const t_type& den)
    {
        t_vector_type div;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            div[dim] = angle[dim] / den;
        return div;
    }

    template<uint01 t_dims, class t_type, class t_vector_type, class t_angle_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<t_angle_type>>, t_vector_type>::value, t_vector_type>::type
    operator/(const t_vector_type& angle, const Vector<t_dims, t_type>& den)
    {
        t_vector_type div;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            div[dim] = angle[dim] / den[dim];
        return div;
    }

    template<uint01 t_dims, class t_vector_type, class t_angle_type>
    constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<t_angle_type>>, t_vector_type>::value, Vector<t_dims, fltp08>>::type
    operator/(const t_vector_type& angle, const Vector<t_dims, Angle<t_angle_type>>& den)
    {
        Vector<t_dims, fltp08> div;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            div[dim] = angle[dim] / den[dim];
        return div;
    }

    template<uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, fltp08> operator/(const Vector<t_dims, Angle<t_angle_type>>& angle, const Vector<t_dims, Angle<t_angle_type>>& den)
    {
        Vector<t_dims, fltp08> div;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            div[dim] = angle[dim] / den[dim];
        return div;
    }
    
    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type> operator/(t_type num, const Angle<t_angle_type>& den)
    {
        return Angle(cast<t_angle_type>(num / cast<t_type>(den.template internal<false>())));
    }

    template<class t_type, class t_vector_type, class t_angle_type>
    constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<t_angle_type>>::value, t_vector_type>::type
    operator/(t_type num, const t_vector_type& angle)
    {
        t_vector_type div;
        for (uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
            div[dim] = num / angle[dim];
        return div;
    }

    template<class t_angle_type>
    constexpr static Angle<t_angle_type>& operator+=(Angle<t_angle_type>& angle, const Angle<t_angle_type>& add)
    {
        angle = angle + add;
        return angle;
    }

    template<class t_angle_type>
    constexpr static Angle<t_angle_type>& operator-=(Angle<t_angle_type>& angle, const Angle<t_angle_type>& sub)
    {
        angle = angle - sub;
        return angle;
    }

    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type>& operator*=(Angle<t_angle_type>& angle, const t_type& mult)
    {
        angle = angle * mult;
        return angle;
    }

    template<uint01 t_dims, class t_type, class t_angle_type>
    constexpr static Vector<t_dims, Angle<t_angle_type>>& operator*=(Vector<t_dims, Angle<t_angle_type>>& angle, const t_type& mult)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
            angle[dim] = angle[dim] * mult;
        return angle;
    }

    template<uint01 t_dims, class t_type, class t_angle_type>
    constexpr static Vector<t_dims, Angle<t_angle_type>>& operator*=(Vector<t_dims, Angle<t_angle_type>>& angle, const Vector<t_dims, t_type>& mult)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
            angle[dim] = angle[dim] * mult[dim];
        return angle;
    }

    template<class t_angle_type>
    constexpr static Angle<t_angle_type>& operator*=(Angle<t_angle_type>& angle, const Angle<t_angle_type>& mult)
    {
        angle = angle * mult;
        return angle;
    }

    template<class t_type, class t_angle_type>
    constexpr static Angle<t_angle_type>& operator/=(Angle<t_angle_type>& angle, const t_type& mult)
    {
        angle = angle / mult;
        return angle;
    }

    template<uint01 t_dims, class t_type, class t_angle_type>
    constexpr static Vector<t_dims, Angle<t_angle_type>>& operator/=(Vector<t_dims, Angle<t_angle_type>>& angle, const t_type& den)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
            angle[dim] = angle[dim] / den;
        return angle;
    }

    template<uint01 t_dims, class t_type, class t_angle_type>
    constexpr static Vector<t_dims, Angle<t_angle_type>>& operator/=(Vector<t_dims, Angle<t_angle_type>>& angle, const Vector<t_dims, t_type>& den)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
            angle[dim] = angle[dim] / den[dim];
        return angle;
    }

    template<uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, Angle<t_angle_type>> operator%(const Vector<t_dims, Angle<t_angle_type>>& vec_a, const Vector<t_dims, Angle<t_angle_type>>& vec_b)
    {
        Vector<t_dims, Angle<t_angle_type>> mod;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            mod[dim] = Angle<t_angle_type>(cast<t_angle_type>(vec_a[dim].template internal<false>() % vec_b[dim].template internal<false>()));
        return mod;
    }

    template<uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, Angle<t_angle_type>> operator%(const Vector<t_dims, Angle<t_angle_type>>& vec_a, const Angle<t_angle_type>& value_b)
    {
        Vector<t_dims, Angle<t_angle_type>> mod;
        for (uint01 dim = 0; dim < t_dims; ++dim)
            mod[dim] = Angle<t_angle_type>(cast<t_angle_type>(vec_a[dim].template internal<false>() % value_b.template internal<false>()));
        return mod;
    }

    template<class t_angle_type>
    constexpr Angle<t_angle_type> abs(const Angle<t_angle_type>& value)
    {
        return value >= Angle<t_angle_type>(0) ? value : -value;
    }
 
    
    template<class t_new_type, uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, Angle<t_new_type>> ToTypeAngle(const Vector<t_dims, Angle<t_angle_type>>& old)
    {
        Vector<t_dims, Angle<t_new_type>> angle;
        for(uint01 dim = 0; dim < t_dims; ++dim)
            angle[dim] = old[dim].template toTypeAngle<t_new_type>();
        return angle;
    }

    class NDEVR_BASE_API AngleDefinitions
    {
    public:
        static Vector<3, Angle<fltp08>> QuaternionToOrientation(const Vector<4, fltp08>& quaternion);
        static  Vector<4, fltp08> OrientationToQuaternion(const Vector<3, Angle<fltp08>>& quaternion);

        template<class t_angle_type, uint01 t_dims, class t_type>
        static Angle<t_angle_type> Rotation(const Vector<t_dims, t_type>& left, const Vector<t_dims, t_type>& middle, const Vector<t_dims, t_type>& right)
        {
            const Vector<t_dims, t_type> v_left(left - middle);
            const Vector<t_dims, t_type> v_right(right - middle);
            return Angle<t_angle_type>(RADIANS, acos(dot(v_left, v_right) / cast<fltp08>(sqrt(v_left.magnitudeSquared() * v_right.magnitudeSquared()))));
        }

        template<class t_angle_type>
        static Angle< t_angle_type> Inclination(const Angle<t_angle_type>& roll, const Angle<t_angle_type>& pitch)
        {
            return Angle<t_angle_type>::atan(cast<fltp08>(sqrt(tan(roll) * tan(roll) + tan(pitch) * tan(pitch))));
        }

        template<class t_angle_type, uint01 t_dims, class t_type>
        static constexpr Angle<t_angle_type> Inclination(const Vector<t_dims, t_type> ray)
        {
            const Vector<2, t_type> xy_diff = ray.template as<2, t_type>();
            return Angle<t_angle_type>::atan2(ray[Z], xy_diff.template magnitude<t_type>());
        }

        template<class t_angle_type, uint01 t_dims, class t_type>
        static constexpr Angle<t_angle_type> Heading(const Vector<t_dims, t_type> ray)
        {
            return Angle<t_angle_type>::atan2(ray[Y], ray[X]);
        }

        template<class t_angle_type, uint01 t_dims, class t_type>
        static constexpr Angle<t_angle_type> AngleBetween(const Vector<t_dims, t_type>& a, const Vector<t_dims, t_type>& b)
        {
            t_type dot_product = dot(a, b);
            t_type length = a.template magnitude<t_type>() * b.template magnitude<t_type>();
            t_type cos_theta = clip(dot_product / length, cast<t_type>(-1), cast<t_type>(1));
            return Angle<t_angle_type>::acos(cos_theta);
        }

        template<class t_angle_type, uint01 t_dims, class t_type>
        static constexpr Vector<t_dims, Angle<t_angle_type>> Orientation(AngleType angle_type, const Vector<t_dims, t_type>& ray)
        {
            Vector<t_dims, Angle<t_angle_type>> orient_vector;
            for (uint01 i = 0; i < t_dims; i++)
                orient_vector[i] = Angle<t_angle_type>(angle_type, ray[i]);
            return orient_vector;
        }

        template<bool t_normalized, uint01 t_dims, class t_angle_type>
        static constexpr Vector<t_dims, fltp08> Orientation(AngleType angle_type, const Vector<t_dims, Angle<t_angle_type>>& ray)
        {
            Vector<t_dims, fltp08> orient_vector;
            for (uint01 i = 0; i < t_dims; i++)
                orient_vector[i] = ray[i].as(angle_type);
            return orient_vector;
        }

        template<class t_angle_type, class t_type>
        static Vector<3, Angle<t_angle_type>> NormalToOrientation(const Vector<3, t_type>& normal, const Angle<t_angle_type>& yaw = Angle<t_angle_type>(0))
        {
            Angle<t_angle_type> pitch(RADIANS, (cast<fltp08>(asin((normal[Y] * sin(yaw) + normal[X] * cos(yaw))))));
            Angle<t_angle_type> roll;
            if (::fabs(cos(pitch)) < 0.001)
                roll = Angle<t_angle_type>(DEGREES, 0.0);
            else if (::fabs(cos(yaw)) < 0.01)
                roll = Angle<t_angle_type>(RADIANS, cast<fltp08>(-asin((cos(yaw) * sin(pitch) - normal[X]) / cos(pitch) * -sin(yaw))));
            else
                roll = Angle<t_angle_type>(RADIANS, cast<fltp08>(-asin((-normal[Y] + sin(yaw) * sin(pitch)) / (cos(pitch) * cos(yaw)))));
            if (normal[Z] < 0)
                roll = Angle<t_angle_type>(DEGREES, 180.0) - roll;
            return Vector<3, Angle<t_angle_type>>(roll, pitch, yaw);
        }

        template<class t_angle_type>
        static Vector<3, Angle<t_angle_type>> NormalizeOrientation(Vector<3, Angle<t_angle_type>> angle, const Vector<3, Angle<t_angle_type>>& reference)
        {
            fltp08 suggested_pitch_value = angle[PITCH].template as<DEGREES>();
            fltp08 our_pitch_value = reference[PITCH].template as<DEGREES>();
 
            fltp08 suggested_yaw_value = angle[YAW].template as<DEGREES>();
            fltp08 our_yaw_value = reference[YAW].template as<DEGREES>();
 
            fltp08 suggested_roll_value = angle[ROLL].template as<DEGREES>();
            fltp08 our_roll_value = reference[ROLL].template as<DEGREES>();
 
            if (suggested_pitch_value - our_pitch_value > 180)
                our_pitch_value += 360;
            else if (our_pitch_value - suggested_pitch_value > 180)
                suggested_pitch_value += 360;
 
            if (suggested_yaw_value - our_yaw_value > 180)
                our_yaw_value += 360;
            else if (our_yaw_value - suggested_yaw_value > 180)
                suggested_yaw_value += 360;
 
            if (suggested_roll_value - our_roll_value > 180)
                our_roll_value += 360;
            else if (our_roll_value - suggested_roll_value > 180)
                suggested_roll_value += 360;
 
            bool flip = std::abs(suggested_roll_value - our_roll_value) >= 90.0 && std::abs(suggested_yaw_value - our_yaw_value) >= 90.0;
            if (flip)
            {
                angle[ROLL] = Angle<fltp08>(DEGREES, angle[ROLL].template as<DEGREES>() - 180);
                angle[YAW] = Angle<fltp08>(DEGREES, angle[YAW].template as<DEGREES>() - 180);
            }
            return angle;
        }
 
    public:
        static const fltp08* CalcIndexSin();
    };

    template<uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, Angle<t_angle_type>> quantize(const Vector<t_dims, Angle<t_angle_type>>& value, Angle<t_angle_type> d = Angle<t_angle_type>(DEGREES, 1.0))
    {
        return AngleDefinitions::Orientation<t_angle_type>(INTERNAL_ANGLE, quantize(AngleDefinitions::Orientation<false>(INTERNAL_ANGLE, value), d.template as<INTERNAL_ANGLE>()));
    }

    template<uint01 t_dims, class t_angle_type>
    constexpr Vector<t_dims, Angle<t_angle_type>> quantize(const Vector<t_dims, Angle<t_angle_type>>& value, Vector<t_dims, Angle<t_angle_type>> d = Vector<t_dims, Angle<t_angle_type>>(Angle<t_angle_type>(DEGREES, 1.0)))
    {
        return AngleDefinitions::Orientation<t_angle_type>(INTERNAL_ANGLE, quantize(AngleDefinitions::Orientation<false>(INTERNAL_ANGLE, value), AngleDefinitions::Orientation<false>(d)));
    }
};