14 #ifndef WPIUTIL_WPI_MATHEXTRAS_H
15 #define WPIUTIL_WPI_MATHEXTRAS_H
17 #include "wpi/Compiler.h"
25 #include <type_traits>
32 unsigned char _BitScanForward(
unsigned long *_Index,
unsigned long _Mask);
33 unsigned char _BitScanForward64(
unsigned long *_Index,
unsigned __int64 _Mask);
34 unsigned char _BitScanReverse(
unsigned long *_Index,
unsigned long _Mask);
35 unsigned char _BitScanReverse64(
unsigned long *_Index,
unsigned __int64 _Mask);
54 return std::numeric_limits<T>::digits;
59 std::size_t ZeroBits = 0;
60 T Shift = std::numeric_limits<T>::digits >> 1;
61 T Mask = (std::numeric_limits<T>::max)() >> Shift;
63 if ((Val & Mask) == 0) {
74 #if __GNUC__ >= 4 || defined(_MSC_VER)
80 #if __has_builtin(__builtin_ctz) || LLVM_GNUC_PREREQ(4, 0, 0)
81 return __builtin_ctz(Val);
82 #elif defined(_MSC_VER)
84 _BitScanForward(&Index, Val);
90 #if !defined(_MSC_VER) || defined(_M_X64)
91 template <
typename T>
struct TrailingZerosCounter<T, 8> {
96 #if __has_builtin(__builtin_ctzll) || LLVM_GNUC_PREREQ(4, 0, 0)
97 return __builtin_ctzll(Val);
98 #elif defined(_MSC_VER)
100 _BitScanForward64(&Index, Val);
116 template <
typename T>
118 static_assert(std::numeric_limits<T>::is_integer &&
119 !std::numeric_limits<T>::is_signed,
120 "Only unsigned integral types are allowed.");
128 return std::numeric_limits<T>::digits;
131 std::size_t ZeroBits = 0;
132 for (T Shift = std::numeric_limits<T>::digits >> 1; Shift; Shift >>= 1) {
133 T Tmp = Val >> Shift;
143 #if __GNUC__ >= 4 || defined(_MSC_VER)
149 #if __has_builtin(__builtin_clz) || LLVM_GNUC_PREREQ(4, 0, 0)
150 return __builtin_clz(Val);
151 #elif defined(_MSC_VER)
153 _BitScanReverse(&Index, Val);
159 #if !defined(_MSC_VER) || defined(_M_X64)
160 template <
typename T>
struct LeadingZerosCounter<T, 8> {
165 #if __has_builtin(__builtin_clzll) || LLVM_GNUC_PREREQ(4, 0, 0)
166 return __builtin_clzll(Val);
167 #elif defined(_MSC_VER)
169 _BitScanReverse64(&Index, Val);
185 template <
typename T>
187 static_assert(std::numeric_limits<T>::is_integer &&
188 !std::numeric_limits<T>::is_signed,
189 "Only unsigned integral types are allowed.");
201 if (ZB ==
ZB_Max && Val == 0)
202 return (std::numeric_limits<T>::max)();
210 static_assert(std::is_unsigned<T>::value,
"Invalid type!");
211 const unsigned Bits = CHAR_BIT *
sizeof(T);
212 assert(N <= Bits &&
"Invalid bit index");
213 return N == 0 ? 0 : (T(-1) >> (Bits - N));
219 return ~maskTrailingOnes<T>(CHAR_BIT *
sizeof(T) - N);
225 return maskLeadingOnes<T>(CHAR_BIT *
sizeof(T) - N);
231 return maskTrailingOnes<T>(CHAR_BIT *
sizeof(T) - N);
242 if (ZB ==
ZB_Max && Val == 0)
243 return (std::numeric_limits<T>::max)();
248 (std::numeric_limits<T>::digits - 1);
255 #define R2(n) n, n + 2 * 64, n + 1 * 64, n + 3 * 64
256 #define R4(n) R2(n), R2(n + 2 * 16), R2(n + 1 * 16), R2(n + 3 * 16)
257 #define R6(n) R4(n), R4(n + 2 * 4), R4(n + 1 * 4), R4(n + 3 * 4)
258 R6(0), R6(2), R6(1), R6(3)
265 template <
typename T>
267 unsigned char in[
sizeof(Val)];
268 unsigned char out[
sizeof(Val)];
269 std::memcpy(in, &Val,
sizeof(Val));
270 for (
unsigned i = 0; i <
sizeof(Val); ++i)
272 std::memcpy(&Val, out,
sizeof(Val));
281 constexpr
inline uint32_t
Hi_32(uint64_t Value) {
282 return static_cast<uint32_t>(Value >> 32);
286 constexpr
inline uint32_t
Lo_32(uint64_t Value) {
287 return static_cast<uint32_t>(Value);
291 constexpr
inline uint64_t
Make_64(uint32_t High, uint32_t Low) {
292 return ((uint64_t)High << 32) | (uint64_t)Low;
296 template <
unsigned N> constexpr
inline bool isInt(int64_t x) {
297 return N >= 64 || (-(INT64_C(1)<<(N-1)) <= x && x < (INT64_C(1)<<(N-1)));
300 template <> constexpr
inline bool isInt<8>(int64_t x) {
301 return static_cast<int8_t>(x) == x;
303 template <> constexpr
inline bool isInt<16>(int64_t x) {
304 return static_cast<int16_t>(x) == x;
306 template <> constexpr
inline bool isInt<32>(int64_t x) {
307 return static_cast<int32_t>(x) == x;
311 template <
unsigned N,
unsigned S>
314 N > 0,
"isShiftedInt<0> doesn't make sense (refers to a 0-bit number.");
315 static_assert(N + S <= 64,
"isShiftedInt<N, S> with N + S > 64 is too wide.");
316 return isInt<N + S>(x) && (x % (UINT64_C(1) << S) == 0);
327 template <
unsigned N>
328 constexpr
inline typename std::enable_if<(N < 64), bool>::type
330 static_assert(N > 0,
"isUInt<0> doesn't make sense");
331 return X < (UINT64_C(1) << (N));
333 template <
unsigned N>
334 constexpr
inline typename std::enable_if<N >= 64,
bool>::type
340 template <> constexpr
inline bool isUInt<8>(uint64_t x) {
341 return static_cast<uint8_t>(x) == x;
343 template <> constexpr
inline bool isUInt<16>(uint64_t x) {
344 return static_cast<uint16_t>(x) == x;
346 template <> constexpr
inline bool isUInt<32>(uint64_t x) {
347 return static_cast<uint32_t>(x) == x;
351 template <
unsigned N,
unsigned S>
354 N > 0,
"isShiftedUInt<0> doesn't make sense (refers to a 0-bit number)");
355 static_assert(N + S <= 64,
356 "isShiftedUInt<N, S> with N + S > 64 is too wide.");
359 return isUInt<N + S>(x) && (x % (UINT64_C(1) << S) == 0);
364 assert(N > 0 && N <= 64 &&
"integer width out of range");
370 return UINT64_MAX >> (64 - N);
374 #pragma warning(push)
375 #pragma warning(disable : 4146)
380 assert(N > 0 && N <= 64 &&
"integer width out of range");
382 return -(UINT64_C(1)<<(N-1));
391 assert(N > 0 && N <= 64 &&
"integer width out of range");
395 return (UINT64_C(1) << (N - 1)) - 1;
404 inline bool isIntN(
unsigned N, int64_t x) {
412 return Value && ((Value + 1) & Value) == 0;
418 return Value && ((Value + 1) & Value) == 0;
424 return Value &&
isMask_32((Value - 1) | Value);
430 return Value &&
isMask_64((Value - 1) | Value);
436 return Value && !(Value & (Value - 1));
441 return Value && !(Value & (Value - 1));
452 template <
typename T>
454 static_assert(std::numeric_limits<T>::is_integer &&
455 !std::numeric_limits<T>::is_signed,
456 "Only unsigned integral types are allowed.");
457 return countLeadingZeros<T>(~Value, ZB);
468 template <
typename T>
470 static_assert(std::numeric_limits<T>::is_integer &&
471 !std::numeric_limits<T>::is_signed,
472 "Only unsigned integral types are allowed.");
473 return countTrailingZeros<T>(~Value, ZB);
478 static unsigned count(T Value) {
480 static_assert(SizeOfT <= 4,
"Not implemented!");
482 return __builtin_popcount(Value);
485 v = v - ((v >> 1) & 0x55555555);
486 v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
487 return ((v + (v >> 4) & 0xF0F0F0F) * 0x1010101) >> 24;
493 static unsigned count(T Value) {
495 return __builtin_popcountll(Value);
498 v = v - ((v >> 1) & 0x5555555555555555ULL);
499 v = (v & 0x3333333333333333ULL) + ((v >> 2) & 0x3333333333333333ULL);
500 v = (v + (v >> 4)) & 0x0F0F0F0F0F0F0F0FULL;
501 return unsigned((uint64_t)(v * 0x0101010101010101ULL) >> 56);
510 template <
typename T>
512 static_assert(std::numeric_limits<T>::is_integer &&
513 !std::numeric_limits<T>::is_signed,
514 "Only unsigned integral types are allowed.");
519 inline double Log2(
double Value) {
520 #if defined(__ANDROID_API__) && __ANDROID_API__ < 18
521 return __builtin_log(Value) / __builtin_log(2.0);
523 return std::log2(Value);
566 static_assert(
sizeof(uint64_t) ==
sizeof(
double),
"Unexpected type sizes");
567 memcpy(&D, &Bits,
sizeof(Bits));
574 static_assert(
sizeof(uint32_t) ==
sizeof(
float),
"Unexpected type sizes");
575 memcpy(&F, &Bits,
sizeof(Bits));
584 static_assert(
sizeof(uint64_t) ==
sizeof(
double),
"Unexpected type sizes");
585 memcpy(&Bits, &Double,
sizeof(Double));
594 static_assert(
sizeof(uint32_t) ==
sizeof(
float),
"Unexpected type sizes");
595 memcpy(&Bits, &Float,
sizeof(Float));
601 constexpr
inline uint64_t
MinAlign(uint64_t A, uint64_t B) {
607 return (A | B) & (1 + ~(A | B));
614 inline uintptr_t
alignAddr(
const void *Addr,
size_t Alignment) {
616 "Alignment is not a power of two!");
618 assert((uintptr_t)Addr + Alignment - 1 >= (uintptr_t)Addr);
620 return (((uintptr_t)Addr + Alignment - 1) & ~(uintptr_t)(Alignment - 1));
626 return alignAddr(Ptr, Alignment) - (uintptr_t)Ptr;
676 inline uint64_t
alignTo(uint64_t Value, uint64_t Align, uint64_t Skew = 0) {
677 assert(Align != 0u &&
"Align can't be 0.");
679 return (Value + Align - 1 - Skew) / Align * Align + Skew;
684 template <u
int64_t Align> constexpr
inline uint64_t
alignTo(uint64_t Value) {
685 static_assert(Align != 0u,
"Align must be non-zero");
686 return (Value + Align - 1) / Align * Align;
690 inline uint64_t
divideCeil(uint64_t Numerator, uint64_t Denominator) {
691 return alignTo(Numerator, Denominator) / Denominator;
698 template <u
int64_t Align>
700 static_assert(Align != 0u,
"Align must be non-zero");
701 template <u
int64_t Value>
703 static const uint64_t value = (Value + Align - 1) / Align * Align;
709 inline uint64_t
alignDown(uint64_t Value, uint64_t Align, uint64_t Skew = 0) {
710 assert(Align != 0u &&
"Align can't be 0.");
712 return (Value - Skew) / Align * Align + Skew;
719 return alignTo(Value, Align) - Value;
724 template <
unsigned B> constexpr
inline int32_t
SignExtend32(uint32_t X) {
725 static_assert(B > 0,
"Bit width can't be 0.");
726 static_assert(B <= 32,
"Bit width out of range.");
727 return int32_t(X << (32 - B)) >> (32 - B);
733 assert(B > 0 &&
"Bit width can't be 0.");
734 assert(B <= 32 &&
"Bit width out of range.");
735 return int32_t(X << (32 - B)) >> (32 - B);
740 template <
unsigned B> constexpr
inline int64_t
SignExtend64(uint64_t x) {
741 static_assert(B > 0,
"Bit width can't be 0.");
742 static_assert(B <= 64,
"Bit width out of range.");
743 return int64_t(x << (64 - B)) >> (64 - B);
749 assert(B > 0 &&
"Bit width can't be 0.");
750 assert(B <= 64 &&
"Bit width out of range.");
751 return int64_t(X << (64 - B)) >> (64 - B);
756 template <
typename T>
757 typename std::enable_if<std::is_unsigned<T>::value, T>::type
759 return (std::max)(X, Y) - (std::min)(X, Y);
765 template <
typename T>
766 typename std::enable_if<std::is_unsigned<T>::value, T>::type
769 bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy;
772 Overflowed = (Z < X || Z < Y);
774 return (std::numeric_limits<T>::max)();
782 template <
typename T>
783 typename std::enable_if<std::is_unsigned<T>::value, T>::type
786 bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy;
799 const T Max = (std::numeric_limits<T>::max)();
801 if (Log2Z < Log2Max) {
804 if (Log2Z > Log2Max) {
813 if (Z & ~(Max >> 1)) {
828 template <
typename T>
829 typename std::enable_if<std::is_unsigned<T>::value, T>::type
832 bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy;
843 template <
typename T>
844 constexpr
int sgn(T val) {
845 return (T(0) < val) - (val < T(0));
bool isUIntN(unsigned N, uint64_t x)
Checks if an unsigned integer fits into the given (dynamic) bit width.
Definition: MathExtras.h:399
Definition: MathExtras.h:51
T reverseBits(T Val)
Reverse the bits in Val.
Definition: MathExtras.h:266
std::size_t countLeadingZeros(T Val, ZeroBehavior ZB=ZB_Width)
Count number of 0's from the most significant bit to the least stopping at the first 1.
Definition: MathExtras.h:186
constexpr uint64_t MinAlign(uint64_t A, uint64_t B)
A and B are either alignments or offsets.
Definition: MathExtras.h:601
T findFirstSet(T Val, ZeroBehavior ZB=ZB_Max)
Get the index of the first set bit starting from the least significant bit.
Definition: MathExtras.h:200
constexpr bool isShiftedMask_64(uint64_t Value)
Return true if the argument contains a non-empty sequence of ones with the remainder zero (64 bit ver...
Definition: MathExtras.h:429
unsigned Log2_32_Ceil(uint32_t Value)
Return the ceil log base 2 of the specified value, 32 if the value is zero.
Definition: MathExtras.h:543
int64_t maxIntN(int64_t N)
Gets the maximum value for a N-bit signed integer.
Definition: MathExtras.h:390
uint64_t alignDown(uint64_t Value, uint64_t Align, uint64_t Skew=0)
Returns the largest uint64_t less than or equal to Value and is Skew mod Align.
Definition: MathExtras.h:709
constexpr int64_t SignExtend64(uint64_t x)
Sign-extend the number in the bottom B bits of X to a 64-bit integer.
Definition: MathExtras.h:740
std::size_t countTrailingZeros(T Val, ZeroBehavior ZB=ZB_Width)
Count number of 0's from the least significant bit to the most stopping at the first 1.
Definition: MathExtras.h:117
std::size_t countLeadingOnes(T Value, ZeroBehavior ZB=ZB_Width)
Count the number of ones from the most significant bit to the first zero bit.
Definition: MathExtras.h:453
Definition: MathExtras.h:125
ZeroBehavior
The behavior an operation has on an input of 0.
Definition: MathExtras.h:41
double Log2(double Value)
Return the log base 2 of the specified value.
Definition: MathExtras.h:519
constexpr bool isPowerOf2_32(uint32_t Value)
Return true if the argument is a power of two > 0.
Definition: MathExtras.h:435
std::enable_if< std::is_unsigned< T >::value, T >::type SaturatingAdd(T X, T Y, bool *ResultOverflowed=nullptr)
Add two unsigned integers, X and Y, of type T.
Definition: MathExtras.h:767
constexpr bool isShiftedUInt(uint64_t x)
Checks if a unsigned integer is an N bit number shifted left by S.
Definition: MathExtras.h:352
uint64_t NextPowerOf2(uint64_t A)
Returns the next power of two (in 64-bits) that is strictly greater than A.
Definition: MathExtras.h:631
bool isIntN(unsigned N, int64_t x)
Checks if an signed integer fits into the given (dynamic) bit width.
Definition: MathExtras.h:404
constexpr bool isMask_32(uint32_t Value)
Return true if the argument is a non-empty sequence of ones starting at the least significant bit wit...
Definition: MathExtras.h:411
std::enable_if< std::is_unsigned< T >::value, T >::type AbsoluteDifference(T X, T Y)
Subtract two unsigned integers, X and Y, of type T and return the absolute value of the result.
Definition: MathExtras.h:758
unsigned Log2_32(uint32_t Value)
Return the floor log base 2 of the specified value, -1 if the value is zero.
Definition: MathExtras.h:530
std::enable_if< std::is_unsigned< T >::value, T >::type SaturatingMultiply(T X, T Y, bool *ResultOverflowed=nullptr)
Multiply two unsigned integers, X and Y, of type T.
Definition: MathExtras.h:784
constexpr bool isShiftedInt(int64_t x)
Checks if a signed integer is an N bit number shifted left by S.
Definition: MathExtras.h:312
uint64_t maxUIntN(uint64_t N)
Gets the maximum value for a N-bit unsigned integer.
Definition: MathExtras.h:363
constexpr bool isMask_64(uint64_t Value)
Return true if the argument is a non-empty sequence of ones starting at the least significant bit wit...
Definition: MathExtras.h:417
T maskTrailingZeros(unsigned N)
Create a bitmask with the N right-most bits set to 0, and all other bits set to 1.
Definition: MathExtras.h:224
uint64_t alignTo(uint64_t Value, uint64_t Align, uint64_t Skew=0)
Returns the next integer (mod 2**64) that is greater than or equal to Value and is a multiple of Alig...
Definition: MathExtras.h:676
alignTo for contexts where a constant expression is required.
Definition: MathExtras.h:699
WPILib C++ utilities (wpiutil) namespace.
Definition: EventLoopRunner.h:17
T maskLeadingOnes(unsigned N)
Create a bitmask with the N left-most bits set to 1, and all other bits set to 0.
Definition: MathExtras.h:218
T maskTrailingOnes(unsigned N)
Create a bitmask with the N right-most bits set to 1, and all other bits set to 0.
Definition: MathExtras.h:209
constexpr std::enable_if<(N< 64), bool >::type isUInt(uint64_t X)
Checks if an unsigned integer fits into the given bit width.
Definition: MathExtras.h:329
uint64_t PowerOf2Floor(uint64_t A)
Returns the power of two which is less than or equal to the given value.
Definition: MathExtras.h:643
uint64_t OffsetToAlignment(uint64_t Value, uint64_t Align)
Returns the offset to the next integer (mod 2**64) that is greater than or equal to Value and is a mu...
Definition: MathExtras.h:718
float BitsToFloat(uint32_t Bits)
This function takes a 32-bit integer and returns the bit equivalent float.
Definition: MathExtras.h:572
constexpr int32_t SignExtend32(uint32_t X)
Sign-extend the number in the bottom B bits of X to a 32-bit integer.
Definition: MathExtras.h:724
constexpr uint32_t Hi_32(uint64_t Value)
Return the high 32 bits of a 64 bit value.
Definition: MathExtras.h:281
std::size_t countTrailingOnes(T Value, ZeroBehavior ZB=ZB_Width)
Count the number of ones from the least significant bit to the first zero bit.
Definition: MathExtras.h:469
double BitsToDouble(uint64_t Bits)
This function takes a 64-bit integer and returns the bit equivalent double.
Definition: MathExtras.h:564
size_t alignmentAdjustment(const void *Ptr, size_t Alignment)
Returns the necessary adjustment for aligning Ptr to Alignment bytes, rounding up.
Definition: MathExtras.h:625
constexpr uint64_t Make_64(uint32_t High, uint32_t Low)
Make a 64-bit integer from a high / low pair of 32-bit integers.
Definition: MathExtras.h:291
constexpr bool isInt(int64_t x)
Checks if an integer fits into the given bit width.
Definition: MathExtras.h:296
constexpr uint32_t Lo_32(uint64_t Value)
Return the low 32 bits of a 64 bit value.
Definition: MathExtras.h:286
constexpr bool isPowerOf2_64(uint64_t Value)
Return true if the argument is a power of two > 0 (64 bit edition.)
Definition: MathExtras.h:440
uint64_t PowerOf2Ceil(uint64_t A)
Returns the power of two which is greater than or equal to the given value.
Definition: MathExtras.h:650
std::enable_if< std::is_unsigned< T >::value, T >::type SaturatingMultiplyAdd(T X, T Y, T A, bool *ResultOverflowed=nullptr)
Multiply two unsigned integers, X and Y, and add the unsigned integer, A to the product.
Definition: MathExtras.h:830
The returned value is numeric_limits<T>::digits.
Definition: MathExtras.h:47
The returned value is undefined.
Definition: MathExtras.h:43
T findLastSet(T Val, ZeroBehavior ZB=ZB_Max)
Get the index of the last set bit starting from the least significant bit.
Definition: MathExtras.h:241
The returned value is numeric_limits<T>::max()
Definition: MathExtras.h:45
unsigned Log2_64(uint64_t Value)
Return the floor log base 2 of the specified value, -1 if the value is zero.
Definition: MathExtras.h:536
uint32_t FloatToBits(float Float)
This function takes a float and returns the bit equivalent 32-bit integer.
Definition: MathExtras.h:592
int64_t minIntN(int64_t N)
Gets the minimum value for a N-bit signed integer.
Definition: MathExtras.h:379
auto count(R &&Range, const E &Element) -> typename std::iterator_traits< decltype(adl_begin(Range))>::difference_type
Wrapper function around std::count to count the number of times an element Element occurs in the give...
Definition: STLExtras.h:1092
uint64_t DoubleToBits(double Double)
This function takes a double and returns the bit equivalent 64-bit integer.
Definition: MathExtras.h:582
T maskLeadingZeros(unsigned N)
Create a bitmask with the N left-most bits set to 0, and all other bits set to 1.
Definition: MathExtras.h:230
static const unsigned char BitReverseTable256[256]
Macro compressed bit reversal table for 256 bits.
Definition: MathExtras.h:254
uint64_t divideCeil(uint64_t Numerator, uint64_t Denominator)
Returns the integer ceil(Numerator / Denominator).
Definition: MathExtras.h:690
uintptr_t alignAddr(const void *Addr, size_t Alignment)
Aligns Addr to Alignment bytes, rounding up.
Definition: MathExtras.h:614
Definition: MathExtras.h:477
constexpr bool isShiftedMask_32(uint32_t Value)
Return true if the argument contains a non-empty sequence of ones with the remainder zero (32 bit ver...
Definition: MathExtras.h:423
Definition: MathExtras.h:702
unsigned countPopulation(T Value)
Count the number of set bits in a value.
Definition: MathExtras.h:511
unsigned Log2_64_Ceil(uint64_t Value)
Return the ceil log base 2 of the specified value, 64 if the value is zero.
Definition: MathExtras.h:549
uint64_t GreatestCommonDivisor64(uint64_t A, uint64_t B)
Return the greatest common divisor of the values using Euclid's algorithm.
Definition: MathExtras.h:554