gba-x3dtest
changeset 4:78d1664c2443
- fixed sin_x16/cos_x16
- added fixed point header
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sat, 14 Jun 2014 03:04:56 +0300 |
| parents | 8e9225853d75 |
| children | 850be43b3135 |
| files | src/fixed.c src/fixed.h src/main.c src/sincos.c src/sincos.h |
| diffstat | 5 files changed, 126 insertions(+), 16 deletions(-) [+] |
line diff
1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 1.2 +++ b/src/fixed.c Sat Jun 14 03:04:56 2014 +0300 1.3 @@ -0,0 +1,101 @@ 1.4 +#include "fixed.h" 1.5 + 1.6 +#if 0 1.7 +/* fast 16.16 division ripped from http://me.henri.net/fp-div.html */ 1.8 +int32_t x16div(register int32_t numerator, register int32_t denominator) 1.9 +{ 1.10 + register int32_t quotient; 1.11 + asm("num .req %[numerator] @ Map Register Equates\n\t" 1.12 + "den .req %[denominator]\n\t" 1.13 + "mod .req r2\n\t" 1.14 + "cnt .req r3\n\t" 1.15 + "quo .req r4\n\t" 1.16 + "sign .req r12\n\t" 1.17 + /* set sign and ensure numerator and denominator are positive */ 1.18 + "cmp den, #0 @ exceptioin if den == zero\n\t" 1.19 + "beq .div0\n\t" 1.20 + "eor sign, num, den @ sign = num ^ den\n\t" 1.21 + "rsbmi den, den, #0 @ den = -den if den < 0\n\t" 1.22 + "subs mod, den, #1 @ mod = den - 1\n\t" 1.23 + "beq .div1 @ return if den == 1\n\t" 1.24 + "movs cnt, num @ num = -num if num < 0\n\t" 1.25 + "rsbmi num, num, #0\n\t" 1.26 + /* skip if deniminator >= numerator */ 1.27 + "movs cnt, num, lsr #16 @ return if den >= num << 16\n\t" 1.28 + "bne .cont\n\t" 1.29 + "cmp den, num, lsl #16\n\t" 1.30 + "bhs .numLeDen\n\t" 1.31 + "\n.cont:\n\t" 1.32 + /* test if denominator is a power of two */ 1.33 + "tst den, mod @ if(den & (den - 1) == 0)\n\t" 1.34 + "beq .powerOf2 @ den is power of 2\n\t" 1.35 + /* count leading zeros */ 1.36 + "stmfd sp!, {r4} @ push r4 (quo) onto the stack\n\t" 1.37 + "mov cnt, #28 @ count difference in leading zeros\n\t" 1.38 + "mov mod, num, lsr #4 @ between num and den\n\t" 1.39 + "cmp den, mod, lsr #12; subls cnt, cnt, #16; movls mod, mod, lsr #16\n\t" 1.40 + "cmp den, mod, lsr #4 ; subls cnt, cnt, #8 ; movls mod, mod, lsr #8\n\t" 1.41 + "cmp den, mod ; subls cnt, cnt, #4 ; movls mod, mod, lsr #4\n\t" 1.42 + /* shift numerator left by cnt bits */ 1.43 + "mov num, num, lsl cnt @ mod:num = num << cnt\n\t" 1.44 + "mov quo, #0\n\t" 1.45 + "rsb den, den, #0 @ negate den for divide loop\n\t" 1.46 + /* skip cnt iterations in the divide loop */ 1.47 + "adds num, num, num @ start: num = mod:num / den\n\t" 1.48 + "add pc, pc, cnt, lsl #4 @ skip cnt x 4 x 4 iterations\n\t" 1.49 + "nop @ nop instruction takes care of pipeline\n\t" 1.50 + /* inner loop unrolled x 48 */ 1.51 + ".rept 47 @ inner loop x 48\n\t" 1.52 + " adcs mod, den, mod, lsl #1\n\t" 1.53 + " subcc mod, mod, den\n\t" 1.54 + " adc quo, quo, quo\n\t" 1.55 + " adds num, num, num\n\t" 1.56 + ".endr\n\t" 1.57 + "adcs mod, den, mod, lsl #1\n\t" 1.58 + "subcc mod, mod, den\n\t" 1.59 + "adc quo, quo, quo\n\t" 1.60 + /* negate quotient if signed */ 1.61 + "cmp sign, #0 @ negate quotient if sign < 0\n\t" 1.62 + "mov num, quo\n\t" 1.63 + "rsbmi num, num, #0\n\t" 1.64 + "ldmfd sp!, {r4} @ pop r4 (quo) off the stack\n\t" 1.65 + "mov pc, lr @return\n\t" 1.66 + /* divide by zero handler */ 1.67 + "\n.div0:\n\t" 1.68 + "mov num, #0\n\t" 1.69 + "mov pc, lr @return\n\t" 1.70 + /* divide by one handler */ 1.71 + "\n.div1:\n\t" 1.72 + "cmp sign, #0\n\t" 1.73 + "mov num, num, asl #16\n\t" 1.74 + "rsbmi num, num, #0\n\t" 1.75 + "mov pc, lr @return\n\t" 1.76 + /* numerator less than or equal to denominator handler */ 1.77 + "\n.numLeDen:\n\t" 1.78 + "mov num, #0 @ quotient = 0 if num < den\n\t" 1.79 + "moveq num, sign, asr #31 @ negate quotient if sign < 0\n\t" 1.80 + "orreq num, num, #1 @ quotient = 1 if num == den\n\t" 1.81 + "mov pc, lr @return\n\t" 1.82 + /* power of two handler */ 1.83 + "\n.powerOf2:\n\t" 1.84 + "mov cnt, #0\n\t" 1.85 + "cmp den, #(1 << 16); movhs cnt, #16 ; movhs den, den, lsr #16\n\t" 1.86 + "cmp den, #(1 << 8) ; addhs cnt, cnt, #8; movhs den, den, lsr #8\n\t" 1.87 + "cmp den, #(1 << 4) ; addhs cnt, cnt, #4; movhs den, den, lsr #4\n\t" 1.88 + "cmp den, #(1 << 2) ; addhi cnt, cnt, #3; addls cnt, cnt, den, lsr #1\n\t" 1.89 + "rsb mod, cnt, #32\n\t" 1.90 + "mov den, num, lsr #16 @ den:num = num << 16\n\t" 1.91 + "mov num, num, lsl #16\n\t" 1.92 + "mov num, num, lsr cnt @ num = num >> cnt | den << mod\n\t" 1.93 + "orr num, num, den, lsl mod\n\t" 1.94 + "cmp sign, #0\n\t" 1.95 + "rsbmi num, num, #0 @ negate quotient if sign < 0" 1.96 + /* output registers */ 1.97 + : [quotient] "=r" (quotient) 1.98 + /* input registers */ 1.99 + : [numerator] "0" (numerator), [denominator] "r" (denominator) 1.100 + /* clobbered registers */ 1.101 + : "r2" /* mod */, "r3" /* cnt */, "r12" /* sign */); 1.102 + return quotient; 1.103 +} 1.104 +#endif
2.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 2.2 +++ b/src/fixed.h Sat Jun 14 03:04:56 2014 +0300 2.3 @@ -0,0 +1,11 @@ 2.4 +#ifndef FIXED_H_ 2.5 +#define FIXED_H_ 2.6 + 2.7 +#include <stdint.h> 2.8 + 2.9 +#define x16mul(a, b) (int32_t)(((int32_t)(a) >> 8) * ((int32_t)(b) >> 8)) 2.10 +/*int32_t x16div(register int32_t num, register int32_t denom);*/ 2.11 + 2.12 +#define x16div(a, b) (int32_t)(((int64_t)(a) << 16) / (b)) 2.13 + 2.14 +#endif /* FIXED_H_ */
3.1 --- a/src/main.c Sat Jun 14 01:56:58 2014 +0300 3.2 +++ b/src/main.c Sat Jun 14 03:04:56 2014 +0300 3.3 @@ -1,3 +1,4 @@ 3.4 +#include <math.h> 3.5 #include <stdint.h> 3.6 #include "gbasys.h" 3.7 #include "logger.h" 3.8 @@ -49,6 +50,8 @@ 3.9 3.10 static int offset; 3.11 3.12 +#define X16INT(x) ((x) << 16) 3.13 + 3.14 static void draw(void) 3.15 { 3.16 int i, j; 3.17 @@ -58,16 +61,12 @@ 3.18 clear_buffer(back_buffer, 0); 3.19 3.20 for(i=0; i<WIDTH; i++) { 3.21 - int16_t x = 2 * (i - offset) * SINLUT_SIZE / WIDTH; 3.22 - int16_t y = sin_int(x); 3.23 - y = HEIGHT - (y * (HEIGHT / 2) / SINLUT_SCALE / 2 + HEIGHT / 2); 3.24 + float x = M_PI * 4.0 * (float)(i - offset) / (float)WIDTH;/*((M_PI_X16 * ((2 * (i - offset)) << 8)) << 8) / WIDTH;*/ 3.25 + float y = (HEIGHT / 4.0) * ((float)sin_x16(x * 65536.0) / 65536.0); 3.26 3.27 - pixels[y * WIDTH + i] = RGB(255, 0, 0); 3.28 + int iy = HEIGHT - (y + HEIGHT / 2.0); 3.29 3.30 - y = cos_int(x); 3.31 - y = HEIGHT - (y * (HEIGHT / 2) / SINLUT_SCALE / 2 + HEIGHT / 2); 3.32 - 3.33 - pixels[y * WIDTH + i] = RGB(0, 255, 0); 3.34 + pixels[iy * WIDTH + i] = RGB(255, 0, 0); 3.35 3.36 if(i == offset) { 3.37 for(j=0; j<HEIGHT; j++) { 3.38 @@ -83,12 +82,10 @@ 3.39 { 3.40 switch(key) { 3.41 case KEY_LEFT: 3.42 - logmsg(LOG_DBG, "foo! (%d)\n", offset); 3.43 --offset; 3.44 break; 3.45 3.46 case KEY_RIGHT: 3.47 - logmsg(LOG_DBG, "bar! (%d)\n", offset); 3.48 ++offset; 3.49 break; 3.50
4.1 --- a/src/sincos.c Sat Jun 14 01:56:58 2014 +0300 4.2 +++ b/src/sincos.c Sat Jun 14 03:04:56 2014 +0300 4.3 @@ -1,4 +1,5 @@ 4.4 #include <math.h> 4.5 +#include "fixed.h" 4.6 #include "sincos.h" 4.7 #include "logger.h" 4.8 4.9 @@ -31,12 +32,12 @@ 4.10 4.11 int32_t sin_x16(int32_t radians) 4.12 { 4.13 - int32_t na = (radians << 16) / (M_PI_X16 * 2); 4.14 - return sin_int((na >> 8) * (SINLUT_SIZE << 8)); 4.15 + int32_t na = x16div(radians, M_PI_X16 * 2); 4.16 + return (sin_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE; 4.17 } 4.18 4.19 int32_t cos_x16(int32_t radians) 4.20 { 4.21 - int32_t na = (radians << 16) / (M_PI_X16 * 2); 4.22 - return cos_int((na >> 8) * (SINLUT_SIZE << 8)); 4.23 + int32_t na = x16div(radians, M_PI_X16 * 2); 4.24 + return (cos_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE; 4.25 }
5.1 --- a/src/sincos.h Sat Jun 14 01:56:58 2014 +0300 5.2 +++ b/src/sincos.h Sat Jun 14 03:04:56 2014 +0300 5.3 @@ -3,8 +3,8 @@ 5.4 5.5 #include <stdint.h> 5.6 5.7 -#define M_PI_X16 (int32_t)(M_PI * 65536.0) 5.8 -/*#define M_PI_X16 (int32_t)((31416 << 16) / 10000)*/ 5.9 +/*#define M_PI_X16 (int32_t)(M_PI * 65536.0) */ 5.10 +#define M_PI_X16 (int32_t)((31416 << 16) / 10000) 5.11 5.12 #define SINLUT_SCALE 512 5.13 #define SINLUT_SIZE 512