From d66092b86da23f7b6b09b24f5c40bf45edd380ba Mon Sep 17 00:00:00 2001 From: lisyarus Date: Thu, 5 Sep 2024 23:16:18 +0300 Subject: [PATCH] Add fixed-point arithmetic implementation --- libs/util/include/psemek/util/fixed_point.hpp | 381 ++++++++++++++++++ 1 file changed, 381 insertions(+) create mode 100644 libs/util/include/psemek/util/fixed_point.hpp diff --git a/libs/util/include/psemek/util/fixed_point.hpp b/libs/util/include/psemek/util/fixed_point.hpp new file mode 100644 index 00000000..115590b4 --- /dev/null +++ b/libs/util/include/psemek/util/fixed_point.hpp @@ -0,0 +1,381 @@ +#pragma once + +#include +#include +#include +#include + +namespace psemek::util +{ + + /** A base template class implementing fixed-point arithmetic. + * + * unsigned_fixed_point 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 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(1.618033); + * float z = static_cast(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 + 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 + H clamp(T x, H min, H max) + { + if (x < static_cast(min)) + return min; + if (x > static_cast(max)) + return max; + return static_cast(x); + } + + } + + template + struct fixed_point; + + template + using unsigned_fixed_point = fixed_point; + + template + using signed_fixed_point = fixed_point; + + template + 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; + + using rep_type = std::conditional_t; + using wide_type = std::conditional_t; + 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 + explicit fixed_point(T value); + + template + explicit fixed_point(T value); + + template + explicit operator T () const; + + template + 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 + template + fixed_point::fixed_point(T value) + : rep_(static_cast(value) << F) + {} + + template + template + fixed_point::fixed_point(T value) + : rep_(static_cast(detail::clamp(std::round(value * (static_cast(1) << F)), min().rep(), max().rep()))) + {} + + template + template + fixed_point::operator T () const + { + return static_cast(rep_) >> F; + } + + template + template + fixed_point::operator T () const + { + return rep_ / static_cast(static_cast(1) << F); + } + + template + fixed_point fixed_point::from_rep(fixed_point::rep_type rep) + { + fixed_point result; + result.rep_ = rep; + return result; + } + + template + fixed_point fixed_point::min() + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + return FP::from_rep(std::numeric_limits::min()); + } + + template + fixed_point fixed_point::max() + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + return FP::from_rep(std::numeric_limits::max()); + } + + template + fixed_point operator - (fixed_point x) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using unsigned_rep_type = typename FP::unsigned_rep_type; + return FP::from_rep(static_cast(static_cast(-x.rep()))); + } + + template + fixed_point operator + (fixed_point x1, fixed_point x2) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using unsigned_rep_type = typename FP::unsigned_rep_type; + return FP::from_rep(static_cast(static_cast(x1.rep()) + static_cast(x2.rep()))); + } + + template + fixed_point operator - (fixed_point x1, fixed_point x2) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using unsigned_rep_type = typename FP::unsigned_rep_type; + return FP::from_rep(static_cast(static_cast(x1.rep()) - static_cast(x2.rep()))); + } + + template + fixed_point operator * (fixed_point x1, fixed_point x2) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using wide_type = typename FP::wide_type; + return FP::from_rep(static_cast((static_cast(x1.rep()) * static_cast(x2.rep())) >> F)); + } + + template + fixed_point operator / (fixed_point x1, fixed_point x2) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using wide_type = typename FP::wide_type; + return FP::from_rep(static_cast((static_cast(x1.rep()) << F) / static_cast(x2.rep()))); + } + + template + fixed_point & fixed_point::operator += (fixed_point x) + { + (*this) = (*this) + x; + return (*this); + } + + template + fixed_point & fixed_point::operator -= (fixed_point x) + { + (*this) = (*this) - x; + return (*this); + } + + template + fixed_point & fixed_point::operator *= (fixed_point x) + { + (*this) = (*this) * x; + return (*this); + } + + template + fixed_point & fixed_point::operator /= (fixed_point x) + { + (*this) = (*this) / x; + return (*this); + } + + template + bool operator == (fixed_point x1, fixed_point x2) + { + return x1.rep() == x2.rep(); + } + + template + auto operator <=> (fixed_point x1, fixed_point x2) + { + return x1.rep() <=> x2.rep(); + } + + template + fixed_point abs(fixed_point x) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + if constexpr (S) + return FP::from_rep(x.rep() & ~(static_cast(1) << (I + F - 1))); + else + return x; + } + + template + fixed_point round(fixed_point x) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + return FP::from_rep(x.rep() & ~((static_cast(1) << F) - 1)); + } + + template + fixed_point sqrt(fixed_point x) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + using wide_type = typename FP::wide_type; + return FP::from_rep(static_cast(std::round(std::sqrt(static_cast(x.rep()) << F)))); + } + + template + fixed_point pow(fixed_point x, int p) + { + if (p < 0) + return fixed_point(1) / pow(x, -p); + + fixed_point result(1); + while (p > 0) + { + if ((p & 1) == 1) + result *= x; + x *= x; + p >>= 1; + } + + return result; + } + + template + fixed_point sin(fixed_point x) + { + using FP = fixed_point; + return static_cast(std::round(std::sin(static_cast(x)))); + } + + template + fixed_point cos(fixed_point x) + { + using FP = fixed_point; + return static_cast(std::round(std::cos(static_cast(x)))); + } + + template + fixed_point exp(fixed_point x) + { + using FP = fixed_point; + return static_cast(std::round(std::exp(static_cast(x)))); + } + + template + fixed_point log(fixed_point x) + { + using FP = fixed_point; + return static_cast(std::round(std::log(static_cast(x)))); + } + +}