Quadrotor from scratch

45 lines
1.0 KiB

/* trig.c */
/* Cosine implementation from:
* http://www.ganssle.com/articles/atrig.htm
*/
double cosine(double x)
{
double p0,p1,p2,p3,p4,p5,y,t,absx,frac,pi2;
int quad;
p0= 0.999999999781;
p1=-0.499999993585;
p2= 0.041666636258;
p3=-0.0013888361399;
p4= 0.00002476016134;
p5=-0.00000026051495;
pi2=1.570796326794896; /* pi/2 */
absx=x;
if (x<0) absx=-absx; /* absolute value of input */
quad=(int) (absx/pi2); /* quadrant (0 to 3) */
if (quad > 3) {
int q = quad & ~3; /* round down to the next multiple of 4 */
absx = absx / (pi2 * quad);
quad -= q;
t = 0.0; /* hello, compiler warnings */
}
frac= (absx/pi2) - quad; /* fractional part of input */
if(quad==0) t=frac * pi2;
if(quad==1) t=(1-frac) * pi2;
if(quad==2) t=frac * pi2;
if(quad==3) t=(frac-1) * pi2;
t=t * t;
y=p0 + (p1*t) + (p2*t*t) + (p3*t*t*t) + (p4*t*t*t*t) + (p5*t*t*t*t*t);
if(quad==2 || quad==1) y=-y; /* correct sign */
return(y);
};
double sine(double x)
{
double pi2=1.570796326794896; /* pi/2 */
x = x - pi2;
if (x < -pi2) x += pi2*4;
return cosine(x);
}