src/utils/Math.hh at main · doomsdayone/openmsx

◆ main openmsx / src / utils / Math.hh

8.4 KiB · C++ · last changed chore(): initial commit 1 month ago source blame history raw

#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>;

/** GLSL fract() function,  https://registry.khronos.org/OpenGL-Refpages/gl4/html/fract.xhtml
 * Return the fractional part of the input, result is in [0, 1).
 */
template <std::floating_point T>
[[nodiscard]] constexpr T fract(T x) noexcept
{
	return x - std::floor(x);
}

/** Returns the smallest number of the form 2^n-1 that is greater or equal
  * to the given number.
  * The resulting number has the same number of leading zeros as the input,
  * but starting from the first 1-bit in the input all bits more to the right
  * are also 1.
  */
[[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.
}

/** Clip x to range [-32768,32767].
  * Optimized for the case when no clipping is needed.
  */
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));
	}
}

/** Clip x to range [0,255].
  * Optimized for the case when no clipping is needed.
  */
[[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));
	}
}

/** Reverse the lower N bits of a given value.
  * The upper 32-N bits from the input are ignored and will be returned as 0.
  * For example reverseNBits('xxxabcde', 5) returns '000edcba' (binary notation).
  */
[[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.
	 */
}

/** Reverse the bits in a byte.
  * This is equivalent to (but faster than) reverseNBits(x, 8);
  */
[[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
}

/** Find the least significant bit that is set.
  * @return 0 if the input is zero (no bits are set),
  *   otherwise the index of the first set bit + 1.
  */
[[nodiscard]] 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;
}

/** Divide one integer by another, rounding towards minus infinity.
  * The normal C/C++ division rounds towards zero.
  * Based on this article:
  *   http://www.microhowto.info/howto/round_towards_minus_infinity_when_dividing_integers_in_c_or_c++.html
  */
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 {.quotient = q, .remainder = 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;
}

/**
 * RAII utility to temporarily disable denormal floating-point handling.
 *
 * Denormal numbers (very small values near zero) can cause 10-100x performance
 * degradation on x86/x64 CPUs. This guard flushes denormals to zero within its
 * scope, trading slight accuracy loss for significant speedup in floating-point
 * intensive code.
 *
 * Only active on x86/x64 platforms with SSE support. No-op on other architectures.
 *
 * Usage: DenormalGuard guard; // at start of performance-critical scope
 */
class DenormalGuard {
private:
#if (defined(__SSE__) && (defined(__GNUC__) || defined(__clang__)))
    // MXCSR bit positions
    static constexpr int DAZ_BIT = 6;   // Denormals-Are-Zero
    static constexpr int FTZ_BIT = 15;  // Flush-To-Zero

    uint32_t saved_mxcsr;
#endif

public:
    [[nodiscard]] DenormalGuard() {
#if (defined(__SSE__) && (defined(__GNUC__) || defined(__clang__)))
        // Read current MXCSR
        asm volatile("stmxcsr %0" : "=m"(saved_mxcsr));

        // Modify and write back
        auto new_mxcsr = saved_mxcsr | (1u << DAZ_BIT) | (1u << FTZ_BIT);
        asm volatile("ldmxcsr %0" : : "m"(new_mxcsr));
#endif
    }

    ~DenormalGuard() {
#if (defined(__SSE__) && (defined(__GNUC__) || defined(__clang__)))
        asm volatile("ldmxcsr %0" : : "m"(saved_mxcsr));
#endif
    }

    DenormalGuard(const DenormalGuard&) = delete;
    DenormalGuard& operator=(const DenormalGuard&) = delete;
    DenormalGuard(DenormalGuard&&) = delete;
    DenormalGuard& operator=(DenormalGuard&&) = delete;
};

} // namespace Math

#endif // MATH_HH

`/`	Search
`g` `c`	Code
`g` `h`	History
`g` `b`	Bookmarks
`g` `t`	Trust
`y`	Permalink to this state (files and trees)
`?`	This sheet

e8555c89 Andy McCall 1 month ago	1	#ifndef MATH_HH
	2	#define MATH_HH
	3
	4	#include "narrow.hh"
	5	#include <bit>
	6	#include <cassert>
	7	#include <climits>
	8	#include <cmath>
	9	#include <concepts>
	10	#include <cstdint>
	11	#include <numbers>
	12	#include <span>
	13
	14	#ifdef _MSC_VER
	15	#include <intrin.h>
	16	#pragma intrinsic(_BitScanForward)
	17	#endif
	18
	19	namespace Math {
	20
	21	inline constexpr double e = std::numbers::e_v <double>;
	22	inline constexpr double ln2 = std::numbers::ln2_v <double>;
	23	inline constexpr double ln10 = std::numbers::ln10_v<double>;
	24	inline constexpr double pi = std::numbers::pi_v <double>;
	25
	26	/** GLSL fract() function, https://registry.khronos.org/OpenGL-Refpages/gl4/html/fract.xhtml
	27	* Return the fractional part of the input, result is in [0, 1).
	28	*/
	29	template <std::floating_point T>
	30	[[nodiscard]] constexpr T fract(T x) noexcept
	31	{
	32	return x - std::floor(x);
	33	}
	34
	35	/** Returns the smallest number of the form 2^n-1 that is greater or equal
	36	* to the given number.
	37	* The resulting number has the same number of leading zeros as the input,
	38	* but starting from the first 1-bit in the input all bits more to the right
	39	* are also 1.
	40	*/
	41	[[nodiscard]] constexpr auto floodRight(std::unsigned_integral auto x) noexcept
	42	{
	43	x \|= x >> 1;
	44	x \|= x >> 2;
	45	x \|= x >> 4;
	46	x \|= x >> ((sizeof(x) >= 2) ? 8 : 0); // Written in a weird way to
	47	x \|= x >> ((sizeof(x) >= 4) ? 16 : 0); // suppress compiler warnings.
	48	x \|= x >> ((sizeof(x) >= 8) ? 32 : 0); // Generates equally efficient
	49	return x; // code.
	50	}
	51
	52	/** Clip x to range [-32768,32767].
	53	* Optimized for the case when no clipping is needed.
	54	*/
	55	template<std::signed_integral T>
	56	[[nodiscard]] inline int16_t clipToInt16(T x)
	57	{
	58	static_assert((T(-1) >> 1) == T(-1), "right-shift must preserve sign");
	59	if (int16_t(x) == x) [[likely]] {
	60	return narrow_cast<int16_t>(x);
	61	} else {
	62	constexpr int SHIFT = (sizeof(T) * CHAR_BIT) - 1;
	63	return narrow_cast<int16_t>(0x7FFF - (x >> SHIFT));
	64	}
65	}
66
67	/** Clip x to range [0,255].
68	* Optimized for the case when no clipping is needed.
69	*/
70	[[nodiscard]] inline uint8_t clipIntToByte(int x)
71	{
72	static_assert((-1 >> 1) == -1, "right-shift must preserve sign");
73	if (uint8_t(x) == x) [[likely]] {
74	return narrow_cast<uint8_t>(x);
75	} else {
76	return narrow_cast<uint8_t>(~(x >> 31));
77	}
78	}
79
80	/** Reverse the lower N bits of a given value.
81	* The upper 32-N bits from the input are ignored and will be returned as 0.
82	* For example reverseNBits('xxxabcde', 5) returns '000edcba' (binary notation).
83	*/
84	[[nodiscard]] constexpr unsigned reverseNBits(unsigned x, unsigned bits)
85	{
86	unsigned ret = 0;
87	while (bits--) {
88	ret = (ret << 1) \| (x & 1);
89	x >>= 1;
90	}
91	return ret;
92
93	/* Just for fun I tried the asm version below (the carry-flag trick
94	* cannot be described in plain C). It's correct and generates shorter
95	* code (both less instructions and less bytes). But it doesn't
96	* actually run faster on the machine I tested on, or only a tiny bit
97	* (possibly because of dependency chains and processor stalls???).
98	* However a big disadvantage of this asm version is that when called
99	* with compile-time constant arguments, this version performs exactly
100	* the same, while the version above can be further optimized by the
101	* compiler (constant-propagation, loop unrolling, ...).
102	unsigned ret = 0;
103	if (bits) {
104	asm (
105	"1: shr %[VAL]\n"
106	" adc %[RET],%[RET]\n"
107	" dec %[BITS]\n"
108	" jne 1b\n"
109	: [VAL] "+r" (val)
110	, [BITS] "+r" (bits)
111	, [RET] "+r" (ret)
112	);
113	}
114	return ret;
115	*/
116
117	/* Maarten suggested the following approach with O(lg(N)) time
118	* complexity (the version above is O(N)).
119	* - reverse full (32-bit) word: O(lg(N))
120	* - shift right over 32-N bits: O(1)
121	* Note: In some lower end CPU the shift-over-N-bits instruction itself
122	* is O(N), in that case this whole algorithm is O(N)
123	* Note2: Instead of '32' it's also possible to use a lower power of 2,
124	* as long as it's bigger than or equal to N.
125	* This algorithm may or may not be faster than the version above, I
126	* didn't try it yet. Also because this routine is _NOT_ performance
127	* critical _AT_ALL_ currently.
128	*/
129	}
130
131	/** Reverse the bits in a byte.
132	* This is equivalent to (but faster than) reverseNBits(x, 8);
133	*/
134	[[nodiscard]] constexpr uint8_t reverseByte(uint8_t a)
135	{
136	// Classical implementation (can be extended to 16 and 32 bits)
137	// a = ((a & 0xF0) >> 4) \| ((a & 0x0F) << 4);
138	// a = ((a & 0xCC) >> 2) \| ((a & 0x33) << 2);
139	// a = ((a & 0xAA) >> 1) \| ((a & 0x55) << 1);
140	// return a;
141
142	// The versions below are specific to reverse a single byte (can't
143	// easily be extended to wider types). Found these tricks on:
144	// http://graphics.stanford.edu/~seander/bithacks.html
145	#ifdef __x86_64
146	// on 64-bit systems this is slightly faster
147	return narrow_cast<uint8_t>((((a * 0x80200802ULL) & 0x0884422110ULL) * 0x0101010101ULL) >> 32);
148	#else
149	// on 32-bit systems this is faster
150	return narrow_cast<uint8_t>((((a * 0x0802 & 0x22110) \| (a * 0x8020 & 0x88440)) * 0x10101) >> 16);
151	#endif
152	}
153
154	/** Find the least significant bit that is set.
155	* @return 0 if the input is zero (no bits are set),
156	* otherwise the index of the first set bit + 1.
157	*/
158	[[nodiscard]] constexpr unsigned findFirstSet(uint32_t x)
159	{
160	return x ? std::countr_zero(x) + 1 : 0;
161	}
162
163	// Cubic Hermite Interpolation:
164	// Given 4 points: (-1, y[0]), (0, y[1]), (1, y[2]), (2, y[3])
165	// Fit a polynomial: f(x) = ax^3 + bx^2 + c*x + d
166	// which passes through the given points at x=0 and x=1
167	// f(0) = y[0]
168	// f(1) = y[1]
169	// and which has specific derivatives at x=0 and x=1
170	// f'(0) = (y[1] - y[-1]) / 2
171	// f'(1) = (y[2] - y[ 0]) / 2
172	// Then evaluate this polynomial at the given x-position (x in [0, 1]).
173	// For more details see:
174	// https://en.wikipedia.org/wiki/Cubic_Hermite_spline
175	// https://www.paulinternet.nl/?page=bicubic
176	[[nodiscard]] constexpr float cubicHermite(std::span<const float, 4> y, float x)
177	{
178	assert(0.0f <= x); assert(x <= 1.0f);
179	float a = -0.5fy[0] + 1.5fy[1] - 1.5fy[2] + 0.5fy[3];
180	float b = y[0] - 2.5fy[1] + 2.0fy[2] - 0.5f*y[3];
181	float c = -0.5fy[0] + 0.5fy[2];
182	float d = y[1];
183	float x2 = x * x;
184	float x3 = x * x2;
185	return ax3 + bx2 + c*x + d;
186	}
187
188	/** Divide one integer by another, rounding towards minus infinity.
189	* The normal C/C++ division rounds towards zero.
190	* Based on this article:
191	* http://www.microhowto.info/howto/round_towards_minus_infinity_when_dividing_integers_in_c_or_c++.html
192	*/
193	struct QuotientRemainder {
194	int quotient;
195	int remainder;
196	};
197	constexpr QuotientRemainder div_mod_floor(int dividend, int divisor) {
198	int q = dividend / divisor;
199	int r = dividend % divisor;
200	if ((r != 0) && ((r < 0) != (divisor < 0))) {
201	--q;
202	r += divisor;
203	}
204	return {.quotient = q, .remainder = r};
205	}
206	constexpr int div_floor(int dividend, int divisor) {
207	return div_mod_floor(dividend, divisor).quotient;
208	}
209	constexpr int mod_floor(int dividend, int divisor) {
210	return div_mod_floor(dividend, divisor).remainder;
211	}
212
213	/**
214	* RAII utility to temporarily disable denormal floating-point handling.
215	*
216	* Denormal numbers (very small values near zero) can cause 10-100x performance
217	* degradation on x86/x64 CPUs. This guard flushes denormals to zero within its
218	* scope, trading slight accuracy loss for significant speedup in floating-point
219	* intensive code.
220	*
221	* Only active on x86/x64 platforms with SSE support. No-op on other architectures.
222	*
223	* Usage: DenormalGuard guard; // at start of performance-critical scope
224	*/
225	class DenormalGuard {
226	private:
227	#if (defined(__SSE__) && (defined(__GNUC__) \|\| defined(__clang__)))
228	// MXCSR bit positions
229	static constexpr int DAZ_BIT = 6; // Denormals-Are-Zero
230	static constexpr int FTZ_BIT = 15; // Flush-To-Zero
231
232	uint32_t saved_mxcsr;
233	#endif
234
235	public:
236	[[nodiscard]] DenormalGuard() {
237	#if (defined(__SSE__) && (defined(__GNUC__) \|\| defined(__clang__)))
238	// Read current MXCSR
239	asm volatile("stmxcsr %0" : "=m"(saved_mxcsr));
240
241	// Modify and write back
242	auto new_mxcsr = saved_mxcsr \| (1u << DAZ_BIT) \| (1u << FTZ_BIT);
243	asm volatile("ldmxcsr %0" : : "m"(new_mxcsr));
244	#endif
245	}
246
247	~DenormalGuard() {
248	#if (defined(__SSE__) && (defined(__GNUC__) \|\| defined(__clang__)))
249	asm volatile("ldmxcsr %0" : : "m"(saved_mxcsr));
250	#endif
251	}
252
253	DenormalGuard(const DenormalGuard&) = delete;
254	DenormalGuard& operator=(const DenormalGuard&) = delete;
255	DenormalGuard(DenormalGuard&&) = delete;
256	DenormalGuard& operator=(DenormalGuard&&) = delete;
257	};
258
259	} // namespace Math
260
261	#endif // MATH_HH

Keyboard