Rev 256 | Rev 330 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 256 | Rev 293 | ||
---|---|---|---|
1 | #include "91x_lib.h" |
1 | #include "91x_lib.h" |
2 | #include "mymath.h" |
2 | #include "mymath.h" |
3 | 3 | ||
4 | // discrete mathematics |
4 | // discrete mathematics |
5 | 5 | ||
6 | // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
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}; |
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 | 8 | ||
9 | s16 c_sin_8192(s16 angle) |
9 | s16 c_sin_8192(s16 angle) |
10 | { |
10 | { |
11 | s8 m,n; |
11 | s8 m,n; |
12 | s16 sinus; |
12 | s16 sinus; |
13 | 13 | ||
14 | // avoid negative angles |
14 | // avoid negative angles |
15 | if (angle < 0) |
15 | if (angle < 0) |
16 | { |
16 | { |
17 | m = -1; |
17 | m = -1; |
18 | angle = -angle; |
18 | angle = -angle; |
19 | } |
19 | } |
20 | else m = +1; |
20 | else m = +1; |
21 | 21 | ||
22 | // fold angle to interval 0 to 359 |
22 | // fold angle to interval 0 to 359 |
23 | angle %= 360; |
23 | angle %= 360; |
24 | 24 | ||
25 | // check quadrant |
25 | // check quadrant |
26 | if (angle <= 90) n = 1; // first quadrant |
26 | if (angle <= 90) n = 1; // first quadrant |
27 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second 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 |
28 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
29 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
29 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
30 | // get lookup value |
30 | // get lookup value |
31 | sinus = sinlookup[angle]; |
31 | sinus = sinlookup[angle]; |
32 | // calculate sinus value |
32 | // calculate sinus value |
33 | return (sinus * m * n); |
33 | return (sinus * m * n); |
34 | } |
34 | } |
35 | 35 | ||
36 | // cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
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) |
37 | s16 c_cos_8192(s16 angle) |
38 | { |
38 | { |
39 | return (c_sin_8192(90 - angle)); |
39 | return (c_sin_8192(90 - angle)); |
40 | } |
40 | } |
- | 41 | ||
- | 42 | // higher resolution angle in deg is arg/div |
|
- | 43 | s16 c_sin_8192_res(s16 arg, s16 div) |
|
- | 44 | { |
|
- | 45 | s16 angle, rest; |
|
- | 46 | s32 tmp; |
|
- | 47 | ||
- | 48 | angle = arg/div; |
|
- | 49 | rest = arg%div; |
|
- | 50 | ||
- | 51 | if(rest>0) |
|
- | 52 | { |
|
- | 53 | tmp = (div-rest)*(s32)c_sin_8192(angle); |
|
- | 54 | tmp += rest * (s32)c_sin_8192(angle+1); |
|
- | 55 | tmp /= div; |
|
- | 56 | return(tmp); |
|
- | 57 | } |
|
- | 58 | else if(rest<0) |
|
- | 59 | { |
|
- | 60 | tmp = (div+rest)*(s32)c_sin_8192(angle); |
|
- | 61 | tmp -= rest * (s32)c_sin_8192(angle-1); |
|
- | 62 | tmp /= div; |
|
- | 63 | return(tmp); |
|
- | 64 | } |
|
- | 65 | else |
|
- | 66 | { |
|
- | 67 | return(c_sin_8192(angle)); |
|
- | 68 | } |
|
- | 69 | ||
- | 70 | } |
|
- | 71 | ||
- | 72 | s16 c_cos_8192_res(s16 arg, s16 div) |
|
- | 73 | { |
|
- | 74 | return(c_sin_8192_res(90*div - arg, div)); |
|
- | 75 | } |
|
- | 76 | ||
41 | 77 | ||
42 | // integer based atan2 that returns angle in counts of 1/546.13° |
78 | // integer based atan2 that returns angle in counts of 1/546.13° |
43 | s32 c_tan2_546(s32 y, s32 x) |
79 | s32 c_atan2_546(s32 y, s32 x) |
44 | { |
80 | { |
45 | s32 qx, qy, q; |
81 | s32 qx, qy, q; |
46 | 82 | ||
47 | if( x < 0) qx = -x; |
83 | if( x < 0) qx = -x; |
48 | else qx = x; |
84 | else qx = x; |
49 | if( y < 0) qy = -y; |
85 | if( y < 0) qy = -y; |
50 | else qy = y; |
86 | else qy = y; |
51 | if(qy <= qx) |
87 | if(qy <= qx) |
52 | { // scale down to avoid overflow in quadratic interpolation |
88 | { // scale down to avoid overflow in quadratic interpolation |
53 | while(qy > (1<<15)) |
89 | while(qy > (1<<15)) |
54 | { |
90 | { |
55 | qy/=2; |
91 | qy/=2; |
56 | qx/=2; |
92 | qx/=2; |
57 | } |
93 | } |
58 | // calculate the quatratic interpolation |
94 | // calculate the quatratic interpolation |
59 | q = (((qy<<13)/qx)*qy)/qx; |
95 | q = (((qy<<13)/qx)*qy)/qx; |
60 | q = (qy<<15)/qx-q; |
96 | q = (qy<<15)/qx-q; |
61 | } |
97 | } |
62 | else |
98 | else |
63 | { // scale down to avoid overflow in quadratic interpolation |
99 | { // scale down to avoid overflow in quadratic interpolation |
64 | while(qx>(1<<15)) |
100 | while(qx>(1<<15)) |
65 | { |
101 | { |
66 | qy/=2; |
102 | qy/=2; |
67 | qx/=2; |
103 | qx/=2; |
68 | } |
104 | } |
69 | // calculate the quatratic interpolation |
105 | // calculate the quatratic interpolation |
70 | q = (((qx<<13)/qy)*qx)/qy; |
106 | q = (((qx<<13)/qy)*qx)/qy; |
71 | q = (qx<<15)/qy-q; |
107 | q = (qx<<15)/qy-q; |
72 | q = 2*((1<<15)-(1<<13)) - q; |
108 | q = 2*((1<<15)-(1<<13)) - q; |
73 | } |
109 | } |
74 | if(y < 0) |
110 | if(y < 0) |
75 | { |
111 | { |
76 | if(x < 0) q = q - 4*((1<<15)-(1<<13)); |
112 | if(x < 0) q = q - 4*((1<<15)-(1<<13)); |
77 | else q = -q; |
113 | else q = -q; |
78 | } |
114 | } |
79 | else if( x < 0) q = 4*((1<<15)-(1<<13)) - q; |
115 | else if( x < 0) q = 4*((1<<15)-(1<<13)) - q; |
80 | return(q); |
116 | return(q); |
81 | } |
117 | } |
82 | 118 |