Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AH/STL/cmath>
       4             : #include <AH/STL/cstdint>
       5             : #include <AH/STL/limits>
       6             : #include <AH/STL/type_traits>
       7             : 
       8             : #include <AH/Error/Error.hpp>
       9             : 
      10             : template <class T>
      11             : struct DoubleWidthInt {
      12             :     using type = void;
      13             : };
      14             : 
      15             : template <>
      16             : struct DoubleWidthInt<uint8_t> {
      17             :     using type = uint16_t;
      18             : };
      19             : 
      20             : template <>
      21             : struct DoubleWidthInt<int8_t> {
      22             :     using type = int16_t;
      23             : };
      24             : 
      25             : template <>
      26             : struct DoubleWidthInt<uint16_t> {
      27             :     using type = uint32_t;
      28             : };
      29             : 
      30             : template <>
      31             : struct DoubleWidthInt<int16_t> {
      32             :     using type = int32_t;
      33             : };
      34             : 
      35             : template <>
      36             : struct DoubleWidthInt<uint32_t> {
      37             :     using type = uint64_t;
      38             : };
      39             : 
      40             : template <>
      41             : struct DoubleWidthInt<int32_t> {
      42             :     using type = int64_t;
      43             : };
      44             : 
      45             : /// @addtogroup FixedPoint
      46             : /// @{
      47             : 
      48             : /// Get the integer type that has twice the number of bits as the given type.
      49             : template <class T>
      50             : using DoubleWidthInt_t = typename DoubleWidthInt<T>::type;
      51             : 
      52             : /**
      53             :  * @brief   Very basic fixed-point integer implementation.
      54             :  * 
      55             :  * @tparam  T 
      56             :  *          The integer type to use.
      57             :  * @tparam  N 
      58             :  *          The number of fractional bits.
      59             :  * @tparam  T2 
      60             :  *          The integer type to use for intermediate values when mutliplying or
      61             :  *          dividing.
      62             :  */
      63             : template <class T, uint8_t N, class T2 = DoubleWidthInt_t<T>>
      64             : class FixedPoint {
      65             :   public:
      66             :     /// Fixed-point representation of the number one.
      67             :     constexpr static T one = 1 << N;
      68             : 
      69             :     /// Default constructor
      70       30399 :     FixedPoint() : val(0) {}
      71             : 
      72             :     /// Initialize U from a numeric value.
      73             :     template <class U>
      74         212 :     FixedPoint(U f) : val(std::llrint(f * one)) {}
      75             : 
      76             :     /// Convert a raw integer representation to fixed point type.
      77       29302 :     static constexpr FixedPoint raw(T t) {
      78       29302 :         FixedPoint res;
      79       29302 :         res.val = t;
      80       29302 :         return res;
      81             :     }
      82             : 
      83             :     /// Addition.
      84       11522 :     FixedPoint operator+(FixedPoint rhs) const {
      85             :         // if (rhs.val > 0 &&
      86             :         //     this->val > std::numeric_limits<T>::max() - rhs.val) {
      87             :         //     ERROR("Integer overflow", 0x0FF0);
      88             :         //     return raw(std::numeric_limits<T>::max());
      89             :         // }
      90             :         // if (rhs.val < 0 &&
      91             :         //     this->val < std::numeric_limits<T>::min() - rhs.val) {
      92             :         //     ERROR("Integer underflow", 0x0FF1);
      93             :         //     return raw(std::numeric_limits<T>::min());
      94             :         // }
      95       11522 :         return raw(static_cast<T>(
      96       11522 :             static_cast<typename std::make_unsigned<T>::type>(this->val) +
      97       11522 :             static_cast<typename std::make_unsigned<T>::type>(rhs.val)));
      98             :     }
      99             : 
     100             :     /// Subtraction.
     101          65 :     FixedPoint operator-(FixedPoint rhs) const {
     102             :         // return raw(this->val - rhs.val);
     103          65 :         return *this + -rhs;
     104             :     }
     105             : 
     106             :     /// Invert.
     107         125 :     FixedPoint operator-() const { return raw(-this->val); }
     108             : 
     109             :     /// Multiplication.
     110       14305 :     FixedPoint operator*(FixedPoint rhs) const {
     111       14305 :         return raw(div_N(T2(this->val) * T2(rhs.val)));
     112             :     }
     113             : 
     114             :     /// Multiplication with normal integer.
     115           2 :     T2 operator*(T2 rhs) const { return div_N(T2(this->val) * T2(rhs)); }
     116             : 
     117             :     /// Division.
     118        2880 :     FixedPoint operator/(FixedPoint rhs) const {
     119        2880 :         if (rhs.val == one)
     120        2848 :             return raw(this->val);
     121          32 :         return raw(T2(this->val) * one / rhs.val);
     122             :     }
     123             : 
     124             :     /// Divide the given integer by @f$ 2^N @f$.
     125       14307 :     static T div_N(T2 val) {
     126             :         static_assert(std::is_unsigned<T2>::value || (-97 * 2) >> 1 == -97,
     127             :                       "Negative signed right shift incorrect");
     128       14307 :         int neg = val < 0 ? 1 : 0;
     129       14307 :         return (val + (1 << (N - 1)) - neg) >> N;
     130             :     }
     131             : 
     132             :     explicit operator long double() const { return (long double)val / one; }
     133         714 :     explicit operator double() const { return (double)val / one; }
     134         224 :     explicit operator float() const { return (float)val / one; }
     135             : 
     136             :   private:
     137             :     T val;
     138             : };
     139             : 
     140             : /// Multiply normal integer with fixed point integer.
     141             : template <class T, uint8_t N, class T2>
     142           1 : T2 operator*(T lhs, FixedPoint<T, N, T2> rhs) {
     143           1 :     return rhs * lhs;
     144             : }
     145             : 
     146             : /// Multiply normal integer with fixed point integer.
     147             : template <class T, uint8_t N, class T2>
     148             : T2 operator*(T2 lhs, FixedPoint<T, N, T2> rhs) {
     149             :     return rhs * lhs;
     150             : }
     151             : 
     152             : #ifndef ARDUINO
     153             : 
     154             : #include <iosfwd>
     155             : 
     156             : /// Printing a fixed-point integer.
     157             : template <class T, class T2, uint8_t N>
     158             : std::ostream &operator<<(std::ostream &os, FixedPoint<T, N, T2> fp) {
     159             :     return os << double(fp);
     160             : }
     161             : 
     162             : #else
     163             : 
     164             : #include <AH/PrintStream/PrintStream.hpp>
     165             : 
     166             : /// Printing a fixed-point integer.
     167             : template <class T, uint8_t N, class T2>
     168             : Print &operator<<(Print &os, FixedPoint<T, N, T2> fp) {
     169             :     return os << double(fp);
     170             : }
     171             : 
     172             : #endif
     173             : 
     174             : /// @}