diff --git a/libs/util/include/psemek/util/fixed_point.hpp b/libs/util/include/psemek/util/fixed_point.hpp index 8827092a..116e480a 100644 --- a/libs/util/include/psemek/util/fixed_point.hpp +++ b/libs/util/include/psemek/util/fixed_point.hpp @@ -106,7 +106,7 @@ namespace psemek::util }; template - H clamp(T x, H min, H max) + constexpr H clamp(T x, H min, H max) { if (x < static_cast(min)) return min; @@ -147,32 +147,37 @@ namespace psemek::util 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; + constexpr fixed_point() = default; + constexpr fixed_point(fixed_point const &) = default; + + constexpr fixed_point & operator = (fixed_point const &) = default; template - explicit fixed_point(T value); + constexpr explicit fixed_point(T value); template - explicit fixed_point(T value); + constexpr explicit fixed_point(T value); + + template + constexpr explicit fixed_point(fixed_point value); template - explicit operator T () const; + constexpr explicit operator T () const; template - explicit operator T () const; + constexpr explicit operator T () const; - fixed_point & operator += (fixed_point); - fixed_point & operator -= (fixed_point); - fixed_point & operator *= (fixed_point); - fixed_point & operator /= (fixed_point); + constexpr fixed_point & operator += (fixed_point); + constexpr fixed_point & operator -= (fixed_point); + constexpr fixed_point & operator *= (fixed_point); + constexpr fixed_point & operator /= (fixed_point); - rep_type rep() const { return rep_; } + constexpr rep_type rep() const { return rep_; } - static fixed_point from_rep(rep_type rep); + static constexpr fixed_point from_rep(rep_type rep); - static fixed_point min(); - static fixed_point max(); + static constexpr fixed_point min(); + static constexpr fixed_point max(); private: rep_type rep_; @@ -180,32 +185,47 @@ namespace psemek::util template template - fixed_point::fixed_point(T value) + constexpr fixed_point::fixed_point(T value) : rep_(static_cast(value) << F) {} template template - fixed_point::fixed_point(T value) + constexpr fixed_point::fixed_point(T value) : rep_(static_cast(detail::clamp(std::round(value * (static_cast(1) << F)), min().rep(), max().rep()))) {} + template + template + constexpr fixed_point::fixed_point(fixed_point value) + { + // TODO: what to do with signs? + if constexpr (F >= F2) + { + rep_ = static_cast(value.rep_) << (F - F2); + } + else + { + rep_ = static_cast(value.rep_) >> (F - F2); + } + } + template template - fixed_point::operator T () const + constexpr fixed_point::operator T () const { return static_cast(rep_) >> F; } template template - fixed_point::operator T () const + constexpr 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) + constexpr fixed_point fixed_point::from_rep(fixed_point::rep_type rep) { fixed_point result; result.rep_ = rep; @@ -213,7 +233,7 @@ namespace psemek::util } template - fixed_point fixed_point::min() + constexpr fixed_point fixed_point::min() { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -221,7 +241,7 @@ namespace psemek::util } template - fixed_point fixed_point::max() + constexpr fixed_point fixed_point::max() { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -229,7 +249,7 @@ namespace psemek::util } template - fixed_point operator - (fixed_point x) + constexpr fixed_point operator - (fixed_point x) { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -238,7 +258,7 @@ namespace psemek::util } template - fixed_point operator + (fixed_point x1, fixed_point x2) + constexpr fixed_point operator + (fixed_point x1, fixed_point x2) { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -247,7 +267,7 @@ namespace psemek::util } template - fixed_point operator - (fixed_point x1, fixed_point x2) + constexpr fixed_point operator - (fixed_point x1, fixed_point x2) { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -256,16 +276,16 @@ namespace psemek::util } template - fixed_point operator * (fixed_point x1, fixed_point x2) + constexpr 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)); + using unsigned_wide_type = typename FP::unsigned_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) + constexpr fixed_point operator / (fixed_point x1, fixed_point x2) { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -274,47 +294,47 @@ namespace psemek::util } template - fixed_point & fixed_point::operator += (fixed_point x) + constexpr fixed_point & fixed_point::operator += (fixed_point x) { (*this) = (*this) + x; return (*this); } template - fixed_point & fixed_point::operator -= (fixed_point x) + constexpr fixed_point & fixed_point::operator -= (fixed_point x) { (*this) = (*this) - x; return (*this); } template - fixed_point & fixed_point::operator *= (fixed_point x) + constexpr fixed_point & fixed_point::operator *= (fixed_point x) { (*this) = (*this) * x; return (*this); } template - fixed_point & fixed_point::operator /= (fixed_point x) + constexpr fixed_point & fixed_point::operator /= (fixed_point x) { (*this) = (*this) / x; return (*this); } template - bool operator == (fixed_point x1, fixed_point x2) + constexpr bool operator == (fixed_point x1, fixed_point x2) { return x1.rep() == x2.rep(); } template - auto operator <=> (fixed_point x1, fixed_point x2) + constexpr auto operator <=> (fixed_point x1, fixed_point x2) { return x1.rep() <=> x2.rep(); } template - fixed_point abs(fixed_point x) + constexpr fixed_point abs(fixed_point x) { using FP = fixed_point; using rep_type = typename FP::rep_type; @@ -325,11 +345,42 @@ namespace psemek::util } template - fixed_point round(fixed_point x) + constexpr fixed_point floor(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)); + constexpr auto fixed_point_one = static_cast(1) << F; + constexpr auto fractional_mask = fixed_point_one - static_cast(1); + return FP::from_rep(x.rep() & ~fractional_mask); + } + + template + constexpr fixed_point ceil(fixed_point x) + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + constexpr auto fixed_point_one = static_cast(1) << F; + constexpr auto fractional_mask = fixed_point_one - static_cast(1); + return FP::from_rep(((x.rep() - static_cast(1)) & ~fractional_mask) + fixed_point_one); + } + + template + constexpr fixed_point round(fixed_point x) + { + if constexpr (F == 0) + { + return x; + } + else + { + using FP = fixed_point; + using rep_type = typename FP::rep_type; + constexpr auto fixed_point_half = static_cast(1) << (F - 1); + if ((x.rep() & fixed_point_half) == 0) + return floor(x); + else + return ceil(x); + } } template @@ -342,7 +393,7 @@ namespace psemek::util } template - fixed_point pow(fixed_point x, int p) + constexpr fixed_point pow(fixed_point x, int p) { if (p < 0) return fixed_point(1) / pow(x, -p);