fk0, легенда (31.10.2011 12:55, просмотров: 733)Самодельный велосипед atan2(), прошу покритиковать (fixed point конечно, иначе какой смысл):
/* (C) Kirill Frolov <fk0@fk0.pp.ru> 2010 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#define STEPS 12
uint_fast16_t A[STEPS];
#define FP_PI 8192
int_fast16_t fp_atan_pi4(int_fast16_t y, int_fast16_t x)
{
uint_fast8_t n;
int_fast16_t a; /* -pi <= a <=pi */
int_fast16_t xs, ys;
#define CORDIC(s, u) do { \
ys=(y>>(s+1)), xs=(x>>(s+1)); \
if (y<0) x-=ys, y+=xs, a-=(u); \
else x+=ys, y-=xs, a+=(u); \
} while(0)
a=0;
for (n=0; n<STEPS; n++) {
printf("%u: a=%d (%f), x=%d, y=%d\n", n, a, M_PI*a/FP_PI, x, y);
CORDIC(n, A[n]);
}
return a;
#undef CORDIC
}
int_fast16_t fp_atan_pi2(int_fast16_t y, int_fast16_t x)
{
if (y < x) { /* 0..pi/4 */
return fp_atan_pi4(y, x);
} else { /* pi/4..pi/2 */
return FP_PI/2 - fp_atan_pi4(x, y);
}
}
int_fast16_t fp_atan2(int_fast16_t y, int_fast16_t x)
{
if (x>=0) {
if (y>=0) return fp_atan_pi2(y, x);
else /* y<0 */ return -fp_atan_pi2(-y, x);
} else /* x<0 */ {
if (y>=0) return FP_PI-fp_atan_pi2(y, -x);
else /* y<0 */ return -FP_PI+fp_atan_pi2(-y, -x);
}
}
int main(int argc, char *argv[])
{
int n;
/* precompute A[STEPS] */
for (n=1; n<=STEPS; n++) {
double a;
a=atan(powf(2.0, -(int)n));
A[n-1]=FP_PI*a/M_PI;
//printf("%f -> %u\n", a, angles[n-1]);
}
//puts("");
if (argc<3) {
fprintf(stderr, "%s: <a> <b>\n", argv[0]);
exit(1);
}
float y, x;
sscanf(argv[1], "%f", &y);
sscanf(argv[2], "%f", &x);
int_fast16_t a;
a=fp_atan2(y*16384, x*16384);
printf("fixed atan2(%f,%f): a=%d (%f)\n", y, x, a, M_PI*a/FP_PI);
double atan = atan2(y, x);
printf("float atan2(%f,%f): a=%f\n", y, x, atan);
return 0;
}
[ZX]