Author Message
Ttelmah

Joined: 11 Mar 2010
Posts: 14408

Fast trigonometric approximations Posted: Fri Jun 09, 2017 2:50 pm Some time ago, I posted a much faster atan2 in the general forum.
Decided to post it here together with another fast set of code for sin and cosine:

The accuracy is better than half a degree, and it is _fast_. Takes about 70uSec on a 40Mhz PIC, while atan2 takes at least ten times longer. It is also smaller.... It only gives positive angles, from 0 to 360 as it's result. This works in degrees but is easy to convert to radians if required.

 Code: //Processor defines here.... #include float angle(float X, float Y) {    //routine to give a fast solution for angle, from X/Y co-ordinates - result in degrees    float AX,AY,ival,oval,aival;    int8 quad;    AX=fabs(X);    AY=fabs(Y);    //Now the approximation used works for tan from -1 to 1, so we have to keep the    //values inside this range and adjust the input/output.    //Four 'quadrants' are decoded -1 to 1, (315 to 45 degrees), then 45 to 135,    //135 to 225, 225 to 315    If (X >= 0) //Right hand half of the circle    {       If (AY > X)       {          If (Y < 0)          {              quad = 4;              ival = -X / Y;          }          Else          {              quad = 2;              ival = X / -Y;          }       }       Else       {          If (AY > X)          {              quad = 4;              ival = -Y / X;          }          else          {              quad = 1;              ival = Y / X;          }       }    }    else    {       //Now the others       If (Y > AX)       {          quad = 2;          ival = X / -Y;       }       Else       {          If (AY > AX)          {                       quad = 4;              ival = -X / Y;          }          Else          {              quad = 3;              ival = -Y / -X;          }       }    }    //A lot of lines of code, but small and quick really.....    //Now the solution    //Now approximation for atan from -1 to +1, giving an answer in degrees.    aival = fAbs(ival);    oval = 45 * ival - ival * (aival - 1) * (14.02 + 3.79 * aival);        //Now solve back to the final result    If (quad != 1)    {       If (quad == 2)           oval = oval + 90;       Else       {           If (quad == 3)               oval = oval + 180;           Else               oval = oval + 270;       }    }    if (oval<0)       oval+=360;    return oval; }    //Demo program using pairs of numbers from the array to test void main() {    const signed int16 source[] = {0,300,600,1000,0,-300,-600,-1000};    int8 ctr,ctr2=0;    signed int16 X,Y;    while (TRUE)    {       for (ctr=0;ctr<8;ctr++)       {          //Now loop through the array, using pairs from two counters as X/Y          X=source[ctr];          Y=source[ctr2];          printf("X %ld, Y %ld, angle %5.1f\n\r", X,Y,angle(X,Y));       }       if (ctr2<7)          ctr2++;       else          ctr2=0;    } }

Now I have added this code for sin/cosine. This is in radians for +PI to -PI input range.
 Code: //A fast float approximation to sin over the range +/-PI #define PI2 (PI*PI) #define INV4  (4/PI) #define INV4SQ  (-4/(PI2)) #define P  0.225 float fast_sin(float x) {     float y;     y = (INV4 * x) + (INV4SQ * x * fabs(x));     return P * (y * fabs(y) - y) + y;    } float fast_cos(float x) {  //This is done by shifting the quadrants    x += PIDIV2;    if(x > PI)   // Original x > pi/2    {       x -= PI2;   // Wrap: cos(x) = cos(x - 2 pi)    }    return fast_sin(x); } void main(void) {   //Test the fast sin algorithm for angles from 0 to PI in steps of PI/128   float an, res, sres,;   for (an=0.0;an<(PI);an+=(PI/128))   {      //res1=fast_sin1(an);      res=fast_sin(an);      sres=sin(an);      printf ("AN=%5.3f sinfast=%5.3f sin=%5.3f\n",an,res,sres);   }   while(TRUE)      delay_cycles(1); }

I don't show a test for the cos, but this works exactly the same. Slightly slower. Display posts from previous: All Posts1 Day7 Days2 Weeks1 Month3 Months6 Months1 Year Oldest FirstNewest First
 All times are GMT - 6 Hours Page 1 of 1

 Jump to: Select a forum Software----------------General CCS C DiscussionCode LibraryEZ App Lynx Hardware----------------CCS ICD / Mach X / Load-n-Go
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum