diff --git a/sdrbase/util/fixed.h b/sdrbase/util/fixed.h index 582c5e05d..6cc3a5d19 100644 --- a/sdrbase/util/fixed.h +++ b/sdrbase/util/fixed.h @@ -27,22 +27,31 @@ #include #include +#include #include #include "fixedtraits.h" -// Internally 1 is 2^28. 28 is the highest power of two that can represent 9.99999... safely on 64 bits internally - -unsigned const fixed_resolution_shift=28; -int64_t const fixed_resolution=1LL< class Fixed { private: IntType m_nVal; + static int64_t scale_cordic_result(int64_t a); + static int64_t right_shift(int64_t val,int shift); + static void perform_cordic_rotation(int64_t&px, int64_t&py, int64_t theta); + static void perform_cordic_polarization(int64_t& argx, int64_t&argy); + public: + static const Fixed fixed_max; + static const Fixed fixed_one; + static const Fixed fixed_zero; + static const Fixed fixed_half; + static const Fixed fixed_pi; + static const Fixed fixed_two_pi; + static const Fixed fixed_half_pi; + static const Fixed fixed_quarter_pi; struct internal {}; @@ -128,7 +137,7 @@ public: operator bool() const { - return m_nVal?true:false; + return m_nVal ? true : false; } inline operator double() const @@ -136,68 +145,68 @@ public: return as_double(); } - int64_t as_internal() const + IntType as_internal() const { return m_nVal; } float as_float() const { - return m_nVal/(float)fixed_resolution; + return m_nVal / (float) FixedTraits::fixed_resolution; } double as_double() const { - return m_nVal/(double)fixed_resolution; + return m_nVal / (double) FixedTraits::fixed_resolution; } int64_t as_long() const { - return (int64_t)(m_nVal/fixed_resolution); + return (int64_t) (m_nVal / FixedTraits::fixed_resolution); } int64_t as_int64() const { - return m_nVal/fixed_resolution; + return m_nVal / FixedTraits::fixed_resolution; } int as_int() const { - return (int)(m_nVal/fixed_resolution); + return (int) (m_nVal / FixedTraits::fixed_resolution); } uint64_t as_unsigned_long() const { - return (uint64_t)(m_nVal/fixed_resolution); + return (uint64_t) (m_nVal / FixedTraits::fixed_resolution); } uint64_t as_unsigned_int64() const { - return (uint64_t)m_nVal/fixed_resolution; + return (uint64_t) (m_nVal / FixedTraits::fixed_resolution); } unsigned int as_unsigned_int() const { - return (unsigned int)(m_nVal/fixed_resolution); + return (unsigned int) (m_nVal / FixedTraits::fixed_resolution); } short as_short() const { - return (short)(m_nVal/fixed_resolution); + return (short) (m_nVal / FixedTraits::fixed_resolution); } unsigned short as_unsigned_short() const { - return (unsigned short)(m_nVal/fixed_resolution); + return (unsigned short) (m_nVal / FixedTraits::fixed_resolution); } Fixed operator++() { - m_nVal += fixed_resolution; + m_nVal += FixedTraits::fixed_resolution; return *this; } Fixed operator--() { - m_nVal -= fixed_resolution; + m_nVal -= FixedTraits::fixed_resolution; return *this; } @@ -224,99 +233,99 @@ public: Fixed& operator*=(double val) { - return (*this)*=Fixed(val); + return (*this) *= Fixed(val); } Fixed& operator*=(float val) { - return (*this)*=Fixed(val); + return (*this) *= Fixed(val); } Fixed& operator*=(int64_t val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(int val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(short val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(char val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(uint64_t val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(unsigned int val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(unsigned short val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator*=(unsigned char val) { - m_nVal*=val; + m_nVal *= val; return *this; } Fixed& operator/=(double val) { - return (*this)/=Fixed(val); + return (*this) /= Fixed(val); } Fixed& operator/=(float val) { - return (*this)/=Fixed(val); + return (*this) /= Fixed(val); } Fixed& operator/=(int64_t val) { - m_nVal/=val; + m_nVal /= val; return *this; } Fixed& operator/=(int val) { - m_nVal/=val; + m_nVal /= val; return *this; } Fixed& operator/=(short val) { - m_nVal/=val; + m_nVal /= val; return *this; } Fixed& operator/=(char val) { - m_nVal/=val; + m_nVal /= val; return *this; } Fixed& operator/=(uint64_t val) { - m_nVal/=val; + m_nVal /= val; return *this; } @@ -328,19 +337,19 @@ public: Fixed& operator/=(unsigned short val) { - m_nVal/=val; + m_nVal /= val; return *this; } Fixed& operator/=(unsigned char val) { - m_nVal/=val; + m_nVal /= val; return *this; } bool operator!() const { - return m_nVal==0; + return m_nVal == 0; } Fixed modf(Fixed* integral_part) const; @@ -362,6 +371,434 @@ inline std::ostream& operator<<(std::ostream& os,fixed const& value) return os< +Fixed& Fixed::operator%=(Fixed const& other) +{ + m_nVal = m_nVal%other.m_nVal; + return *this; +} + +template +Fixed& Fixed::operator*=(Fixed const& val) +{ + bool const val_negative = val.m_nVal < 0; + bool const this_negative = m_nVal < 0; + bool const negate = val_negative ^ this_negative; + uint64_t const other = val_negative ? -val.m_nVal : val.m_nVal; + uint64_t const self = this_negative ? -m_nVal : m_nVal; + + if (uint64_t const self_upper = (self >> 32)) { + m_nVal = (self_upper*other) << (32 - FixedTraits::fixed_resolution_shift); + } else { + m_nVal = 0; + } + + if (uint64_t const self_lower = (self&0xffffffff)) + { + uint64_t const other_upper = static_cast(other >> 32); + uint64_t const other_lower = static_cast(other & 0xffffffff); + uint64_t const lower_self_upper_other_res = self_lower*other_upper; + uint64_t const lower_self_lower_other_res = self_lower*other_lower; + m_nVal += (lower_self_upper_other_res << (32 - FixedTraits::fixed_resolution_shift)) + + (lower_self_lower_other_res >> FixedTraits::fixed_resolution_shift); + } + + if (negate) { + m_nVal=-m_nVal; + } + + return *this; +} + +template +Fixed& Fixed::operator/=(Fixed const& divisor) +{ + if( !divisor.m_nVal) + { + m_nVal = fixed_max.m_nVal; + } + else + { + bool const negate_this = (m_nVal < 0); + bool const negate_divisor = (divisor.m_nVal < 0); + bool const negate = negate_this ^ negate_divisor; + uint64_t a = negate_this ? -m_nVal : m_nVal; + uint64_t b = negate_divisor ? -divisor.m_nVal : divisor.m_nVal; + + uint64_t res = 0; + + uint64_t temp = b; + bool const a_large = a > b; + unsigned shift = FixedTraits::fixed_resolution_shift; + + if(a_large) + { + uint64_t const half_a = a>>1; + + while (temp < half_a) + { + temp <<= 1; + ++shift; + } + } + + uint64_t d = 1LL << shift; + + if (a_large) + { + a -= temp; + res += d; + } + + while (a && temp && shift) + { + unsigned right_shift = 0; + + while(right_shift < shift && (temp > a)) + { + temp >>= 1; + ++right_shift; + } + + d >>= right_shift; + shift -= right_shift; + a -= temp; + res += d; + } + + m_nVal = (negate ? -(int64_t) res : res); + } + + return *this; +} + +template +Fixed Fixed::sqrt() const +{ + unsigned const max_shift = 62; + uint64_t a_squared = 1LL << max_shift; + unsigned b_shift = (max_shift + FixedTraits::fixed_resolution_shift) / 2; + uint64_t a = 1LL << b_shift; + + uint64_t x = m_nVal; + + while (b_shift && (a_squared > x)) + { + a >>= 1; + a_squared >>= 2; + --b_shift; + } + + uint64_t remainder = x - a_squared; + --b_shift; + + while (remainder && b_shift) + { + uint64_t b_squared = 1LL << (2*b_shift - FixedTraits::fixed_resolution_shift); + int const two_a_b_shift = b_shift + 1 - FixedTraits::fixed_resolution_shift; + uint64_t two_a_b = (two_a_b_shift > 0) ? (a << two_a_b_shift) : (a >> -two_a_b_shift); + + while (b_shift && (remainder < (b_squared+two_a_b))) + { + b_squared >>= 2; + two_a_b >>= 1; + --b_shift; + } + + uint64_t const delta = b_squared + two_a_b; + + if ((2*remainder) > delta) + { + a += (1LL << b_shift); + remainder -= delta; + + if (b_shift) { + --b_shift; + } + } + } + + return Fixed(Fixed::internal(), a); +} + +template +Fixed Fixed::exp() const +{ + if (m_nVal >= FixedTraits::log_two_power_n_reversed[0]) + { + return fixed_max; + } + + if (m_nVal < -FixedTraits::log_two_power_n_reversed[63 - 2*FixedTraits::fixed_resolution_shift]) + { + return Fixed(internal(), 0); + } + + if (!m_nVal) + { + return Fixed(internal(), FixedTraits::fixed_resolution); + } + + int64_t res = FixedTraits::fixed_resolution; + + if (m_nVal > 0) + { + int power = FixedTraits::max_power; + int64_t const* log_entry = FixedTraits::log_two_power_n_reversed; + int64_t temp=m_nVal; + + while (temp && power>(-(int)FixedTraits::fixed_resolution_shift)) + { + while (!power || (temp<*log_entry)) + { + if (!power) { + log_entry = FixedTraits::log_one_plus_two_power_minus_n; + } else { + ++log_entry; + } + + --power; + } + + temp -= *log_entry; + + if (power < 0) { + res += (res >> (-power)); + } else { + res <<= power; + } + } + } + else + { + int power = FixedTraits::fixed_resolution_shift; + int64_t const* log_entry = FixedTraits::log_two_power_n_reversed + (FixedTraits::max_power-power); + int64_t temp=m_nVal; + + while (temp && (power > (-(int) FixedTraits::fixed_resolution_shift))) + { + while (!power || (temp > (-*log_entry))) + { + if(!power) { + log_entry = FixedTraits::log_one_over_one_minus_two_power_minus_n; + } else { + ++log_entry; + } + + --power; + } + + temp += *log_entry; + + if (power <0 ) { + res -= (res >> (-power)); + } else { + res >>= power; + } + } + } + + return Fixed(Fixed::internal(), res); +} + +template +Fixed Fixed::log() const +{ + if ( m_nVal <= 0) { + return -fixed_max; + } + + if (m_nVal == FixedTraits::fixed_resolution) { + return fixed_zero; + } + + uint64_t temp = m_nVal; + int left_shift = 0; + uint64_t const scale_position = 0x8000000000000000ULL; + + while (temp < scale_position) + { + ++left_shift; + temp <<= 1; + } + + int64_t res = (left_shift < FixedTraits::max_power) ? + FixedTraits::log_two_power_n_reversed[left_shift] : + -FixedTraits::log_two_power_n_reversed[2*FixedTraits::max_power - left_shift]; + unsigned int right_shift = 1; + uint64_t shifted_temp = temp >> 1; + + while (temp && (right_shift < FixedTraits::fixed_resolution_shift)) + { + while ((right_shift < FixedTraits::fixed_resolution_shift) && (temp < (shifted_temp + scale_position))) + { + shifted_temp >>= 1; + ++right_shift; + } + + temp -= shifted_temp; + shifted_temp = temp >> right_shift; + res += FixedTraits::log_one_over_one_minus_two_power_minus_n[right_shift-1]; + } + + return Fixed(Fixed::internal(), res); +} + +template +int64_t Fixed::scale_cordic_result(int64_t a) +{ + int64_t const cordic_scale_factor = 0x22C2DD1C; /* 0.271572 * 2^31*/ + return (int64_t)((((int64_t)a)*cordic_scale_factor) >> 31); +} + +template +int64_t Fixed::right_shift(int64_t val, int shift) +{ + return (shift < 0) ? (val << -shift) : (val >> shift); +} + +template +void Fixed::perform_cordic_rotation(int64_t&px, int64_t&py, int64_t theta) +{ + int64_t x = px, y = py; + int64_t const *arctanptr = FixedTraits::arctantab; + for (int i = -1; i <= (int) FixedTraits::fixed_resolution_shift; ++i) + { + int64_t const yshift = right_shift(y,i); + int64_t const xshift = right_shift(x,i); + + if (theta < 0) + { + x += yshift; + y -= xshift; + theta += *arctanptr++; + } + else + { + x -= yshift; + y += xshift; + theta -= *arctanptr++; + } + } + + px = scale_cordic_result(x); + py = scale_cordic_result(y); +} + +template +void Fixed::perform_cordic_polarization(int64_t& argx, int64_t&argy) +{ + int64_t theta = 0; + int64_t x = argx, y = argy; + int64_t const *arctanptr = FixedTraits::arctantab; + + for (int i = -1; i <= (int) FixedTraits::fixed_resolution_shift; ++i) + { + int64_t const yshift = right_shift(y,i); + int64_t const xshift = right_shift(x,i); + + if(y < 0) + { + y += xshift; + x -= yshift; + theta -= *arctanptr++; + } + else + { + y -= xshift; + x += yshift; + theta += *arctanptr++; + } + } + + argx = scale_cordic_result(x); + argy = theta; +} + +template +void Fixed::sin_cos(Fixed const& theta, Fixed* s, Fixed*c) +{ + int64_t x = theta.m_nVal % FixedTraits::internal_two_pi; + + if (x < 0) { + x += FixedTraits::internal_two_pi; + } + + bool negate_cos = false; + bool negate_sin = false; + + if (x > FixedTraits::internal_pi) + { + x = FixedTraits::internal_two_pi - x; + negate_sin=true; + } + + if (x > FixedTraits::internal_half_pi) + { + x = FixedTraits::internal_pi - x; + negate_cos=true; + } + + int64_t x_cos = 1<<28, x_sin = 0; + + perform_cordic_rotation(x_cos, x_sin, (int64_t) x); + + if (s) { + s->m_nVal = negate_sin ? -x_sin : x_sin; + } + + if(c) { + c->m_nVal = negate_cos ? -x_cos : x_cos; + } +} + +template +Fixed Fixed::atan() const +{ + Fixed r, theta; + to_polar(1, *this, &r, &theta); + return theta; +} + +template +void Fixed::to_polar(Fixed const& x, Fixed const& y, Fixed* r, Fixed *theta) +{ + bool const negative_x = x.m_nVal < 0; + bool const negative_y = y.m_nVal < 0; + + uint64_t a = negative_x ? -x.m_nVal : x.m_nVal; + uint64_t b = negative_y ? -y.m_nVal : y.m_nVal; + + unsigned int right_shift = 0; + unsigned const max_value = 1U << FixedTraits::fixed_resolution_shift; + + while ((a >= max_value) || (b >= max_value)) + { + ++right_shift; + a >>= 1; + b >>= 1; + } + + int64_t xtemp = (int64_t) a; + int64_t ytemp = (int64_t) b; + + perform_cordic_polarization(xtemp, ytemp); + + r->m_nVal = int64_t(xtemp) << right_shift; + theta->m_nVal = ytemp; + + if (negative_x && negative_y) { + theta->m_nVal -= FixedTraits::internal_pi; + } else if (negative_x) { + theta->m_nVal = FixedTraits::internal_pi - theta->m_nVal; + } + + else if(negative_y) { + theta->m_nVal = -theta->m_nVal; + } +} + template inline Fixed operator-(double a, Fixed const& b) { @@ -462,7 +899,7 @@ inline Fixed operator-(Fixed const& a, int64 } template -inline Fixed operator-(Fixed const& a, unsigned b) +inline Fixed operator-(Fixed const& a, unsigned b) { Fixed temp(a); return temp -= b; @@ -541,7 +978,7 @@ inline Fixed operator%(int64_t a, Fixed cons template inline Fixed operator%(unsigned a, Fixed const& b) { - Fixed temp(a); + Fixed temp(a); return temp %= b; } @@ -728,7 +1165,7 @@ inline Fixed operator+(char a, Fixed const& } template -inline Fixed operator+(Fixed const& a, double b) +inline Fixed operator+(Fixed const& a, double b) { Fixed temp(a); return temp += b; @@ -1381,9 +1818,9 @@ inline bool operator<(unsigned short a, Fixed const& b) } template -inline bool operator<(short a, Fixed const& b) +inline bool operator<(short a, Fixed const& b) { - return Fixed(a)(a) < b; } template @@ -1881,12 +2318,9 @@ inline Fixed modf(Fixed const& x, Fixed inline Fixed Fixed::ceil() const { - if(m_nVal%fixed_resolution) - { + if (m_nVal % FixedTraits::fixed_resolution) { return floor()+1; - } - else - { + } else { return *this; } } @@ -1895,15 +2329,17 @@ template inline Fixed Fixed::floor() const { Fixed res(*this); - int64_t const remainder=m_nVal%fixed_resolution; - if(remainder) + int64_t const remainder = m_nVal % FixedTraits::fixed_resolution; + + if (remainder) { - res.m_nVal-=remainder; - if(m_nVal<0) - { + res.m_nVal -= remainder; + + if(m_nVal<0) { res-=1; } } + return res; } @@ -1911,7 +2347,7 @@ template inline Fixed Fixed::sin() const { Fixed res; - sin_cos(*this,&res,0); + sin_cos(*this,&res, 0); return res; } @@ -1919,44 +2355,44 @@ template inline Fixed Fixed::cos() const { Fixed res; - sin_cos(*this,0,&res); + sin_cos(*this,0, &res); return res; } template inline Fixed Fixed::tan() const { - Fixed s,c; - sin_cos(*this,&s,&c); + Fixed s, c; + sin_cos(*this, &s, &c); return s/c; } template inline Fixed Fixed::operator-() const { - return Fixed(internal(),-m_nVal); + return Fixed(internal(), -m_nVal); } template inline Fixed Fixed::abs() const { - return Fixed(internal(),m_nVal<0?-m_nVal:m_nVal); + return Fixed(Fixed::internal(), m_nVal < 0 ? -m_nVal : m_nVal); } template -inline Fixed Fixed::modf(Fixed*integral_part) const +inline Fixed Fixed::modf(Fixed *integral_part) const { - int64_t fractional_part=m_nVal%fixed_resolution; - if(m_nVal<0 && fractional_part>0) - { - fractional_part-=fixed_resolution; + int64_t fractional_part = m_nVal % FixedTraits::fixed_resolution; + if ((m_nVal < 0) && (fractional_part > 0)) { + fractional_part -= FixedTraits::fixed_resolution; } - integral_part->m_nVal=m_nVal-fractional_part; - return Fixed(internal(),fractional_part); + + integral_part->m_nVal = m_nVal - fractional_part; + return Fixed(Fixed::internal(), fractional_part); } template -inline Fixed arg(const std::complex>& val) +inline Fixed arg(const std::complex >& val) { Fixed r,theta; Fixed::to_polar(val.real(),val.imag(),&r,&theta); @@ -1964,20 +2400,35 @@ inline Fixed arg(const std::complex>& val) } template -inline std::complex> polar(Fixed const& rho, Fixed const& theta) +inline std::complex > polar(const Fixed& rho, const Fixed& theta) { Fixed s,c; Fixed::sin_cos(theta,&s,&c); - return std::complex>(rho * c, rho * s); + return std::complex >(rho * c, rho * s); } -Fixed const fixed_max(Fixed::internal(),0x7fffffffffffffffLL); -Fixed const fixed_one(Fixed::internal(),1LL<<(fixed_resolution_shift)); -Fixed const fixed_zero(Fixed::internal(),0); -Fixed const fixed_half(Fixed::internal(),1LL<<(fixed_resolution_shift-1)); -extern Fixed const fixed_pi; -extern Fixed const fixed_two_pi; -extern Fixed const fixed_half_pi; -extern Fixed const fixed_quarter_pi; +template +const Fixed Fixed::fixed_max(Fixed::internal(), std::numeric_limits::max()); + +template +const Fixed Fixed::fixed_one(Fixed::internal(), 1LL << (FixedTraits::fixed_resolution_shift)); + +template +const Fixed Fixed::fixed_zero(Fixed::internal(), 0); + +template +const Fixed Fixed::fixed_half(Fixed::internal(), 1LL << (FixedTraits::fixed_resolution_shift-1)); + +template +const Fixed Fixed::fixed_pi(Fixed::internal(), FixedTraits::internal_pi); + +template +const Fixed Fixed::fixed_two_pi(Fixed::internal(), FixedTraits::internal_two_pi); + +template +const Fixed Fixed::fixed_half_pi(Fixed::internal(), FixedTraits::internal_half_pi); + +template +const Fixed Fixed::fixed_quarter_pi(Fixed::internal(), FixedTraits::internal_quarter_pi); #endif /* SDRBASE_UTIL_FIXED_H_ */ diff --git a/sdrbase/util/fixedtraits.h b/sdrbase/util/fixedtraits.h index 3af812573..5b2d26be5 100644 --- a/sdrbase/util/fixedtraits.h +++ b/sdrbase/util/fixedtraits.h @@ -26,10 +26,11 @@ class FixedTraits }; template<> -class FixedTraits<28> +struct FixedTraits<28> { static const uint32_t fixed_resolution_shift = 28; //!< 1.0 representation. 28 is the highest power of two that can represent 9.99999... safely on 64 bits internally - static const int64_t fixed_resolution = 1LL << 28; + static const int64_t fixed_resolution = 1LL << fixed_resolution_shift; + static const int32_t max_power = 63 - fixed_resolution_shift; static const int64_t internal_pi = 0x3243f6a8; static const int64_t internal_two_pi = 0x6487ed51; static const int64_t internal_half_pi = 0x1921fb54;