diff --git a/sdrbase/util/fixed.cpp b/sdrbase/util/fixed.cpp new file mode 100644 index 000000000..8bc9ddcfd --- /dev/null +++ b/sdrbase/util/fixed.cpp @@ -0,0 +1,461 @@ +// Distributed under the Boost Software License, Version 1.0. (See +// accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) +// (C) Copyright 2007 Anthony Williams +#include "fixed.hpp" + +// all constants are scaled to 2^28 (as 1) +int64_t const internal_pi=0x3243f6a8; +int64_t const internal_two_pi=0x6487ed51; +int64_t const internal_half_pi=0x1921fb54; +int64_t const internal_quarter_pi=0xc90fdaa; + +extern fixed const fixed_pi(fixed::internal(),internal_pi); +extern fixed const fixed_two_pi(fixed::internal(),internal_two_pi); +extern fixed const fixed_half_pi(fixed::internal(),internal_half_pi); +extern fixed const fixed_quarter_pi(fixed::internal(),internal_quarter_pi); + +fixed& fixed::operator%=(fixed const& other) +{ + m_nVal = m_nVal%other.m_nVal; + return *this; +} + +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-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-fixed_resolution_shift)) + + (lower_self_lower_other_res>>fixed_resolution_shift); + } + + if(negate) + { + m_nVal=-m_nVal; + } + return *this; +} + + +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=fixed_resolution_shift; + + if(a_large) + { + uint64_t const half_a=a>>1; + while(tempa)) + { + temp>>=1; + ++right_shift; + } + d>>=right_shift; + shift-=right_shift; + a-=temp; + res+=d; + } + m_nVal=(negate?-(int64_t)res:res); + } + + return *this; +} + + +fixed fixed::sqrt() const +{ + unsigned const max_shift=62; + uint64_t a_squared=1LL<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-fixed_resolution_shift); + int const two_a_b_shift=b_shift+1-fixed_resolution_shift; + uint64_t two_a_b=(two_a_b_shift>0)?(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<=log_two_power_n_reversed[0]) + { + return fixed_max; + } + if(m_nVal<-log_two_power_n_reversed[63-2*fixed_resolution_shift]) + { + return fixed(internal(),0); + } + if(!m_nVal) + { + return fixed(internal(),fixed_resolution); + } + + int64_t res=fixed_resolution; + + if(m_nVal>0) + { + int power=max_power; + int64_t const* log_entry=log_two_power_n_reversed; + int64_t temp=m_nVal; + while(temp && power>(-(int)fixed_resolution_shift)) + { + while(!power || (temp<*log_entry)) + { + if(!power) + { + log_entry=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=fixed_resolution_shift; + int64_t const* log_entry=log_two_power_n_reversed+(max_power-power); + int64_t temp=m_nVal; + + while(temp && power>(-(int)fixed_resolution_shift)) + { + while(!power || (temp>(-*log_entry))) + { + if(!power) + { + log_entry=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(internal(),res); +} + +fixed fixed::log() const +{ + if(m_nVal<=0) + { + return -fixed_max; + } + if(m_nVal==fixed_resolution) + { + return fixed_zero; + } + uint64_t temp=m_nVal; + int left_shift=0; + uint64_t const scale_position=0x8000000000000000; + while(temp>1; + while(temp && (right_shift>=1; + ++right_shift; + } + + temp-=shifted_temp; + shifted_temp=temp>>right_shift; + res+=log_one_over_one_minus_two_power_minus_n[right_shift-1]; + } + return fixed(fixed::internal(),res); +} + + +namespace +{ + const int64_t arctantab[32] = { + 297197971, 210828714, 124459457, 65760959, 33381290, 16755422, 8385879, + 4193963, 2097109, 1048571, 524287, 262144, 131072, 65536, 32768, 16384, + 8192, 4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1, 0, 0, + }; + + + int64_t 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); + } + + int64_t right_shift(int64_t val,int shift) + { + return (shift<0)?(val<<-shift):(val>>shift); + } + + void perform_cordic_rotation(int64_t&px, int64_t&py, int64_t theta) + { + int64_t x = px, y = py; + int64_t const *arctanptr = arctantab; + for (int i = -1; i <= (int)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); + } + + + void perform_cordic_polarization(int64_t& argx, int64_t&argy) + { + int64_t theta=0; + int64_t x = argx, y = argy; + int64_t const *arctanptr = arctantab; + for(int i = -1; i <= (int)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; + } +} + +void fixed::sin_cos(fixed const& theta,fixed* s,fixed*c) +{ + int64_t x=theta.m_nVal%internal_two_pi; + if( x < 0 ) + x += internal_two_pi; + + bool negate_cos=false; + bool negate_sin=false; + + if( x > internal_pi ) + { + x =internal_two_pi-x; + negate_sin=true; + } + if(x>internal_half_pi) + { + x=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; + } +} + +fixed fixed::atan() const +{ + fixed r,theta; + to_polar(1,*this,&r,&theta); + return theta; +} + +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 right_shift=0; + unsigned const max_value=1U<=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)<m_nVal=ytemp; + + if(negative_x && negative_y) + { + theta->m_nVal-=internal_pi; + } + else if(negative_x) + { + theta->m_nVal=internal_pi-theta->m_nVal; + } + else if(negative_y) + { + theta->m_nVal=-theta->m_nVal; + } +} + diff --git a/sdrbase/util/fixed.hpp b/sdrbase/util/fixed.hpp new file mode 100644 index 000000000..b9f97bd01 --- /dev/null +++ b/sdrbase/util/fixed.hpp @@ -0,0 +1,1591 @@ +#ifndef FIXED_HPP +#define FIXED_HPP +// Distributed under the Boost Software License, Version 1.0. (See +// accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) +// (C) Copyright 2007 Anthony Williams + +#include +#include +#include + +// 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<(nVal*static_cast(fixed_resolution))) + {} + fixed(float nVal): + m_nVal(static_cast(nVal*static_cast(fixed_resolution))) + {} + + template + fixed& operator=(T other) + { + m_nVal=fixed(other).m_nVal; + return *this; + } + fixed& operator=(fixed const& other) + { + m_nVal=other.m_nVal; + return *this; + } + friend bool operator==(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal==rhs.m_nVal; + } + friend bool operator!=(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal!=rhs.m_nVal; + } + friend bool operator<(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal>rhs.m_nVal; + } + friend bool operator<=(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal<=rhs.m_nVal; + } + friend bool operator>=(fixed const& lhs,fixed const& rhs) + { + return lhs.m_nVal>=rhs.m_nVal; + } + operator bool() const + { + return m_nVal?true:false; + } + inline operator double() const + { + return as_double(); + } + int64_t as_internal() const + { + return m_nVal; + } + float as_float() const + { + return m_nVal/(float)fixed_resolution; + } + + double as_double() const + { + return m_nVal/(double)fixed_resolution; + } + + int64_t as_long() const + { + return (int64_t)(m_nVal/fixed_resolution); + } + int64_t as_int64() const + { + return m_nVal/fixed_resolution; + } + + int as_int() const + { + return (int)(m_nVal/fixed_resolution); + } + + uint64_t as_unsigned_long() const + { + return (uint64_t)(m_nVal/fixed_resolution); + } + uint64_t as_unsigned_int64() const + { + return (uint64_t)m_nVal/fixed_resolution; + } + + unsigned int as_unsigned_int() const + { + return (unsigned int)(m_nVal/fixed_resolution); + } + + short as_short() const + { + return (short)(m_nVal/fixed_resolution); + } + + unsigned short as_unsigned_short() const + { + return (unsigned short)(m_nVal/fixed_resolution); + } + + fixed operator++() + { + m_nVal += fixed_resolution; + return *this; + } + + fixed operator--() + { + m_nVal -= fixed_resolution; + return *this; + } + + fixed floor() const; + fixed ceil() const; + fixed sqrt() const; + fixed exp() const; + fixed log() const; + fixed& operator%=(fixed const& other); + fixed& operator*=(fixed const& val); + fixed& operator/=(fixed const& val); + fixed& operator-=(fixed const& val) + { + m_nVal -= val.m_nVal; + return *this; + } + + fixed& operator+=(fixed const& val) + { + m_nVal += val.m_nVal; + return *this; + } + fixed& operator*=(double val) + { + return (*this)*=fixed(val); + } + fixed& operator*=(float val) + { + return (*this)*=fixed(val); + } + fixed& operator*=(int64_t val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(int val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(short val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(char val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(uint64_t val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(unsigned int val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(unsigned short val) + { + m_nVal*=val; + return *this; + } + fixed& operator*=(unsigned char val) + { + m_nVal*=val; + return *this; + } + fixed& operator/=(double val) + { + return (*this)/=fixed(val); + } + fixed& operator/=(float val) + { + return (*this)/=fixed(val); + } + fixed& operator/=(int64_t val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(int val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(short val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(char val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(uint64_t val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(unsigned int val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(unsigned short val) + { + m_nVal/=val; + return *this; + } + fixed& operator/=(unsigned char val) + { + m_nVal/=val; + return *this; + } + + + bool operator!() const + { + return m_nVal==0; + } + + fixed modf(fixed* integral_part) const; + fixed atan() const; + + static void sin_cos(fixed const& theta,fixed* s,fixed*c); + static void to_polar(fixed const& x,fixed const& y,fixed* r,fixed*theta); + + fixed sin() const; + fixed cos() const; + fixed tan() const; + fixed operator-() const; + fixed abs() const; +}; + +/* why always as double ? +inline std::ostream& operator<<(std::ostream& os,fixed const& value) +{ + return os<(double a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(float a, fixed const& b) +{ + return fixed(a)>b; +} + +inline bool operator>(uint64_t a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(int64_t a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(unsigned a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(int a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(unsigned short a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(short a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(unsigned char a, fixed const& b) +{ + return fixed(a)>b; +} +inline bool operator>(char a, fixed const& b) +{ + return fixed(a)>b; +} + +inline bool operator>(fixed const& a,double b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,float b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,uint64_t b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,int64_t b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,unsigned b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,int b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,unsigned short b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,short b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,unsigned char b) +{ + return a>fixed(b); +} +inline bool operator>(fixed const& a,char b) +{ + return a>fixed(b); +} + +inline bool operator<=(double a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(float a, fixed const& b) +{ + return fixed(a)<=b; +} + +inline bool operator<=(uint64_t a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(int64_t a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(unsigned a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(int a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(unsigned short a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(short a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(unsigned char a, fixed const& b) +{ + return fixed(a)<=b; +} +inline bool operator<=(char a, fixed const& b) +{ + return fixed(a)<=b; +} + +inline bool operator<=(fixed const& a,double b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,float b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,uint64_t b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,int64_t b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,unsigned b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,int b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,unsigned short b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,short b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,unsigned char b) +{ + return a<=fixed(b); +} +inline bool operator<=(fixed const& a,char b) +{ + return a<=fixed(b); +} + +inline bool operator>=(double a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(float a, fixed const& b) +{ + return fixed(a)>=b; +} + +inline bool operator>=(uint64_t a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(int64_t a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(unsigned a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(int a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(unsigned short a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(short a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(unsigned char a, fixed const& b) +{ + return fixed(a)>=b; +} +inline bool operator>=(char a, fixed const& b) +{ + return fixed(a)>=b; +} + +inline bool operator>=(fixed const& a,double b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,float b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a, uint64_t b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a, int64_t b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,unsigned b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,int b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,unsigned short b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,short b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,unsigned char b) +{ + return a>=fixed(b); +} +inline bool operator>=(fixed const& a,char b) +{ + return a>=fixed(b); +} + +inline fixed sin(fixed const& x) +{ + return x.sin(); +} +inline fixed cos(fixed const& x) +{ + return x.cos(); +} +inline fixed tan(fixed const& x) +{ + return x.tan(); +} + +inline fixed sqrt(fixed const& x) +{ + return x.sqrt(); +} + +inline fixed exp(fixed const& x) +{ + return x.exp(); +} + +inline fixed log(fixed const& x) +{ + return x.log(); +} + +inline fixed floor(fixed const& x) +{ + return x.floor(); +} + +inline fixed ceil(fixed const& x) +{ + return x.ceil(); +} + +inline fixed abs(fixed const& x) +{ + return x.abs(); +} + +inline fixed modf(fixed const& x,fixed*integral_part) +{ + return x.modf(integral_part); +} + +inline fixed fixed::ceil() const +{ + if(m_nVal%fixed_resolution) + { + return floor()+1; + } + else + { + return *this; + } +} + +inline fixed fixed::floor() const +{ + fixed res(*this); + int64_t const remainder=m_nVal%fixed_resolution; + if(remainder) + { + res.m_nVal-=remainder; + if(m_nVal<0) + { + res-=1; + } + } + return res; +} + + +inline fixed fixed::sin() const +{ + fixed res; + sin_cos(*this,&res,0); + return res; +} + +inline fixed fixed::cos() const +{ + fixed res; + sin_cos(*this,0,&res); + return res; +} + +inline fixed fixed::tan() const +{ + fixed s,c; + sin_cos(*this,&s,&c); + return s/c; +} + +inline fixed fixed::operator-() const +{ + return fixed(internal(),-m_nVal); +} + +inline fixed fixed::abs() const +{ + return fixed(internal(),m_nVal<0?-m_nVal:m_nVal); +} + +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; + } + integral_part->m_nVal=m_nVal-fractional_part; + return fixed(internal(),fractional_part); +} + +inline fixed arg(const std::complex& val) +{ + fixed r,theta; + fixed::to_polar(val.real(),val.imag(),&r,&theta); + return theta; +} + +inline std::complex polar(fixed const& rho, fixed const& theta) +{ + fixed s,c; + fixed::sin_cos(theta,&s,&c); + 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; + +#endif