Advertisement
Guest User

Untitled

a guest
Apr 4th, 2012
96
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #ifndef run_time_fixed_pt_hpp
  2. #define run_time_fixed_pt_hpp
  3.  
  4. #include <boost/constrained_value.hpp>
  5. #include <boost/operators.hpp>
  6. #include <cmath> // ldexp
  7. #include <iostream>
  8. #include <boost/serialization/nvp.hpp>
  9. #include <boost/type_traits/is_floating_point.hpp>
  10. #include <boost/utility/enable_if.hpp>
  11. #include <limits>
  12.  
  13. namespace cv = boost::constrained_value;
  14.  
  15. namespace {
  16. template<typename base_type=int>
  17. struct rnd {
  18.   static base_type apply (base_type x, int frac_bits) {
  19.     return ((x >> (frac_bits-1)) + 1) >> 1;
  20.   }
  21. };
  22.  
  23. template<typename int_t, typename flt_t>
  24. inline int_t nint (flt_t x) { return (x >= 0) ? int_t (x + 0.5) : int_t (x - 0.5); }
  25.  
  26. template<typename int_t>
  27. inline int_t shift_right (int_t x, int bits) {
  28.   return x >> bits;
  29. }
  30. template<typename int_t>
  31. inline int_t shift_left (int_t x, int bits) {
  32.   return x << bits;
  33. }
  34.  
  35. // positive shift is <<
  36. template <typename T>
  37. inline T shift(T val, int bits) {
  38.   if (bits<0) {
  39.     return shift_right (val, -bits);
  40.   } else {
  41.     return shift_left (val, bits);
  42.   }
  43. }
  44.  
  45. }
  46.  
  47. namespace boost { namespace constrained_value {
  48. template<typename V, typename C, typename E>
  49. cv::constrained<V,C,E> operator- (cv::constrained<V,C,E> c) {
  50.   V tmp (c.value());
  51.   c = -tmp;
  52.   return c;
  53. }
  54. template<typename V, typename C, typename E>
  55. cv::constrained<V,C,E> operator~ (cv::constrained<V,C,E> c) {
  56.   V tmp (c.value());
  57.   c = ~tmp;
  58.   return c;
  59. }
  60.   }}
  61.  
  62. //! signed fixed pt
  63. template<typename base_type=int, typename error_policy=cv::throw_exception<>, typename round_policy=rnd<base_type> >
  64. struct fixed_pt  :
  65.   boost::ordered_euclidian_ring_operators< fixed_pt<base_type,error_policy,round_policy>, base_type>,
  66.           //  boost::ordered_euclidian_ring_operators< fixed_pt<base_type,error_policy,round_policy>, double>,
  67.   boost::ordered_euclidian_ring_operators< fixed_pt<base_type,error_policy,round_policy> >,
  68.   boost::shiftable<fixed_pt<base_type,error_policy,round_policy>, base_type>
  69.  
  70. {
  71.   typedef fixed_pt<base_type,error_policy,round_policy> self;
  72.  
  73.   typedef base_type base_t; // export
  74.  
  75.   typedef typename cv::bounded<base_type,base_type,base_type,error_policy>::type val_t;
  76.  
  77.   int int_bits;
  78.   int frac_bits;
  79.   val_t val;
  80.   static const bool is_signed = std::numeric_limits<base_type>::is_signed;
  81.  
  82.   int total_bits() const { return int_bits + frac_bits; }
  83.   base_type max() const { return is_signed ? (~(base_type(-1) << (total_bits()-1))) : (~(base_type (-1) << total_bits())) ; }
  84.   base_type min() const { return is_signed ? ((base_type(-1) << (total_bits()-1))) : 0 ; }
  85.   typedef typename cv::bounded<base_type,base_type,base_type,error_policy>::type::constraint_type constraint_type;
  86.  
  87.   /* explicit */ fixed_pt () {}
  88.  
  89.   /* explicit */ fixed_pt (int _int_bits, int _frac_bits, val_t v) :
  90.     int_bits (_int_bits),
  91.     frac_bits (_frac_bits),
  92.     val (v) {}
  93.  
  94.   /* explicit */ fixed_pt (int _int_bits, int _frac_bits, base_type x, bool scale=true) :
  95.     int_bits (_int_bits),
  96.     frac_bits (_frac_bits),
  97.     val (scale ? (shiftup_or_rnddn (x, frac_bits)) : x, constraint_type (min(), max())) {}
  98.  
  99.   /* explicit */ fixed_pt (int _int_bits, int _frac_bits) :
  100.     int_bits (_int_bits),
  101.     frac_bits (_frac_bits),
  102.     val (constraint_type (min(), max()))
  103.     {}
  104.  
  105.   template<typename _base_type, typename _error_policy, typename _round_policy>
  106.   /* explicit */ fixed_pt (int _int_bits, int _frac_bits, fixed_pt<_base_type, _error_policy, _round_policy>const& other) :
  107.     int_bits (_int_bits),
  108.     frac_bits (_frac_bits),
  109.     val (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits),
  110.      constraint_type (min(), max()))
  111.   {}
  112.  
  113.   // re-construct (useful for default constructed)
  114.   template<typename flt_t>
  115.   self& re_init (int _int_bits, int _frac_bits, flt_t x, typename boost::enable_if<boost::is_float<flt_t> >::type* dummy=0) {
  116.     int_bits = _int_bits;
  117.     frac_bits = _frac_bits;
  118.     change_constraint (val, constraint_type (min(), max()));
  119.     val = nint<base_type> (x * std::pow(flt_t(2), frac_bits));
  120.  
  121.     return *this;
  122.   }
  123.  
  124.   self&  operator= (self const& other) {
  125.     val = (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits));
  126.     return *this;
  127.   }
  128.  
  129.   template<typename _base_type, typename _error_policy, typename _round_policy>
  130.   self&  operator= (fixed_pt<_base_type, _error_policy, _round_policy>const& other) {
  131.     val = (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits));
  132.     return *this;
  133.   }
  134.  
  135.   template<typename flt_t>
  136.   fixed_pt (int _int_bits, int _frac_bits, flt_t x, typename boost::enable_if<boost::is_float<flt_t> >::type* dummy=0) :
  137.     int_bits (_int_bits),
  138.     frac_bits (_frac_bits),
  139.     val (nint<base_type> (x * std::pow(flt_t(2), frac_bits)), constraint_type (min(), max())){};
  140.  
  141.  
  142.   // void set_bits (int _int_bits, int _frac_bits) {
  143.   //   base_type old_val = val.value();
  144.   //   int_bits = _int_bits;
  145.   //   frac_bits = _frac_bits;
  146.   //   val = val_t (nint<base_type> (old_val * (1 << frac_bits)), constraint_type (min(), max()));
  147.   // }
  148.  
  149.   //operator base_type() const { return round_policy::apply (val); }
  150.  
  151.   //! convert other fixed_pt to our scaling
  152.   base_type convert (self const& x) const {
  153.     return shiftup_or_rnddn (base_type (x.val), frac_bits - x.frac_bits);
  154.   }
  155.  
  156.   self& operator+=(self const& x) {
  157.     val += convert (x);
  158.     return *this;
  159.   }
  160.   self& operator-=(self const& x) {
  161.       val -= convert (x);
  162.     return *this;
  163.   }
  164.   self& operator*=(self const& x) {
  165.     base_type tmp = (base_type)val * (base_type)convert(x);
  166.     val = tmp >> (frac_bits);
  167.     return *this;
  168.   }
  169.  
  170.   self& operator/=(self const& x) {
  171.     val = ((base_type)val << frac_bits) / (base_type)x.val;
  172.     return *this; }
  173.  
  174.   self from_base (base_type x) const {
  175.     return self (int_bits, frac_bits, x);
  176.   }
  177.  
  178.   // self from_double (double x) const { // const?
  179.   //   return self (int_bits, frac_bits, x);
  180.   // }
  181.  
  182.   // Don't do these, force conversion to fixed_pt and use above
  183.   self& operator+=(base_type x) { *this += from_base (x) ; return *this; }
  184.   self& operator-=(base_type x) { *this -= from_base (x); return *this; }
  185.   self& operator*=(base_type x) { val *= x; return *this; }
  186.   self& operator/=(base_type x) { val /= x; return *this; }
  187.  
  188.   self& operator<<=(int x) { val <<= x; return *this; }
  189.   self& operator>>=(int x) { val >>= x; return *this; }
  190.  
  191.   bool operator==(self const& x) const { return val == convert (x); }
  192.   bool operator<(self const& x) const { return val < convert (x); }
  193.  
  194.   self& operator |=(self const& x) { val |= convert (x); return *this; }
  195.   self& operator &=(self const& x) { val &= convert (x); return *this; }
  196.   self& operator ^=(self const& x) { val ^= convert (x); return *this; }
  197.  
  198.   //  self operator %=(base_type x) { val %= x; return *this; } What should this do?
  199.   self operator ~() const { self f = *this; f.val = ~f.val; return f; }
  200.   self operator -() const { self f = *this; f.val = -f.val; return f; }
  201.   //  template<int _int_bits, int _frac_bits, typename _base_type, typename _round_policy>
  202.   // self operator+= (fixed_pt<_int_bits, _frac_bits, _base_type, _round_policy>const& other) {
  203.   // }
  204.  
  205.   template<typename flt_t>
  206.   flt_t as_flt() const { return std::ldexp (static_cast<flt_t>(val), -frac_bits); }
  207.   base_type as_base() const { return shiftup_or_rnddn (val, -frac_bits); }
  208.  
  209.   base_type shiftup_or_rnddn (base_type x, int bits) const {
  210.     if (bits > 0)
  211.       return shift_left (x, bits);
  212.     else if (bits < 0)
  213.       return round_policy::apply (x, -bits);
  214.     else
  215.       return x;
  216.   }
  217.  
  218.   friend class boost::serialization::access;
  219.  
  220.   template<typename Archive>
  221.   void serialize (Archive & ar, const unsigned int /* file_verison */) {
  222.     ar & boost::serialization::make_nvp ("int_bits", int_bits);
  223.     ar & boost::serialization::make_nvp ("frac_bits", frac_bits);
  224.     ar & boost::serialization::make_nvp ("val", val); // this isn't useful till val_t has serialization
  225.   }
  226.  
  227. };
  228.  
  229. template<typename base_type, typename error_policy, typename round_policy>
  230. std::ostream& operator<< (std::ostream& os, fixed_pt<base_type,error_policy,round_policy> const& fp) {
  231.   os << '[' << fp.val << ',' << fp.int_bits << ',' << fp.frac_bits << ']';
  232.   return os;
  233. }
  234.  
  235. #endif
Advertisement
RAW Paste Data Copied
Advertisement