607 lines
20 KiB
C++
607 lines
20 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();
|
|
}
|
|
|
|
// 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(x * log2(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) 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), '.');
|
|
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;
|
|
}
|
|
}
|