CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to support@ccsinfo.com

Fast trigonometric approximations

 
Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message
Ttelmah



Joined: 11 Mar 2010
Posts: 19195

View user's profile Send private message

Fast trigonometric approximations
PostPosted: Fri Jun 09, 2017 2:50 pm     Reply with quote

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 <math.h>

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.
tinley



Joined: 09 May 2006
Posts: 66

View user's profile Send private message Visit poster's website

Great code thanks
PostPosted: Tue Jan 07, 2020 7:03 am     Reply with quote

Thanks Ttelmah. This code is exactly what I was looking for. You mention that the quadrants may not be correct. They don't match CCS atan2 function and I think you were right to suspect. I substituted the following:

result = (atan2( input_value_X , input_value_Y ) * 57.2958 ); // 180/pi = 57.2958

With:

result = angle(input_value_X, input_value_Y);

And I had to swap the X and Y to get it to work.

But it does work very well and quickly! Thank you!
tinley



Joined: 09 May 2006
Posts: 66

View user's profile Send private message Visit poster's website

Fast COS
PostPosted: Tue Jan 07, 2020 9:07 am     Reply with quote

As it happens, I can also use your fast COS in the same project. But this did have a couple of bugs which are fixed in the following:

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
#define PIDIV2 (PI/2)
#define PIX2 (PI*2)

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 -= PIX2;   // Wrap: cos(x) = cos(x - 2 pi)
   }

   return fast_sin(x);
}


But still save me heaps of time trying to save time! Thank you!
Ttelmah



Joined: 11 Mar 2010
Posts: 19195

View user's profile Send private message

PostPosted: Mon Jan 27, 2020 1:16 pm     Reply with quote

Well done.
Glad you got it sorted. Very Happy
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
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


Powered by phpBB © 2001, 2005 phpBB Group