diff options
Diffstat (limited to 'maths.c')
| -rw-r--r-- | maths.c | 327 |
1 files changed, 229 insertions, 98 deletions
@@ -1,115 +1,246 @@ #include "maths.h" +#include "plat.h" -#define sin_table_count 256 - -static const int sin_table[sin_table_count] = { - 0, 25, 50, 75, - 100, 125, 150, 175, - 200, 224, 249, 273, - 297, 321, 345, 369, - 392, 415, 438, 460, - 483, 505, 526, 548, - 569, 590, 610, 630, - 650, 669, 688, 706, - 724, 742, 759, 775, - 792, 807, 822, 837, - 851, 865, 878, 891, - 903, 915, 926, 936, - 946, 955, 964, 972, - 980, 987, 993, 999, - 1004, 1009, 1013, 1016, - 1019, 1021, 1023, 1024, - 1024, 1024, 1023, 1021, - 1019, 1016, 1013, 1009, - 1004, 999, 993, 987, - 980, 972, 964, 955, - 946, 936, 926, 915, - 903, 891, 878, 865, - 851, 837, 822, 807, - 792, 775, 759, 742, - 724, 706, 688, 669, - 650, 630, 610, 590, - 569, 548, 526, 505, - 483, 460, 438, 415, - 392, 369, 345, 321, - 297, 273, 249, 224, - 200, 175, 150, 125, - 100, 75, 50, 25, - 0, -25, -50, -75, - -100, -125, -150, -175, - -200, -224, -249, -273, - -297, -321, -345, -369, - -392, -415, -438, -460, - -483, -505, -526, -548, - -569, -590, -610, -630, - -650, -669, -688, -706, - -724, -742, -759, -775, - -792, -807, -822, -837, - -851, -865, -878, -891, - -903, -915, -926, -936, - -946, -955, -964, -972, - -980, -987, -993, -999, - -1004, -1009, -1013, -1016, - -1019, -1021, -1023, -1024, - -1024, -1024, -1023, -1021, - -1019, -1016, -1013, -1009, - -1004, -999, -993, -987, - -980, -972, -964, -955, - -946, -936, -926, -915, - -903, -891, -878, -865, - -851, -837, -822, -807, - -792, -775, -759, -742, - -724, -706, -688, -669, - -650, -630, -610, -590, - -569, -548, -526, -505, - -483, -460, -438, -415, - -392, -369, -345, -321, - -297, -273, -249, -224, - -200, -175, -150, -125, - -100, -75, -50, -25 +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]; -int fsqrt(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; +int sqrt_table[sqrt_table_count] = { + 0x0000, 0x0200, 0x02d4, 0x0376, 0x0400, 0x0478, 0x04e6, 0x054a, + 0x05a8, 0x0600, 0x0653, 0x06a2, 0x06ed, 0x0736, 0x077b, 0x07be, + 0x0800, 0x083f, 0x087c, 0x08b7, 0x08f1, 0x092a, 0x0961, 0x0997, + 0x09cc, 0x0a00, 0x0a32, 0x0a64, 0x0a95, 0x0ac5, 0x0af4, 0x0b22, + 0x0b50, 0x0b7d, 0x0ba9, 0x0bd5, 0x0c00, 0x0c2a, 0x0c54, 0x0c7d, + 0x0ca6, 0x0cce, 0x0cf6, 0x0d1d, 0x0d44, 0x0d6a, 0x0d90, 0x0db6, + 0x0ddb, 0x0e00, 0x0e24, 0x0e48, 0x0e6c, 0x0e8f, 0x0eb2, 0x0ed5, + 0x0ef7, 0x0f19, 0x0f3b, 0x0f5c, 0x0f7d, 0x0f9e, 0x0fbf, 0x0fdf, + 0x1000, 0x101f, 0x103f, 0x105e, 0x107e, 0x109c, 0x10bb, 0x10da, + 0x10f8, 0x1116, 0x1134, 0x1152, 0x116f, 0x118c, 0x11a9, 0x11c6, + 0x11e3, 0x1200, 0x121c, 0x1238, 0x1254, 0x1270, 0x128c, 0x12a7, + 0x12c2, 0x12de, 0x12f9, 0x1314, 0x132e, 0x1349, 0x1364, 0x137e, + 0x1398, 0x13b2, 0x13cc, 0x13e6, 0x1400, 0x1419, 0x1432, 0x144c, + 0x1465, 0x147e, 0x1497, 0x14b0, 0x14c8, 0x14e1, 0x14f9, 0x1512, + 0x152a, 0x1542, 0x155a, 0x1572, 0x158a, 0x15a2, 0x15b9, 0x15d1, + 0x15e8, 0x1600, 0x1617, 0x162e, 0x1645, 0x165c, 0x1673, 0x1689, +}; + +#define mtx_stack_max 0x20 + +int mtx_stack[mtx_stack_max][0x10]; +int mtx_stack_top; + +void init_maths(void) { + int i, hs = sin_table_count >> 1; + int m[0x10]; + for ( + i = hs; + i < sin_table_count; + i++ + ) { + sin_table[i] = -sin_table[i - hs]; } - return mid; + hs >>= 1; + for (i = 0; i < sin_table_count; i++) { + cos_table[i] = sin_table[(i + hs) & sin_table_mask]; + } + mtx_stack_top = 1; + mtx_iden(m); + mtx_cpy(mtx_stack[0], m); +} + +int* mtx_iden(int* m) { + register int o = f1; + m[0] = o; + m[1] = 0; + m[2] = 0; + m[3] = 0; + m[4] = 0; + m[5] = o; + m[6] = 0; + m[7] = 0; + m[8] = 0; + m[9] = 0; + m[10] = o; + m[11] = 0; + m[12] = 0; + m[13] = 0; + m[14] = 0; + m[15] = o; + return m; +} + +int* mtx_cpy(int* d, const int* s) { + d[0]=s[0];d[1]=s[1];d[2]=s[2];d[3]=s[3]; + d[4]=s[4];d[5]=s[5];d[6]=s[6];d[7]=s[7]; + d[8]=s[8];d[9]=s[9];d[10]=s[10];d[11]=s[11]; + d[12]=s[12];d[13]=s[13];d[14]=s[14];d[15]=s[15]; + return d; +} + +int* mtx_mul(int* d, const int* a, const int* b) { +#define mul(a, b) \ + ((a * b) >> fbits) +#define dot(x1, y1, z1, w1, x2, y2, z2, w2) \ + mul(x1, x2) + mul(y1, y2) + mul(z1, z2) + mul(w1, w2) + d[0] = dot(a[0], a[1], a[2], a[3], b[0],b[4],b[8], b[12]); + d[1] = dot(a[0], a[1], a[2], a[3], b[1],b[5],b[9], b[13]); + d[2] = dot(a[0], a[1], a[2], a[3], b[2],b[6],b[10],b[14]); + d[3] = dot(a[0], a[1], a[2], a[3], b[3],b[7],b[11],b[15]); + d[4] = dot(a[4], a[5], a[6], a[7], b[0],b[4],b[8], b[12]); + d[5] = dot(a[4], a[5], a[6], a[7], b[1],b[5],b[9], b[13]); + d[6] = dot(a[4], a[5], a[6], a[7], b[2],b[6],b[10],b[14]); + d[7] = dot(a[4], a[5], a[6], a[7], b[3],b[7],b[11],b[15]); + d[8] = dot(a[8], a[9], a[10],a[11], b[0],b[4],b[8], b[12]); + d[9] = dot(a[8], a[9], a[10],a[11], b[1],b[5],b[9], b[13]); + d[10] = dot(a[8], a[9], a[10],a[11], b[2],b[6],b[10],b[14]); + d[11] = dot(a[8], a[9], a[10],a[11], b[3],b[7],b[11],b[15]); + d[12] = dot(a[12],a[13],a[14],a[15], b[0],b[4],b[8], b[12]); + d[13] = dot(a[12],a[13],a[14],a[15], b[1],b[5],b[9], b[13]); + d[14] = dot(a[12],a[13],a[14],a[15], b[2],b[6],b[10],b[14]); + d[15] = dot(a[12],a[13],a[14],a[15], b[3],b[7],b[11],b[15]); +#undef dot +#undef mul + return d; +} + +void mtx_push(const int* m) { + int* d, i; + assert(mtx_stack_top < mtx_stack_max); + i = mtx_stack_top++; + d = mtx_stack[i]; + mtx_iden(d); + mtx_mul(d, mtx_stack[i - 1], m); +} + +void mtx_push_trans(int* v) { + int m[0x10]; + mtx_trans(m, v); + mtx_push(m); +} + +void mtx_push_rot_x(int a) { + int m[0x10]; + mtx_rot_x(m, a); + mtx_push(m); +} + +void mtx_push_rot_y(int a) { + int m[0x10]; + mtx_rot_y(m, a); + mtx_push(m); +} + +void mtx_push_rot_z(int a) { + int m[0x10]; + mtx_rot_z(m, a); + mtx_push(m); +} + +int* mtx_pop(void) { + assert(mtx_stack_top > 0); + return mtx_stack[--mtx_stack_top]; +} + +int* mtx_popn(int n) { + int* t = mtx_pop(); n--; + while (n--) + mtx_pop(); + return t; +} + +int* mtx_peek(void) { + return mtx_stack[mtx_stack_top - 1]; +} + +int* mtx_trans(int* d, int* v) { + mtx_iden(d); + d[3] = v[0]; + d[7] = v[1]; + d[11] = v[2]; + return d; +} + +int* mtx_rot_x(int* d, int a) { + mtx_iden(d); + a &= sin_table_mask; + d[5] = cos_table[a]; + d[6] = -sin_table[a]; + d[9] = sin_table[a]; + d[10] = cos_table[a]; + return d; } -int absolute(int x) { - return x >= 0 ? x : -x; +int* mtx_rot_y(int* d, int a) { + mtx_iden(d); + a &= sin_table_mask; + d[0] = cos_table[a]; + d[2] = sin_table[a]; + d[8] = -sin_table[a]; + d[10] = cos_table[a]; + return d; } -int fsin(int t) { - int index, sign; - sign = t >= 0 ? 1 : -1; - t = absolute(t); - index = (t * (sin_table_count) / ((1 << fbits) * 2)); - index = index % sin_table_count; - return (sign * sin_table[index]) / 2; +int* mtx_rot_z(int* d, int a) { + mtx_iden(d); + a &= sin_table_mask; + d[0] = cos_table[a]; + d[1] = -sin_table[a]; + d[4] = sin_table[a]; + d[5] = cos_table[a]; + return d; } -int fcos(int t) { - return fsin((1 << fbits) / 2 - t); +int* mtx_apply(const int* m, int* v) { + int s[4]; + vec_cpy(s, v, 4); +#define mul(a, b) \ + ((a * b) >> fbits) +#define dot(x1, y1, z1, w1, x2, y2, z2, w2) \ + mul(x1, x2) + mul(y1, y2) + mul(z1, z2) + mul(w1, w2) + v[0] = dot(s[0],s[1],s[2],s[3], m[0], m[1], m[2], m[3]); + v[1] = dot(s[0],s[1],s[2],s[3], m[4], m[5], m[6], m[7]); + v[2] = dot(s[0],s[1],s[2],s[3], m[8], m[9], m[10],m[11]); + v[3] = dot(s[0],s[1],s[2],s[3], m[12],m[13],m[14],m[15]); +#undef mul +#undef dot + return v; } -int ftan(int t) { - return (fsin(t) << fbits) / fcos(t); +int* vec_cpy(int* d, const int* s, int c) { + int i; + for (i = 0; i < c; i++) d[i] = s[i]; + return d; } -int flerp(int a, int b, int t) { - return a + ((t * (b - a)) >> fbits); +void persp(int* v, int asp) { + v[0] = ((v[0] << fbits) / v[2]); + v[1] = ((v[1] << fbits) / v[2]) * asp >> fbits; + v[2] = f1; } +void ndc2clip(int* c, int* p) { + register int hw = c[2] >> 1; + register int hh = c[3] >> 1; + p[0] = ((hw << fbits) * p[0]) >> fbits; + p[1] = ((hh << fbits) * p[1]) >> fbits; + p[0] >>= fbits; + p[1] >>= fbits; + p[0] += hw; + p[1] += hh; +} |