665 lines
22 KiB
C++
665 lines
22 KiB
C++
#pragma once
|
||
#include "Helpers/StringHelpers.hpp"
|
||
#include <cmath>
|
||
#include <cstdint>
|
||
#include <string>
|
||
#include <Eigen/Core>
|
||
#include <boost/multiprecision/cpp_int.hpp>
|
||
#include <cereal/types/vector.hpp>
|
||
|
||
namespace FixedPoint {
|
||
template <typename T> T abs(const T& a) {
|
||
if(a < T(0))
|
||
return -a;
|
||
return a;
|
||
}
|
||
|
||
template <typename T> T sqrt(const T& a) {
|
||
return T(boost::multiprecision::sqrt(a.get_underlying_val() << a.bit_fraction_count()), true);
|
||
}
|
||
|
||
template <typename T> T lerp(const T& a, const T& b, const T& t) {
|
||
return a + t * (b - a);
|
||
}
|
||
|
||
template <typename T> T lerp_double(const T& a, const T& b, double t) {
|
||
if(t == 0.0)
|
||
return a;
|
||
else
|
||
return a + (b - a) / T(1.0 / t);
|
||
}
|
||
|
||
template <typename T> T trunc(const T& a) {
|
||
return (a >> a.bit_fraction_count()) << a.bit_fraction_count();
|
||
}
|
||
|
||
template <typename T> T negative_round(const T& a) {
|
||
T truncatedVal = FixedPoint::trunc(a);
|
||
if(truncatedVal != a && a < T(0))
|
||
return truncatedVal + T(1);
|
||
return truncatedVal;
|
||
}
|
||
|
||
// Gets highest bit in UNDERLYING VALUE, not the integer portion of the number
|
||
template <typename T> int to_highest_bit(const T& a) {
|
||
typename T::UnderlyingType underNum = a.get_underlying_val();
|
||
if(underNum == 0)
|
||
return 0;
|
||
|
||
int b = 0;
|
||
while(underNum >>= 1)
|
||
b++;
|
||
|
||
return b;
|
||
}
|
||
|
||
template <typename T> T log2_int(const T& a) {
|
||
typename T::UnderlyingType b(typename T::UnderlyingType(1u) << (a.bit_fraction_count() - 1));
|
||
typename T::UnderlyingType y(0);
|
||
typename T::UnderlyingType x = a.get_underlying_val();
|
||
|
||
if(a < T(0))
|
||
throw std::runtime_error("[FixedPoint::log2] Number outside domain");
|
||
|
||
while(x < (typename T::UnderlyingType(1u) << a.bit_fraction_count()))
|
||
{
|
||
x <<= 1;
|
||
y -= (typename T::UnderlyingType(1u)) << a.bit_fraction_count();
|
||
}
|
||
|
||
while(x >= (typename T::UnderlyingType(2u) << a.bit_fraction_count()))
|
||
{
|
||
x >>= 1;
|
||
y += (typename T::UnderlyingType(1u)) << a.bit_fraction_count();
|
||
}
|
||
|
||
return T(y, true);
|
||
}
|
||
|
||
template <typename T> T exp2_int(T x) {
|
||
if(x < T(0))
|
||
return T(1);
|
||
|
||
typename T::UnderlyingType x_int = x.get_underlying_val() >> x.bit_fraction_count();
|
||
x -= T(x_int, false);
|
||
assert(x >= T(0) && x < T(1));
|
||
|
||
return T(typename T::UnderlyingType(1) << static_cast<int>(x_int), false);
|
||
}
|
||
|
||
// Base should be a power of 2 to be accurate
|
||
template <typename T> T log_int(const T& a, const T& base) {
|
||
return log2_int(a) / log2_int(base);
|
||
}
|
||
|
||
// Base should be a power of 2 to be accurate
|
||
template <typename T> T exp_int(const T& x, const T& base) {
|
||
return exp2_int(x * log2_int(base));
|
||
}
|
||
|
||
template <typename T> T exp_int_accurate(uint64_t x, uint64_t base) {
|
||
T toRet(1);
|
||
T tBase(base);
|
||
for(uint64_t i = 0; i < x; i++)
|
||
toRet *= tBase;
|
||
return toRet;
|
||
}
|
||
|
||
// https://github.com/Koishi-Satori/EirinFixed/blob/main/include/eirin/math.hpp
|
||
template <typename T> T log2(const T& a) {
|
||
typename T::UnderlyingType b(typename T::UnderlyingType(1u) << (a.bit_fraction_count() - 1));
|
||
typename T::UnderlyingType y(0);
|
||
typename T::UnderlyingType x = a.get_underlying_val();
|
||
|
||
if(a < T(0))
|
||
throw std::runtime_error("[FixedPoint::log2] Number outside domain");
|
||
|
||
while(x < (typename T::UnderlyingType(1u) << a.bit_fraction_count()))
|
||
{
|
||
x <<= 1;
|
||
y -= (typename T::UnderlyingType(1u)) << a.bit_fraction_count();
|
||
}
|
||
|
||
while(x >= (typename T::UnderlyingType(2u) << a.bit_fraction_count()))
|
||
{
|
||
x >>= 1;
|
||
y += (typename T::UnderlyingType(1u)) << a.bit_fraction_count();
|
||
}
|
||
|
||
typename T::UnderlyingType z = x;
|
||
for(int i = 0; i < a.bit_fraction_count(); ++i)
|
||
{
|
||
z = (z * z) >> a.bit_fraction_count();
|
||
if(z >= (typename T::UnderlyingType(2u) << a.bit_fraction_count()))
|
||
{
|
||
z >>= 1;
|
||
y += b;
|
||
}
|
||
b = b >> 1;
|
||
}
|
||
|
||
return T(y, true);
|
||
}
|
||
|
||
// https://github.com/MikeLankamp/fpm/blob/master/include/fpm/math.hpp
|
||
template <typename T> T exp2(T x) {
|
||
if(x < T(0))
|
||
return T(1) / exp2(-x);
|
||
|
||
typename T::UnderlyingType x_int = x.get_underlying_val() >> x.bit_fraction_count();
|
||
x -= T(x_int, false);
|
||
assert(x >= T(0) && x < T(1));
|
||
|
||
T fA(1.8964611454333148e-3);
|
||
T fB(8.9428289841091295e-3);
|
||
T fC(5.5866246304520701e-2);
|
||
T fD(2.4013971109076949e-1);
|
||
T fE(6.9315475247516736e-1);
|
||
T fF(9.9999989311082668e-1);
|
||
return T(typename T::UnderlyingType(1) << static_cast<int>(x_int), false) * (((((fA * x + fB) * x + fC) * x + fD) * x + fE) * x + fF);
|
||
}
|
||
|
||
template <typename T> T log(const T& a, const T& base) {
|
||
return log2(a) / log2(base);
|
||
}
|
||
|
||
template <typename T> T exp(const T& x, const T& base) {
|
||
return exp2(x * log2(base));
|
||
}
|
||
|
||
template <typename T, int bitFractionCount> class Number {
|
||
public:
|
||
typedef T UnderlyingType;
|
||
|
||
static int bit_fraction_count() {
|
||
return bitFractionCount;
|
||
}
|
||
|
||
Number():
|
||
val(0) {}
|
||
|
||
explicit Number(double initVal) {
|
||
val = T(initVal * std::pow(2, bitFractionCount));
|
||
}
|
||
|
||
explicit Number(float initVal) {
|
||
val = T(initVal * std::pow(2, bitFractionCount));
|
||
}
|
||
|
||
explicit Number(int64_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(int32_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(int16_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(uint64_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(uint32_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(uint16_t initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
explicit Number(const std::string& initVal) {
|
||
val = T(initVal) << bitFractionCount;
|
||
}
|
||
|
||
template <typename Archive> void load(Archive& a) {
|
||
bool isNegative;
|
||
std::vector<uint8_t> v;
|
||
a(isNegative, v);
|
||
boost::multiprecision::import_bits(val, v.begin(), v.end(), 8, true);
|
||
if(isNegative)
|
||
val = -val;
|
||
}
|
||
|
||
template <typename Archive> void save(Archive& a) const {
|
||
bool isNegative = val < 0;
|
||
std::vector<uint8_t> v;
|
||
T absVal = boost::multiprecision::abs(val);
|
||
boost::multiprecision::export_bits(absVal, std::back_inserter(v), 8, true);
|
||
a(isNegative, v);
|
||
}
|
||
|
||
Number operator-(const Number& other) const {
|
||
return Number(val - other.val, true);
|
||
}
|
||
|
||
Number operator-() const {
|
||
return Number(-val, true);
|
||
}
|
||
|
||
Number operator/(const Number& other) const {
|
||
return Number((val << bitFractionCount) / other.val, true);
|
||
}
|
||
|
||
Number operator*(const Number& other) const {
|
||
return Number((val * other.val) >> bitFractionCount, true);
|
||
}
|
||
|
||
template <int otherDecimalCount> Number multiply_different_precision(const Number<T, otherDecimalCount>& other) const {
|
||
return Number(((val * other.get_underlying_val()) >> other.bit_fraction_count()), true);
|
||
}
|
||
|
||
Number multiply_double(double a) const {
|
||
if(std::fabs(a) < 1.0)
|
||
a = 1.0 / a;
|
||
return (*this) * Number(a);
|
||
}
|
||
|
||
Number divide_double(double a) const {
|
||
if(std::fabs(a) < 1.0)
|
||
a = 1.0 / a;
|
||
return (*this) / Number(a);
|
||
}
|
||
|
||
const T& get_underlying_val() const {
|
||
return val;
|
||
}
|
||
|
||
Number operator+(const Number& other) const {
|
||
return Number(val + other.val, true);
|
||
}
|
||
|
||
Number operator%(const Number& other) const {
|
||
return Number(val % other.val, true);
|
||
}
|
||
|
||
Number& operator+=(const Number& other) {
|
||
val += other.val;
|
||
return *this;
|
||
}
|
||
|
||
Number& operator-=(const Number& other) {
|
||
val -= other.val;
|
||
return *this;
|
||
}
|
||
|
||
Number& operator*=(const Number& other) {
|
||
val *= other.val;
|
||
val >>= bitFractionCount;
|
||
return *this;
|
||
}
|
||
|
||
Number& operator/=(const Number& other) {
|
||
val <<= bitFractionCount;
|
||
val /= other.val;
|
||
return *this;
|
||
}
|
||
|
||
Number& operator%=(const Number& other) {
|
||
val %= other.val;
|
||
return *this;
|
||
}
|
||
|
||
explicit operator float() const {
|
||
return static_cast<float>(val) / std::pow(2, bitFractionCount);
|
||
}
|
||
|
||
explicit operator double() const {
|
||
return static_cast<double>(val) / std::pow(2, bitFractionCount);
|
||
}
|
||
|
||
explicit operator int64_t() const {
|
||
return static_cast<int64_t>(val >> bitFractionCount);
|
||
}
|
||
|
||
explicit operator uint64_t() const {
|
||
return static_cast<uint64_t>(val >> bitFractionCount);
|
||
}
|
||
|
||
std::string to_underlying_str() const {
|
||
return val.str();
|
||
}
|
||
|
||
void from_underlying_str(const std::string& str) {
|
||
val = T(str);
|
||
}
|
||
|
||
std::string display_int_str(size_t scientificNotationDigits, bool fancy = false) const {
|
||
if(scientificNotationDigits <= 0) {
|
||
T shiftedVal = val >> bitFractionCount;
|
||
return shiftedVal.str();
|
||
}
|
||
else {
|
||
T shiftedVal = val >> bitFractionCount;
|
||
std::string a = shiftedVal.str();
|
||
if(a.size() > (scientificNotationDigits + 2)) {
|
||
bool isNegative = val < 0;
|
||
size_t exponent = a.size() - (isNegative ? 2 : 1);
|
||
a = a.substr(0, scientificNotationDigits + (isNegative ? 2 : 1));
|
||
a.insert(a.begin() + (isNegative ? 2 : 1), '.');
|
||
if(fancy) {
|
||
for(;;) {
|
||
if(a.back() == '0') {
|
||
a.pop_back();
|
||
continue;
|
||
}
|
||
else if(a.back() == '.')
|
||
a.pop_back();
|
||
break;
|
||
}
|
||
a += "×10";
|
||
std::string expStr = std::to_string(exponent);
|
||
for(char eChar : expStr) {
|
||
switch(eChar) {
|
||
case '0':
|
||
a += "⁰";
|
||
break;
|
||
case '1':
|
||
a += "¹";
|
||
break;
|
||
case '2':
|
||
a += "²";
|
||
break;
|
||
case '3':
|
||
a += "³";
|
||
break;
|
||
case '4':
|
||
a += "⁴";
|
||
break;
|
||
case '5':
|
||
a += "⁵";
|
||
break;
|
||
case '6':
|
||
a += "⁶";
|
||
break;
|
||
case '7':
|
||
a += "⁷";
|
||
break;
|
||
case '8':
|
||
a += "⁸";
|
||
break;
|
||
case '9':
|
||
a += "⁹";
|
||
break;
|
||
default:
|
||
a.push_back(eChar);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
a += "e" + std::to_string(exponent);
|
||
}
|
||
return a;
|
||
}
|
||
}
|
||
|
||
friend std::ostream& operator<<(std::ostream& os, Number const& m) {
|
||
return os << static_cast<double>(m);
|
||
}
|
||
|
||
bool operator<(const Number& other) const {
|
||
return val < other.val;
|
||
}
|
||
|
||
bool operator>(const Number& other) const {
|
||
return val > other.val;
|
||
}
|
||
|
||
bool operator>=(const Number& other) const {
|
||
return val >= other.val;
|
||
}
|
||
|
||
bool operator<=(const Number& other) const {
|
||
return val <= other.val;
|
||
}
|
||
|
||
bool operator==(const Number& other) const {
|
||
return val == other.val;
|
||
}
|
||
|
||
Number operator>>(int shiftNum) const {
|
||
return Number(val >> shiftNum, true);
|
||
}
|
||
|
||
Number operator<<(int shiftNum) const {
|
||
return Number(val << shiftNum, true);
|
||
}
|
||
|
||
Number(const T& initVal, bool isUnderlyingVal):
|
||
val(initVal)
|
||
{
|
||
if(isUnderlyingVal)
|
||
val = initVal;
|
||
else
|
||
val = initVal << bitFractionCount;
|
||
}
|
||
|
||
private:
|
||
T val;
|
||
};
|
||
|
||
template <typename T, int bitFractionCount> class Multiplier {
|
||
public:
|
||
typedef Number<T, bitFractionCount> UnderlyingNumber;
|
||
|
||
Multiplier():
|
||
val(1),
|
||
isReciprocal(false)
|
||
{}
|
||
|
||
Multiplier(const UnderlyingNumber& initVal):
|
||
val(initVal),
|
||
isReciprocal(false)
|
||
{
|
||
if(val != UnderlyingNumber(0) && abs(val) < UnderlyingNumber(1)) {
|
||
val = UnderlyingNumber(1) / val;
|
||
isReciprocal = true;
|
||
}
|
||
}
|
||
|
||
Multiplier(const UnderlyingNumber& initVal, bool isReciprocal):
|
||
val(initVal),
|
||
isReciprocal(isReciprocal)
|
||
{
|
||
if(abs(val) == UnderlyingNumber(1))
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(double initVal) {
|
||
if(std::abs(initVal) < 1.0) {
|
||
val = UnderlyingNumber(1.0 / initVal);
|
||
isReciprocal = true;
|
||
}
|
||
else {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
}
|
||
|
||
explicit Multiplier(float initVal) {
|
||
if(std::abs(initVal) < 1.0) {
|
||
val = UnderlyingNumber(1.0 / initVal);
|
||
isReciprocal = true;
|
||
}
|
||
else {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
}
|
||
|
||
explicit Multiplier(int64_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(int32_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(int16_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(uint64_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(uint32_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit Multiplier(uint16_t initVal) {
|
||
val = UnderlyingNumber(initVal);
|
||
isReciprocal = false;
|
||
}
|
||
|
||
explicit operator float() const {
|
||
if(isReciprocal && val != UnderlyingNumber(0))
|
||
return 1.0f / static_cast<float>(val);
|
||
else
|
||
return static_cast<float>(val);
|
||
}
|
||
|
||
explicit operator double() const {
|
||
if(isReciprocal && val != UnderlyingNumber(0))
|
||
return 1.0 / static_cast<double>(val);
|
||
else
|
||
return static_cast<double>(val);
|
||
}
|
||
|
||
explicit operator int64_t() const {
|
||
if(isReciprocal)
|
||
return 0;
|
||
else
|
||
return static_cast<int64_t>(val);
|
||
}
|
||
|
||
explicit operator uint64_t() const {
|
||
if(isReciprocal)
|
||
return 0;
|
||
else
|
||
return static_cast<uint64_t>(val);
|
||
}
|
||
|
||
explicit operator UnderlyingNumber() const {
|
||
if(isReciprocal && val != UnderlyingNumber(0))
|
||
return UnderlyingNumber(1) / val;
|
||
else
|
||
return val;
|
||
}
|
||
|
||
Multiplier multiply_double(double a) const {
|
||
return (*this) * Multiplier(a);
|
||
}
|
||
|
||
Multiplier divide_double(double a) const {
|
||
return (*this) / Multiplier(a);
|
||
}
|
||
|
||
Multiplier operator*(const Multiplier& a) const {
|
||
if(a.val == UnderlyingNumber(0))
|
||
return Multiplier(0);
|
||
return (*this) / Multiplier(a.val, !a.isReciprocal);
|
||
}
|
||
|
||
Multiplier operator/(const Multiplier& a) const {
|
||
if(val == UnderlyingNumber(0))
|
||
return Multiplier(0);
|
||
|
||
if(!isReciprocal && !a.isReciprocal) {
|
||
if(abs(val) < abs(a.val))
|
||
return Multiplier(a.val / val, true);
|
||
else
|
||
return Multiplier(val / a.val, false);
|
||
}
|
||
else if(!isReciprocal && a.isReciprocal) {
|
||
return Multiplier(val * a.val, false);
|
||
}
|
||
else if(isReciprocal && !a.isReciprocal) {
|
||
return Multiplier(val * a.val, true);
|
||
}
|
||
else {
|
||
if(abs(val) < abs(a.val))
|
||
return Multiplier(a.val / val, false);
|
||
else
|
||
return Multiplier(val / a.val, true);
|
||
}
|
||
}
|
||
|
||
bool operator==(const Multiplier& other) const {
|
||
return (val == other.val) && (isReciprocal == other.isReciprocal || val == UnderlyingNumber(1));
|
||
}
|
||
|
||
bool operator!=(const Multiplier& other) const {
|
||
return !((*this) == other);
|
||
}
|
||
private:
|
||
Number<T, bitFractionCount> val;
|
||
bool isReciprocal;
|
||
};
|
||
|
||
template <typename Mult> Mult::UnderlyingNumber operator*(const Mult& a, const typename Mult::UnderlyingNumber& b) {
|
||
return static_cast<Mult::UnderlyingNumber>(a * Multiplier(b));
|
||
}
|
||
|
||
template <typename Mult> Mult::UnderlyingNumber operator*(const typename Mult::UnderlyingNumber& b, const Mult& a) {
|
||
return static_cast<Mult::UnderlyingNumber>(a * Multiplier(b));
|
||
}
|
||
|
||
template <typename Mult> Mult::UnderlyingNumber operator/(const Mult& a, const typename Mult::UnderlyingNumber& b) {
|
||
return static_cast<Mult::UnderlyingNumber>(a / Multiplier(b));
|
||
}
|
||
|
||
template <typename Mult> Mult::UnderlyingNumber operator/(const typename Mult::UnderlyingNumber& a, const Mult& b) {
|
||
return static_cast<Mult::UnderlyingNumber>(Multiplier(a) / b);
|
||
}
|
||
}
|
||
|
||
typedef FixedPoint::Number<boost::multiprecision::cpp_int, 32> WorldScalar;
|
||
typedef FixedPoint::Multiplier<boost::multiprecision::cpp_int, 32> WorldMultiplier;
|
||
|
||
namespace Eigen {
|
||
template<> struct NumTraits<WorldScalar> : GenericNumTraits<WorldScalar>
|
||
{
|
||
typedef WorldScalar Real;
|
||
typedef WorldScalar NonInteger;
|
||
typedef WorldScalar Nested;
|
||
|
||
static inline Real epsilon() { return Real(0); }
|
||
static inline Real dummy_precision() { return Real(0); }
|
||
static inline int digits10() { return 0; }
|
||
|
||
enum {
|
||
IsInteger = 0,
|
||
IsSigned = 1,
|
||
IsComplex = 0,
|
||
RequireInitialization = 1,
|
||
ReadCost = 6,
|
||
AddCost = 150,
|
||
MulCost = 100
|
||
};
|
||
};
|
||
}
|
||
|
||
namespace FixedPoint {
|
||
template <typename Mult> Eigen::Vector<typename Mult::UnderlyingNumber, 2> multiplier_vec_mult(const Eigen::Vector<typename Mult::UnderlyingNumber, 2>& a, const Mult& m) {
|
||
auto toRet = a;
|
||
toRet.x() = toRet.x() * m;
|
||
toRet.y() = toRet.y() * m;
|
||
return toRet;
|
||
}
|
||
|
||
template <typename Mult> Eigen::Vector<typename Mult::UnderlyingNumber, 2> multiplier_vec_div(const Eigen::Vector<typename Mult::UnderlyingNumber, 2>& a, const Mult& m) {
|
||
auto toRet = a;
|
||
toRet.x() = toRet.x() / m;
|
||
toRet.y() = toRet.y() / m;
|
||
return toRet;
|
||
}
|
||
}
|