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