Code: Select all
//lut-based atan2, returns angle in 0.1 degree units, granularity varies between 0.1 and 0.2 degrees, max absolute error 0.05 degrees
int16_t atan2_tbl[256] = {0,2,4,7,9,11,13,16,18,20,22,25,27,29,31,34,36,38,40,42,45,47,49,51,54,56,58,60,62,
65,67,69,71,73,76,78,80,82,84,87,89,91,93,95,98,100,102,104,106,108,111,113,115,117,119,121,123,126,128,130,
132,134,136,138,140,142,145,147,149,151,153,155,157,159,161,163,165,167,169,171,174,176,178,180,182,184,186,
188,190,192,194,196,198,200,202,204,206,208,209,211,213,215,217,219,221,223,225,227,229,231,233,234,236,238,
240,242,244,246,247,249,251,253,255,257,258,260,262,264,266,267,269,271,273,275,276,278,280,282,283,285,287,
288,290,292,294,295,297,299,300,302,304,305,307,309,310,312,314,315,317,318,320,322,323,325,326,328,330,331,
333,334,336,337,339,341,342,344,345,347,348,350,351,353,354,356,357,359,360,361,363,364,366,367,369,370,372,
373,374,376,377,379,380,381,383,384,386,387,388,390,391,392,394,395,396,398,399,400,402,403,404,405,407,408,
409,411,412,413,414,416,417,418,419,421,422,423,424,426,427,428,429,430,432,433,434,435,436,437,439,440,441,
442,443,444,445,447,448,449};
typedef union {
int32_t i;
uint32_t ui;
unsigned char b[4];
}
i2b ;
int16_t _atan2(int32_t y, int32_t x) {
int8_t ny=0, nx=0;
int16_t result;
i2b xx,yy;
if(y < 0) {
y = -y;
ny=1;
}
if(x < 0) {
x = -x;
nx=1;
}
xx.i=x; yy.i=y; //assigments do not emit assembly code, yet allow addressing individual byte in int32s shaving off about 10 cycles
if(y==x)
result=450;
else if(x>y) { // 0-45 octant, we want y/(x/256) or (y*256)/x
if(xx.b[3]) //shortest, emits only 2 instructions (and, breq); alternatives like if(x&0xff000000) or if((uint_32)x>>24) emit at least 8 instructions
result=atan2_tbl[(uint8_t)(y/(x>>8))];
else
result=atan2_tbl[(uint8_t)((y<<8)/x)]; // safe because y<x<=0xffff
}
else { // 45-90 octant, we want x/(y/256) or (x*256)/y
if(yy.b[3])
result=atan2_tbl[(uint8_t)(x/(y>>8))];
else
result=atan2_tbl[(uint8_t)((x<<8)/y)];
result = 900 - result;
}
if(ny)
if(nx) result -= 1800;
else result=-result;
else
if(nx) result = 1800-result;
return result;
}