FixedPoint.hpp

Go to the documentation of this file.

//
//  CeresEngine - A game development framework
//
//  Created by Rogiel Sulzbach.
//  Copyright (c) 2018-2022 Rogiel Sulzbach. All rights reserved.
//
 
#pragma once
 
#include "CeresEngine/DataTypes.hpp"
 
#include "CeresEngine/Foundation/Hash.hpp"
#include "CeresEngine/Foundation/Container/Array.hpp"
#include "CeresEngine/Math/Math.hpp"
 
#include <cmath>
#include <limits>
 
namespace CeresEngine::inline Math {
 
    template<typename T = Int64, unsigned int P = 20> struct FixedPoint {
        static_assert(P <= sizeof(T) * 8);
 
        using Self = FixedPoint<T, P>;
 
        // NOLINTNEXTLINE(hicpp-signed-bitwise)
        static const constexpr T one = T(1) << P;
 
        T raw; // NOLINT(misc-non-private-member-variables-in-classes)
 
        constexpr FixedPoint() = default;
 
        constexpr explicit FixedPoint(T raw) noexcept : raw(raw) {}
 
        constexpr FixedPoint(const FixedPoint&) = default;
 
        constexpr FixedPoint& operator=(const FixedPoint&) = default;
 
        constexpr FixedPoint( // NOLINT(google-explicit-constructor,hicpp-explicit-conversions)
            float f) noexcept
                : raw(static_cast<T>(f * one)) {}
 
        [[nodiscard]] explicit constexpr operator float() const { return static_cast<float>(raw) / one; }
 
        constexpr FixedPoint( // NOLINT(google-explicit-constructor,hicpp-explicit-conversions)
            const double d) noexcept
                : raw(static_cast<T>(d * double(one))) {}
 
        [[nodiscard]] explicit constexpr operator double() const { return double(raw) / double(one); }
 
        constexpr FixedPoint( // NOLINT(google-explicit-constructor,hicpp-explicit-conversions)
            long double d) noexcept
                : raw(static_cast<T>(d * one)) {}
 
        [[nodiscard]] explicit constexpr operator long double() const { return static_cast<long double>(raw) / one; }
 
        template<typename TT, unsigned int TP>
        constexpr FixedPoint( // NOLINT(google-explicit-constructor,hicpp-explicit-conversions)
            const FixedPoint<TT, TP>& other)
                : raw(other.template convert<T, P>()) {}
 
        template<typename TT, unsigned int TP> [[nodiscard]] TT convert() const {
            if constexpr(TP > P) {
                return static_cast<TT>(raw) << (TP - P);
            } else {
                return static_cast<TT>(raw) >> (P - TP);
            }
        }
    };
 
    using Fixed32x32 = FixedPoint<Int64, 32>;
 
    using Fixed44x20 = FixedPoint<Int64, 20>;
 
    using Fixed20x44 = FixedPoint<Int64, 44>;
 
    using Fixed48x16 = Math::FixedPoint<Int64, 16>;
 
    using Fixed16x48 = Math::FixedPoint<Int64, 48>;
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> operator-(const FixedPoint<T, P> a) noexcept {
        return FixedPoint<T, P>(T(-a.raw));
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> operator+(const FixedPoint<T, P> a) noexcept { return a; }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr FixedPoint<T, P> operator+(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return FixedPoint<T, P>(T(a.raw + b.raw));
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr FixedPoint<T, P> operator-(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return FixedPoint<T, P>(T(a.raw - b.raw));
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr FixedPoint<T, P> operator*(const FixedPoint<T, P> x, const typename FixedPoint<T, P>::Self y) noexcept {
        using UT = typename std::make_unsigned<T>::type;
 
        constexpr size_t bits = sizeof(T) * 8;
        constexpr size_t halfBits = bits / 2;
        constexpr UT halfMask = (UT(1) << (4 * sizeof(T))) - UT(1);
 
        UT a = UT(x.raw) >> halfBits;
        UT b = UT(x.raw) & halfMask;
        UT c = UT(y.raw) >> halfBits;
        UT d = UT(y.raw) & halfMask;
 
        UT ac = a * c;
        UT bc = b * c;
        UT ad = a * d;
        UT bd = b * d;
 
        UT mid34 = (bd >> halfBits) + (bc & halfMask) + (ad & halfMask);
 
        UT upper64 = ac + (bc >> halfBits) + (ad >> halfBits) + (mid34 >> halfBits);
        UT lower64 = (mid34 << halfBits) | (bd & halfMask);
 
        if constexpr(std::is_signed<T>::value) {
            T upper64s = T(upper64);
            if(x.raw < T(0))
                upper64s -= y.raw;
            if(y.raw < T(0))
                upper64s -= x.raw;
            upper64 = UT(upper64s);
 
            UT raw = ((upper64 << (bits - P + 1)) >> 1) | (lower64 >> P);
            if(upper64s < T(0)) {
                raw |= UT(std::numeric_limits<T>::lowest());
            }
            return FixedPoint<T, P>((T)raw);
        } else {
            return FixedPoint<T, P>(T((upper64 << (bits - P)) | (lower64 >> P)));
        }
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr FixedPoint<T, P> operator/(const FixedPoint<T, P> x, const typename FixedPoint<T, P>::Self y) noexcept {
        if constexpr(sizeof(T) * 4 <= P) {
            return FixedPoint<T, P>(T((x.raw << P) / y.raw));
        } else {
            return x * FixedPoint<T, P>(T((FixedPoint<T, P>::one << P) / y.raw));
        }
    }
 
    // ---------------------------------------------------------------------------------------------
 
    template<typename T, unsigned int P> inline constexpr FixedPoint<T, P>& operator+=(FixedPoint<T, P>& a, const typename FixedPoint<T, P>::Self b) noexcept {
        a = a + b;
        return a;
    }
 
    template<typename T, unsigned int P> inline constexpr FixedPoint<T, P>& operator++(FixedPoint<T, P>& a) noexcept {
        a.raw = a.raw + FixedPoint<T, P>::one;
        return a;
    }
 
    template<typename T, unsigned int P>
    inline constexpr FixedPoint<T, P> // NOLINT(cert-dcl21-cpp)
        operator++(FixedPoint<T, P>& a, int) noexcept {
        FixedPoint<T, P> copy = a;
        ++a;
        return copy;
    }
 
    template<typename T, unsigned int P> inline constexpr FixedPoint<T, P>& operator-=(FixedPoint<T, P>& a, const typename FixedPoint<T, P>::Self b) noexcept {
        a = a - b;
        return a;
    }
 
    template<typename T, unsigned int P> inline constexpr FixedPoint<T, P>& operator--(FixedPoint<T, P>& a) noexcept {
        a.raw = a.raw - FixedPoint<T, P>::one;
        return a;
    }
 
    template<typename T, unsigned int P>
    inline constexpr FixedPoint<T, P> // NOLINT(cert-dcl21-cpp)
        operator--(FixedPoint<T, P>& a, int) noexcept {
        FixedPoint<T, P> copy = a;
        --a;
        return copy;
    }
 
    template<typename T, unsigned int P> inline constexpr FixedPoint<T, P>& operator*=(FixedPoint<T, P>& a, const typename FixedPoint<T, P>::Self b) noexcept {
        a = a * b;
        return a;
    }
 
    // ---------------------------------------------------------------------------------------------
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator==(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw == b.raw;
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator!=(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw != b.raw;
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator<(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw < b.raw;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr bool operator<(const FixedPoint<T, P> a, double b) noexcept {
        return a.raw < FixedPoint<T, P>(b).raw;
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator<=(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw <= b.raw;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr bool operator<=(const FixedPoint<T, P> a, double b) noexcept {
        return a.raw <= FixedPoint<T, P>(b).raw;
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator>(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw > b.raw;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr bool operator>(const FixedPoint<T, P> a, double b) noexcept {
        return a.raw > FixedPoint<T, P>(b).raw;
    }
 
    template<typename T, unsigned int P>
    [[nodiscard]] inline constexpr bool operator>=(const FixedPoint<T, P> a, const typename FixedPoint<T, P>::Self b) noexcept {
        return a.raw >= b.raw;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr bool operator>=(const FixedPoint<T, P> a, double b) noexcept {
        return a.raw >= FixedPoint<T, P>(b).raw;
    }
 
    // ---------------------------------------------------------------------------------------------
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> abs(const FixedPoint<T, P>& x) {
        if constexpr(std::is_signed<T>::value) {
            return x.raw < T(0) ? -x : x;
        } else {
            return x;
        }
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> frac(const FixedPoint<T, P>& x) {
        if constexpr(std::is_signed<T>::value) {
            return x.raw < T(0) ? FixedPoint<T, P>((-x.raw) % FixedPoint<T, P>::one) : FixedPoint<T, P>(x.raw % FixedPoint<T, P>::one);
        } else {
            return FixedPoint<T, P>(x.raw % FixedPoint<T, P>::one);
        }
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> sign(const FixedPoint<T, P>& x) {
        if(x.raw == T(0)) {
            return x;
        }
 
        if constexpr(std::is_signed<T>::value) {
            return (x.raw < T(0)) ? FixedPoint<T, P>(-FixedPoint<T, P>::one) : FixedPoint<T, P>(FixedPoint<T, P>::one);
        } else {
            return FixedPoint<T, P>(FixedPoint<T, P>::one);
        }
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> floor(const FixedPoint<T, P>& x) {
        return FixedPoint<T, P>((x.raw >> P) << P);
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> ceil(const FixedPoint<T, P>& x) {
        return floor(x / FixedPoint<T, P>(FixedPoint<T, P>::one) + FixedPoint<T, P>(FixedPoint<T, P>::one - T(1)));
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> round(const FixedPoint<T, P>& x) {
        return FixedPoint<T, P>(((x.raw + FixedPoint<T, P>::one / T(2)) >> P) << P);
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> sqrt(const FixedPoint<T, P>& x) {
        T r = x.raw;
        T b = T(1) << ((sizeof(T) * 8) - 2);
        T q = 0;
        while(b > 0x40) {
            T t = q + b;
            if(r >= t) {
                r -= t;
                q = t + b; // equivalent to q += 2*b
            }
            r <<= 1;
            b >>= 1;
        }
        return FixedPoint<T, P>(q >> (P - 1));
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> acos(FixedPoint<T, P> x) {
        const FixedPoint<T, P> negate = FixedPoint<T, P>(x.raw < T(0) ? FixedPoint<T, P>::one : T(0));
        x = abs(x);
        FixedPoint<T, P> ret = FixedPoint<T, P>(-0.0187293);
        ret = ret * x;
        ret = ret + FixedPoint<T, P>(0.0742610);
        ret = ret * x;
        ret = ret - FixedPoint<T, P>(0.2121144);
        ret = ret * x;
        ret = ret + FixedPoint<T, P>(1.5707288);
        ret = ret * sqrt(FixedPoint<T, P>(FixedPoint<T, P>::one) - x);
        ret = ret - FixedPoint<T, P>(2.0) * negate * ret;
        return negate * FixedPoint<T, P>(3.14159265358979) + ret;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> asin(FixedPoint<T, P> x) {
        const FixedPoint<T, P> negate = FixedPoint<T, P>(x.raw < T(0) ? FixedPoint<T, P>::one : T(0));
        x = abs(x);
        FixedPoint<T, P> ret = FixedPoint<T, P>(-0.0187293);
        ret = ret * x;
        ret = ret + FixedPoint<T, P>(0.0742610);
        ret = ret * x;
        ret = ret - FixedPoint<T, P>(0.2121144);
        ret = ret * x;
        ret = ret + FixedPoint<T, P>(1.5707288);
        ret = FixedPoint<T, P>(3.14159265358979) * FixedPoint<T, P>(0.5) - sqrt(FixedPoint<T, P>(FixedPoint<T, P>::one) - x) * ret;
        return ret - FixedPoint<T, P>(FixedPoint<T, P>::one * 2) * negate * ret;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> atan2(const FixedPoint<T, P>& y, const FixedPoint<T, P>& x) {
        static_assert(std::is_signed_v<T>);
 
        using Type = FixedPoint<T, P>;
 
        Type t0, t1, t3, t4;
 
        t3 = abs(x);
        t1 = abs(y);
        t0 = max(t3, t1);
        t1 = min(t3, t1);
        t3 = Type(1.0) / t0;
        t3 = t1 * t3;
 
        t4 = t3 * t3;
        t0 = -Type(0.013480470);
        t0 = t0 * t4 + Type(0.057477314);
        t0 = t0 * t4 - Type(0.121239071);
        t0 = t0 * t4 + Type(0.195635925);
        t0 = t0 * t4 - Type(0.332994597);
        t0 = t0 * t4 + Type(0.999995630);
        t3 = t0 * t3;
 
        t3 = (abs(y) > abs(x)) ? Type(1.570796327) - t3 : t3;
        t3 = (x < 0) ? Type(3.141592654) - t3 : t3;
        t3 = (y < 0) ? -t3 : t3;
 
        return t3;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> atan(const FixedPoint<T, P>& x) {
        return atan2(x, FixedPoint<T, P>(FixedPoint<T, P>::one));
    }
 
 
    template <typename B, unsigned int F>
    constexpr inline FixedPoint<B, F> fmod(FixedPoint<B, F> x, FixedPoint<B, F> y) noexcept
    {
        return
            assert(y.raw != 0),
            FixedPoint<B, F>(x.raw % y.raw);
    }
 
    template <typename B, unsigned int F>
    FixedPoint<B, F> sin(FixedPoint<B, F> x) noexcept
    {
        // This sine uses a fifth-order curve-fitting approximation originally
        // described by Jasper Vijn on coranac.com which has a worst-case
        // relative error of 0.07% (over [-pi:pi]).
        using Fixed = FixedPoint<B, F>;
 
        // Turn x from [0..2*PI] domain into [0..4] domain
        x = fmod(x, Fixed(2.0 / Math::PI));
        x = x / Fixed(Math::PI / 2.0);
 
        // Take x modulo one rotation, so [-4..+4].
        if (x < Fixed(0.0)) {
            x += Fixed(4.0);
        }
 
        int sign = +1;
        if (x > Fixed(2.0)) {
            // Reduce domain to [0..2].
            sign = -1;
            x -= Fixed(2.0);
        }
 
        if (x > Fixed(1.0)) {
            // Reduce domain to [0..1].
            x = Fixed(2.0) - x;
        }
 
        const Fixed x2 = x*x;
        return Fixed(sign * 1.0) * x * (Fixed(Math::PI) - x2*(Fixed(2.0 / Math::PI) - 5.0 - x2*(Fixed(Math::PI) - 3.0)))/2.0;
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> cos(const FixedPoint<T, P>& x) {
//        static_assert(std::is_signed_v<T>::value);
 
        using Fixed = FixedPoint<T, P>;
        if (x > Fixed(0.0)) {  // Prevent an overflow due to the addition of π/2
            return sin(x - (Fixed(Math::PI * 2.0) - Fixed(Math::PI / 2.0)));
        } else {
            return sin(Fixed(Math::PI / 2.0) + x);
        }
    }
 
    template<typename T, unsigned int P> [[nodiscard]] inline constexpr FixedPoint<T, P> tan(const FixedPoint<T, P>& x) {
        const FixedPoint<T, P> sine = sin(x);
        const FixedPoint<T, P> cosine = cos(x);
        if(cosine.raw == T(0)) {
            return std::numeric_limits<FixedPoint<T, P>>::infinity();
        }
 
        return sine / cosine;
    }
 
    // ---------------------------------------------------------------------------------------------
 
    [[nodiscard]] constexpr FixedPoint<Int64, sizeof(Int64) * 8 - std::numeric_limits<double>::digits> operator"" _fp(const long double v) { return v; }
 
} // namespace CeresEngine::inline Math
 
template<typename T, unsigned int P> struct std::is_integral<CeresEngine::Math::FixedPoint<T, P>> : std::false_type {};
 
template<typename T, unsigned int P> struct std::is_signed<CeresEngine::Math::FixedPoint<T, P>> : std::is_signed<T> {};
 
template<typename T, unsigned int P> struct std::is_unsigned<CeresEngine::Math::FixedPoint<T, P>> : std::is_unsigned<T> {};
 
template<typename T, unsigned int P> struct std::make_signed<CeresEngine::Math::FixedPoint<T, P>> {
    typedef CeresEngine::Math::FixedPoint<typename std::make_signed<T>::type, P> type;
};
 
template<typename T, unsigned int P> struct std::make_unsigned<CeresEngine::Math::FixedPoint<T, P>> {
    typedef CeresEngine::Math::FixedPoint<typename std::make_unsigned<T>::type, P> type;
};
 
template<typename T, unsigned int P> struct std::hash<CeresEngine::Math::FixedPoint<T, P>> {
    using Type = CeresEngine::Math::FixedPoint<T, P>;
    constexpr size_t operator()(const Type& obj) const { return CeresEngine::hash(obj.raw); }
};
 
template<typename T, unsigned int P> struct std::numeric_limits<CeresEngine::Math::FixedPoint<T, P>> {
    using Tp = CeresEngine::Math::FixedPoint<T, P>;
 
    static inline constexpr bool is_specialized = true;
 
    static inline constexpr bool is_integer = false;
 
    static inline constexpr bool is_signed = numeric_limits<T>::is_signed;
 
    static inline constexpr bool is_exact = true;
 
    static inline constexpr bool has_infinity = false;
 
    static inline constexpr bool has_quiet_NaN = false;
 
    static inline constexpr bool has_signaling_NaN = false;
 
    static inline constexpr float_denorm_style has_denorm = denorm_absent;
 
    static inline constexpr bool has_denorm_loss = false;
 
    static inline constexpr float_round_style round_style = round_toward_zero;
 
    static inline constexpr bool is_iec559 = true;
 
    static inline constexpr bool is_bounded = true;
 
    static inline constexpr bool is_modulo = false;
 
    static inline constexpr int digits = static_cast<int>(P);
 
    static inline constexpr int digits10 = static_cast<int>(digits * 0.3010299956639812);
 
    static inline constexpr int max_digits10 = 0;
 
    static inline constexpr int radix = 2;
 
    static inline constexpr int min_exponent = 0;
 
    static inline constexpr int min_exponent10 = 0;
 
    static inline constexpr int max_exponent = 0;
 
    static inline constexpr int max_exponent10 = 0;
 
    static inline constexpr bool traps = std::numeric_limits<T>::traps;
 
    static inline constexpr bool tinyness_before = false;
 
    [[nodiscard]] static constexpr Tp min() noexcept { return Tp(T(0)); }
 
    [[nodiscard]] static constexpr Tp lowest() noexcept { return Tp(std::numeric_limits<T>::lowest()); }
 
    [[nodiscard]] static constexpr Tp max() noexcept { return Tp(std::numeric_limits<T>::max()); }
 
    [[nodiscard]] static constexpr Tp epsilon() noexcept { return Tp(T(1)); }
 
    [[nodiscard]] static constexpr Tp round_error() noexcept { return Tp(0); }
 
    [[nodiscard]] static constexpr Tp infinity() noexcept { return max(); }
 
    [[nodiscard]] static constexpr Tp quiet_NaN() noexcept { return Tp(0); }
 
    [[nodiscard]] static constexpr Tp signaling_NaN() noexcept { return Tp(0); }
 
    [[nodiscard]] static constexpr Tp denorm_min() noexcept { return Tp(0); }
};