388 lines
12 KiB
C++
388 lines
12 KiB
C++
#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 1 / 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. Note that the result is still truncated
|
|
* to the original size, i.e. multiplying two 16.16 fixed-points gives a 16.16 fixed-point
|
|
* as well.
|
|
*
|
|
* 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 <typename FloatingPoint, typename Function, typename FixedPoint1, typename ... FixedPoint>
|
|
auto apply_via_floating_point(Function && function, FixedPoint1 const & x1, FixedPoint const & ... xs)
|
|
{
|
|
static_assert((std::is_same_v<FixedPoint1, FixedPoint> && ...));
|
|
return static_cast<FixedPoint1>(std::forward<Function>(function)(static_cast<FloatingPoint>(x1), static_cast<FloatingPoint>(xs) ...));
|
|
}
|
|
|
|
}
|
|
|
|
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);
|
|
|
|
// Binary exponentiation algorithm
|
|
|
|
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)
|
|
{
|
|
return detail::apply_via_floating_point<double>(&std::sin, x);
|
|
}
|
|
|
|
template <unsigned int I, unsigned int F, bool S>
|
|
fixed_point<I, F, S> cos(fixed_point<I, F, S> x)
|
|
{
|
|
return detail::apply_via_floating_point<double>(&std::cos, x);
|
|
}
|
|
|
|
template <unsigned int I, unsigned int F, bool S>
|
|
fixed_point<I, F, S> exp(fixed_point<I, F, S> x)
|
|
{
|
|
return detail::apply_via_floating_point<double>(&std::exp, x);
|
|
}
|
|
|
|
template <unsigned int I, unsigned int F, bool S>
|
|
fixed_point<I, F, S> log(fixed_point<I, F, S> x)
|
|
{
|
|
return detail::apply_via_floating_point<double>(&std::log, x);
|
|
}
|
|
|
|
}
|