CRNTrigonometry.h

Go to the documentation of this file.

/* Copyright 2006-2015 Yann LEYDIER, CoReNum, Université Paris Descartes
 * 
 * This file is part of libcrn.
 * 
 * libcrn is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * libcrn is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with libcrn.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * file: CRNTrigonometry.h
 * \author Yann LEYDIER
 */

#ifndef CRNTrigonometry_HEADER
#define CRNTrigonometry_HEADER

#include <CRN.h>
#include <CRNException.h>
#include <cmath>
#include <vector>

namespace crn
{
    struct Radian
    {
        using type = double;
        static constexpr type MAXVAL = 2.0 * M_PI;
        static constexpr TypeInfo<type>::SumType VAL2PI = 2.0 * M_PI;
    };
    struct Degree
    {
        using type = double;
        static constexpr type MAXVAL = 360;
        static constexpr TypeInfo<type>::SumType VAL2PI = 360;
    };
    struct ByteAngle
    {
        using type = uint8_t;
        static constexpr type MAXVAL = 255;
        static constexpr TypeInfo<type>::SumType VAL2PI = 256;
    };

    template<typename Unit> inline double Cosine(typename Unit::type angle) noexcept
    {
        return cos(angle * Radian::MAXVAL / Unit::MAXVAL);
    }

    template<typename Unit> inline double Sine(typename Unit::type angle) noexcept
    {
        return sin(angle * Radian::MAXVAL / Unit::MAXVAL);
    }

    template<typename Unit> inline double Tangent(typename Unit::type angle) noexcept
    {
        return tan(angle * Radian::MAXVAL / Unit::MAXVAL);
    }

    template<> inline double Cosine<ByteAngle>(uint8_t angle) noexcept
    {
        static double costab[256];
        static bool init = false;
        if (!init)
        {
            init = true;
            for (int tmp = 0; tmp < 256; ++tmp)
                costab[tmp] = cos(tmp * Radian::MAXVAL / ByteAngle::MAXVAL);
        }
        return costab[angle];
    }

    template<> inline double Sine<ByteAngle>(uint8_t angle) noexcept
    {
        static double sintab[256];
        static bool init = false;
        if (!init)
        {
            init = true;
            for (int tmp = 0; tmp < 256; ++tmp)
                sintab[tmp] = sin(tmp * Radian::MAXVAL / ByteAngle::MAXVAL);
        }
        return sintab[angle];
    }

    template<> inline double Tangent<ByteAngle>(uint8_t angle) noexcept
    {
        static double tantab[256];
        static bool init = false;
        if (!init)
        {
            init = true;
            for (int tmp = 0; tmp < 256; ++tmp)
                tantab[tmp] = tan(tmp * Radian::MAXVAL / ByteAngle::MAXVAL);
        }
        return tantab[angle];
    }

    template<typename Unit> struct Angle
    {
        using unit = Unit;
        constexpr Angle() noexcept(std::is_nothrow_constructible<typename Unit::type>::value) 
            : value(0) { }
        inline Angle(typename Unit::type val) noexcept(std::is_nothrow_constructible<typename Unit::type>::value)
            : value(val)
        {
            typename TypeInfo<typename Unit::type>::SumType tmp(value);
            while (tmp >= Unit::VAL2PI)
                tmp -= typename TypeInfo<typename Unit::type>::SumType(Unit::VAL2PI);
            while (tmp < 0)
                tmp += Unit::VAL2PI;
            value = typename Unit::type(tmp);
        }
        template<typename U> inline Angle(const Angle<U> &other) noexcept(
                std::is_nothrow_constructible<typename U::type>::value && 
                std::is_nothrow_constructible<typename TypeInfo<typename Unit::type>::SumType>::value) 
            : value(other.template Get<Unit>())
        {
            typename TypeInfo<typename Unit::type>::SumType tmp(value);
            while (tmp >= Unit::VAL2PI)
                tmp -= Unit::VAL2PI;
            while (tmp < 0)
                tmp += Unit::VAL2PI;
            value = typename Unit::type(tmp);
        }
        Angle(const Angle&) = default;
        Angle(Angle&&) = default;
        Angle& operator=(const Angle&) = default;
        Angle& operator=(Angle&&) = default;

        template<typename U> inline typename U::type Get() const noexcept(
                std::is_nothrow_constructible<typename U::type>::value && 
                std::is_nothrow_constructible<typename TypeInfo<typename Unit::type>::SumType>::value)
        {
            return typename U::type(((typename TypeInfo<typename Unit::type>::SumType)value * U::MAXVAL) / Unit::MAXVAL);
        }

        inline Angle operator-() const noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value) 
        { Angle a(Unit::VAL2PI); return a -= *this; }

        inline const Angle& operator+=(typename Unit::type angle) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value)
        {
            typename TypeInfo<typename Unit::type>::SumType tmp(value);
            tmp += angle;
            while (tmp >= Unit::VAL2PI)
                tmp -= Unit::VAL2PI;
            value = typename Unit::type(tmp);
            return *this;
        }
        inline const Angle& operator+=(const Angle &other) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value) { return this->operator+=(other.value); }
        inline Angle operator+(const Angle &other) const  noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value){ Angle a(*this); return  a+= other; }

        inline const Angle& operator-=(typename Unit::type angle) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value)
        {
            typename TypeInfo<typename Unit::type>::DiffType tmp(value);
            tmp -= angle;
            while (tmp < 0)
                tmp += Unit::VAL2PI;
            value = typename Unit::type(tmp);
            return *this;
        }
        inline const Angle& operator-=(const Angle &other) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value) 
        { return this->operator-=(other.value); }
        inline Angle operator-(const Angle &other) const noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value)
        { Angle a(*this); return a -= other; }

        inline Angle& operator*=(double f) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value)
        {
            auto tmp = double(value) * f; 
            while (tmp >= Unit::VAL2PI)
                tmp -= Unit::VAL2PI;
            while (tmp < 0)
                tmp += Unit::VAL2PI;
            value = typename Unit::type(tmp);
            return *this;
        }
        inline Angle& operator*(double f) noexcept(std::is_nothrow_copy_assignable<typename Unit::type>::value) { auto a(*this); return a *= f; }

        inline bool operator==(const Angle &other) const noexcept { return value == other.value; }

        inline double Cos() const noexcept { return crn::Cosine<Unit>(value); }
        inline double Sin() const noexcept { return crn::Sine<Unit>(value); }
        inline double Tan() const noexcept { return crn::Tangent<Unit>(value); }

        static inline Angle Acos(double cosine) noexcept(std::is_nothrow_constructible<typename Unit::type>::value) { return Angle<Radian>(acos(cosine)); }
        static inline Angle Asin(double sine) noexcept(std::is_nothrow_constructible<typename Unit::type>::value) { return Angle<Radian>(asin(sine)); }
        static inline Angle Atan(double tangent) noexcept(std::is_nothrow_constructible<typename Unit::type>::value) { return Angle<Radian>(atan(tangent)); }
        static inline Angle Atan(double y, double x) noexcept(std::is_nothrow_constructible<typename Unit::type>::value) { return Angle<Radian>(atan2(y, x)); }

        static constexpr Angle<Unit> LEFT() { return typename Unit::type(0); } 
        static constexpr Angle<Unit> RIGHT() { return typename Unit::type(Unit::MAXVAL / 2); } 
        static constexpr Angle<Unit> TOP() { return typename Unit::type(Unit::MAXVAL / 4); } 
        static constexpr Angle<Unit> BOTTOM() { return typename Unit::type((3 * (typename TypeInfo<typename Unit::type>::SumType)Unit::MAXVAL) / 4); } 
        typename Unit::type value; 
    };

    template<class ANGLE> using Unit = typename ANGLE::unit;

    template<typename Unit> inline Angle<Unit> operator+(const Angle<Unit> &a1, const Angle<Unit> &a2) 
        noexcept(
                std::is_nothrow_constructible<typename Unit::type>::value && 
                std::is_nothrow_copy_assignable<typename Unit::type>::value)
    {
        Angle<Unit> tmp(a1);
        tmp += a2;
        return tmp;
    }

    template<typename Unit> inline Angle<Unit> operator-(const Angle<Unit> &a1, const Angle<Unit> &a2) 
        noexcept(
                std::is_nothrow_constructible<typename Unit::type>::value && 
                std::is_nothrow_copy_assignable<typename Unit::type>::value)
    {
        Angle<Unit> tmp(a1);
        tmp -= a2;
        return tmp;
    }

    template<typename Unit> inline Angle<Unit> operator*(double f, const Angle<Unit> &a) 
        noexcept(
                std::is_nothrow_constructible<typename Unit::type>::value && 
                std::is_nothrow_copy_assignable<typename Unit::type>::value)
    {
        Angle<Unit> tmp(a);
        return tmp *= f;
    }

    namespace literals
    {
        inline Angle<Degree> operator"" _deg(long double val)
        {
            return Angle<Degree>{double(val)};
        }
        inline Angle<Radian> operator"" _rad(long double val)
        {
            return Angle<Radian>{double(val)};
        }
        inline Angle<ByteAngle> operator"" _ba(unsigned long long int val)
        {
            return Angle<ByteAngle>{uint8_t(val % 256)};
        }
    }

    template<class A> inline double Cos(const A &a) noexcept { return a.Cos(); }
    inline double Cos(double a) noexcept { return cos(a); }
    inline double Cos(uint8_t a) noexcept { return Cosine<ByteAngle>(a); }
    template<class A> inline double Sin(const A &a) noexcept { return a.Sin(); }
    inline double Sin(double a) noexcept { return sin(a); }
    inline double Sin(uint8_t a) noexcept { return Sine<ByteAngle>(a); }
    template<class A> inline double Tan(const A &a) noexcept { return a.Tan(); }
    inline double Tan(double a) noexcept { return Tan(a); }
    inline double Tan(uint8_t a) noexcept { return Tangent<ByteAngle>(a); }
    template<class A> inline A Atan(double s, double c) noexcept { return A::Atan(s, c); }
    template<> inline double Atan<double>(double s, double c) noexcept { return atan2(s, c); }

    template<typename Unit> inline typename Unit::type AngularDistance(const Angle<Unit> &a1, const Angle<Unit> &a2) noexcept(
            std::is_nothrow_constructible<typename TypeInfo<typename Unit::type>::DiffType>::value && 
            std::is_nothrow_copy_assignable<typename TypeInfo<typename Unit::type>::DiffType>::value &&
            std::is_nothrow_constructible<typename Unit::type>::value)
    {
    // TODO what is the fastest? min(a-b, 2PI - (a-b)) or PI - abs(PI - abs(a-b))
        typename TypeInfo<typename Unit::type>::DiffType dist(Abs((typename TypeInfo<typename Unit::type>::DiffType)a1.value - (typename TypeInfo<typename Unit::type>::DiffType)a2.value));
        if (dist > Unit::MAXVAL / 2)
            dist = Unit::MAXVAL - dist;
        return typename Unit::type(dist);
    }
    inline double AngularDistance(double a1, double a2) noexcept
    {
        const auto dist = Abs(a1 - a2);
        return (dist > M_PI) ? (M_PI - dist) : dist;
    }

    template<typename ITER> typename std::iterator_traits<ITER>::value_type AngularMean(ITER beg, ITER en)
    {
        using return_type = typename std::iterator_traits<ITER>::value_type;
        if (beg == en)
            throw ExceptionDomain("AngularMean(): empty set of angles.");
        double s = 0, c = 0;
        for (ITER it = beg; it != en; ++it)
        {
            s += Sin(*it);
            c += Cos(*it);
        }
        return Atan<return_type>(s, c);
    }

    template<typename ITER> ITER AngularMedian(ITER beg, ITER en)
    {
        if (beg == en)
            throw ExceptionDomain("AngularMedian(): empty set of angles.");
        auto mdist = std::vector<double>(std::distance(beg, en), 0.0);
        auto cnt1 = size_t(0);
        for (auto it1 = beg; it1 != en; ++cnt1, ++it1)
        {
            auto cnt2 = cnt1 + 1;
            for (auto it2 = std::next(it1); it2 != en; ++cnt2, ++it2)
            {
                const auto d = AngularDistance(*it1, *it2);
                mdist[cnt1] += d;
                mdist[cnt2] += d;
            }
        }
        return std::next(beg, std::min_element(mdist.begin(), mdist.end()) - mdist.begin());
    }

    template<typename ITER> double AngularVariance(ITER beg, ITER en)
    {
        using angle_type = typename std::iterator_traits<ITER>::value_type;
        if (beg == en)
            throw ExceptionDomain("AngularVariance(): empty set of angles.");
        angle_type mean(AngularMean(beg, en));
        double acc = 0;
        for (ITER it = beg; it != en; ++it)
        {
            acc += Sqr(AngularDistance<typename angle_type::unit>(mean, *it));
        }
        return acc / double(std::distance(beg, en));
    }

    template<typename ITER> double AngularVariance(ITER beg, ITER en, typename std::iterator_traits<ITER>::value_type mean)
    {
        using angle_type = typename std::iterator_traits<ITER>::value_type;
        if (beg == en)
            throw ExceptionDomain("AngularVariance(): empty set of angles.");
        double acc = 0;
        for (ITER it = beg; it != en; ++it)
        {
            acc += Sqr(AngularDistance<typename angle_type::unit>(mean, *it));
        }
        return acc / double(std::distance(beg, en));
    }

    template<typename ITER> double CircularVariance(ITER beg, ITER en)
    {
        if (beg == en)
            throw ExceptionDomain("CircularVariance(): empty set of angles.");
        auto c = 0.0, s = 0.0;
        for (ITER it = beg; it != en; ++it)
        {
            c += it->Cos();
            s += it->Sin();
        }
        auto n = double(std::distance(beg, en));
        return 1.0 - sqrt(Sqr(c / n) + Sqr(s / n));
    }

    template<typename ITER> double CircularStdDev(ITER beg, ITER en)
    {
        return sqrt(-2.0 * log(1.0 - CircularVariance(beg, en)));
    }

    template<typename ITER> typename std::complex<double> TrigonometricMoment(ITER beg, ITER en, typename std::iterator_traits<ITER>::value_type refer, size_t p)
    {
        if (beg == en)
            throw ExceptionDomain("TrigonometricMoment(): empty set of angles.");
        if (p == 0)
            throw ExceptionInvalidArgument("TrigonometricMoment(): null order.");
        auto c = 0.0, s = 0.0;
        for (auto it = beg; it != en; ++it)
        {
            c += Cos(double(p) * (*it - refer));
            s += Sin(double(p) * (*it - refer));
        }
        const auto n = double(std::distance(beg, en));
        c /= n;
        s /= n;
        return std::complex<double>(c, s);
    }

    template<typename ITER> double AngularSkewness(ITER beg, ITER en)
    {
        if (beg == en)
            throw ExceptionDomain("AngularSkewness(): empty set of angles.");
        const auto m = AngularMean(beg, en);
        const auto m1 = TrigonometricMoment(beg, en, m, 1);
        const auto m2 = TrigonometricMoment(beg, en, m, 2);
        return std::abs(m2) * Sin(Angle<Radian>(std::arg(m2)) - 2 * m) / pow(1 - std::abs(m1), 1.5);
    }

    template<typename ITER> double AngularKurtosis(ITER beg, ITER en)
    {
        if (beg == en)
            throw ExceptionDomain("AngularKurtosis(): empty set of angles.");
        const auto m = AngularMean(beg, en);
        const auto m1 = TrigonometricMoment(beg, en, m, 1);
        const auto m2 = TrigonometricMoment(beg, en, m, 2);
        return (std::abs(m2) * Cos(Angle<Radian>(std::arg(m2)) - 2 * m) - pow(std::abs(m1), 4)) / Sqr(1 - std::abs(m1));
    }

}

#endif