| 2,6 → 2,7 |
|---|
| #include <avr/pgmspace.h> |
| #include "mymath.h" |
| |
| |
| // discrete mathematics |
| |
| // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| 40,3 → 41,81 |
|---|
| return (c_sin_8192(90 - angle)); |
| } |
| |
| |
| // higher resolution angle in deg is arg/div |
| int16_t c_sin_8192_res(int16_t arg, int16_t div) |
| { |
| int16_t angle, rest; |
| int32_t tmp; |
| |
| angle = arg/div; |
| rest = arg%div; |
| |
| if(rest>0) |
| { |
| tmp = (div-rest)*(int32_t)c_sin_8192(angle); |
| tmp += rest * (int32_t)c_sin_8192(angle+1); |
| tmp /= div; |
| return(tmp); |
| } |
| else if(rest<0) |
| { |
| tmp = (div+rest)*(int32_t)c_sin_8192(angle); |
| tmp -= rest * (int32_t)c_sin_8192(angle-1); |
| tmp /= div; |
| return(tmp); |
| } |
| else |
| { |
| return(c_sin_8192(angle)); |
| } |
| |
| } |
| |
| int16_t c_cos_8192_res(int16_t arg, int16_t div) |
| { |
| return(c_sin_8192_res(90*div - arg, div)); |
| } |
| |
| |
| |
| // fast integer based atan2 that returns angle in counts of 1/546.13° |
| int32_t c_atan2_546(int32_t y, int32_t x) |
| { |
| int32_t qx, qy, q; |
| |
| if( x < 0) qx = -x; |
| else qx = x; |
| if( y < 0) qy = -y; |
| else qy = y; |
| if(qy <= qx) |
| { // scale down to avoid overflow in quadratic interpolation |
| while(qy > (1L<<15)) |
| { |
| qy /= 2; |
| qx /= 2; |
| } |
| // calculate the quatratic interpolation |
| q = (((qy<<13)/qx) * qy) / qx; |
| q = (qy<<15) / qx - q; |
| } |
| else |
| { // scale down to avoid overflow in quadratic interpolation |
| while(qx>(1L<<15)) |
| { |
| qy /= 2; |
| qx /= 2; |
| } |
| // calculate the quatratic interpolation |
| q = (((qx<<13) / qy) * qx) / qy; |
| q = (qx<<15) / qy - q; |
| q = 2 * ((1L<<15) - (1L<<13)) - q; |
| } |
| if(y < 0) |
| { |
| if(x < 0) q = q - 4 * ((1L<<15) - (1L<<13)); |
| else q = -q; |
| } |
| else if( x < 0) q = 4 * ((1L<<15) - (1L<<13)) - q; |
| return(q); |
| } |