Rev 261 | Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 256 | killagreg | 1 | #include "91x_lib.h" |
| 2 | #include "mymath.h" |
||
| 3 | |||
| 4 | // discrete mathematics |
||
| 5 | |||
| 6 | // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
||
| 7 | const s16 sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
||
| 8 | |||
| 9 | s16 c_sin_8192(s16 angle) |
||
| 10 | { |
||
| 11 | s8 m,n; |
||
| 12 | s16 sinus; |
||
| 13 | |||
| 14 | // avoid negative angles |
||
| 15 | if (angle < 0) |
||
| 16 | { |
||
| 17 | m = -1; |
||
| 18 | angle = -angle; |
||
| 19 | } |
||
| 20 | else m = +1; |
||
| 21 | |||
| 22 | // fold angle to interval 0 to 359 |
||
| 23 | angle %= 360; |
||
| 24 | |||
| 25 | // check quadrant |
||
| 26 | if (angle <= 90) n = 1; // first quadrant |
||
| 27 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
||
| 28 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
||
| 29 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
||
| 30 | // get lookup value |
||
| 31 | sinus = sinlookup[angle]; |
||
| 32 | // calculate sinus value |
||
| 33 | return (sinus * m * n); |
||
| 34 | } |
||
| 35 | |||
| 36 | // cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
||
| 37 | s16 c_cos_8192(s16 angle) |
||
| 38 | { |
||
| 39 | return (c_sin_8192(90 - angle)); |
||
| 40 | } |
||
| 41 | |||
| 42 | // integer based atan2 that returns angle in counts of 1/546.13° |
||
| 43 | s32 c_tan2_546(s32 y, s32 x) |
||
| 44 | { |
||
| 45 | s32 qx, qy, q; |
||
| 46 | |||
| 47 | if( x < 0) qx = -x; |
||
| 48 | else qx = x; |
||
| 49 | if( y < 0) qy = -y; |
||
| 50 | else qy = y; |
||
| 51 | if(qy <= qx) |
||
| 52 | { // scale down to avoid overflow in quadratic interpolation |
||
| 53 | while(qy > (1<<15)) |
||
| 54 | { |
||
| 55 | qy/=2; |
||
| 56 | qx/=2; |
||
| 57 | } |
||
| 58 | // calculate the quatratic interpolation |
||
| 59 | q = (((qy<<13)/qx)*qy)/qx; |
||
| 60 | q = (qy<<15)/qx-q; |
||
| 61 | } |
||
| 62 | else |
||
| 63 | { // scale down to avoid overflow in quadratic interpolation |
||
| 64 | while(qx>(1<<15)) |
||
| 65 | { |
||
| 66 | qy/=2; |
||
| 67 | qx/=2; |
||
| 68 | } |
||
| 69 | // calculate the quatratic interpolation |
||
| 70 | q = (((qx<<13)/qy)*qx)/qy; |
||
| 71 | q = (qx<<15)/qy-q; |
||
| 72 | q = 2*((1<<15)-(1<<13)) - q; |
||
| 73 | } |
||
| 74 | if(y < 0) |
||
| 75 | { |
||
| 76 | if(x < 0) q = q - 4*((1<<15)-(1<<13)); |
||
| 77 | else q = -q; |
||
| 78 | } |
||
| 79 | else if( x < 0) q = 4*((1<<15)-(1<<13)) - q; |
||
| 80 | return(q); |
||
| 81 | } |