s16x16.h

Go to the documentation of this file.

#pragma once
 
// Signed 16.16 fixed-point arithmetic and trigonometry.
// All operations are integer-only in the hot path.
 
#include "fl/stl/stdint.h"
#include "fl/math/sin32.h"
#include "fl/math/fixed_point/isqrt.h"
#include "fl/stl/compiler_control.h"
#include "fl/math/fixed_point/traits.h"
#include "fl/stl/noexcept.h"
#include "fl/stl/undef.h"  // Undefine abs/min/max macros from Arduino.h
 
FL_OPTIMIZATION_LEVEL_O3_BEGIN
 
namespace fl {
 
// Signed 16.16 fixed-point value type.
class s16x16 {
  public:
    static constexpr int INT_BITS = 16;
    static constexpr int FRAC_BITS = 16;
    static constexpr i32 SCALE = static_cast<i32>(1) << FRAC_BITS;
 
    // ---- Construction ------------------------------------------------------
 
    constexpr s16x16() FL_NOEXCEPT = default;
 
    explicit constexpr s16x16(float f) FL_NOEXCEPT
        : mValue(static_cast<i32>(f * (SCALE))) {}
 
    // Integer constructor — any integer width (portable: AVR 16-bit int, ARM/x86 32-bit).
    // Compile error if constexpr value exceeds INT_BITS range.
    template <typename IntT, detail::enable_if_integer_t<IntT> = 0>
    explicit constexpr s16x16(IntT n) FL_NOEXCEPT
        : mValue(detail::int_to_fixed<INT_BITS, FRAC_BITS>::from_signed(n)) {}
 
    // Auto-promotion from other fixed-point types
    // Enabled only when both INT_BITS and FRAC_BITS can be promoted (no demotion)
    template <typename OtherFP>
    constexpr s16x16(const OtherFP& other,
                     typename fl::enable_if<
                         (OtherFP::INT_BITS <= INT_BITS) &&
                         (OtherFP::FRAC_BITS <= FRAC_BITS) &&
                         (OtherFP::INT_BITS != INT_BITS || OtherFP::FRAC_BITS != FRAC_BITS),
                         int>::type = 0)
 FL_NOEXCEPT : mValue(static_cast<i32>(
            static_cast<i64>(other.raw()) << (FRAC_BITS - OtherFP::FRAC_BITS))) {}
 
    // Raw constructor for C++11 constexpr from_raw
    struct RawTag {};
    constexpr explicit s16x16(i32 raw, RawTag) FL_NOEXCEPT : mValue(raw) {}
 
    static constexpr FASTLED_FORCE_INLINE s16x16 from_raw(i32 raw) FL_NOEXCEPT {
        return s16x16(raw, RawTag());
    }
 
    // ---- Access ------------------------------------------------------------
 
    constexpr i32 raw() const FL_NOEXCEPT { return mValue; }
    constexpr i32 to_int() const FL_NOEXCEPT { return mValue >> FRAC_BITS; }
    constexpr float to_float() const FL_NOEXCEPT { return static_cast<float>(mValue) / (SCALE); }
 
    // ---- Fixed-point arithmetic --------------------------------------------
 
#if defined(__riscv) && (__riscv_xlen == 32)
    // RV32: Use mul+mulh inline asm to avoid expensive 64-bit widening multiply.
    // Extracts bits [47:16] of the 64-bit product for Q16.16 fixed-point.
    FASTLED_FORCE_INLINE s16x16 operator*(s16x16 b) const {
        i32 lo, hi;
        asm("mul  %0, %2, %3\n\t"
            "mulh %1, %2, %3"
            : "=&r"(lo), "=&r"(hi)
            : "r"(mValue), "r"(b.mValue));
        return from_raw(static_cast<i32>(
            (static_cast<u32>(lo) >> 16) | (static_cast<u32>(hi) << 16)));
    }
#else
    constexpr FASTLED_FORCE_INLINE s16x16 operator*(s16x16 b) const FL_NOEXCEPT {
        return from_raw(static_cast<i32>(
            (static_cast<i64>(mValue) * b.mValue) >> FRAC_BITS));
    }
#endif
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator/(s16x16 b) const FL_NOEXCEPT {
        return from_raw(static_cast<i32>(
            (static_cast<i64>(mValue) * (static_cast<i64>(SCALE))) / b.mValue));
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator+(s16x16 b) const FL_NOEXCEPT {
        return from_raw(static_cast<i32>(
            static_cast<u32>(mValue) + static_cast<u32>(b.mValue)));
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator-(s16x16 b) const FL_NOEXCEPT {
        return from_raw(static_cast<i32>(
            static_cast<u32>(mValue) - static_cast<u32>(b.mValue)));
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator-() const FL_NOEXCEPT {
        return from_raw(static_cast<i32>(0u - static_cast<u32>(mValue)));
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator>>(int shift) const FL_NOEXCEPT {
        return from_raw(mValue >> shift);
    }
 
    // ---- Scalar multiply (no fixed-point shift) ----------------------------
 
    constexpr FASTLED_FORCE_INLINE s16x16 operator*(i32 scalar) const FL_NOEXCEPT {
        return from_raw(mValue * scalar);
    }
 
    friend constexpr s16x16 operator*(i32 scalar, s16x16 fp) FL_NOEXCEPT {
        return s16x16::from_raw(scalar * fp.mValue);
    }
 
    // ---- Comparisons -------------------------------------------------------
 
    constexpr bool operator<(s16x16 b) const FL_NOEXCEPT { return mValue < b.mValue; }
    constexpr bool operator>(s16x16 b) const FL_NOEXCEPT { return mValue > b.mValue; }
    constexpr bool operator<=(s16x16 b) const FL_NOEXCEPT { return mValue <= b.mValue; }
    constexpr bool operator>=(s16x16 b) const FL_NOEXCEPT { return mValue >= b.mValue; }
    constexpr bool operator==(s16x16 b) const FL_NOEXCEPT { return mValue == b.mValue; }
    constexpr bool operator!=(s16x16 b) const FL_NOEXCEPT { return mValue != b.mValue; }
 
    // ---- Math ---------------------------------------------------------------
 
    static constexpr FASTLED_FORCE_INLINE s16x16 mod(s16x16 a, s16x16 b) FL_NOEXCEPT {
        return from_raw(a.mValue % b.mValue);
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 floor(s16x16 x) FL_NOEXCEPT {
        return from_raw(x.mValue & ~(i32((SCALE) - 1)));
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 ceil(s16x16 x) FL_NOEXCEPT {
        return from_raw((x.mValue & ~(i32((SCALE) - 1))) +
                        ((x.mValue & i32((SCALE) - 1)) ? (SCALE) : 0));
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 fract(s16x16 x) FL_NOEXCEPT {
        return from_raw(x.mValue & i32((SCALE) - 1));
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 abs(s16x16 x) FL_NOEXCEPT {
        return from_raw(x.mValue < 0 ? -x.mValue : x.mValue);
    }
 
    static constexpr FASTLED_FORCE_INLINE int sign(s16x16 x) FL_NOEXCEPT {
        return x.mValue > 0 ? 1 : (x.mValue < 0 ? -1 : 0);
    }
 
#if defined(__riscv) && (__riscv_xlen == 32)
    static FASTLED_FORCE_INLINE s16x16 lerp(s16x16 a, s16x16 b, s16x16 t) {
#else
    static constexpr FASTLED_FORCE_INLINE s16x16 lerp(s16x16 a, s16x16 b, s16x16 t) FL_NOEXCEPT {
#endif
        return a + (b - a) * t;
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 clamp(s16x16 x, s16x16 lo, s16x16 hi) FL_NOEXCEPT {
        return x < lo ? lo : (x > hi ? hi : x);
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 step(s16x16 edge, s16x16 x) FL_NOEXCEPT {
        return x < edge ? s16x16() : s16x16(1.0f);
    }
 
    static FASTLED_FORCE_INLINE s16x16 smoothstep(s16x16 edge0, s16x16 edge1, s16x16 x) FL_NOEXCEPT {
        constexpr s16x16 zero(0.0f);
        constexpr s16x16 one(1.0f);
        constexpr s16x16 two(2.0f);
        constexpr s16x16 three(3.0f);
        s16x16 t = clamp((x - edge0) / (edge1 - edge0), zero, one);
        return t * t * (three - two * t);
    }
 
    // ---- Inverse Trigonometry (pure fixed-point) ----------------------------
 
    static FASTLED_FORCE_INLINE s16x16 atan(s16x16 x) FL_NOEXCEPT {
        constexpr s16x16 one(1.0f);
        constexpr s16x16 pi_over_2(1.5707963f);
        bool neg = x.mValue < 0;
        s16x16 ax = abs(x);
        s16x16 result;
        if (ax <= one) {
            result = atan_unit(ax);
        } else {
            result = pi_over_2 - atan_unit(one / ax);
        }
        return neg ? -result : result;
    }
 
    static FASTLED_FORCE_INLINE s16x16 atan2(s16x16 y, s16x16 x) FL_NOEXCEPT {
        constexpr s16x16 pi(3.1415926f);
        constexpr s16x16 pi_over_2(1.5707963f);
        if (x.mValue == 0 && y.mValue == 0) return s16x16();
        if (x.mValue == 0) return y.mValue > 0 ? pi_over_2 : -pi_over_2;
        if (y.mValue == 0) return x.mValue > 0 ? s16x16() : pi;
        s16x16 ax = abs(x);
        s16x16 ay = abs(y);
        s16x16 a;
        if (ax >= ay) {
            a = atan_unit(ay / ax);
        } else {
            a = pi_over_2 - atan_unit(ax / ay);
        }
        if (x.mValue < 0) a = pi - a;
        if (y.mValue < 0) a = -a;
        return a;
    }
 
    static FASTLED_FORCE_INLINE s16x16 asin(s16x16 x) FL_NOEXCEPT {
        constexpr s16x16 one(1.0f);
        return atan2(x, sqrt(one - x * x));
    }
 
    static FASTLED_FORCE_INLINE s16x16 acos(s16x16 x) FL_NOEXCEPT {
        constexpr s16x16 one(1.0f);
        return atan2(sqrt(one - x * x), x);
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 sqrt(s16x16 x) FL_NOEXCEPT {
        return x.mValue <= 0 ? s16x16() : from_raw(static_cast<i32>(
            fl::isqrt64(static_cast<u64>(x.mValue) << FRAC_BITS)));
    }
 
    static constexpr FASTLED_FORCE_INLINE s16x16 rsqrt(s16x16 x) FL_NOEXCEPT {
        return sqrt(x).mValue == 0
            ? s16x16()
            : from_raw(SCALE) / sqrt(x);
    }
 
    static FASTLED_FORCE_INLINE s16x16 pow(s16x16 base, s16x16 exp) FL_NOEXCEPT {
        if (base.mValue <= 0) return s16x16();
        constexpr s16x16 one(1.0f);
        if (exp.mValue == 0) return one;
        if (base == one) return one;
        // Snap base values within ~2 ULPs of 1.0 to exactly 1.0 to dodge the
        // log2(1+t) minimax polynomial's upper-endpoint residual (#2969).
        constexpr i32 kOneRaw = static_cast<i32>(SCALE);
        if (base.mValue >= (kOneRaw - 2) && base.mValue <= kOneRaw) {
            return one;
        }
        return exp2_fp(exp * log2_fp(base));
    }
 
    // ---- Member function versions (operate on *this) -----------------------
 
    constexpr FASTLED_FORCE_INLINE s16x16 floor() const FL_NOEXCEPT {
        return floor(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 ceil() const FL_NOEXCEPT {
        return ceil(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 fract() const FL_NOEXCEPT {
        return fract(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 abs() const FL_NOEXCEPT {
        return abs(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE int sign() const FL_NOEXCEPT {
        return sign(*this);
    }
 
    FASTLED_FORCE_INLINE s16x16 sin() const FL_NOEXCEPT {
        return sin(*this);
    }
 
    FASTLED_FORCE_INLINE s16x16 cos() const FL_NOEXCEPT {
        return cos(*this);
    }
 
    FASTLED_FORCE_INLINE s16x16 atan() const FL_NOEXCEPT {
        return atan(*this);
    }
 
    FASTLED_FORCE_INLINE s16x16 asin() const FL_NOEXCEPT {
        return asin(*this);
    }
 
    FASTLED_FORCE_INLINE s16x16 acos() const FL_NOEXCEPT {
        return acos(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 sqrt() const FL_NOEXCEPT {
        return sqrt(*this);
    }
 
    constexpr FASTLED_FORCE_INLINE s16x16 rsqrt() const FL_NOEXCEPT {
        return rsqrt(*this);
    }
 
    // ---- Trigonometry ------------------------------------------------------
 
    static FASTLED_FORCE_INLINE s16x16 sin(s16x16 angle) FL_NOEXCEPT {
        return from_raw(fl::sin32(angle_to_a24(angle)) >> 15);
    }
 
    static FASTLED_FORCE_INLINE s16x16 cos(s16x16 angle) FL_NOEXCEPT {
        return from_raw(fl::cos32(angle_to_a24(angle)) >> 15);
    }
 
    // Combined sin+cos from s16x16 radians. Output in s16x16 [-1, 1].
    // Uses sincos32 which shares angle decomposition for ~30% fewer ops.
    static FASTLED_FORCE_INLINE void sincos(s16x16 angle, s16x16 &out_sin,
                                            s16x16 &out_cos) FL_NOEXCEPT {
        u32 a24 = angle_to_a24(angle);
        fl::SinCos32 sc = fl::sincos32(a24);
        out_sin = from_raw(sc.sin_val >> 15);
        out_cos = from_raw(sc.cos_val >> 15);
    }
 
  private:
    i32 mValue = 0;
 
    // Returns 0-based position of highest set bit, or -1 if v==0.
    static constexpr FASTLED_FORCE_INLINE int highest_bit(u32 v) FL_NOEXCEPT {
        return v == 0 ? -1 : _highest_bit_step(v, 0);
    }
 
    static constexpr int _highest_bit_step(u32 v, int r) FL_NOEXCEPT {
        return (v & 0xFFFF0000u) ? _highest_bit_step(v >> 16, r + 16)
             : (v & 0x0000FF00u) ? _highest_bit_step(v >> 8,  r + 8)
             : (v & 0x000000F0u) ? _highest_bit_step(v >> 4,  r + 4)
             : (v & 0x0000000Cu) ? _highest_bit_step(v >> 2,  r + 2)
             : (v & 0x00000002u) ? r + 1
             : r;
    }
 
    // Fixed-point log base 2 for positive values.
    // Uses 4-term minimax polynomial for log2(1+t), t in [0,1).
    // Horner evaluation uses i64 intermediates (24 frac bits) to minimize
    // rounding error, then converts back to 16 frac bits.
    static FASTLED_FORCE_INLINE s16x16 log2_fp(s16x16 x) FL_NOEXCEPT {
        u32 val = static_cast<u32>(x.mValue);
        int msb = highest_bit(val);
        i32 int_part = msb - FRAC_BITS;
        i32 t;
        if (msb >= FRAC_BITS) {
            t = static_cast<i32>(
                (val >> (msb - FRAC_BITS)) - (SCALE));
        } else {
            t = static_cast<i32>(
                (val << (FRAC_BITS - msb)) - (SCALE));
        }
        // 4-term minimax coefficients for log2(1+t), t in [0,1).
        // Stored as i64 with 24 fractional bits. Max product ~2^49, fits i64.
        constexpr int IFRAC = 24;
        constexpr i64 c0 = 24189248LL;  // 1.44179 * 2^24
        constexpr i64 c1 = -11728384LL; // -0.69907 * 2^24
        constexpr i64 c2 = 6098176LL;   // 0.36348 * 2^24
        constexpr i64 c3 = -1788416LL;  // -0.10660 * 2^24
        // Extend t from 16 to 24 frac bits.
        i64 t24 = static_cast<i64>(t) << (IFRAC - FRAC_BITS);
        // Horner: t * (c0 + t * (c1 + t * (c2 + t * c3)))
        i64 acc = c3;
        acc = c2 + ((acc * t24) >> IFRAC);
        acc = c1 + ((acc * t24) >> IFRAC);
        acc = c0 + ((acc * t24) >> IFRAC);
        i64 frac_part = (acc * t24) >> IFRAC;
        // Convert from 24 frac bits back to 16.
        i32 frac16 = static_cast<i32>(frac_part >> (IFRAC - FRAC_BITS));
        return from_raw(static_cast<i32>(static_cast<u32>(int_part) << FRAC_BITS) + frac16);
    }
 
    // Fixed-point 2^x. Uses 4-term minimax polynomial for 2^t, t in [0,1).
    // Horner evaluation uses i64 intermediates (24 frac bits) to minimize
    // rounding error, then converts back to 16 frac bits.
    static FASTLED_FORCE_INLINE s16x16 exp2_fp(s16x16 x) FL_NOEXCEPT {
        s16x16 fl_val = floor(x);
        s16x16 fr = x - fl_val;
        i32 n = fl_val.mValue >> FRAC_BITS;
        if (n >= INT_BITS - 1) return from_raw(0x7FFFFFFF);
        if (n < -FRAC_BITS) return s16x16();
        i32 int_pow;
        if (n >= 0) {
            int_pow = static_cast<i32>(SCALE) << n;
        } else {
            int_pow = static_cast<i32>(SCALE) >> (-n);
        }
        // 4-term minimax coefficients for 2^t - 1, t in [0,1).
        // Stored as i64 with 24 fractional bits. Max product ~2^48, fits i64.
        constexpr int IFRAC = 24;
        constexpr i64 d0 = 11629376LL;  // 0.69316 * 2^24
        constexpr i64 d1 = 4038400LL;   // 0.24071 * 2^24
        constexpr i64 d2 = 895232LL;    // 0.05336 * 2^24
        constexpr i64 d3 = 214016LL;    // 0.01276 * 2^24
        // Extend fr from 16 to 24 frac bits.
        i64 fr24 = static_cast<i64>(fr.mValue) << (IFRAC - FRAC_BITS);
        // Horner: 1 + fr * (d0 + fr * (d1 + fr * (d2 + fr * d3)))
        i64 acc = d3;
        acc = d2 + ((acc * fr24) >> IFRAC);
        acc = d1 + ((acc * fr24) >> IFRAC);
        acc = d0 + ((acc * fr24) >> IFRAC);
        constexpr i64 one24 = 1LL << IFRAC;
        i64 frac_pow24 = one24 + ((acc * fr24) >> IFRAC);
        // Convert from 24 frac bits to 16 frac bits, then scale by int_pow.
        i32 frac_pow16 = static_cast<i32>(frac_pow24 >> (IFRAC - FRAC_BITS));
        i64 result =
            (static_cast<i64>(int_pow) * frac_pow16) >> FRAC_BITS;
        return from_raw(static_cast<i32>(result));
    }
 
    // Converts s16x16 radians to sin32/cos32 input format.
    // 256/(2*PI) — converts radians to sin32/cos32 format.
    static constexpr i32 RAD_TO_24 = 2670177;
    static constexpr FASTLED_FORCE_INLINE u32 angle_to_a24(s16x16 angle) FL_NOEXCEPT {
        return static_cast<u32>(
            (static_cast<i64>(angle.mValue) * RAD_TO_24) >> FRAC_BITS);
    }
 
    // Polynomial atan for t in [0, 1]. Returns [0, π/4].
    // 7th-order minimax: atan(t) ≈ t * (c0 + t² * (c1 + t² * (c2 + t² * c3)))
    // Coefficients optimized via coordinate descent on s16x16 quantization grid.
    static FASTLED_FORCE_INLINE s16x16 atan_unit(s16x16 t) FL_NOEXCEPT {
        constexpr s16x16 c0(0.9998779297f);
        constexpr s16x16 c1(-0.3269348145f);
        constexpr s16x16 c2(0.1594085693f);
        constexpr s16x16 c3(-0.0472106934f);
        s16x16 t2 = t * t;
        return t * (c0 + t2 * (c1 + t2 * (c2 + t2 * c3)));
    }
};
 
} // namespace fl
 
FL_OPTIMIZATION_LEVEL_O3_END