fast_math.cpp

00001 #include "mbed.h"
00002 #include "fast_math.h"
00003 
00004 unsigned short  sin60[DEG60+1];  // 0～60度, 振幅65535のsinテーブル(最大誤差0.003%) from 0 to 60 deg. (max precision error is 0.003%)
00005 
00006 long _sin(unsigned short th){    // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI)
00007 // 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI)
00008 // 出力 : 65535*sin(th)
00009 //    init_fast_math();
00010 //    if( th>2.*PI ){ th -= 2*PI*(float)((int)(th/(2.*PI)));}
00011 //    th_int = (unsigned short)(th/(PI/3.)*(float)DEG60+0.5);  // rad to deg
00012 //    sin = (float)_sin(th)/65535.;
00013     unsigned short f_minus;
00014     long    x;
00015 
00016     // sinがマイナスのとき、thから180度引いて、f_minus=1にする
00017     if( th>=DEG60*3){   f_minus = 1;    th -= DEG60*3;} // if th>=180deg, th = th - 180deg;
00018     else{               f_minus = 0;}                   // else         , f_minus = on. 
00019 
00020     if( th<DEG60 ){         // th<60度のとき
00021         x = sin60[th];                              // sin(th)
00022     }else if( th<DEG60*2 ){ // 60≦th<120度のとき
00023         x = sin60[DEG60*2-th] + sin60[th-DEG60];    // sin(th)=sin(th+60)+sin(th-60)=sin(180-(th+60))+sin(th-60) because sin(th+60)=s/2+c*root(3)/2, sin(th-60)=s/2-c*root(3)/2.
00024     }else{                  // 120≦th<180度のとき
00025         x = sin60[DEG60*3-th];                      // sin(60-(th-120))=sin(180-th)
00026     }
00027     if( f_minus==1 ){   x = -x;}  // sinがマイナスのときマイナスにする
00028     return(x);
00029 }
00030 
00031 long _cos(unsigned short th){    // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI)
00032 // 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI)
00033 // 出力 : 65535*cos(th)
00034     th += DEG60*3/2;
00035     th += DEG60*3/2;
00036     if( th>=DEG60*6 ){  th -= DEG60*6;}
00037     return( _sin(th) );
00038 }
00039 
00040 void init_fast_math(){  // sin0-sin60deg; 0deg=0, 60deg=512
00041     int i;
00042     
00043     for( i=0;i<=DEG60;i++ ){    // 0～60度までのsinテーブルをつくるset sin table from 0 to 60 deg..
00044 //        sin60[i] = (unsigned short)(sin((float)i/512.*PI/3.));
00045         sin60[i] = (unsigned short)(65535.*sinf((float)i/(float)DEG60*PI/3.));
00046     }
00047 }
00048 
00049 #if 0
00050 //float  norm(float x[0], float x[1]){  // ２ノルムを計算
00051 //    return(sqrt(x[0]*x[0]+x[1]*x[1]));
00052 //}
00053 float  sqrt2(float x){    // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
00054 //  return((1+x)*0.5);      // 一次近似
00055     return(x+(1-x*x)*0.25); // 二次近似
00056 }
00057 #endif