# HG changeset patch # User John Tsiombikas # Date 1402704296 -10800 # Node ID 78d1664c24437a5a50a19baff8606b2266ac4653 # Parent 8e9225853d7518c09fe529099e7cd43ffe1fd384 - fixed sin_x16/cos_x16 - added fixed point header diff -r 8e9225853d75 -r 78d1664c2443 src/fixed.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/fixed.c Sat Jun 14 03:04:56 2014 +0300 @@ -0,0 +1,101 @@ +#include "fixed.h" + +#if 0 +/* fast 16.16 division ripped from http://me.henri.net/fp-div.html */ +int32_t x16div(register int32_t numerator, register int32_t denominator) +{ + register int32_t quotient; + asm("num .req %[numerator] @ Map Register Equates\n\t" + "den .req %[denominator]\n\t" + "mod .req r2\n\t" + "cnt .req r3\n\t" + "quo .req r4\n\t" + "sign .req r12\n\t" + /* set sign and ensure numerator and denominator are positive */ + "cmp den, #0 @ exceptioin if den == zero\n\t" + "beq .div0\n\t" + "eor sign, num, den @ sign = num ^ den\n\t" + "rsbmi den, den, #0 @ den = -den if den < 0\n\t" + "subs mod, den, #1 @ mod = den - 1\n\t" + "beq .div1 @ return if den == 1\n\t" + "movs cnt, num @ num = -num if num < 0\n\t" + "rsbmi num, num, #0\n\t" + /* skip if deniminator >= numerator */ + "movs cnt, num, lsr #16 @ return if den >= num << 16\n\t" + "bne .cont\n\t" + "cmp den, num, lsl #16\n\t" + "bhs .numLeDen\n\t" + "\n.cont:\n\t" + /* test if denominator is a power of two */ + "tst den, mod @ if(den & (den - 1) == 0)\n\t" + "beq .powerOf2 @ den is power of 2\n\t" + /* count leading zeros */ + "stmfd sp!, {r4} @ push r4 (quo) onto the stack\n\t" + "mov cnt, #28 @ count difference in leading zeros\n\t" + "mov mod, num, lsr #4 @ between num and den\n\t" + "cmp den, mod, lsr #12; subls cnt, cnt, #16; movls mod, mod, lsr #16\n\t" + "cmp den, mod, lsr #4 ; subls cnt, cnt, #8 ; movls mod, mod, lsr #8\n\t" + "cmp den, mod ; subls cnt, cnt, #4 ; movls mod, mod, lsr #4\n\t" + /* shift numerator left by cnt bits */ + "mov num, num, lsl cnt @ mod:num = num << cnt\n\t" + "mov quo, #0\n\t" + "rsb den, den, #0 @ negate den for divide loop\n\t" + /* skip cnt iterations in the divide loop */ + "adds num, num, num @ start: num = mod:num / den\n\t" + "add pc, pc, cnt, lsl #4 @ skip cnt x 4 x 4 iterations\n\t" + "nop @ nop instruction takes care of pipeline\n\t" + /* inner loop unrolled x 48 */ + ".rept 47 @ inner loop x 48\n\t" + " adcs mod, den, mod, lsl #1\n\t" + " subcc mod, mod, den\n\t" + " adc quo, quo, quo\n\t" + " adds num, num, num\n\t" + ".endr\n\t" + "adcs mod, den, mod, lsl #1\n\t" + "subcc mod, mod, den\n\t" + "adc quo, quo, quo\n\t" + /* negate quotient if signed */ + "cmp sign, #0 @ negate quotient if sign < 0\n\t" + "mov num, quo\n\t" + "rsbmi num, num, #0\n\t" + "ldmfd sp!, {r4} @ pop r4 (quo) off the stack\n\t" + "mov pc, lr @return\n\t" + /* divide by zero handler */ + "\n.div0:\n\t" + "mov num, #0\n\t" + "mov pc, lr @return\n\t" + /* divide by one handler */ + "\n.div1:\n\t" + "cmp sign, #0\n\t" + "mov num, num, asl #16\n\t" + "rsbmi num, num, #0\n\t" + "mov pc, lr @return\n\t" + /* numerator less than or equal to denominator handler */ + "\n.numLeDen:\n\t" + "mov num, #0 @ quotient = 0 if num < den\n\t" + "moveq num, sign, asr #31 @ negate quotient if sign < 0\n\t" + "orreq num, num, #1 @ quotient = 1 if num == den\n\t" + "mov pc, lr @return\n\t" + /* power of two handler */ + "\n.powerOf2:\n\t" + "mov cnt, #0\n\t" + "cmp den, #(1 << 16); movhs cnt, #16 ; movhs den, den, lsr #16\n\t" + "cmp den, #(1 << 8) ; addhs cnt, cnt, #8; movhs den, den, lsr #8\n\t" + "cmp den, #(1 << 4) ; addhs cnt, cnt, #4; movhs den, den, lsr #4\n\t" + "cmp den, #(1 << 2) ; addhi cnt, cnt, #3; addls cnt, cnt, den, lsr #1\n\t" + "rsb mod, cnt, #32\n\t" + "mov den, num, lsr #16 @ den:num = num << 16\n\t" + "mov num, num, lsl #16\n\t" + "mov num, num, lsr cnt @ num = num >> cnt | den << mod\n\t" + "orr num, num, den, lsl mod\n\t" + "cmp sign, #0\n\t" + "rsbmi num, num, #0 @ negate quotient if sign < 0" + /* output registers */ + : [quotient] "=r" (quotient) + /* input registers */ + : [numerator] "0" (numerator), [denominator] "r" (denominator) + /* clobbered registers */ + : "r2" /* mod */, "r3" /* cnt */, "r12" /* sign */); + return quotient; +} +#endif diff -r 8e9225853d75 -r 78d1664c2443 src/fixed.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/fixed.h Sat Jun 14 03:04:56 2014 +0300 @@ -0,0 +1,11 @@ +#ifndef FIXED_H_ +#define FIXED_H_ + +#include + +#define x16mul(a, b) (int32_t)(((int32_t)(a) >> 8) * ((int32_t)(b) >> 8)) +/*int32_t x16div(register int32_t num, register int32_t denom);*/ + +#define x16div(a, b) (int32_t)(((int64_t)(a) << 16) / (b)) + +#endif /* FIXED_H_ */ diff -r 8e9225853d75 -r 78d1664c2443 src/main.c --- a/src/main.c Sat Jun 14 01:56:58 2014 +0300 +++ b/src/main.c Sat Jun 14 03:04:56 2014 +0300 @@ -1,3 +1,4 @@ +#include #include #include "gbasys.h" #include "logger.h" @@ -49,6 +50,8 @@ static int offset; +#define X16INT(x) ((x) << 16) + static void draw(void) { int i, j; @@ -58,16 +61,12 @@ clear_buffer(back_buffer, 0); for(i=0; i +#include "fixed.h" #include "sincos.h" #include "logger.h" @@ -31,12 +32,12 @@ int32_t sin_x16(int32_t radians) { - int32_t na = (radians << 16) / (M_PI_X16 * 2); - return sin_int((na >> 8) * (SINLUT_SIZE << 8)); + int32_t na = x16div(radians, M_PI_X16 * 2); + return (sin_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE; } int32_t cos_x16(int32_t radians) { - int32_t na = (radians << 16) / (M_PI_X16 * 2); - return cos_int((na >> 8) * (SINLUT_SIZE << 8)); + int32_t na = x16div(radians, M_PI_X16 * 2); + return (cos_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE; } diff -r 8e9225853d75 -r 78d1664c2443 src/sincos.h --- a/src/sincos.h Sat Jun 14 01:56:58 2014 +0300 +++ b/src/sincos.h Sat Jun 14 03:04:56 2014 +0300 @@ -3,8 +3,8 @@ #include -#define M_PI_X16 (int32_t)(M_PI * 65536.0) -/*#define M_PI_X16 (int32_t)((31416 << 16) / 10000)*/ +/*#define M_PI_X16 (int32_t)(M_PI * 65536.0) */ +#define M_PI_X16 (int32_t)((31416 << 16) / 10000) #define SINLUT_SCALE 512 #define SINLUT_SIZE 512