CORDIC算法的C语言实现
简单的实现了CORDIC算法。
#include
#include
#include
/* g_atan_table[i] = atan(2^-i). when i >= 25 abs((atan(2^-i)-2^-i)/atan(2^-i)) < 1E-15. */
static const double g_atan_table[64] = {
7.853981633974483E-1,4.636476090008061E-1,2.449786631268642E-1,1.243549945467614E-1,
6.241880999595735E-2,3.123983343026828E-2,1.562372862047683E-2,7.812341060101111E-3,
3.906230131966972E-3,1.953122516478819E-3,9.765621895593194E-4,4.882812111948983E-4,
2.441406201493618E-4,1.220703118936702E-4,6.103515617420878E-5,3.05175781155261E-5,
1.525878906131576E-5,7.62939453110197E-6,3.814697265606496E-6,1.907348632810187E-6,
9.536743164059609E-7,4.768371582030889E-7,2.38418579101558E-7,1.192092895507807E-7,
5.960464477539055E-8,2.98023223876953E-8,1.490116119384766E-8,7.450580596923828E-9,
3.725290298461914E-9,1.862645149230957E-9,9.313225746154785E-10,4.656612873077393E-10,
2.328306436538696E-10,1.164153218269348E-10,5.820766091346741E-11,2.91038304567337E-11,
1.455191522836685E-11,7.275957614183426E-12,3.637978807091713E-12,1.818989403545856E-12,
9.094947017729282E-13,4.547473508864641E-13,2.273736754432321E-13,1.13686837721616E-13,
5.684341886080801E-14,2.842170943040401E-14,1.4210854715202E-14,7.105427357601002E-15,
3.552713678800501E-15,1.77635683940025E-15,8.881784197001252E-16,4.440892098500626E-16,
2.220446049250313E-16,1.110223024625157E-16,5.551115123125783E-17,2.775557561562891E-17,
1.387778780781446E-17,6.938893903907228E-18,3.469446951953614E-18,1.734723475976807E-18,
8.673617379884035E-19,4.336808689942018E-19,2.168404344971009E-19,1.084202172485504E-19};
/* g_product_cos_table[i] = product(1/sqrt(1+2^(-2*k)),k,0,i); */
static const double g_product_cos_table[64] = {
7.071067811865475E-1,6.324555320336759E-1,6.135719910778963E-1,6.088339125177524E-1,
6.076482562561681E-1,6.073517701412958E-1,6.072776440935258E-1,6.072591122988925E-1,
6.07254479332562E-1,6.072533210898748E-1,6.072530315291339E-1,6.072529591389442E-1,
6.072529410413965E-1,6.072529365170094E-1,6.072529353859126E-1,6.072529351031383E-1,
6.072529350324446E-1,6.072529350147711E-1,6.072529350103526E-1,6.072529350092478E-1,
6.072529350089715E-1,6.072529350089023E-1,6.072529350088848E-1,6.072529350088803E-1,
6.072529350088791E-1,6.072529350088786E-1,6.072529350088783E-1,6.072529350088781E-1,
6.072529350088778E-1,6.072529350088776E-1,6.072529350088773E-1,6.072529350088771E-1,
6.072529350088768E-1,6.072529350088765E-1,6.072529350088762E-1,6.072529350088759E-1,
6.072529350088756E-1,6.072529350088753E-1,6.07252935008875E-1,6.072529350088746E-1,
6.072529350088743E-1,6.07252935008874E-1,6.072529350088736E-1,6.072529350088733E-1,
6.072529350088729E-1,6.072529350088725E-1,6.072529350088721E-1,6.072529350088717E-1,
6.072529350088713E-1,6.072529350088709E-1,6.072529350088705E-1,6.0725293500887E-1,
6.072529350088696E-1,6.072529350088692E-1,6.072529350088687E-1,6.072529350088683E-1,
6.072529350088678E-1,6.072529350088673E-1,6.072529350088668E-1,6.072529350088663E-1,
6.072529350088658E-1,6.072529350088653E-1,6.072529350088648E-1,6.072529350088642E-1};
/* g_atanh_table[i] = atanh(2^-(i+1)). */
static const double g_atanh_table[64] = {
5.493061443340549E-1,2.554128118829953E-1,1.25657214140453E-1,6.258157147700301E-2,
3.126017849066699E-2,1.562627175205221E-2,7.812658951540421E-3,3.906269868396826E-3,
1.95312748353255E-3,9.765628104410358E-4,4.882812888051128E-4,2.441406298506386E-4,
1.220703131063297E-4,6.103515632579116E-5,3.051757813447389E-5,1.525878906368418E-5,
7.629394531398012E-6,3.814697265643522E-6,1.907348632814768E-6,9.536743164065301E-7,
4.768371582032244E-7,2.384185791015377E-7,1.192092895507544E-7,5.960464477539066E-8,
2.980232238766821E-8,1.490116119384766E-8,7.450580596923828E-9,3.725290298407704E-9,
1.862645149176747E-9,9.313225745612684E-10,4.656612873077393E-10,2.32830643613212E-10,
1.164153218303229E-10,5.820766086010433E-11,2.910383045694546E-11,1.455191517420968E-11,
7.275957559986552E-12,3.637978752884913E-12,1.818989349336575E-12,9.094946475630264E-13,
4.547472966764072E-13,2.27373675443245E-13,1.136867835115106E-13,5.684341886080882E-14,
2.842165522029559E-14,1.421080050509343E-14,7.105427357601014E-15,3.552713678800504E-15,
1.776302629291627E-15,8.881242095915012E-16,4.440349997414384E-16,2.21990394816407E-16,
1.110223024625157E-16,5.545694112263355E-17,2.770136550700464E-17,6.938893903907228E-18,
6.938893903907228E-18,0.0E0,0.0E0,0.0E0,0.0E0,0.0E0,0.0E0,0.0E0};
/* 4,13,40,121,364,1093... */
static const int g_atanh_twice_ind[4] = {3,12,39,120};
/* g_product_cos_table[i] = product(1/sqrt(1+2^(-2*(k+1))),k,0,i)*product(1/sqrt(1+2^(-2*(k+1))), k <= i && k in g_atanh_twice_ind ); */
static const double g_product_cosh_table[64] = {
1.154700538379252E0,1.192569587999888E0,1.201997162280557E0,1.206710876642441E0,
1.207300522842615E0,1.207447925385481E0,1.207484775458747E0,1.207493987941918E0,
1.207496291060514E0,1.207496866840026E0,1.207497010784895E0,1.207497046771112E0,
1.207497064764221E0,1.207497067013359E0,1.207497067575644E0,1.207497067716215E0,
1.207497067751358E0,1.207497067760143E0,1.20749706776234E0,1.207497067762889E0,
1.207497067763026E0,1.20749706776306E0,1.207497067763069E0,1.207497067763071E0,
1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,
1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,
1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,
1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,
1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,1.207497067763072E0,
1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,
1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,
1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,
1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,
1.207497067763071E0,1.207497067763071E0,1.207497067763071E0,1.207497067763071E0};
/* my_cos() and my_sin() share the same code expect the return vairable */
/* return cos(t) */
double my_cos(const double t){
const int n = 50 ;
double x = g_product_cos_table[n-1];
double xt;
double y = 0.0;
double z = 0.0;
double k = 1.0;
int i;
if(t < 0 || t > M_PI/2){
return 0.0;
}
for(i = 0; i < 25; i++){
xt = x;
if(t > z){
x = x - y*k;
y = y + xt*k;
z = z + g_atan_table[i];
}
else {
x = x + y*k;
y = y - xt*k;
z = z - g_atan_table[i];
}
k *= 0.5;
}
/* when i is large enough we can replace atan(2^-i) with 2^-i */
for(; i < n; i++){
xt = x;
if(t > z){
x = x - y*k;
y = y + xt*k;
z = z + k;
}
else {
x = x + y*k;
y = y - xt*k;
z = z - k;
}
k *= 0.5;
}
return x;
}
/* return sin(t) */
double my_sin(const double t){
const int n = 52;
double x = g_product_cos_table[n-1];
double xt;
double y = 0.0;
double z = 0.0;
double k = 1.0;
int i;
if(t < 0 || t > M_PI/2){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(t > z){
x = x - y*k;
y = y + xt*k;
z = z + g_atan_table[i];
}
else {
x = x + y*k;
y = y - xt*k;
z = z - g_atan_table[i];
}
k *= 0.5;
}
return y;
}
/* my_atan2() and my_hypot() share the same code expect the return vairable */
/* return arctan(y/x) */
double my_atan2(double y, double x){
const int n = 52;
double xt;
double z = 0.0;
double k = 1.0;
int i;
if(x < 0.0 || y < 0.0 || y > x){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(y > 0.0){
x = x + y*k;
y = y - xt*k;
z = z + g_atan_table[i];
}
else {
x = x - y*k;
y = y + xt*k;
z = z - g_atan_table[i];
}
k *= 0.5;
}
return z;
}
/* return sqrt(x*x+y*y) */
double my_hypot(double y, double x){
const int n = 52;
double xt;
double z = 0.0;
double k = 1.0;
int i;
if(x < 0.0 || y < 0.0){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(y > 0.0){
x = x + y*k;
y = y - xt*k;
z = z + g_atan_table[i];
}
else {
x = x - y*k;
y = y + xt*k;
z = z - g_atan_table[i];
}
k *= 0.5;
}
return x*g_product_cos_table[n-1];
}
/* my_cosh(),my_sinh() and my_exp() share the same code expect the return vairable */
/* return cosh(t) */
double my_cosh(const double t){
const int n = 50;
double x = g_product_cosh_table[n];
double y = 0;
double z = 0;
double k = 0.5;
double xt;
int i;
int twice_ind_ind = 0;
int twice_ind = g_atanh_twice_ind[twice_ind_ind];
/* 1.1181730155265 */
if(t > 1.11 || t < -1.11){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
if(i == twice_ind){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
twice_ind_ind++;
twice_ind = g_atanh_twice_ind[twice_ind_ind];
}
k *= 0.5;
}
return x;
}
/* return sinh(t) */
double my_sinh(const double t){
const int n = 50;
double x = g_product_cosh_table[n];
double y = 0;
double z = 0;
double k = 0.5;
double xt;
int i;
int twice_ind_ind = 0;
int twice_ind = g_atanh_twice_ind[twice_ind_ind];
/* 1.1181730155265 */
if(t > 1.11 || t < -1.11){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
if(i == twice_ind){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
twice_ind_ind++;
twice_ind = g_atanh_twice_ind[twice_ind_ind];
}
k *= 0.5;
}
return y;
}
/* return exp(t) */
double my_exp(const double t){
const int n = 50;
double x = g_product_cosh_table[n];
double y = 0;
double z = 0;
double k = 0.5;
double xt;
int i;
int twice_ind_ind = 0;
int twice_ind = g_atanh_twice_ind[twice_ind_ind];
/* 1.1181730155265 */
if(t > 1.11 || t < -1.11){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
if(i == twice_ind){
xt = x;
if(t > z){
x = x + y*k;
y = y + xt*k;
z = z + g_atanh_table[i];
}
else {
x = x - y*k;
y = y - xt*k;
z = z - g_atanh_table[i];
}
twice_ind_ind++;
twice_ind = g_atanh_twice_ind[twice_ind_ind];
}
k *= 0.5;
}
return x+y;
}
/* return atanh(y/x) */
double my_atanh2(double y, double x){
const int n = 50;
double z = 0;
double k = 0.5;
double xt;
int i;
int twice_ind_ind = 0;
int twice_ind = g_atanh_twice_ind[twice_ind_ind];
if(y < 0 || x < 0){
return 0.0;
}
for(i = 0; i < n; i++){
xt = x;
if(y > 0){
x = x - y*k;
y = y - xt*k;
z = z + g_atanh_table[i];
}
else {
x = x + y*k;
y = y + xt*k;
z = z - g_atanh_table[i];
}
if(i == twice_ind){
xt = x;
if(y > 0){
x = x - y*k;
y = y - xt*k;
z = z + g_atanh_table[i];
}
else {
x = x + y*k;
y = y + xt*k;
z = z - g_atanh_table[i];
}
twice_ind_ind++;
twice_ind = g_atanh_twice_ind[twice_ind_ind];
}
k *= 0.5;
}
return z;
}
/* return log(x) */
double my_log(double x){
return 2.0*my_atanh2((x-1.0)/(x+1.0),1);
}
/* return sqrt(x0) */
double my_sqrt(const double x0){
const int n = 50;
double x ;
double y ;
double d;
double z = 0;
double k = 0.5;
double xt;
int i;
int twice_ind_ind = 0;
int twice_ind = g_atanh_twice_ind[twice_ind_ind];
d = 0.25*g_product_cosh_table[n]*g_product_cosh_table[n];
x = x0 + d;
y = x0 - d;
for(i = 0; i < n; i++){
xt = x;
if(y > 0){
x = x - y*k;
y = y - xt*k;
z = z + g_atanh_table[i];
}
else {
x = x + y*k;
y = y + xt*k;
z = z - g_atanh_table[i];
}
if(i == twice_ind){
xt = x;
if(y > 0){
x = x - y*k;
y = y - xt*k;
z = z + g_atanh_table[i];
}
else {
x = x + y*k;
y = y + xt*k;
z = z - g_atanh_table[i];
}
twice_ind_ind++;
twice_ind = g_atanh_twice_ind[twice_ind_ind];
}
k *= 0.5;
}
return x;
}
int main(int argc,char* argv[]){
double ds[] = {0.1,0.2,0.3,0.4,0.5,0.6,sqrt(2)/2};
int n = sizeof(ds)/sizeof(ds[0]);
double c,s,d,t;
int i;
for(i = 0; i < n; i++){
d = ds[i];
c = my_cos(d);
s = my_sin(d);
printf("my_cos(%g)=%g, error=%g. my_sin(%g)=%g, error=%g\n",d,c,c-cos(d),d,s,s-sin(d));
}
printf("\n");
for(i = 0; i < n; i++){
d = ds[i];
t = my_atan2(15.0*d,15.0);
printf("my_atan2(%g,15.0)=%g, error=%g\n",15.0*d,t,t-atan2(15.0*d,15.0));
}
printf("\n");
for(i = 0; i < n; i++){
d = ds[i];
c = my_cosh(d);
s = my_sinh(d);
printf("my_cosh(%g)=%g, error=%g. my_sinh(%g)=%g, error=%g\n",d,c,c-cosh(d),d,s,s-sinh(d));
}
printf("\n");
for(i = 0; i < n; i++){
d = ds[i];
t = my_atanh2(d,1.0);
printf("my_atanh2(%g,1)=%g, error=%g\n",d,t,t-atanh(d));
}
printf("\n");
printf("my_hypot(3,4)=%g.\n",my_hypot(3,4));
d = 1.0;
t = my_exp(d);
printf("my_exp(%g)=%g, error=%g.\n",d,t,t-exp(d));
printf("my_log(sqrt(exp(1)))=%g.\n",my_log(sqrt(M_E)));
d = 2.0;
t = my_sqrt(d);
printf("my_sqrt(%g)=%g, error=%g.\n",d,t,t-sqrt(2.0));
return 0;
}
输出结果还是比较满意的
my_cos(0.1)=0.995004, error=-1.67644e-014. my_sin(0.1)=0.0998334, error=-1.77636e-015
my_cos(0.2)=0.980067, error=-1.62093e-014. my_sin(0.2)=0.198669, error=-3.9968e-015
my_cos(0.3)=0.955336, error=-1.66533e-014. my_sin(0.3)=0.29552, error=-5.38458e-015
my_cos(0.4)=0.921061, error=-1.57652e-014. my_sin(0.4)=0.389418, error=-7.10543e-015
my_cos(0.5)=0.877583, error=-1.46549e-014. my_sin(0.5)=0.479426, error=-8.77076e-015
my_cos(0.6)=0.825336, error=-1.4877e-014. my_sin(0.6)=0.564642, error=-1.06581e-014
my_cos(0.707107)=0.760245, error=-1.33227e-014. my_sin(0.707107)=0.649637, error=-1.23235e-014
my_atan2(1.5,15.0)=0.0996687, error=-4.85723e-016
my_atan2(3,15.0)=0.197396, error=-3.05311e-016
my_atan2(4.5,15.0)=0.291457, error=2.22045e-016
my_atan2(6,15.0)=0.380506, error=2.77556e-016
my_atan2(7.5,15.0)=0.463648, error=1.11022e-016
my_atan2(9,15.0)=0.54042, error=4.44089e-016
my_atan2(10.6066,15.0)=0.61548, error=0
my_cosh(0.1)=1.005, error=-4.44089e-016. my_sinh(0.1)=0.100167, error=-5.27356e-016
my_cosh(0.2)=1.02007, error=-6.66134e-016. my_sinh(0.2)=0.201336, error=4.44089e-016
my_cosh(0.3)=1.04534, error=-8.88178e-016. my_sinh(0.3)=0.30452, error=4.996e-016
my_cosh(0.4)=1.08107, error=-1.77636e-015. my_sinh(0.4)=0.410752, error=-1.44329e-015
my_cosh(0.5)=1.12763, error=-8.88178e-016. my_sinh(0.5)=0.521095, error=-1.77636e-015
my_cosh(0.6)=1.18547, error=-1.77636e-015. my_sinh(0.6)=0.636654, error=1.11022e-016
my_cosh(0.707107)=1.26059, error=-8.88178e-016. my_sinh(0.707107)=0.767523, error=-5.55112e-016
my_atanh2(0.1,1)=0.100335, error=7.07767e-016
my_atanh2(0.2,1)=0.202733, error=-2.498e-016
my_atanh2(0.3,1)=0.30952, error=6.10623e-016
my_atanh2(0.4,1)=0.423649, error=6.10623e-016
my_atanh2(0.5,1)=0.549306, error=-7.77156e-016
my_atanh2(0.6,1)=0.693147, error=-6.66134e-016
my_atanh2(0.707107,1)=0.881374, error=-5.55112e-016
my_hypot(3,4)=5.
my_exp(1)=2.71828, error=-2.22045e-015.
my_log(sqrt(exp(1)))=0.5.
my_sqrt(2)=1.41421, error=-1.77636e-015.
参考： Elementary Functions Algorithms and Implementations