PowerOfTwo.h

#pragma once
#ifdef _MSC_VER
#include <intrin.h>
#endif
#include <cassert>
#include <cstdint>
#include <type_traits>
 
namespace Cogs
{
 
  template<typename T>
  inline bool isPowerOfTwo(T x)
  {
    static_assert(std::is_integral<T>::value, "Integer required.");
    // x-1 inverts all bits from the lowest set bit and down inclusive,
    // while everything above remains unchanged. Thus, the expression
    // removes the lowest set bit. If the number is a power of two, then
    // the number consists of a single bit set, the lowest bit set.
    return (x & (x-1)) == 0;
  }
 
 
  inline uint8_t roundUpToPowerOfTwo(uint8_t x)
  {
#if defined(_MSC_VER)
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint8_t)1 << (((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
#ifdef _DEBUG
    uint16_t o = x;
#endif
    x--;            // If we are a power-of-two, this will pop one leading bit. Otherwise leading bit will remain.
    x |= x >> 1u;   // Make leading bit a pair of leading bits.
    x |= x >> 2u;   // And than in turn into a quadruplet of leading bits.
    x |= x >> 4u;   // And now all bits below the leading bit are set.
    x++;            // The next number is a power of two, equal or larger than the input.
 
    // Also note that if x was zero, first instruction sets all bits, then already set bits
    // gets propagated down with no effect, and the last add flips everything back to zero.
#ifdef _DEBUG
    assert(o <= x);
    assert((o == 0) || (o / 2 < x));
#endif
    return x;
#endif
  }
 
  inline uint16_t roundUpToPowerOfTwo(uint16_t x)
  {
#if defined(_MSC_VER)
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint16_t)1 << (((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
#ifdef _DEBUG
    uint16_t o = x;
#endif
    x--;
    x |= x >> 1u;
    x |= x >> 2u;
    x |= x >> 4u;
    x |= x >> 8u;
    x++;
#ifdef _DEBUG
    assert(o <= x);
    assert((o == 0) || (o/2 < x));
#endif
    return x;
#endif
  }
 
  inline uint16_t roundDownToPowerOfTwo(uint16_t x)
  {
#if defined(_MSC_VER)
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint16_t)(1 << index);
    }
    return 0;
#else
#ifdef _DEBUG
    uint16_t o = x;
#endif
    x |= x >> 1u;   // Make leading bit a pair of leading bits.
    x |= x >> 2u;
    x |= x >> 4u;
    x |= x >> 8u;
    x = x - (x >> 1);
#ifdef _DEBUG
    assert(x <= o);
#endif
    return x;
#endif
  }
 
 
  inline uint32_t roundUpToPowerOfTwo(uint32_t x)
  {
#if defined(_MSC_VER)
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint32_t)1 << (((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
#ifdef _DEBUG
    uint32_t o = x;
#endif
    x--;
    x |= x >> 1u;
    x |= x >> 2u;
    x |= x >> 4u;
    x |= x >> 8u;
    x |= x >> 16u;
    x++;
#ifdef _DEBUG
    assert(o <= x);
    assert((o == 0) || (o / 2 < x));
#endif
    return x;
#endif
  }
 
  inline uint64_t roundUpToPowerOfTwo(uint64_t x)
  {
#if defined(_MSC_VER) && defined(_WIN64)
    unsigned long index;
    if (_BitScanReverse64(&index, x)) {
      return (uint64_t)1 << (((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
#ifdef _DEBUG
    uint64_t o = x;
#endif
    x--;
    x |= x >> 1u;
    x |= x >> 2u;
    x |= x >> 4u;
    x |= x >> 8u;
    x |= x >> 16u;
    x |= x >> 32u;
    x++;
#ifdef _DEBUG
    assert(o <= x);
    assert((o == 0) || (o / 2 < x));
#endif
    return x;
#endif
  }
 
  inline uint8_t roundUpToPowerOfTwoShift(uint8_t x)
  {
#ifdef _MSC_VER
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint8_t)(((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
    uint8_t t = roundUpToPowerOfTwo(x);
 
    // Find shift of most significant bit (i.e. only bit).
    uint8_t s = 0;
    if (t & 0xF0u) s += 4;  // If any bits in 1111 0000, shift is at least 4
    if (t & 0xCCu) s += 2;  // If any bits in 1100 1100, shift is at least 2, plus previous.
    if (t & 0xAAu) s += 1;  // If any bits in 1010 1010, shift is at least 1, plus previous.
 
#ifdef _DEBUG
    assert(x <= (1u << s));
    assert((s == 0u) || ((1u << (s - 1u) < x)));
#endif
    return s;
#endif
  }
 
  inline uint16_t roundUpToPowerOfTwoShift(uint16_t x)
  {
#ifdef _MSC_VER
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint16_t)(((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
    uint16_t t = roundUpToPowerOfTwo(x);
 
    uint16_t s = 0;
    if (t & 0xFF00u) s += 8;
    if (t & 0xF0F0u) s += 4;
    if (t & 0xCCCCu) s += 2;
    if (t & 0xAAAAu) s += 1;
 
#ifdef _DEBUG
    assert(x <= (1u << s));
    assert((s == 0u) || ((1u << (s - 1u) < x)));
#endif
    return s;
#endif
  }
 
  inline uint32_t roundUpToPowerOfTwoShift(uint32_t x)
  {
#ifdef _MSC_VER
    unsigned long index;
    if (_BitScanReverse(&index, x)) {
      return (uint32_t)(((x & (x - 1)) == 0) ? index : index + 1);
    }
    return 0;
#else
    uint32_t t = roundUpToPowerOfTwo(x);
 
    uint32_t s = 0;
    if (t & 0xFFFF0000u) s += 16;
    if (t & 0xFF00FF00u) s += 8;
    if (t & 0xF0F0F0F0u) s += 4;
    if (t & 0xCCCCCCCCu) s += 2;
    if (t & 0xAAAAAAAAu) s += 1;
 
#ifdef _DEBUG
    assert(x <= (1u << s));
    assert((s == 0u) || ((1u << (s - 1u) < x)));
#endif
    return s;
#endif
  }
 
  inline uint64_t roundUpToPowerOfTwoShift(uint64_t x)
  {
#if defined(_MSC_VER) && defined(_WIN64)
    unsigned long index;
    if (_BitScanReverse64(&index, x)) {
      return ((x & (x - 1)) == 0) ? index : index + 1;
    }
    return 0;
#else
    uint64_t t = roundUpToPowerOfTwo(x);
 
    uint64_t s = 0;
    if (t & 0xFFFFFFFF00000000u) s += 32;
    if (t & 0xFFFF0000FFFF0000u) s += 16;
    if (t & 0xFF00FF00FF00FF00u) s += 8;
    if (t & 0xF0F0F0F0F0F0F0F0u) s += 4;
    if (t & 0xCCCCCCCCCCCCCCCCu) s += 2;
    if (t & 0xAAAAAAAAAAAAAAAAu) s += 1;
 
#ifdef _DEBUG
    assert(x <= (1ull << s));
    assert((s == 0u) || ((1ull << (s - 1u) < x)));
#endif
    return s;
#endif
  }
 
}