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