fast_math.hpp

Go to the documentation of this file.

#pragma once

 
#include <pulp/signal/denormal.hpp>
 
#include <algorithm>  // std::max / std::min, used by clamp()
#include <bit>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <type_traits>
#include <cmath>
 
#if defined(__APPLE__) && defined(__clang__)
#include <simd/simd.h>
#endif
 
// Clang's IEEE 754-2019 maximum/minimum builtins propagate NaN, as the
// AArch64 FMAX/FMIN instructions do. Nested so that a compiler without
// __has_builtin never parses the call.
#if defined(__clang__) && defined(__has_builtin)
#if __has_builtin(__builtin_elementwise_maximum) && __has_builtin(__builtin_elementwise_minimum)
#define PULP_FAST_MATH_HAS_IEEE_MINMAX 1
#endif
#endif
#ifndef PULP_FAST_MATH_HAS_IEEE_MINMAX
#define PULP_FAST_MATH_HAS_IEEE_MINMAX 0
#endif
 
namespace pulp::signal {

enum class FastTrigProfile : std::uint8_t {
    reference,
    realtime_efficient,
    realtime_precise,
};

struct FastMath {

    template <FastTrigProfile Profile>
    static float sin_cycles(float phase_cycles) noexcept {
        if constexpr (Profile == FastTrigProfile::reference) {
            constexpr float two_pi = 6.28318530717958647692f;
            return std::sin(two_pi * phase_cycles);
        } else {
            float x = phase_cycles;
            if (x > 0.5f)
                x -= 1.0f;
            if (x > 0.25f)
                x = 0.5f - x;
            else if (x < -0.25f)
                x = -0.5f - x;
            const float x2 = x * x;
 
            if constexpr (Profile == FastTrigProfile::realtime_efficient) {
                return x * (6.28250560008354834003487064338056964f +
                            x2 * (-41.1664423308903732524414077000881397f +
                                  74.4524187069072428211419543641796188f * x2));
            } else {
                static_assert(Profile == FastTrigProfile::realtime_precise);
                return x *
                       (precise_c0 +
                        x2 *
                            (precise_c1 +
                             x2 *
                                 (precise_c2 +
                                  x2 *
                                      (precise_c3 + precise_c4 * x2))));
            }
        }
    }

    static float sin_cycles(float phase_cycles, FastTrigProfile profile) noexcept {
        switch (profile) {
            case FastTrigProfile::realtime_efficient:
                return sin_cycles<FastTrigProfile::realtime_efficient>(phase_cycles);
            case FastTrigProfile::realtime_precise:
                return sin_cycles<FastTrigProfile::realtime_precise>(phase_cycles);
            case FastTrigProfile::reference:
            default: return sin_cycles<FastTrigProfile::reference>(phase_cycles);
        }
    }
 
#if defined(__APPLE__) && defined(__clang__)
    static simd_float4 sin_cycles_precise(simd_float4 phase_cycles) noexcept {
        simd_float4 x = phase_cycles - ::simd::floor(phase_cycles + 0.5f);
        const simd_float4 magnitude = ::simd::abs(x);
        x = ::simd::copysign(::simd::min(magnitude, 0.5f - magnitude), x);
        const simd_float4 x2 = x * x;
        return x *
               (precise_c0 +
                x2 *
                    (precise_c1 +
                     x2 *
                         (precise_c2 +
                          x2 *
                              (precise_c3 + precise_c4 * x2))));
    }
#endif

    static float tanh(float x) {
        const float c = tanh_clamp(x);
        constexpr float kScale = 1.0f - 0x1p-21f;
        constexpr float a0 = 0.9997622141279201f * kScale;
        constexpr float a1 = 0.10092608875679396f * kScale;
        constexpr float a2 = 0.0006321601824242231f * kScale;
        constexpr float b1 = 0.4335808440239807f;
        constexpr float b2 = 0.01240458813388444f;
        const float t = c * c;
        return c * (a0 + t * (a1 + t * a2)) / (1.0f + t * (b1 + t * b2));
    }

    static constexpr float kTanhClamp = 5.24f;

    static float tanh_clamp(float x) noexcept {
#if PULP_FAST_MATH_HAS_IEEE_MINMAX
        return __builtin_elementwise_minimum(__builtin_elementwise_maximum(x, -kTanhClamp),
                                             kTanhClamp);
#else
        const float upper = x > kTanhClamp ? kTanhClamp : x;
        return upper < -kTanhClamp ? -kTanhClamp : upper;
#endif
    }

    static float tanh_accurate(float x) {
        // Clamping x^2 rather than x keeps the sign path short; past |x| = 9
        // the ratio exceeds 1 and the output clamp saturates it.
        const float u = std::fmin(x * x, 81.0f);
        // Ordered compares rather than fmin/fmax: a NaN fails both and
        // propagates, as std::tanh's does, instead of becoming a full-scale
        // constant that hides the upstream fault.
        const float r = tanh_accurate_rational(x, u);
        const float upper = r > 1.0f ? 1.0f : r;
        return upper < -1.0f ? -1.0f : upper;
    }

    static float tanh_accurate_rational(float x, float u) {
        const float u2 = u * u;
        // Both polynomials in Estrin form: shorter dependency chains than
        // Horner, which matters inside a serial per-sample recursion.
        const float p = (1.0f + u * 0.133803148f) +
                        u2 * ((0.00349475497f + u * 2.05960245e-05f) + u2 * 1.33379685e-08f);
        const float q = (1.0f + u * 0.467136322f) +
                        u2 * ((0.0258737793f + u * 0.000328429886f) + u2 * 7.7694232e-07f);
        return x * p / q;
    }

    static float tanh_minimax(float x) {
        if (x < -kTanhMinimaxClamp)
            return -1.0f;
        if (x > kTanhMinimaxClamp)
            return 1.0f;
        return tanh_minimax_rational(x);
    }

    static void tanh_minimax(const float* input, float* output, std::size_t count) noexcept {
        std::size_t i = 0;
#if defined(__clang__) || defined(__GNUC__)
        using Lanes = float __attribute__((vector_size(16)));
        constexpr std::size_t kVectors = 4;
        constexpr std::size_t kStep = kVectors * 4;
        for (; i + kStep <= count; i += kStep) {
            Lanes v[kVectors];
            std::memcpy(v, input + i, sizeof v);
            for (auto& lane : v) {
#if defined(__clang__)
                const Lanes c = __builtin_elementwise_min(
                    __builtin_elementwise_max(lane, Lanes{} - kTanhMinimaxClamp),
                    Lanes{} + kTanhMinimaxClamp);
#else
                const Lanes lo = Lanes{} - kTanhMinimaxClamp;
                const Lanes hi = Lanes{} + kTanhMinimaxClamp;
                const Lanes floored = lane < lo ? lo : lane;
                const Lanes c = floored > hi ? hi : floored;
#endif
                lane = tanh_minimax_rational(c);
            }
            std::memcpy(output + i, v, sizeof v);
        }
#endif
        for (; i < count; ++i)
            output[i] = tanh_minimax_rational(
                std::fmin(std::fmax(input[i], -kTanhMinimaxClamp), kTanhMinimaxClamp));
    }

    template <typename T> static T tanh_minimax_rational(T x) noexcept {
        constexpr float kScale = 1.0f - 0x1p-21f;
        const T t = x * x;
        const T num = x * (kScale + t * (1.2183626499918065e-01f * kScale +
                                         t * (2.1714800492144003e-03f * kScale +
                                              t * (3.4510574788349538e-06f * kScale))));
        const T den = 1.0f + t * (4.5515217250939749e-01f +
                                  t * (2.0577605545641264e-02f + t * 1.3105688057792639e-04f));
        return num / den;
    }

    static float tanh_minimax_recursive(float x) noexcept {
        if (x < -kTanhMinimaxClamp)
            return -1.0f;
        if (x > kTanhMinimaxClamp)
            return 1.0f;
        constexpr float kScale = 1.0f - 0x1p-21f;
        constexpr float n1 = 1.2183626499918065e-01f * kScale;
        constexpr float n2 = 2.1714800492144003e-03f * kScale;
        constexpr float n3 = 3.4510574788349538e-06f * kScale;
        constexpr float d1 = 4.5515217250939749e-01f;
        constexpr float d2 = 2.0577605545641264e-02f;
        constexpr float d3 = 1.3105688057792639e-04f;
        const float t = x * x;
        const float t2 = t * t;
        const float num = x * ((kScale + t * n1) + t2 * (n2 + t * n3));
        const float den = (1.0f + t * d1) + t2 * (d2 + t * d3);
        return num / den;
    }
 
    static constexpr float kTanhMinimaxClamp = 7.0f;

    static float sin(float x) {
        constexpr float pi = 3.14159265f;
        constexpr float two_pi = 6.28318530f;
        // Wrap to [0, 2*pi)
        x = std::fmod(x, two_pi);
        if (x < 0) x += two_pi;
        // Map to [-pi, pi]
        if (x > pi) x -= two_pi;
        // Parabolic approximation with correction
        constexpr float B = 4.0f / pi;
        constexpr float C = -4.0f / (pi * pi);
        float y = B * x + C * x * std::abs(x);
        // Extra precision via Corrected Parabola
        y = 0.225f * (y * std::abs(y) - y) + y;
        return y;
    }

    static float cos(float x) {
        return sin(x + 1.5707963f); // x + pi/2
    }

    static float exp2(float x) {
        return std::exp2(x);
    }

    static float log2(float x) {
        // Extract exponent and mantissa via IEEE 754
        uint32_t bits;
        std::memcpy(&bits, &x, sizeof(bits));
        int32_t exponent = static_cast<int32_t>((bits >> 23) & 0xFF) - 127;
        bits = (bits & 0x007FFFFF) | 0x3F800000; // set exponent to 0 (mantissa in [1,2))
        float m;
        std::memcpy(&m, &bits, sizeof(m));
        // Polynomial for log2(m) where m in [1, 2)
        float result = static_cast<float>(exponent);
        m -= 1.0f;
        result += m * (1.4423904f + m * (-0.7210953f + m * 0.4778948f));
        return result;
    }

    static float pow(float base, float exp) {
        if (base <= 0) return 0;
        return exp2(exp * log2(base));
    }

    static float db_to_gain(float db) {
        return exp2(db * 0.16609640f); // log2(10)/20 ≈ 0.16609640
    }

    static float gain_to_db(float gain) {
        if (gain <= 0) return -200.0f;
        return log2(gain) * 6.0205999f; // 20 / log2(10) ≈ 6.0205999
    }
 
    // Branch-free decibel conversions for per-sample dynamics paths: plain
    // arithmetic and bit manipulation with no calls and no branches, so a loop
    // over them auto-vectorizes, unlike the libm calls they replace. Accurate
    // enough to keep a compressor's gain within a few parts in 10^7 of the
    // libm path (a residual below -120 dBFS), unlike the coarse log2-based
    // db_to_gain() / gain_to_db() above.

    static float log10_accurate(float x) noexcept {
        constexpr std::uint32_t kSqrtHalfBits = 0x3f3504f3u; // bits of sqrt(1/2)
        const std::uint32_t offset = std::bit_cast<std::uint32_t>(x) - kSqrtHalfBits;
        const float k = static_cast<float>(static_cast<std::int32_t>(offset) >> 23);
        const float m = std::bit_cast<float>((offset & 0x007fffffu) + kSqrtHalfBits);
        const float f = m - 1.0f; // exact: m is within a factor of 2 of 1
        const float s = f / (2.0f + f);
        const float z = s * s;
        const float series =
            z * (1.0f / 3.0f + z * (1.0f / 5.0f + z * (1.0f / 7.0f + z * (1.0f / 9.0f))));
        const float two_s = 2.0f * s;
        const float ln_m = two_s * series + two_s;
        // log10(2) and 1/ln(10). The high part of log10(2) has few enough
        // significant bits that k * hi is exact for every float exponent.
        constexpr float kLog10TwoHi = 0.30102539062f; // 0x3e9a2000
        constexpr float kLog10TwoLo = 4.6050390e-06f; // log10(2) - hi
        constexpr float kInvLn10 = 0.43429448190f;
        return k * kLog10TwoHi + (ln_m * kInvLn10 + k * kLog10TwoLo);
    }

    static float exp10_accurate(float x) noexcept {
        constexpr float kLog2Ten = 3.32192809489f;
        constexpr float kLog10TwoHi = 0.30102539062f; // 0x3e9a2000, 11 significant bits
        constexpr float kLog10TwoLo = 4.6050390e-06f; // log10(2) - hi
        constexpr float kLnTen = 2.30258509299f;
        // Past 10^+/-75 (about 250 octaves) the result is inf or 0 anyway;
        // clamping there keeps |n| <= 250 (so n * hi stays exact) and each half
        // of 2^n inside the normal exponent range.
        const float xc = std::fmin(std::fmax(x, -75.0f), 75.0f);
        const float n = std::nearbyint(xc * kLog2Ten);
        const float d = (xc - n * kLog10TwoHi) - n * kLog10TwoLo;
        const float t = d * kLnTen;
        const float p =
            1.0f +
            t * (1.0f + t * (0.5f + t * (1.0f / 6.0f +
                                         t * (1.0f / 24.0f +
                                              t * (1.0f / 120.0f +
                                                   t * (1.0f / 720.0f + t * (1.0f / 5040.0f)))))));
        const std::int32_t e = static_cast<std::int32_t>(n);
        const std::int32_t e_half = e / 2;
        const float scale_a = std::bit_cast<float>(static_cast<std::uint32_t>(e_half + 127) << 23);
        const float scale_b =
            std::bit_cast<float>(static_cast<std::uint32_t>(e - e_half + 127) << 23);
        const float result = (p * scale_a) * scale_b;
        return x == x ? result : x;
    }

    static float gain_to_db_accurate(float gain) noexcept {
        return 20.0f * log10_accurate(gain);
    }

    static float db_to_gain_accurate(float db) noexcept {
        return exp10_accurate(db / 20.0f);
    }

    static float rcp(float x) {
        // One Newton-Raphson iteration from initial estimate
        float estimate = 1.0f / x; // compiler will often use rcpss
        return estimate;
    }

    static float rsqrt(float x) {
        // Quake-style fast inverse sqrt with one NR iteration
        uint32_t i;
        std::memcpy(&i, &x, sizeof(i));
        i = 0x5f3759df - (i >> 1);
        float y;
        std::memcpy(&y, &i, sizeof(y));
        y = y * (1.5f - 0.5f * x * y * y); // Newton-Raphson
        return y;
    }
 
private:
    static constexpr float precise_c0 =
        6.28318527379078585274731929079414949f;
    static constexpr float precise_c1 =
        -41.3416774783915252855640244027643612f;
    static constexpr float precise_c2 =
        81.6022312427274226421465134076212909f;
    static constexpr float precise_c3 =
        -76.5749921819992128192000934020817094f;
    static constexpr float precise_c4 =
        39.7109181438058471453004860893416233f;
 
public:

    static float clamp_unit(float x) {
        return std::max(-1.0f, std::min(1.0f, x));
    }

    static float soft_clip(float x) {
        if (x <= -1.5f) return -1.0f;
        if (x >= 1.5f) return 1.0f;
        return x - (x * x * x) / 6.75f;
    }
};

enum class LadderSaturation { exact, accurate, realtime_efficient };

#if defined(PULP_SIGNAL_LADDER_SATURATION)
static_assert(PULP_SIGNAL_LADDER_SATURATION == 0 || PULP_SIGNAL_LADDER_SATURATION == 1 ||
                  PULP_SIGNAL_LADDER_SATURATION == 2,
              "PULP_SIGNAL_LADDER_SATURATION must be 0 (exact), 1 (accurate) or "
              "2 (realtime_efficient)");
inline constexpr LadderSaturation kDefaultLadderSaturation =
    PULP_SIGNAL_LADDER_SATURATION == 0   ? LadderSaturation::exact
    : PULP_SIGNAL_LADDER_SATURATION == 2 ? LadderSaturation::realtime_efficient
                                         : LadderSaturation::accurate;
#elif defined(PULP_SIGNAL_FAST_LADDER_TANH)
inline constexpr LadderSaturation kDefaultLadderSaturation =
    PULP_SIGNAL_FAST_LADDER_TANH ? LadderSaturation::realtime_efficient : LadderSaturation::exact;
#else
inline constexpr LadderSaturation kDefaultLadderSaturation = LadderSaturation::accurate;
#endif

template <auto Saturation>
    requires std::is_same_v<decltype(Saturation), LadderSaturation>
inline float ladder_tanh_profiled(float x) {
    if constexpr (Saturation == LadderSaturation::realtime_efficient)
        return FastMath::tanh_minimax_recursive(x);
    else if constexpr (Saturation == LadderSaturation::accurate)
        return FastMath::tanh_accurate(x);
    else
        return std::tanh(x);
}

template <auto RealtimeEfficient>
    requires std::is_same_v<decltype(RealtimeEfficient), bool>
[[deprecated("use ladder_tanh_profiled<LadderSaturation>")]]
inline float ladder_tanh_profiled(float x) {
    return ladder_tanh_profiled<RealtimeEfficient ? LadderSaturation::realtime_efficient
                                                  : LadderSaturation::exact>(x);
}

inline float ladder_tanh(float x) {
    return ladder_tanh_profiled<kDefaultLadderSaturation>(x);
}

inline double lipschitz_tanh(double v) noexcept {
    const double x = std::clamp(v, -3.0, 3.0);
    const double x2 = x * x;
    return x * (27.0 + x2) / (27.0 + 9.0 * x2);
}
 
namespace detail {

template <typename T> inline T select_bits(bool condition, T if_true, T if_false) noexcept {
    static_assert(sizeof(T) == 4 || sizeof(T) == 8);
    using U = std::conditional_t<sizeof(T) == 4, std::uint32_t, std::uint64_t>;
    const U mask = U{0} - static_cast<U>(condition);
    return std::bit_cast<T>((std::bit_cast<U>(if_true) & mask) |
                            (std::bit_cast<U>(if_false) & ~mask));
}

template <typename T> inline T snap_to_zero_bits(T x) noexcept {
#if PULP_DSP_ENABLE_SNAP_TO_ZERO
    return select_bits(std::abs(x) < snap_threshold<T>(), T(0), x);
#else
    return x;
#endif
}

inline float tanh_accurate_bits(float x) noexcept {
    const float u = std::fmin(x * x, 81.0f);
    const float r = FastMath::tanh_accurate_rational(x, u);
    const float upper = select_bits(r > 1.0f, 1.0f, r);
    return select_bits(upper < -1.0f, -1.0f, upper);
}
 
} // namespace detail
 
} // namespace pulp::signal