Add fixed-point arithmetic implementation
This commit is contained in:
parent
8617fac987
commit
d66092b86d
1 changed files with 381 additions and 0 deletions
381
libs/util/include/psemek/util/fixed_point.hpp
Normal file
381
libs/util/include/psemek/util/fixed_point.hpp
Normal file
|
|
@ -0,0 +1,381 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <concepts>
|
||||
#include <limits>
|
||||
|
||||
namespace psemek::util
|
||||
{
|
||||
|
||||
/** A base template class implementing fixed-point arithmetic.
|
||||
*
|
||||
* unsigned_fixed_point<I, F> with I integer bits and F fractional bits is
|
||||
* represented as an (I+F)-bit-wide standard unsigned integer type, and the
|
||||
* stored value is interpreted as representing the fraction (value / 2^F).
|
||||
* It is capable of representing values in the range from 0 to (2^(I+F) - 1) / 2^F,
|
||||
* with a fixed precision of 1 / 2^F.
|
||||
*
|
||||
* signed_fixed_point<I, F> with I integer bits and F fractional bits is
|
||||
* represented as an (I+F)-bit-wide standard signed integer type, and the
|
||||
* stored value is interpreted as representing the fraction (value / 2^F).
|
||||
* It is capable of representing values in the range from (- 2^(I+F-1)) / 2^F
|
||||
* to (2^(I+F-1) - 1) / 2^F, with a fixed precision of 2^F.
|
||||
*
|
||||
* For example,
|
||||
*
|
||||
* unsigned_fixed_point<8, 8> can represent the range [0, 255.996094]
|
||||
* with a fixed precision of 0.00390625
|
||||
*
|
||||
* signed_fixed_point<16, 16> can represent the range [-32768, 32767.9999847]
|
||||
* with a fixed precision of 0.00001525878.
|
||||
*
|
||||
* Note that for both the signed and unsigned versions, (I+F) must be equal to
|
||||
* 8, 16 or 32. To add more options, add a specialization for the
|
||||
* detail::fixed_point_traits class below.
|
||||
*
|
||||
* Addition and subtraction are implemented by casting to unsigned types,
|
||||
* meaning integer overflow doesn't invoke undefined behavior, but instead
|
||||
* always overflows correctly (whether this is a meaningful behavior is a
|
||||
* separate question...).
|
||||
*
|
||||
* Multiplication and division are implemented by casting to a 2x wider integer type,
|
||||
* to increase precision and prevent overflows.
|
||||
*
|
||||
* Typical usage of this class would be to make a type alias like
|
||||
*
|
||||
* using fp = signed_fixed_point<16, 16>;
|
||||
*
|
||||
* and then using this alias in place of other arithmetic types;
|
||||
*
|
||||
* Only explicit casts from and to other standard types are allowed, to prevent
|
||||
* accidental conversions. For example, this will compile:
|
||||
*
|
||||
* fp x(1);
|
||||
* fp y(3.1415f);
|
||||
* x = static_cast<fp>(1.618033);
|
||||
* float z = static_cast<float>(y);
|
||||
*
|
||||
* while this won't compile:
|
||||
*
|
||||
* fp x = 1;
|
||||
* fp y = 3.1415f;
|
||||
* x = 1.618033;
|
||||
* float z = y;
|
||||
*
|
||||
* To initialize the fixed_point value directly from the internal representation type,
|
||||
* use the static fixed_point::from_rep() method.
|
||||
*/
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <unsigned int TotalBits>
|
||||
struct fixed_point_traits;
|
||||
|
||||
template <>
|
||||
struct fixed_point_traits<8>
|
||||
{
|
||||
using signed_rep_type = std::int8_t;
|
||||
using unsigned_rep_type = std::uint8_t;
|
||||
|
||||
using signed_wide_type = std::int16_t;
|
||||
using unsigned_wide_type = std::uint16_t;
|
||||
};
|
||||
|
||||
template <>
|
||||
struct fixed_point_traits<16>
|
||||
{
|
||||
using signed_rep_type = std::int16_t;
|
||||
using unsigned_rep_type = std::uint16_t;
|
||||
|
||||
using signed_wide_type = std::int32_t;
|
||||
using unsigned_wide_type = std::uint32_t;
|
||||
};
|
||||
|
||||
template <>
|
||||
struct fixed_point_traits<32>
|
||||
{
|
||||
using signed_rep_type = std::int32_t;
|
||||
using unsigned_rep_type = std::uint32_t;
|
||||
|
||||
using signed_wide_type = std::int64_t;
|
||||
using unsigned_wide_type = std::uint64_t;
|
||||
};
|
||||
|
||||
template <typename T, typename H>
|
||||
H clamp(T x, H min, H max)
|
||||
{
|
||||
if (x < static_cast<T>(min))
|
||||
return min;
|
||||
if (x > static_cast<T>(max))
|
||||
return max;
|
||||
return static_cast<H>(x);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template <unsigned int IntegerBits, unsigned int FractionalBits, bool Signed>
|
||||
struct fixed_point;
|
||||
|
||||
template <unsigned int IntegerBits, unsigned int FractionalBits>
|
||||
using unsigned_fixed_point = fixed_point<IntegerBits, FractionalBits, false>;
|
||||
|
||||
template <unsigned int IntegerBits, unsigned int FractionalBits>
|
||||
using signed_fixed_point = fixed_point<IntegerBits, FractionalBits, true>;
|
||||
|
||||
template <unsigned int IntegerBits, unsigned int FractionalBits, bool Signed>
|
||||
struct fixed_point
|
||||
{
|
||||
static constexpr unsigned int integer_bits = IntegerBits;
|
||||
static constexpr unsigned int fractional_bits = FractionalBits;
|
||||
static constexpr unsigned int total_bits = integer_bits + fractional_bits;
|
||||
|
||||
using traits = detail::fixed_point_traits<total_bits>;
|
||||
|
||||
using rep_type = std::conditional_t<Signed, typename traits::signed_rep_type, typename traits::unsigned_rep_type>;
|
||||
using wide_type = std::conditional_t<Signed, typename traits::signed_wide_type, typename traits::unsigned_wide_type>;
|
||||
using unsigned_rep_type = typename traits::unsigned_rep_type;
|
||||
using unsigned_wide_type = typename traits::unsigned_wide_type;
|
||||
|
||||
fixed_point() = default;
|
||||
fixed_point(fixed_point const &) = default;
|
||||
|
||||
template <std::integral T>
|
||||
explicit fixed_point(T value);
|
||||
|
||||
template <std::floating_point T>
|
||||
explicit fixed_point(T value);
|
||||
|
||||
template <std::integral T>
|
||||
explicit operator T () const;
|
||||
|
||||
template <std::floating_point T>
|
||||
explicit operator T () const;
|
||||
|
||||
fixed_point & operator += (fixed_point);
|
||||
fixed_point & operator -= (fixed_point);
|
||||
fixed_point & operator *= (fixed_point);
|
||||
fixed_point & operator /= (fixed_point);
|
||||
|
||||
rep_type rep() const { return rep_; }
|
||||
|
||||
static fixed_point from_rep(rep_type rep);
|
||||
|
||||
static fixed_point min();
|
||||
static fixed_point max();
|
||||
|
||||
private:
|
||||
rep_type rep_;
|
||||
};
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
template <std::integral T>
|
||||
fixed_point<I, F, S>::fixed_point(T value)
|
||||
: rep_(static_cast<rep_type>(value) << F)
|
||||
{}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
template <std::floating_point T>
|
||||
fixed_point<I, F, S>::fixed_point(T value)
|
||||
: rep_(static_cast<rep_type>(detail::clamp(std::round(value * (static_cast<rep_type>(1) << F)), min().rep(), max().rep())))
|
||||
{}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
template <std::integral T>
|
||||
fixed_point<I, F, S>::operator T () const
|
||||
{
|
||||
return static_cast<T>(rep_) >> F;
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
template <std::floating_point T>
|
||||
fixed_point<I, F, S>::operator T () const
|
||||
{
|
||||
return rep_ / static_cast<T>(static_cast<rep_type>(1) << F);
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> fixed_point<I, F, S>::from_rep(fixed_point<I, F, S>::rep_type rep)
|
||||
{
|
||||
fixed_point<I, F, S> result;
|
||||
result.rep_ = rep;
|
||||
return result;
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> fixed_point<I, F, S>::min()
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
return FP::from_rep(std::numeric_limits<rep_type>::min());
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> fixed_point<I, F, S>::max()
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
return FP::from_rep(std::numeric_limits<rep_type>::max());
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> operator - (fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using unsigned_rep_type = typename FP::unsigned_rep_type;
|
||||
return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(-x.rep())));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> operator + (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using unsigned_rep_type = typename FP::unsigned_rep_type;
|
||||
return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(x1.rep()) + static_cast<unsigned_rep_type>(x2.rep())));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> operator - (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using unsigned_rep_type = typename FP::unsigned_rep_type;
|
||||
return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(x1.rep()) - static_cast<unsigned_rep_type>(x2.rep())));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> operator * (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using wide_type = typename FP::wide_type;
|
||||
return FP::from_rep(static_cast<rep_type>((static_cast<wide_type>(x1.rep()) * static_cast<wide_type>(x2.rep())) >> F));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> operator / (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using wide_type = typename FP::wide_type;
|
||||
return FP::from_rep(static_cast<rep_type>((static_cast<wide_type>(x1.rep()) << F) / static_cast<wide_type>(x2.rep())));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> & fixed_point<I, F, S>::operator += (fixed_point<I, F, S> x)
|
||||
{
|
||||
(*this) = (*this) + x;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> & fixed_point<I, F, S>::operator -= (fixed_point<I, F, S> x)
|
||||
{
|
||||
(*this) = (*this) - x;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> & fixed_point<I, F, S>::operator *= (fixed_point<I, F, S> x)
|
||||
{
|
||||
(*this) = (*this) * x;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> & fixed_point<I, F, S>::operator /= (fixed_point<I, F, S> x)
|
||||
{
|
||||
(*this) = (*this) / x;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
bool operator == (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
return x1.rep() == x2.rep();
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
auto operator <=> (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
|
||||
{
|
||||
return x1.rep() <=> x2.rep();
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> abs(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
if constexpr (S)
|
||||
return FP::from_rep(x.rep() & ~(static_cast<rep_type>(1) << (I + F - 1)));
|
||||
else
|
||||
return x;
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> round(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
return FP::from_rep(x.rep() & ~((static_cast<rep_type>(1) << F) - 1));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> sqrt(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
using rep_type = typename FP::rep_type;
|
||||
using wide_type = typename FP::wide_type;
|
||||
return FP::from_rep(static_cast<rep_type>(std::round(std::sqrt(static_cast<wide_type>(x.rep()) << F))));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> pow(fixed_point<I, F, S> x, int p)
|
||||
{
|
||||
if (p < 0)
|
||||
return fixed_point<I, F, S>(1) / pow(x, -p);
|
||||
|
||||
fixed_point<I, F, S> result(1);
|
||||
while (p > 0)
|
||||
{
|
||||
if ((p & 1) == 1)
|
||||
result *= x;
|
||||
x *= x;
|
||||
p >>= 1;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> sin(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
return static_cast<FP>(std::round(std::sin(static_cast<double>(x))));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> cos(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
return static_cast<FP>(std::round(std::cos(static_cast<double>(x))));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> exp(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
return static_cast<FP>(std::round(std::exp(static_cast<double>(x))));
|
||||
}
|
||||
|
||||
template <unsigned int I, unsigned int F, bool S>
|
||||
fixed_point<I, F, S> log(fixed_point<I, F, S> x)
|
||||
{
|
||||
using FP = fixed_point<I, F, S>;
|
||||
return static_cast<FP>(std::round(std::log(static_cast<double>(x))));
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue