Math.hh

Go to the documentation of this file.

#ifndef MATH_HH
#define MATH_HH
 
#include "narrow.hh"
#include <bit>
#include <cassert>
#include <climits>
#include <cmath>
#include <concepts>
#include <cstdint>
#include <numbers>
#include <span>
 
#ifdef _MSC_VER
#include <intrin.h>
#pragma intrinsic(_BitScanForward)
#endif
 
namespace Math {
 
inline constexpr double e    = std::numbers::e_v   <double>;
inline constexpr double ln2  = std::numbers::ln2_v <double>;
inline constexpr double ln10 = std::numbers::ln10_v<double>;
inline constexpr double pi   = std::numbers::pi_v  <double>;
 
[[nodiscard]] constexpr auto floodRight(std::unsigned_integral auto x) noexcept
{
        x |= x >> 1;
        x |= x >> 2;
        x |= x >> 4;
        x |= x >> ((sizeof(x) >= 2) ?  8 : 0); // Written in a weird way to
        x |= x >> ((sizeof(x) >= 4) ? 16 : 0); // suppress compiler warnings.
        x |= x >> ((sizeof(x) >= 8) ? 32 : 0); // Generates equally efficient
        return x;                              // code.
}
 
template<std::signed_integral T>
[[nodiscard]] inline int16_t clipToInt16(T x)
{
        static_assert((T(-1) >> 1) == T(-1), "right-shift must preserve sign");
        if (int16_t(x) == x) [[likely]] {
                return narrow_cast<int16_t>(x);
        } else {
                constexpr int SHIFT = (sizeof(T) * CHAR_BIT) - 1;
                return narrow_cast<int16_t>(0x7FFF - (x >> SHIFT));
        }
}
 
[[nodiscard]] inline uint8_t clipIntToByte(int x)
{
        static_assert((-1 >> 1) == -1, "right-shift must preserve sign");
        if (uint8_t(x) == x) [[likely]] {
                return narrow_cast<uint8_t>(x);
        } else {
                return narrow_cast<uint8_t>(~(x >> 31));
        }
}
 
[[nodiscard]] constexpr unsigned reverseNBits(unsigned x, unsigned bits)
{
        unsigned ret = 0;
        while (bits--) {
                ret = (ret << 1) | (x & 1);
                x >>= 1;
        }
        return ret;
 
        /* Just for fun I tried the asm version below (the carry-flag trick
         * cannot be described in plain C). It's correct and generates shorter
         * code (both less instructions and less bytes). But it doesn't
         * actually run faster on the machine I tested on, or only a tiny bit
         * (possibly because of dependency chains and processor stalls???).
         * However a big disadvantage of this asm version is that when called
         * with compile-time constant arguments, this version performs exactly
         * the same, while the version above can be further optimized by the
         * compiler (constant-propagation, loop unrolling, ...).
        unsigned ret = 0;
        if (bits) {
                asm (
                "1:     shr     %[VAL]\n"
                "       adc     %[RET],%[RET]\n"
                "       dec     %[BITS]\n"
                "       jne     1b\n"
                        : [VAL]  "+r" (val)
                        , [BITS] "+r" (bits)
                        , [RET]  "+r" (ret)
                );
        }
        return ret;
        */
 
        /* Maarten suggested the following approach with O(lg(N)) time
         * complexity (the version above is O(N)).
         *  - reverse full (32-bit) word: O(lg(N))
         *  - shift right over 32-N bits: O(1)
         * Note: In some lower end CPU the shift-over-N-bits instruction itself
         *       is O(N), in that case this whole algorithm is O(N)
         * Note2: Instead of '32' it's also possible to use a lower power of 2,
         *        as long as it's bigger than or equal to N.
         * This algorithm may or may not be faster than the version above, I
         * didn't try it yet. Also because this routine is _NOT_ performance
         * critical _AT_ALL_ currently.
         */
}
 
[[nodiscard]] constexpr uint8_t reverseByte(uint8_t a)
{
        // Classical implementation (can be extended to 16 and 32 bits)
        //   a = ((a & 0xF0) >> 4) | ((a & 0x0F) << 4);
        //   a = ((a & 0xCC) >> 2) | ((a & 0x33) << 2);
        //   a = ((a & 0xAA) >> 1) | ((a & 0x55) << 1);
        //   return a;
 
        // The versions below are specific to reverse a single byte (can't
        // easily be extended to wider types). Found these tricks on:
        //    http://graphics.stanford.edu/~seander/bithacks.html
#ifdef __x86_64
        // on 64-bit systems this is slightly faster
        return narrow_cast<uint8_t>((((a * 0x80200802ULL) & 0x0884422110ULL) * 0x0101010101ULL) >> 32);
#else
        // on 32-bit systems this is faster
        return narrow_cast<uint8_t>((((a * 0x0802 & 0x22110) | (a * 0x8020 & 0x88440)) * 0x10101) >> 16);
#endif
}
 
[[nodiscard]] inline /*constexpr*/ unsigned findFirstSet(uint32_t x)
{
        return x ? std::countr_zero(x) + 1 : 0;
}
 
// Cubic Hermite Interpolation:
//   Given 4 points: (-1, y[0]), (0, y[1]), (1, y[2]), (2, y[3])
//   Fit a polynomial:  f(x) = a*x^3 + b*x^2 + c*x + d
//     which passes through the given points at x=0 and x=1
//       f(0) = y[0]
//       f(1) = y[1]
//     and which has specific derivatives at x=0 and x=1
//       f'(0) = (y[1] - y[-1]) / 2
//       f'(1) = (y[2] - y[ 0]) / 2
//   Then evaluate this polynomial at the given x-position (x in [0, 1]).
// For more details see:
//   https://en.wikipedia.org/wiki/Cubic_Hermite_spline
//   https://www.paulinternet.nl/?page=bicubic
[[nodiscard]] constexpr float cubicHermite(std::span<const float, 4> y, float x)
{
        assert(0.0f <= x); assert(x <= 1.0f);
        float a = -0.5f*y[0] + 1.5f*y[1] - 1.5f*y[2] + 0.5f*y[3];
        float b =       y[0] - 2.5f*y[1] + 2.0f*y[2] - 0.5f*y[3];
        float c = -0.5f*y[0]             + 0.5f*y[2];
        float d =                   y[1];
        float x2 = x * x;
        float x3 = x * x2;
        return a*x3 + b*x2 + c*x + d;
}
 
struct QuotientRemainder {
    int quotient;
    int remainder;
};
constexpr QuotientRemainder div_mod_floor(int dividend, int divisor) {
    int q = dividend / divisor;
    int r = dividend % divisor;
    if ((r != 0) && ((r < 0) != (divisor < 0))) {
        --q;
        r += divisor;
    }
    return {q, r};
}
constexpr int div_floor(int dividend, int divisor) {
    return div_mod_floor(dividend, divisor).quotient;
}
constexpr int mod_floor(int dividend, int divisor) {
    return div_mod_floor(dividend, divisor).remainder;
}
 
} // namespace Math
 
#endif // MATH_HH