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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
|
#include "maths.h"
int sin_table[sin_table_count] = {
0x000, 0x00c, 0x019, 0x025, 0x032, 0x03e, 0x04b, 0x057,
0x063, 0x070, 0x07c, 0x088, 0x094, 0x0a0, 0x0ac, 0x0b8,
0x0c3, 0x0cf, 0x0da, 0x0e6, 0x0f1, 0x0fc, 0x107, 0x111,
0x11c, 0x126, 0x130, 0x13a, 0x144, 0x14e, 0x157, 0x161,
0x16a, 0x172, 0x17b, 0x183, 0x18b, 0x193, 0x19b, 0x1a2,
0x1a9, 0x1b0, 0x1b7, 0x1bd, 0x1c3, 0x1c9, 0x1ce, 0x1d4,
0x1d9, 0x1dd, 0x1e2, 0x1e6, 0x1e9, 0x1ed, 0x1f0, 0x1f3,
0x1f6, 0x1f8, 0x1fa, 0x1fc, 0x1fd, 0x1fe, 0x1ff, 0x1ff,
0x200, 0x1ff, 0x1ff, 0x1fe, 0x1fd, 0x1fc, 0x1fa, 0x1f8,
0x1f6, 0x1f3, 0x1f0, 0x1ed, 0x1e9, 0x1e6, 0x1e2, 0x1dd,
0x1d9, 0x1d4, 0x1ce, 0x1c9, 0x1c3, 0x1bd, 0x1b7, 0x1b0,
0x1a9, 0x1a2, 0x19b, 0x193, 0x18b, 0x183, 0x17b, 0x172,
0x16a, 0x161, 0x157, 0x14e, 0x144, 0x13a, 0x130, 0x126,
0x11c, 0x111, 0x107, 0x0fc, 0x0f1, 0x0e6, 0x0da, 0x0cf,
0x0c3, 0x0b8, 0x0ac, 0x0a0, 0x094, 0x088, 0x07c, 0x070,
0x063, 0x057, 0x04b, 0x03e, 0x032, 0x025, 0x019, 0x00c,
};
int cos_table[sin_table_count];
void init_maths(void) {
int i, hs = sin_table_count >> 1;
for (
i = hs;
i < sin_table_count;
i++
) {
sin_table[i] = -sin_table[i - hs];
}
hs >>= 1;
for (i = 0; i < sin_table_count; i++) {
cos_table[i] = sin_table[(i + hs) & sin_table_mask];
}
}
int fxsqrt(int n) {
int lo, hi, mid, i, mm;
if (n <= 0) { return -1; }
lo = mini(1, n);
hi = maxi(1, n);
while ((100 << fbits * lo * lo) >> fbits * 2)
lo *= (10 << fbits) >> fbits;
while ((5 * hi * hi) >> fbits * 2 > n)
hi *= (1 << fbits) / 10;
for (i = 0; i < 100; i++) {
mid = ((lo + hi) * (1 << fbits)) / (2 << fbits);
mm = mid * mid >> fbits;
if (mm == n) return mid;
if (mm > n) hi = mid;
else lo = mid;
}
return mid;
}
|