From 4623bdafca736378c183d0efdb1194cfd7d6388b Mon Sep 17 00:00:00 2001 From: deftio Date: Mon, 15 Apr 2024 12:03:00 -0700 Subject: [PATCH] updated docs comments and formatting --- FR_Math_Example1.cpp | 761 ++++++++++++++++++++++--------------------- FR_defs.h | 58 ++-- FR_math.c | 503 +++++++++++++++------------- FR_math.h | 287 ++++++++-------- FR_math_2D.cpp | 123 ++++--- FR_math_2D.h | 226 ++++++++----- fr_math_test.c | 49 ++- 7 files changed, 1049 insertions(+), 958 deletions(-) diff --git a/FR_Math_Example1.cpp b/FR_Math_Example1.cpp index 8ec52b3..ce3ed6c 100644 --- a/FR_Math_Example1.cpp +++ b/FR_Math_Example1.cpp @@ -1,6 +1,6 @@ /** * @file FR_main.cpp - implementation test file for fixed radix cpp library source - * This program is intended to be run on a 32bit machine to show how the library works. + * This program is intended to be run on a 32bit machine to show how the library works. * * @copy Copyright (C) <2001-2012> * @author M A Chatterjee @@ -29,92 +29,92 @@ #include #include -#include "FR_defs.h" /* platform abstraction, constants, status codes */ -#include "FR_math.h" /* header file for fr_math.h */ -#include "FR_math_2D.h" /* 2D transformations, coordinate transforms */ +#include "FR_defs.h" /* platform abstraction, constants, status codes */ +#include "FR_math.h" /* header file for fr_math.h */ +#include "FR_math_2D.h" /* 2D transformations, coordinate transforms */ -//these next 2 lines are only for this demo and not a runtime dependancy on FR_math routines -#include "math.h" -#define R2D (2.0*3.14159265358/360.0) -#define D2R (360.0/2.0*3.14159265358) +// these next 2 lines are only for this demo and not a runtime dependancy on FR_math routines +#include "math.h" +#define R2D (2.0 * 3.14159265358 / 360.0) +#define D2R (360.0 / 2.0 * 3.14159265358) -#define FR_SML(x,y) (fabs(fabs((double)(x))-fabs((double)(y)))) -#define FR_ERR(item,ref) ((fabs((double)ref)>0.0000001) ?(((((double)item)-((double)ref)))/((double)ref))*(100.0):FR_SML(item,ref)) +#define FR_SML(x, y) (fabs(fabs((double)(x)) - fabs((double)(y)))) +#define FR_ERR(item, ref) ((fabs((double)ref) > 0.0000001) ? (((((double)item) - ((double)ref))) / ((double)ref)) * (100.0) : FR_SML(item, ref)) int gTestRadians = 0; -int gTestRadianMACROS = 0; +int gTestRadianMACROS = 0; int gTestForwardTrig = 0; int gTestInvTrig = 0; int gTestPow2andLog2 = 1; //=============================================== -//Print out the matrix in its pure integer representation -void printMatrixRaw (FR_Matrix2D_CPT *nM) +// Print out the matrix in its pure integer representation +void printMatrixRaw(FR_Matrix2D_CPT *nM) { - printf("%8d %8d %8d\n",(int)nM->m00,(int)nM->m01,(int)nM->m02); - printf("%8d %8d %8d\n",(int)nM->m10,(int)nM->m11,(int)nM->m12); - printf("%8d %8d %8d\n",0,0,1<<(nM->radix)); + printf("%8d %8d %8d\n", (int)nM->m00, (int)nM->m01, (int)nM->m02); + printf("%8d %8d %8d\n", (int)nM->m10, (int)nM->m11, (int)nM->m12); + printf("%8d %8d %8d\n", 0, 0, 1 << (nM->radix)); } //=============================================== -//Print out the matrix in its floating point equivalent for readability -//note this entire representation won't be available on low end micros -void printMatrixFloat (FR_Matrix2D_CPT *nM) +// Print out the matrix in its floating point equivalent for readability +// note this entire representation won't be available on low end micros +void printMatrixFloat(FR_Matrix2D_CPT *nM) { - printf("%10.4f %10.4f %10.4f\n",FR2D(nM->m00,nM->radix),FR2D(nM->m01,nM->radix),FR2D(nM->m02,nM->radix)); - printf("%10.4f %10.4f %10.4f\n",FR2D(nM->m10,nM->radix),FR2D(nM->m11,nM->radix),FR2D(nM->m12,nM->radix)); + printf("%10.4f %10.4f %10.4f\n", FR2D(nM->m00, nM->radix), FR2D(nM->m01, nM->radix), FR2D(nM->m02, nM->radix)); + printf("%10.4f %10.4f %10.4f\n", FR2D(nM->m10, nM->radix), FR2D(nM->m11, nM->radix), FR2D(nM->m12, nM->radix)); printf("%10.4f %10.4f %10.4f\n", 0.0, 0.0, 1.0); } //=============================================== -//Print out the matrix in both its fixed radix point and floating point equivalents -void printMatrixBoth (FR_Matrix2D_CPT *nM) +// Print out the matrix in both its fixed radix point and floating point equivalents +void printMatrixBoth(FR_Matrix2D_CPT *nM) { - printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n",(int)nM->m00,(int)nM->m01,(int)nM->m02,FR2D(nM->m00,nM->radix),FR2D(nM->m01,nM->radix),FR2D(nM->m02,nM->radix)); - printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n",(int)nM->m10,(int)nM->m11,(int)nM->m12,FR2D(nM->m10,nM->radix),FR2D(nM->m11,nM->radix),FR2D(nM->m12,nM->radix)); - printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n",0,0,1<<(nM->radix),0.0, 0.0, 1.0); + printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n", (int)nM->m00, (int)nM->m01, (int)nM->m02, FR2D(nM->m00, nM->radix), FR2D(nM->m01, nM->radix), FR2D(nM->m02, nM->radix)); + printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n", (int)nM->m10, (int)nM->m11, (int)nM->m12, FR2D(nM->m10, nM->radix), FR2D(nM->m11, nM->radix), FR2D(nM->m12, nM->radix)); + printf("%8d %8d %8d ==> %10.4f %10.4f %10.4f\n", 0, 0, 1 << (nM->radix), 0.0, 0.0, 1.0); } //=============================================== -//Print out number in integer, floating point forms -void printNumSigned (s32 nNum, u16 radix) +// Print out number in integer, floating point forms +void printNumSigned(s32 nNum, u16 radix) { - printf("[%8d,%2d] ==> %10.4f",(int)nNum,radix,FR2D(nNum,radix)); + printf("[%8d,%2d] ==> %10.4f", (int)nNum, radix, FR2D(nNum, radix)); } -void printNumUnsigned (u32 nNum, u16 radix) +void printNumUnsigned(u32 nNum, u16 radix) { - printf("[%8d,%2d] ==> %10.4f",(int)nNum,radix,FR2D(nNum,radix)); + printf("[%8d,%2d] ==> %10.4f", (int)nNum, radix, FR2D(nNum, radix)); } -void printNumSigned3 (s32 n1, u16 r1, s32 n2, u16 r2, s32 n3, u16 r3) +void printNumSigned3(s32 n1, u16 r1, s32 n2, u16 r2, s32 n3, u16 r3) { - printf("num,radix,float:[%8d,%2d,%10.4f][%8d,%2d,%10.4f][%8d,%2d,%10.4f]\n",(int)n1,(int)r1,FR2D(n1,r1),(int)n2,(int)r2,FR2D(n2,r2),(int)n3,(int)r3,FR2D(n3,r3)); + printf("num,radix,float:[%8d,%2d,%10.4f][%8d,%2d,%10.4f][%8d,%2d,%10.4f]\n", (int)n1, (int)r1, FR2D(n1, r1), (int)n2, (int)r2, FR2D(n2, r2), (int)n3, (int)r3, FR2D(n3, r3)); } typedef struct { double min_err; double min_err_val; - long min_n; + long min_n; double min_err_pct; - long min_err_pct_n; - + long min_err_pct_n; + double max_err; double max_err_val; double max_err_pct; - long max_err_pct_n; - long max_n; + long max_err_pct_n; + long max_n; double sum_total_err; double sum_total_err2; long n; } FR_TEST_ERR; void err_init(FR_TEST_ERR *e) { - e->sum_total_err=0; - e->sum_total_err2=0; - e->n=0; + e->sum_total_err = 0; + e->sum_total_err2 = 0; + e->n = 0; } void err_accum(FR_TEST_ERR *e, double val, double ref) { - double err = val-ref; - double epc = FR_ERR(val,ref); - if (0==(e->n)) + double err = val - ref; + double epc = FR_ERR(val, ref); + if (0 == (e->n)) { e->min_err = err; e->min_err_val = val; @@ -127,7 +127,7 @@ void err_accum(FR_TEST_ERR *e, double val, double ref) e->max_err_pct = epc; e->max_err_pct_n = 0; e->sum_total_err = err; - e->sum_total_err = (err)*(err); + e->sum_total_err = (err) * (err); } else { @@ -144,7 +144,7 @@ void err_accum(FR_TEST_ERR *e, double val, double ref) } if (err > e->max_err) { - e->max_err = val-ref; + e->max_err = val - ref; e->max_err_val = val; e->max_n = e->n; } @@ -154,104 +154,102 @@ void err_accum(FR_TEST_ERR *e, double val, double ref) e->max_err_pct_n = e->n; } e->sum_total_err += err; - e->sum_total_err2 += (err)*(err); + e->sum_total_err2 += (err) * (err); } - e->n+=1; + e->n += 1; } void err_print(FR_TEST_ERR *e, const char *s) { - if (e->n >0) - printf("%sn[%5ld] min_e: [%11.6f,%11.6f,%5ld] max_e[%11.5f,%11.6f,%5ld] min_pct[%11.5f,%5ld] max_pct [%11.5f,%5ld] tot_e[%13.7f] mse[%13.7f]\n",s,e->n, - e->min_err,e->min_err_val,e->min_n,e->max_err,e->max_err_val,e->max_n, - e->min_err_pct,e->min_err_pct_n,e->max_err_pct,e->max_err_pct_n, - e->sum_total_err,e->sum_total_err2/(double)(e->n)); + if (e->n > 0) + printf("%sn[%5ld] min_e: [%11.6f,%11.6f,%5ld] max_e[%11.5f,%11.6f,%5ld] min_pct[%11.5f,%5ld] max_pct [%11.5f,%5ld] tot_e[%13.7f] mse[%13.7f]\n", s, e->n, + e->min_err, e->min_err_val, e->min_n, e->max_err, e->max_err_val, e->max_n, + e->min_err_pct, e->min_err_pct_n, e->max_err_pct, e->max_err_pct_n, + e->sum_total_err, e->sum_total_err2 / (double)(e->n)); else printf("nodata\n"); } /* s16 const static gFR_COS_TAB_RAD_S0d15[]={ - 32767, 32757, 32727, 32678, 32609, 32520, 32412, 32284, - 32137, 31970, 31785, 31580, 31356, 31113, 30851, 30571, - 30272, 29955, 29621, 29268, 28897, 28510, 28105, 27683, - 27244, 26789, 26318, 25831, 25329, 24811, 24278, 23731, - 23169, 22594, 22004, 21402, 20787, 20159, 19519, 18867, - 18204, 17530, 16845, 16150, 15446, 14732, 14009, 13278, - 12539, 11792, 11038, 10278, 9511, 8739, 7961, 7179, - 6392, 5601, 4807, 4011, 3211, 2410, 1607, 804, - 0 + 32767, 32757, 32727, 32678, 32609, 32520, 32412, 32284, + 32137, 31970, 31785, 31580, 31356, 31113, 30851, 30571, + 30272, 29955, 29621, 29268, 28897, 28510, 28105, 27683, + 27244, 26789, 26318, 25831, 25329, 24811, 24278, 23731, + 23169, 22594, 22004, 21402, 20787, 20159, 19519, 18867, + 18204, 17530, 16845, 16150, 15446, 14732, 14009, 13278, + 12539, 11792, 11038, 10278, 9511, 8739, 7961, 7179, + 6392, 5601, 4807, 4011, 3211, 2410, 1607, 804, + 0 }; #define gFR_COS_TAB_RAD_S0d15_SZ (64) #define gFR_COS_TAB_RAD_S0d15_SZPREC (6) #define gFR_COS_TAB_RAD_S0d15_SZMASK (0x3f) */ -s16 const static gFR_COS_TAB_RAD_S0d15[]={ - 0x007fff, 0x007ffc, 0x007ff5, 0x007fe8, 0x007fd7, 0x007fc1, 0x007fa6, 0x007f86, - 0x007f61, 0x007f37, 0x007f08, 0x007ed4, 0x007e9c, 0x007e5e, 0x007e1c, 0x007dd5, - 0x007d89, 0x007d38, 0x007ce2, 0x007c88, 0x007c29, 0x007bc4, 0x007b5c, 0x007aee, - 0x007a7c, 0x007a04, 0x007989, 0x007908, 0x007883, 0x0077f9, 0x00776b, 0x0076d8, - 0x007640, 0x0075a4, 0x007503, 0x00745e, 0x0073b5, 0x007306, 0x007254, 0x00719d, - 0x0070e1, 0x007022, 0x006f5e, 0x006e95, 0x006dc9, 0x006cf8, 0x006c23, 0x006b4a, - 0x006a6c, 0x00698b, 0x0068a5, 0x0067bc, 0x0066ce, 0x0065dd, 0x0064e7, 0x0063ee, - 0x0062f1, 0x0061f0, 0x0060eb, 0x005fe2, 0x005ed6, 0x005dc6, 0x005cb3, 0x005b9c, - 0x005a81, 0x005963, 0x005842, 0x00571d, 0x0055f4, 0x0054c9, 0x00539a, 0x005268, - 0x005133, 0x004ffa, 0x004ebf, 0x004d80, 0x004c3f, 0x004afa, 0x0049b3, 0x004869, - 0x00471c, 0x0045cc, 0x00447a, 0x004325, 0x0041cd, 0x004073, 0x003f16, 0x003db7, - 0x003c56, 0x003af2, 0x00398c, 0x003824, 0x0036b9, 0x00354d, 0x0033de, 0x00326d, - 0x0030fb, 0x002f86, 0x002e10, 0x002c98, 0x002b1e, 0x0029a3, 0x002826, 0x0026a7, - 0x002527, 0x0023a6, 0x002223, 0x00209f, 0x001f19, 0x001d93, 0x001c0b, 0x001a82, - 0x0018f8, 0x00176d, 0x0015e1, 0x001455, 0x0012c7, 0x001139, 0x000fab, 0x000e1b, - 0x000c8b, 0x000afb, 0x00096a, 0x0007d9, 0x000647, 0x0004b6, 0x000324, 0x000192, - 0x000000 - }; -#define gFR_COS_TAB_RAD_S0d15_SZ (128) +s16 const static gFR_COS_TAB_RAD_S0d15[] = { + 0x007fff, 0x007ffc, 0x007ff5, 0x007fe8, 0x007fd7, 0x007fc1, 0x007fa6, 0x007f86, + 0x007f61, 0x007f37, 0x007f08, 0x007ed4, 0x007e9c, 0x007e5e, 0x007e1c, 0x007dd5, + 0x007d89, 0x007d38, 0x007ce2, 0x007c88, 0x007c29, 0x007bc4, 0x007b5c, 0x007aee, + 0x007a7c, 0x007a04, 0x007989, 0x007908, 0x007883, 0x0077f9, 0x00776b, 0x0076d8, + 0x007640, 0x0075a4, 0x007503, 0x00745e, 0x0073b5, 0x007306, 0x007254, 0x00719d, + 0x0070e1, 0x007022, 0x006f5e, 0x006e95, 0x006dc9, 0x006cf8, 0x006c23, 0x006b4a, + 0x006a6c, 0x00698b, 0x0068a5, 0x0067bc, 0x0066ce, 0x0065dd, 0x0064e7, 0x0063ee, + 0x0062f1, 0x0061f0, 0x0060eb, 0x005fe2, 0x005ed6, 0x005dc6, 0x005cb3, 0x005b9c, + 0x005a81, 0x005963, 0x005842, 0x00571d, 0x0055f4, 0x0054c9, 0x00539a, 0x005268, + 0x005133, 0x004ffa, 0x004ebf, 0x004d80, 0x004c3f, 0x004afa, 0x0049b3, 0x004869, + 0x00471c, 0x0045cc, 0x00447a, 0x004325, 0x0041cd, 0x004073, 0x003f16, 0x003db7, + 0x003c56, 0x003af2, 0x00398c, 0x003824, 0x0036b9, 0x00354d, 0x0033de, 0x00326d, + 0x0030fb, 0x002f86, 0x002e10, 0x002c98, 0x002b1e, 0x0029a3, 0x002826, 0x0026a7, + 0x002527, 0x0023a6, 0x002223, 0x00209f, 0x001f19, 0x001d93, 0x001c0b, 0x001a82, + 0x0018f8, 0x00176d, 0x0015e1, 0x001455, 0x0012c7, 0x001139, 0x000fab, 0x000e1b, + 0x000c8b, 0x000afb, 0x00096a, 0x0007d9, 0x000647, 0x0004b6, 0x000324, 0x000192, + 0x000000}; +#define gFR_COS_TAB_RAD_S0d15_SZ (128) #define gFR_COS_TAB_RAD_s0d15_SZPREC (7) #define gFR_COS_TAB_RAD_S0d15_SZMASK (0x7f) #define gFR_COS_INTRP_PREC (5) -#define gFR_COS_INTRP_MASK ((1< -s16 FR_cos(s32 rad, int prec) +s16 FR_cos(s32 rad, int prec) { - int addr, frac,q,x,y,z,addrc; - //printf("rad:%6d ",rad); - rad= ((FR_kRAD2Q>>1)*rad)>>(FR_kPREC-1); - q = (rad>=0)? (rad>>prec)&0x1 : 2+((rad>>prec)&0x1); - //printf("rad:%6d ",rad); - //printf("q:%2d ",q); - addr = FR_CHRDX(rad,prec,gFR_COS_TAB_RAD_s0d15_SZPREC)&gFR_COS_TAB_RAD_S0d15_SZMASK; + int addr, frac, q, x, y, z, addrc; + // printf("rad:%6d ",rad); + rad = ((FR_kRAD2Q >> 1) * rad) >> (FR_kPREC - 1); + q = (rad >= 0) ? (rad >> prec) & 0x1 : 2 + ((rad >> prec) & 0x1); + // printf("rad:%6d ",rad); + // printf("q:%2d ",q); + addr = FR_CHRDX(rad, prec, gFR_COS_TAB_RAD_s0d15_SZPREC) & gFR_COS_TAB_RAD_S0d15_SZMASK; addrc = gFR_COS_TAB_RAD_S0d15_SZ - addr; - //printf("addr:%6d ",addr); - frac = FR_CHRDX(rad,prec,gFR_COS_TAB_RAD_s0d15_SZPREC+gFR_COS_INTRP_PREC)&(gFR_COS_INTRP_MASK); - //printf("frac:%6d\n",rad); - + // printf("addr:%6d ",addr); + frac = FR_CHRDX(rad, prec, gFR_COS_TAB_RAD_s0d15_SZPREC + gFR_COS_INTRP_PREC) & (gFR_COS_INTRP_MASK); + // printf("frac:%6d\n",rad); + /* if (q < 2) - return (0==q) ? gFR_COS_TAB_RAD_S0d15[addr ]: -gFR_COS_TAB_RAD_S0d15[addrc]; + return (0==q) ? gFR_COS_TAB_RAD_S0d15[addr ]: -gFR_COS_TAB_RAD_S0d15[addrc]; else return (2==q) ? -gFR_COS_TAB_RAD_S0d15[addr ]: gFR_COS_TAB_RAD_S0d15[addrc]; */ if (q < 2) { - x = (0==q) ? gFR_COS_TAB_RAD_S0d15[addr ]: -gFR_COS_TAB_RAD_S0d15[addrc ]; - y = (0==q) ? gFR_COS_TAB_RAD_S0d15[addr+1]: -gFR_COS_TAB_RAD_S0d15[addrc-1]; + x = (0 == q) ? gFR_COS_TAB_RAD_S0d15[addr] : -gFR_COS_TAB_RAD_S0d15[addrc]; + y = (0 == q) ? gFR_COS_TAB_RAD_S0d15[addr + 1] : -gFR_COS_TAB_RAD_S0d15[addrc - 1]; } else { - x = (2==q) ? -gFR_COS_TAB_RAD_S0d15[addr ]: gFR_COS_TAB_RAD_S0d15[addrc ]; - y = (2==q) ? -gFR_COS_TAB_RAD_S0d15[addr+1]: gFR_COS_TAB_RAD_S0d15[addrc-1]; + x = (2 == q) ? -gFR_COS_TAB_RAD_S0d15[addr] : gFR_COS_TAB_RAD_S0d15[addrc]; + y = (2 == q) ? -gFR_COS_TAB_RAD_S0d15[addr + 1] : gFR_COS_TAB_RAD_S0d15[addrc - 1]; } - z= FR_INTERP(x,y,frac,gFR_COS_INTRP_PREC); - //printf("%5d %5d %5d %5d\n",x,y,frac,z); + z = FR_INTERP(x, y, frac, gFR_COS_INTRP_PREC); + // printf("%5d %5d %5d %5d\n",x,y,frac,z); return z; - } -int putSingleChar(char x) +int putSingleChar(char x) { - return printf("%c",(char) x); + return printf("%c", (char)x); } //=============================================== -//main program for testing the functions -int main (int argc, char *argv[]) +// main program for testing the functions +int main(int argc, char *argv[]) { int ret_val = FR_S_OK; int i; @@ -265,339 +263,348 @@ int main (int argc, char *argv[]) printf("These routines were developed for use on ink recognizers, and embedded projects\n"); printf("This sample program uses the C std floating point library to show floating / fixed point operations\n"); printf("On embedded systems floating point won't be available so the radix scaling factors must be used.\n"); - - //Fixed radix math tests - //Simple overflow example demo, see FR_math_docs.txt for discussion, this is not part of the library - char ca=34,cb=3,cc; + + // Fixed radix math tests + // Simple overflow example demo, see FR_math_docs.txt for discussion, this is not part of the library + char ca = 34, cb = 3, cc; printf("Overflow example using 8 bit numbers\n"); - cc=ca*cb; - printf("%d * %d = %d\n",(int)ca,(int)cb,(int)cc); - cb=5; - cc=ca*cb; - printf("%d * %d = %d (!!)\n\n",(int)ca,(int)cb,(int)cc); - - //Fixed radix integer math test + cc = ca * cb; + printf("%d * %d = %d\n", (int)ca, (int)cb, (int)cc); + cb = 5; + cc = ca * cb; + printf("%d * %d = %d (!!)\n\n", (int)ca, (int)cb, (int)cc); + + // Fixed radix integer math test s32 a, b, c, s; - printf("Fixed Radix Integer Routines\n"); - printf("\n"); - a=1234; - printf("Some different interpretations for %d, a signed number, based on different base-2 radixes\n",(int)a); - for(i=0; i<15; i++) - { - printf("radix:%2d ==>",i); - printNumSigned(a,i); - printf(" precision = (1/%5d) or %f",(int)1<", i); + printNumSigned(a, i); + printf(" precision = (1/%5d) or %f", (int)1 << i, FR2D(1, i)); + printf("\n"); + } + printf("\nExamples: Adding two numbers together (signed)\n"); + for (i = 0; i < 5; i++) + { + u16 r = 5; + a = i * 55; + b = 654321; + c = a + b; + s = FR_FixAddSat(a, b); + printNumSigned3(a, r, b, r, c, r); + printf("saturated : "); + printNumSigned(s, r); + printf("\n"); + double af, bf, cf; + af = FR2D(a, r); + bf = FR2D(b, r); + cf = af + bf; + + printf("using doubles: af:%10.4f bf:%10.4f cf:%10.4f \n", af, bf, cf); + printf(" fixed to double result delta(c_FR - cf)=%10.4f\n", FR2D(c, r) - cf); + printf("sat fixed to double result delta(s_FR - cf)=%10.4f\n", FR2D(s, r) - cf); + } + + printf("\nExamples: Multiplying two numbers together (signed)\n"); + printf("Watch where overflow starts errors. Before this fixed radix multiplies work quite well!\n"); + printf("Also look at how saturation prevents wrap-around at the expense of clipping error\n"); + for (i = 1; i < 3; i++) + { + u16 r = 7; + a = i * 313; + b = 654321; + c = a * b; // remember the prec goes radix_a + radix_b + printNumSigned3(a, r, b, r, c, 2 * r); // see notes in FR_math_docs for more on multiplicative precision + s = FR_FixMulSat(a, b); + printf("saturated : "); + printNumSigned(s, r); + printf("\n"); + + double af, bf, cf; + af = FR2D(a, r); + bf = FR2D(b, r); + cf = af * bf; + printf("using doubles: af:%10.4f bf:%10.4f cf:%10.4f \n", af, bf, cf); + printf(" fixed to double result delta(c_FR - cf)=%10.4f\n", FR2D(c, 2 * r) - cf); + printf("sat fixed to double result delta(s_FR - cf)=%10.4f\n", FR2D(s, 2 * r) - cf); + } + + // begin 2D Coordinate point matrix tests + FR_Matrix2D_CPT mA, mB; + s16 x16, y16; + s32 x32, y32; + printf("\nFixed Radix 2D coordinate matrix routines\n"); printf("Create simple transformation & rotation matrix we will call mA and its inverse mB\n"); - x32 = 100; y32 = 200; - mA.XlateI(15,24); - mB=mA; // set B to A (note this uses C++ operator overloading - mB.inv(); // Make B the inverse of A - - printf("Forward Transform Matrix (fixed-%dbit-radix and floating point equivalent\n",mA.radix); - printMatrixBoth(&mA); - printf("\n"); - printf("Reverse Transform Matrix (fixed-%dbit-radix and floating point equivalent\n",mB.radix); - printMatrixBoth(&mB); - - printf("32bit:(%6d,%6d)\n",(int)x32,(int)y32); + x32 = 100; + y32 = 200; + mA.XlateI(15, 24); + mB = mA; // set B to A (note this uses C++ operator overloading + mB.inv(); // Make B the inverse of A + + printf("Forward Transform Matrix (fixed-%dbit-radix and floating point equivalent\n", mA.radix); + printMatrixBoth(&mA); + printf("\n"); + printf("Reverse Transform Matrix (fixed-%dbit-radix and floating point equivalent\n", mB.radix); + printMatrixBoth(&mB); + + printf("32bit:(%6d,%6d)\n", (int)x32, (int)y32); printf("Forward tranform 32 bit (x32,y32)*[mA]\n"); - mA.XFormPtI(x32,y32,&x32,&y32); - - printf("32bit:(%6d,%6d)\n",(int)x32,(int)y32); + mA.XFormPtI(x32, y32, &x32, &y32); + + printf("32bit:(%6d,%6d)\n", (int)x32, (int)y32); printf("Reverse tranform 32 bit (x32,y32)*[mB]\n"); - mB.XFormPtI(x32,y32,&x32,&y32); - printf("32bit:(%6d,%6d)\n",(int)x32,(int)y32); + mB.XFormPtI(x32, y32, &x32, &y32); + printf("32bit:(%6d,%6d)\n", (int)x32, (int)y32); printf("\n"); - x16 = 110; y16 = 210; - printf("16bit:(%6d,%6d)\n",(int)x16,(int)y16); + x16 = 110; + y16 = 210; + printf("16bit:(%6d,%6d)\n", (int)x16, (int)y16); printf("Forward transform 16 bit (x16,y16)*[mA]\n"); - mA.XFormPtI16(x16,y16,&x16,&y16); - - printf("16bit:(%6d,%6d)\n",(int)x16,(int)y16); + mA.XFormPtI16(x16, y16, &x16, &y16); + + printf("16bit:(%6d,%6d)\n", (int)x16, (int)y16); printf("Reverse transform 16 bit (x16,y16)*[mB]\n"); - mB.XFormPtI16(x16,y16,&x16,&y16); - printf("16bit:(%6d,%6d)\n",(int)x16,(int)y16); - + mB.XFormPtI16(x16, y16, &x16, &y16); + printf("16bit:(%6d,%6d)\n", (int)x16, (int)y16); + printf("\nNow that we have translated back and forth a few points we'll do a skew/rotate\n"); printf("to save space I'll only show this with 32 bit points but 16 bit points work too.\n"); - - //add some scale/skew rotation by hand + + // add some scale/skew rotation by hand mA.radix = 6; // we'll use 6 bits of fractional precision or 1/32 per unit - printf("To show the precisions effects of a lower radix we'll set the radix of matrix to %d bits\n\n",mA.radix); - - mA.ID(); // create matrix from scratch with new precision - mA.XlateI(15,24); // add some translation - mA.m01 = (3)<0.001)||(zc<(-0.001))) - zt=tan(((double)i)*(R2D)); + double fc, zc = cos(((double)i) * (R2D)); + double fs, zs = sin(((double)i) * (R2D)); + double ft, zt; + if ((zc > 0.001) || (zc < (-0.001))) + zt = tan(((double)i) * (R2D)); else - zt = zs>0.0? 32767:-32767; - - fc = FR2D(FR_CosI(i),15); - fs = FR2D(FR_SinI(i),15); - ft = FR2D(FR_TanI(i),15); - err_accum(&err_c,fc,zc);err_accum(&err_s,fs,zs);err_accum(&err_t,ft,zt); + zt = zs > 0.0 ? 32767 : -32767; + + fc = FR2D(FR_CosI(i), 15); + fs = FR2D(FR_SinI(i), 15); + ft = FR2D(FR_TanI(i), 15); + err_accum(&err_c, fc, zc); + err_accum(&err_s, fs, zs); + err_accum(&err_t, ft, zt); printf("%4ld : %4d : %9.5f %9.5f %9.5f : %9.5f %9.5f %9.5f : %9.5f %9.5f %9.5f\n", - index,i,fc,zc,FR_ERR(fc,zc),fs,zs,FR_ERR(fs,zs),ft,zt,FR_ERR(ft,zt)); + index, i, fc, zc, FR_ERR(fc, zc), fs, zs, FR_ERR(fs, zs), ft, zt, FR_ERR(ft, zt)); index++; } - err_print(&err_c,"cos:"); - err_print(&err_s,"sin:"); - err_print(&err_t,"tan:"); + err_print(&err_c, "cos:"); + err_print(&err_s, "sin:"); + err_print(&err_t, "tan:"); } - //Radian based trig testing + // Radian based trig testing if (gTestRadians) { printf("begin radian based trig\n"); - index=0; - err_init(&err_c); err_init(&err_s); err_init(&err_t); + index = 0; + err_init(&err_c); + err_init(&err_s); + err_init(&err_t); { - double lcl2pi = 2.0*3.14159265358; - double id;; - for (id = -lcl2pi; id<= lcl2pi; id+= 0.01) + double lcl2pi = 2.0 * 3.14159265358; + double id; + ; + for (id = -lcl2pi; id <= lcl2pi; id += 0.01) { - double fc,zc=cos(id); - double fs=0,zs=sin(id); - double ft=0,zt; - - if ((zc>0.001)||(zc<(-0.001))) - zt=tan(id); + double fc, zc = cos(id); + double fs = 0, zs = sin(id); + double ft = 0, zt; + + if ((zc > 0.001) || (zc < (-0.001))) + zt = tan(id); else - zt = zs>0.0? 32767:-32767; - s32 v = (s32)(id*32768.0); - fc = FR2D(FR_cos(v,15),15); - //fs = FR2D(FR_SinI(i),15); - //ft = FR2D(FR_TanI(i),15); - err_accum(&err_c,fc,zc);//err_accum(&err_s,fs,zs);err_accum(&err_t,ft,zt); + zt = zs > 0.0 ? 32767 : -32767; + s32 v = (s32)(id * 32768.0); + fc = FR2D(FR_cos(v, 15), 15); + // fs = FR2D(FR_SinI(i),15); + // ft = FR2D(FR_TanI(i),15); + err_accum(&err_c, fc, zc); // err_accum(&err_s,fs,zs);err_accum(&err_t,ft,zt); printf("%4ld : %7.3f : %9.5f %9.5f %9.5f : %9.5f %9.5f %9.5f : %9.5f %9.5f %9.5f\n", - index,id*360.0/lcl2pi,fc,zc,FR_ERR(fc,zc),fs,zs,FR_ERR(fs,zs),ft,zt,FR_ERR(ft,zt)); + index, id * 360.0 / lcl2pi, fc, zc, FR_ERR(fc, zc), fs, zs, FR_ERR(fs, zs), ft, zt, FR_ERR(ft, zt)); index++; - } - err_print(&err_c,"cos:"); - err_print(&err_s,"sin:"); - err_print(&err_t,"tan:"); + err_print(&err_c, "cos:"); + err_print(&err_s, "sin:"); + err_print(&err_t, "tan:"); } } if (gTestRadianMACROS) { - //rad to deg, deg to radian macro test - for (i=-233; i<=230; i+=7) + // rad to deg, deg to radian macro test + for (i = -233; i <= 230; i += 7) { - double deg,rad,df,rf; - s32 ir,r = 16; - - ir = I2FR(i,r); - rf = (double)i*( 0.017453292519943295769236); - df = (double)i*( 57.29577951308232087679815); - deg = FR2D(FR_DEG2RAD(ir),r); - rad = FR2D(FR_RAD2DEG(ir),r); - printf("%4d %12.5f %12.5f %12.5f %12.5f %12.5f\n",i,(double)ir,df,deg,rf,rad); + double deg, rad, df, rf; + s32 ir, r = 16; + + ir = I2FR(i, r); + rf = (double)i * (0.017453292519943295769236); + df = (double)i * (57.29577951308232087679815); + deg = FR2D(FR_DEG2RAD(ir), r); + rad = FR2D(FR_RAD2DEG(ir), r); + printf("%4d %12.5f %12.5f %12.5f %12.5f %12.5f\n", i, (double)ir, df, deg, rf, rad); } } if (gTestInvTrig) { - for (i = -32768; i<= 32768; i += 1<<6) + for (i = -32768; i <= 32768; i += 1 << 6) { - double fi = (double)i/(32768.0); - double fac = acos(fi)*57.29577951308232087679815; - double fr_ac =FR2D(FR_acos(i,FR_TRIG_PREC),0); - printf("%6d : %8.4f %8.4f %8.4f %8.4f \n",i,fi,fac,fr_ac,FR_ERR(fac,fr_ac)); - + double fi = (double)i / (32768.0); + double fac = acos(fi) * 57.29577951308232087679815; + double fr_ac = FR2D(FR_acos(i, FR_TRIG_PREC), 0); + printf("%6d : %8.4f %8.4f %8.4f %8.4f \n", i, fi, fac, fr_ac, FR_ERR(fac, fr_ac)); } } { - //quandrant = FR_DEG2Q(FR_CHRDX(x,xr,0))&3; - //quandrant_pos = (FR_DEG2Q(FR_CHRDX(x,xr,0))&3)+4)&3); + // quandrant = FR_DEG2Q(FR_CHRDX(x,xr,0))&3; + // quandrant_pos = (FR_DEG2Q(FR_CHRDX(x,xr,0))&3)+4)&3); } if (gTestPow2andLog2) { printf("begin power2, log2\n"); - index=0; + index = 0; FR_TEST_ERR err_p2, err_pe, err_p10; - err_init(&err_p2); err_init(&err_pe); err_init(&err_p10); + err_init(&err_p2); + err_init(&err_pe); + err_init(&err_p10); { double id; - //test power 2 - for (id = -3.0; id<= 3.0; id+= (1.0/32.0)) + // test power 2 + for (id = -3.0; id <= 3.0; id += (1.0 / 32.0)) { - double fp2,fpe,fp10,zp2=pow(2,id); - double zpe=pow(2.71828182845,id); - double zp10=pow(10,id); - s32 v = (s32)(id*32768); - fp2 = FR2D(FR_pow2(v,15),15); - fpe = FR2D(FR_EXP(v,15),15); - fp10 = FR2D(FR_POW10(v,15),15); - err_accum(&err_p2,fp2,zp2); - err_accum(&err_pe,fpe,zpe); - err_accum(&err_p10,fp10,zp10); - printf("%4ld: %7.3f : %11.5f %11.5f %11.5f : %11.5f %11.5f %11.5f : %11.5f %11.5f %11.5f\n", index,id, - fp2,zp2,FR_ERR(fp2,zp2),fpe,zpe,FR_ERR(fpe,zpe),fp10,zp10,FR_ERR(fp10,zp10) - ); + double fp2, fpe, fp10, zp2 = pow(2, id); + double zpe = pow(2.71828182845, id); + double zp10 = pow(10, id); + s32 v = (s32)(id * 32768); + fp2 = FR2D(FR_pow2(v, 15), 15); + fpe = FR2D(FR_EXP(v, 15), 15); + fp10 = FR2D(FR_POW10(v, 15), 15); + err_accum(&err_p2, fp2, zp2); + err_accum(&err_pe, fpe, zpe); + err_accum(&err_p10, fp10, zp10); + printf("%4ld: %7.3f : %11.5f %11.5f %11.5f : %11.5f %11.5f %11.5f : %11.5f %11.5f %11.5f\n", index, id, + fp2, zp2, FR_ERR(fp2, zp2), fpe, zpe, FR_ERR(fpe, zpe), fp10, zp10, FR_ERR(fp10, zp10)); index++; } err_print(&err_p2, "pow2 :"); err_print(&err_pe, "exp :"); - err_print(&err_p10,"pow10:"); - + err_print(&err_p10, "pow10:"); + /* FR_TEST_ERR err_l2, err_le, err_l10; //test log2, ln, log10 - err_init(&err_l2); + err_init(&err_l2); for (id = 0.1; id<= 25.0; id+= 0.1) { double fl2 ,zl2=log(id)/log(2); s32 v = (s32)(id*32768.0); fl2 = FR2D(FR_log2(v,15,15),15); - + err_accum(&err_l2,fl2,zl2); printf("%4d : %7.3f : %9.5f %9.5f %9.5f\n", index,id,fl2,zl2,FR_ERR(fl2,zl2)); index++; - + } err_print(&err_p2,"pow2:"); */ @@ -607,45 +614,45 @@ int main (int argc, char *argv[]) printf("\nTes FR_printNum(..) fammily of functions showing various prec choices\n"); { int rdx = 13; - int z = (int) ((123.45678)*(1<<13)); - int zn = -z; - printf("z (int) %8d, zn (int) %8d\n",z,zn); - printf("z %9.3f zn %9.3f\n",FR2D(z,rdx),FR2D(zn,rdx)); - - - printf("z using printNumF( , z,%3d,4,3) : ",rdx); - FR_printNumF(putSingleChar, z,rdx,4,3); + int z = (int)((123.45678) * (1 << 13)); + int zn = -z; + printf("z (int) %8d, zn (int) %8d\n", z, zn); + printf("z %9.3f zn %9.3f\n", FR2D(z, rdx), FR2D(zn, rdx)); + + printf("z using printNumF( , z,%3d,4,3) : ", rdx); + FR_printNumF(putSingleChar, z, rdx, 4, 3); printf("\n"); - printf("zn using printNumF( ,zn,%3d,4,3) : ",rdx); - FR_printNumF(putSingleChar,zn,rdx,4,3); + printf("zn using printNumF( ,zn,%3d,4,3) : ", rdx); + FR_printNumF(putSingleChar, zn, rdx, 4, 3); printf("\n"); - printf("z using printNumF( , z,%3d,5,2) :",rdx); - FR_printNumF(putSingleChar, z,rdx,5,2); + printf("z using printNumF( , z,%3d,5,2) :", rdx); + FR_printNumF(putSingleChar, z, rdx, 5, 2); printf("\n"); - printf("zn using printNumF( ,zn,%3d,5,2) :",rdx); - FR_printNumF(putSingleChar,zn,rdx,5,2); + printf("zn using printNumF( ,zn,%3d,5,2) :", rdx); + FR_printNumF(putSingleChar, zn, rdx, 5, 2); printf("\n"); - - printf("z using printNumF( , z,%3d,5,5) :",rdx); - FR_printNumF(putSingleChar, z,rdx,5,5); + printf("z using printNumF( , z,%3d,5,5) :", rdx); + FR_printNumF(putSingleChar, z, rdx, 5, 5); printf("\n"); - printf("zn using printNumF( ,zn,%3d,5,5) :",rdx); - FR_printNumF(putSingleChar,zn,rdx,5,5); + printf("zn using printNumF( ,zn,%3d,5,5) :", rdx); + FR_printNumF(putSingleChar, zn, rdx, 5, 5); printf("\n"); printf(" check floor and ceil macros\n"); - printf("ceil: z, zn (%10.5f,%10.5f) FR:",FR2D(FR_CEIL(z,rdx),rdx), FR2D(FR_CEIL(zn,rdx),rdx)); - FR_printNumF(putSingleChar,FR_CEIL(z,rdx),rdx,5,5); printf(" , "); - FR_printNumF(putSingleChar,FR_CEIL(zn,rdx),rdx,5,5); printf("\n\n"); - - printf("floor: z, zn (%10.5f,%10.5f) FR:",FR2D(FR_FLOOR(z,rdx),rdx), FR2D(FR_FLOOR(zn,rdx),rdx)); - FR_printNumF(putSingleChar,FR_FLOOR(z,rdx),rdx,5,5); printf(" , "); - FR_printNumF(putSingleChar,FR_FLOOR(zn,rdx),rdx,5,5); printf("\n\n"); + printf("ceil: z, zn (%10.5f,%10.5f) FR:", FR2D(FR_CEIL(z, rdx), rdx), FR2D(FR_CEIL(zn, rdx), rdx)); + FR_printNumF(putSingleChar, FR_CEIL(z, rdx), rdx, 5, 5); + printf(" , "); + FR_printNumF(putSingleChar, FR_CEIL(zn, rdx), rdx, 5, 5); + printf("\n\n"); + printf("floor: z, zn (%10.5f,%10.5f) FR:", FR2D(FR_FLOOR(z, rdx), rdx), FR2D(FR_FLOOR(zn, rdx), rdx)); + FR_printNumF(putSingleChar, FR_FLOOR(z, rdx), rdx, 5, 5); + printf(" , "); + FR_printNumF(putSingleChar, FR_FLOOR(zn, rdx), rdx, 5, 5); + printf("\n\n"); } - + return ret_val; } - diff --git a/FR_defs.h b/FR_defs.h index 9ee5fe6..fa75620 100644 --- a/FR_defs.h +++ b/FR_defs.h @@ -1,6 +1,6 @@ /** * @file FR_Defs.h - type definitions used in Fixed-Radix math lib - * + * * @copy Copyright (C) <2001-2012> * @author M A Chatterjee * @version 1.01 M. A. Chatterjee, cleaned up naming @@ -29,48 +29,44 @@ #ifndef __FR_Platform_Defs_H__ #define __FR_Platform_Defs_H__ -typedef unsigned char u8 ; -typedef signed char s8 ; -typedef unsigned short u16 ; -typedef signed short s16 ; -typedef unsigned long u32 ; -typedef signed long s32 ; - - -typedef short FR_bool; -typedef u16 FR_RESULT; +typedef unsigned char u8; +typedef signed char s8; +typedef unsigned short u16; +typedef signed short s16; +typedef unsigned long u32; +typedef signed long s32; -#define FR_SWAP_BYTES(x) (((x>>8)&0xff)|((x<<8)&0xff00)) +typedef short FR_bool; +typedef u16 FR_RESULT; +#define FR_SWAP_BYTES(x) (((x >> 8) & 0xff) | ((x << 8) & 0xff00)) // Return codes #ifdef WIN32 -#pragma warning( disable : 4001 4514) +#pragma warning(disable : 4001 4514) #endif -//generic error codes -#define FR_S_OK (0x0000) +// generic error codes +#define FR_S_OK (0x0000) -#define FR_E_FAIL (0x8000) -#define FR_E_BADARGUMENTS (0x8001) -#define FR_E_NULLPOINTER (0x8002) -#define FR_E_INDEXOUTOFRANGE (0x8003) -#define FR_E_BUFFERFULL (0x8004) -#define FR_E_NOTIMPLEMENTED (0x8005) -#define FR_E_MEMALLOCFAILED (0x8006) -#define FR_E_UNKNOWNOBJECT (0x8007) +#define FR_E_FAIL (0x8000) +#define FR_E_BADARGUMENTS (0x8001) +#define FR_E_NULLPOINTER (0x8002) +#define FR_E_INDEXOUTOFRANGE (0x8003) +#define FR_E_BUFFERFULL (0x8004) +#define FR_E_NOTIMPLEMENTED (0x8005) +#define FR_E_MEMALLOCFAILED (0x8006) +#define FR_E_UNKNOWNOBJECT (0x8007) -//math specific -#define FR_E_UNABLE (0x8100) +// math specific +#define FR_E_UNABLE (0x8100) -#define FR_FAILED(x) (x&0x8000) -#define FR_SUCCEEDED(x) (!(x&0x8000)) +#define FR_FAILED(x) (x & 0x8000) +#define FR_SUCCEEDED(x) (!(x & 0x8000)) /*******************************/ -#define FR_FALSE (0) -#define FR_TRUE (!FR_FALSE) - +#define FR_FALSE (0) +#define FR_TRUE (!FR_FALSE) #endif // __FR_Platform_Defs_H__ - diff --git a/FR_math.c b/FR_math.c index 0b2698d..47ceafd 100644 --- a/FR_math.c +++ b/FR_math.c @@ -1,8 +1,8 @@ /** * - * @file FR_math.c - c implementation file for basic fixed + * @file FR_math.c - c implementation file for basic fixed * radix math routines - * + * * @copy Copyright (C) <2001-2014> * @author M A Chatterjee * @version 1.01 M. A. Chatterjee, cleaned up naming @@ -18,7 +18,7 @@ * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * -* 1. The origin of this software must not be misrepresented; you must not + * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, please place an acknowledgment in the product documentation. * @@ -29,11 +29,9 @@ * distribution. * */ - #include "FR_math.h" - /*======================================================= * FR_FixMuls (x*y signed NOT saturated mul) * User must keep track of new radix point after using this function @@ -41,13 +39,13 @@ s32 FR_FixMuls(s32 x, s32 y) /* signed not saturated */ { - s32 z,sign = (x < 0)? (y>0) : (y < 0); + s32 z, sign = (x < 0) ? (y > 0) : (y < 0); x = FR_ABS(x); y = FR_ABS(y); - z = FR_FIXMUL32u(x,y); - - return sign?-z:z; + z = FR_FIXMUL32u(x, y); + + return sign ? -z : z; } /*======================================================= @@ -57,28 +55,28 @@ s32 FR_FixMuls(s32 x, s32 y) /* signed not saturated */ s32 FR_FixMulSat(s32 x, s32 y) { - s32 z,h,l,m1,m2; - int sign = ( ((x<0)&& (y>0)) || ((x>0)&&(y<0)) ) ?1:0; - + s32 z, h, l, m1, m2; + int sign = (((x < 0) && (y > 0)) || ((x > 0) && (y < 0))) ? 1 : 0; + x = FR_ABS(x); y = FR_ABS(y); - h = ((x>>16)*(y>>16)); - m1 = ((x>>16)*(y&0xffff)); - m2 = ((y>>16)*(x&0xffff)); - l = (((x&0xffff)*(y&0xffff))); - - z = (h<<16)+m1+m2+l; - - if (h&0xffff8000) /* TODO: make better saturation check -- this might be a bug */ + h = ((x >> 16) * (y >> 16)); + m1 = ((x >> 16) * (y & 0xffff)); + m2 = ((y >> 16) * (x & 0xffff)); + l = (((x & 0xffff) * (y & 0xffff))); + + z = (h << 16) + m1 + m2 + l; + + if (h & 0xffff8000) /* TODO: make better saturation check -- this might be a bug */ z = 0x7fffffff; else { - z = (h<<16) + m1; + z = (h << 16) + m1; if (z < 0) z = 0x7fffffff; else { - z+= m2; + z += m2; if (z < 0) z = 0x7fffffff; else @@ -89,7 +87,7 @@ s32 FR_FixMulSat(s32 x, s32 y) } } } - return sign?-z:z; + return sign ? -z : z; } /*======================================================= @@ -98,174 +96,184 @@ s32 FR_FixMulSat(s32 x, s32 y) */ s32 FR_FixAddSat(s32 x, s32 y) { - s32 sum = x+y; + s32 sum = x + y; if (x < 0) { if (y < 0) - return (sum >= 0)?0x80000000:sum; + return (sum >= 0) ? 0x80000000 : sum; } else { if (y >= 0) - return (sum <= 0)?0x7fffffff:sum; + return (sum <= 0) ? 0x7fffffff : sum; } return sum; } - /* Cosine table in s0.15 format, 1 entry per degree * used in all trig functions (sin,cos,tan, atan2 etc) */ -s16 const static gFR_COS_TAB_S0d15[]={ - 32767,32762,32747,32722,32687,32642,32587,32522,32448,32363, - 32269,32164,32050,31927,31793,31650,31497,31335,31163,30981, - 30790,30590,30381,30162,29934,29696,29450,29195,28931,28658, - 28377,28086,27787,27480,27165,26841,26509,26168,25820,25464, - 25100,24729,24350,23964,23570,23169,22761,22347,21925,21497, - 21062,20620,20173,19719,19259,18794,18323,17846,17363,16876, - 16383,15885,15383,14875,14364,13847,13327,12803,12274,11742, - 11207,10668,10125, 9580, 9032, 8481, 7927, 7371, 6813, 6252, - 5690, 5126, 4560, 3993, 3425, 2856, 2286, 1715, 1144, 572, - 0}; - +s16 const static gFR_COS_TAB_S0d15[] = { + 32767, 32762, 32747, 32722, 32687, 32642, 32587, 32522, 32448, 32363, + 32269, 32164, 32050, 31927, 31793, 31650, 31497, 31335, 31163, 30981, + 30790, 30590, 30381, 30162, 29934, 29696, 29450, 29195, 28931, 28658, + 28377, 28086, 27787, 27480, 27165, 26841, 26509, 26168, 25820, 25464, + 25100, 24729, 24350, 23964, 23570, 23169, 22761, 22347, 21925, 21497, + 21062, 20620, 20173, 19719, 19259, 18794, 18323, 17846, 17363, 16876, + 16383, 15885, 15383, 14875, 14364, 13847, 13327, 12803, 12274, 11742, + 11207, 10668, 10125, 9580, 9032, 8481, 7927, 7371, 6813, 6252, + 5690, 5126, 4560, 3993, 3425, 2856, 2286, 1715, 1144, 572, + 0}; + /* cosine with integer input precision in degrees, returns s0.15 result */ -s16 FR_CosI(s16 deg) +s16 FR_CosI(s16 deg) { - deg = deg%360; /* this is an expensive operation*/ - if (deg > 180) { deg -= 360; } - else if (deg < -180) { deg += 360;} - + deg = deg % 360; /* this is an expensive operation*/ + if (deg > 180) + { + deg -= 360; + } + else if (deg < -180) + { + deg += 360; + } + if (deg >= 0) - return deg <= (90) ? gFR_COS_TAB_S0d15[ deg ]:-gFR_COS_TAB_S0d15[180-deg]; + return deg <= (90) ? gFR_COS_TAB_S0d15[deg] : -gFR_COS_TAB_S0d15[180 - deg]; else - return deg >= (-90) ? gFR_COS_TAB_S0d15[-deg ]:-gFR_COS_TAB_S0d15[180+deg]; + return deg >= (-90) ? gFR_COS_TAB_S0d15[-deg] : -gFR_COS_TAB_S0d15[180 + deg]; } /* sin with integer input precision in degrees, returns s0.15 result */ s16 FR_SinI(s16 deg) { - return FR_CosI(deg-90); + return FR_CosI(deg - 90); } /* cos() with fixed radix precision, returns interpolated s0.15 result */ -s16 FR_Cos (s16 deg,u16 radix) +s16 FR_Cos(s16 deg, u16 radix) { - s16 i,j; - i = FR_CosI(deg>>radix); - j= FR_CosI((deg>>radix)+1); - return i+(((j-i)*(deg&((1<>radix); + s16 i, j; + i = FR_CosI(deg >> radix); + j = FR_CosI((deg >> radix) + 1); + return i + (((j - i) * (deg & ((1 << radix) - 1))) >> radix); } /* sin() with fixed radix precision, returns interpolated s0.15 result * could be a macro.. */ -s16 FR_Sin (s16 deg,u16 radix) +s16 FR_Sin(s16 deg, u16 radix) { - return FR_Cos(deg-(90<=0)?(FR_TRIG_MAXVAL< 180) { deg -= 360; } - else if (deg < -180) { deg += 360;} + deg = deg % 360; /* this is an expensive operation */ + if (deg > 180) + { + deg -= 360; + } + else if (deg < -180) + { + deg += 360; + } - if (90 == deg) - return (FR_TRIG_MAXVAL<= 0) - return deg <= (90) ? FR_TN( deg ):-FR_TN(180-deg); + return deg <= (90) ? FR_TN(deg) : -FR_TN(180 - deg); else - return deg >= (-90) ? -FR_TN(-deg ): FR_TN(180+deg); - -} + return deg >= (-90) ? -FR_TN(-deg) : FR_TN(180 + deg); +} /* Tan with s15.16 result with fixed radix input precision, returns interpolated s15.16 result */ - -s32 FR_Tan (s16 deg, u16 radix) + +s32 FR_Tan(s16 deg, u16 radix) { - s16 i,j; - i = FR_TanI(deg>>radix); - j= FR_TanI((deg>>radix)+1); - return FR_INTERPI(i,j,deg,radix);/*i+(((j-i)*(deg&((1<>radix);*/ -} + s16 i, j; + i = FR_TanI(deg >> radix); + j = FR_TanI((deg >> radix) + 1); + return FR_INTERPI(i, j, deg, radix); /*i+(((j-i)*(deg&((1<>radix);*/ +} /* Inverse Trig * Ugly looking acos with bin search (working): */ -s16 FR_acos (s32 input, u16 radix) +s16 FR_acos(s32 input, u16 radix) { - s16 r=45, s=input, x=46,y,z; - - input = FR_CHRDX(input,radix,FR_TRIG_PREC); /* chg radix to s0.15 */ - - // +or- 1.0000 is special case as it doesn't fit in table search - if ((input&0xffff)==0x8000) //? shouldn't it be: (input&7fff)!=0 - return (input<0) ? 180: 0; - input = (FR_ABS(input)) & ((1<>=1) - r+= (input < gFR_COS_TAB_S0d15[r]) ? x : -x; - - r+= (input < gFR_COS_TAB_S0d15[r]) ? 1 : -1; - r+= (input < gFR_COS_TAB_S0d15[r]) ? 1 : -1; - + s16 r = 45, s = input, x = 46, y, z; + + input = FR_CHRDX(input, radix, FR_TRIG_PREC); /* chg radix to s0.15 */ + + // +or- 1.0000 is special case as it doesn't fit in table search + if ((input & 0xffff) == 0x8000) //? shouldn't it be: (input&7fff)!=0 + return (input < 0) ? 180 : 0; + input = (FR_ABS(input)) & ((1 << radix) - 1); + while (x >>= 1) + r += (input < gFR_COS_TAB_S0d15[r]) ? x : -x; + + r += (input < gFR_COS_TAB_S0d15[r]) ? 1 : -1; + r += (input < gFR_COS_TAB_S0d15[r]) ? 1 : -1; + x = FR_ABS(input - gFR_COS_TAB_S0d15[r]); - y = FR_ABS(input - gFR_COS_TAB_S0d15[r+1]); - z = FR_ABS(input - gFR_COS_TAB_S0d15[r-1]); - r = (x0) ? r:180-r;; + return (s > 0) ? r : 180 - r; + ; } - -s16 FR_asin (s32 input, u16 radix) +s16 FR_asin(s32 input, u16 radix) { - return 90-FR_acos(input,radix); + return 90 - FR_acos(input, radix); } -s16 FR_atan2 (s32 y, s32 x, u16 radix) +s16 FR_atan2(s32 y, s32 x, u16 radix) { int q; - if (0==x) + if (0 == x) return y < 0 ? FR_TRIG_MINVAL : FR_TRIG_MAXVAL; - - q = (y >= 0) ? ( (x>= 0) ? 0 : 1) : ((x>=0) ? 3 : 2); + + q = (y >= 0) ? ((x >= 0) ? 0 : 1) : ((x >= 0) ? 3 : 2); return q; } /* Raise x to the xth power - */ + */ -#define FR_kPOW2COEF_PREC (4) -u16 static const gPOW2_ICOEF[17] = { - 0, 2902, 5932, 9096,12400,15850,19454,23216,27146, - 31249,35534,40009,44682,49562,54658,59979,65535}; /* 16bit prec */ +#define FR_kPOW2COEF_PREC (4) +u16 static const gPOW2_ICOEF[17] = { + 0, 2902, 5932, 9096, 12400, 15850, 19454, 23216, 27146, + 31249, 35534, 40009, 44682, 49562, 54658, 59979, 65535}; /* 16bit prec */ /* Accuracy vs space tradeoff: * can save some space by comment out above coef and using smaller table here. @@ -274,40 +282,40 @@ u16 static const gPOW2_ICOEF[17] = { u16 static const gPOW2_ICOEF[9] = {0,5932,12400,19454,27146,35534,44682,54658,65535}; // 16bit prec */ -//TODO: keep prec up... should be input# of bits + 14 (includes interp error etc..) -// pow2: 383 bytes of 32bit x86 code under gcc cygwin with tables counted (and before output radix stuff added) -//e.g. outputradix = input+14 bits of prec so -// if input > 30 output = MAXINTu32 -// if input+14 < 20 output radix = 30-input -// if input+14 < 16 ouptut radix = 16 (e.g. don't just slam the radix point way to the left we don't have that precision anyway. +// TODO: keep prec up... should be input# of bits + 14 (includes interp error etc..) +// pow2: 383 bytes of 32bit x86 code under gcc cygwin with tables counted (and before output radix stuff added) +// e.g. outputradix = input+14 bits of prec so +// if input > 30 output = MAXINTu32 +// if input+14 < 20 output radix = 30-input +// if input+14 < 16 ouptut radix = 16 (e.g. don't just slam the radix point way to the left we don't have that precision anyway. s32 FR_pow2(s32 input, u16 radix) { - s32 flr,frac,k,j,sc; - flr = FR_INT(input,radix); - frac = FR_FRACS(input,radix,FR_kPOW2COEF_PREC); + s32 flr, frac, k, j, sc; + flr = FR_INT(input, radix); + frac = FR_FRACS(input, radix, FR_kPOW2COEF_PREC); k = (s32)(gPOW2_ICOEF[frac]); - j = (s32)(gPOW2_ICOEF[frac+1]); - sc = ((j-k)>>8)+((j-k)>>10)+((j-k)>>11); /* slope correction */ - frac = ((FR_FRACS(input,radix,15))-(FR_FRACS(frac,FR_kPOW2COEF_PREC,15)))<> 8) + ((j - k) >> 10) + ((j - k) >> 11); /* slope correction */ + frac = ((FR_FRACS(input, radix, 15)) - (FR_FRACS(frac, FR_kPOW2COEF_PREC, 15))) << FR_kPOW2COEF_PREC; + if (frac <= (1 << (15 - 1))) + sc = FR_INTERP(0, sc, frac, 14); else - sc = FR_INTERP(sc,0,frac-(1<<(15-1)),14); + sc = FR_INTERP(sc, 0, frac - (1 << (15 - 1)), 14); - k = FR_INTERP(k,j,frac,15)-sc; + k = FR_INTERP(k, j, frac, 15) - sc; - if (flr >=0) /* positive powers of 2 */ + if (flr >= 0) /* positive powers of 2 */ { - k= FR_CHRDX((k<>flr),16,radix); - j= (radix-flr)>=0 ? 1<<(radix-flr):0; + k = FR_CHRDX((k >> flr), 16, radix); + j = (radix - flr) >= 0 ? 1 << (radix - flr) : 0; } - return j+k; + return j + k; } /* s32 FR_exp(s32 input, u16 radix) @@ -319,14 +327,14 @@ s32 FR_pow10(s32 input, u16 radix) { if (FR_FRAC(input,radix)) return FR_pow2(FR_SLOG2_10(input),radix); - else - { + else + { input = FR_INT(input,radix); if (input >=0) { - s32 x=10; + s32 x=10; while (input--) - x = FR_SMUL10(x); + x = FR_SMUL10(x); return x<>=1) { - if (input>(1<>=h;} // i+=h; input>>=h + s32 h = 16; //,frac = FR_FRAC(input,radix); + while (h >>= 1) + { + if (input > (1 << h)) + { + input >>= h; + } // i+=h; input>>=h } - return input <=0 ? FR_LOG2MIN : FR_CHRDX(input,radix,output_radix); + return input <= 0 ? FR_LOG2MIN : FR_CHRDX(input, radix, output_radix); } s32 FR_ln(s32 input, u16 radix, u16 output_radix) { - s32 r = FR_log2(input,radix,output_radix); + s32 r = FR_log2(input, radix, output_radix); return FR_SrLOG2E(r); /* Note: return FR_SrLOG2E(FR_log2()) would be a very ugly macro expansion! */ } s32 FR_log10(s32 input, u16 radix, u16 output_radix) { - s32 r = FR_log2(input,radix,output_radix); - return FR_SrLOG2_10(r);/* Note: return FR_SrLOG2_10(FR_log2()) would be a very ugly macro expansion! */ + s32 r = FR_log2(input, radix, output_radix); + return FR_SrLOG2_10(r); /* Note: return FR_SrLOG2_10(FR_log2()) would be a very ugly macro expansion! */ } /*************************************** FR_printNum write out fixed radix number with space padding equiavlent ot %f in printf family myNum = 12.34 // in fixed num - e.g. printf("%4.2f",myNum ) ==> " 12" - + e.g. printf("%4.2f",myNum ) ==> " 12" + - printf("test fr math rad \n"); + printf("test fr math rad \n"); - FR_printNumF (putchar, 123456 , 0, 3, 0); printf("\n"); - FR_printNumF (putchar, 123456<<13 , 13, 3, 4); printf(":\n"); - FR_printNumF (putchar, D2FR(1234.5678,13) , 13, 3, 6); printf(":\n"); - FR_printNumF (putchar, D2FR(-1234.5678,13) , 13, 3, 6); printf(":\n"); + FR_printNumF (putchar, 123456 , 0, 3, 0); printf("\n"); + FR_printNumF (putchar, 123456<<13 , 13, 3, 4); printf(":\n"); + FR_printNumF (putchar, D2FR(1234.5678,13) , 13, 3, 6); printf(":\n"); + FR_printNumF (putchar, D2FR(-1234.5678,13) , 13, 3, 6); printf(":\n"); */ -int FR_printNumF(int(*f)(char), s32 n, int radix, int pad, int prec) +int FR_printNumF(int (*f)(char), s32 n, int radix, int pad, int prec) { - int t=10,s=0,r=FR_FRAC(n,radix); - n=FR_INT(n,radix); - if (f) { - if(n < 0) { n = -n, s=1;pad--;} - while ( (n/t) > 0) { t*=10; pad--;} - while (pad-- > 0) {f(' ');} - if (s) f('-'); - while ( t>=10) { - t/=10; - f((char)((n/t)%10)+'0'); - } - if(prec || r) - { - f('.'); - s = 1<<(radix); - if (r) { - while (s) { - r*=10; - s/=10; - } - } - r>>=radix; - //r++; - t=1; - while ( (r/t) > 0) { t*=10;} - while ((t>=10)&&(prec)) { - t/=10; prec--; - f((char)((r/t)%10)+'0'); - } - while (prec-- > 0) {f('0');} - } - return 0; - } - return -1; + int t = 10, s = 0, r = FR_FRAC(n, radix); + n = FR_INT(n, radix); + if (f) + { + if (n < 0) + { + n = -n, s = 1; + pad--; + } + while ((n / t) > 0) + { + t *= 10; + pad--; + } + while (pad-- > 0) + { + f(' '); + } + if (s) + f('-'); + while (t >= 10) + { + t /= 10; + f((char)((n / t) % 10) + '0'); + } + if (prec || r) + { + f('.'); + s = 1 << (radix); + if (r) + { + while (s) + { + r *= 10; + s /= 10; + } + } + r >>= radix; + // r++; + t = 1; + while ((r / t) > 0) + { + t *= 10; + } + while ((t >= 10) && (prec)) + { + t /= 10; + prec--; + f((char)((r / t) % 10) + '0'); + } + while (prec-- > 0) + { + f('0'); + } + } + return 0; + } + return -1; } /*************************************** FR_printNumD write out a decimal integer with space padding equiavlent ot %d in printf family int num = 12 - e.g. printf("%4d",num) ==> " 12" - + e.g. printf("%4d",num) ==> " 12" + */ -int FR_printNumD( int (*f)(char), int n, int pad ) +int FR_printNumD(int (*f)(char), int n, int pad) { - int t=10,s=0; - if (f) { - if(n < 0) { n = -n, s=1;pad--;} - while ( (n/t) > 0) { t*=10; pad--;} - while (pad-- > 0) {f(' ');} - if (s) f('-'); - while ( t>=10) { - t/=10; - f((char)((n/t)%10)+'0'); - } - return FR_S_OK; - } - return FR_E_FAIL; + int t = 10, s = 0; + if (f) + { + if (n < 0) + { + n = -n, s = 1; + pad--; + } + while ((n / t) > 0) + { + t *= 10; + pad--; + } + while (pad-- > 0) + { + f(' '); + } + if (s) + f('-'); + while (t >= 10) + { + t /= 10; + f((char)((n / t) % 10) + '0'); + } + return FR_S_OK; + } + return FR_E_FAIL; } /*************************************** FR_printNumH write out a integer as hex - if (showPrefix == true ) - print "0x" + if (showPrefix == true ) + print "0x" else - //no prefix printed + //no prefix printed */ -int FR_printNumH( int (*f)(char), int n, int showPrefix ){ - int d,x = ((sizeof(int))<<1)-1; - if (f) { - if (showPrefix) { - f('0'); - f('x'); - } - - do { - d = (n >> (x<<2))&0xf; - d = (d > 9) ? (d-0xa + 'a') : (d + '0'); - f(d); - }while (x--); - return FR_S_OK; - } - return FR_E_FAIL; +int FR_printNumH(int (*f)(char), int n, int showPrefix) +{ + int d, x = ((sizeof(int)) << 1) - 1; + if (f) + { + if (showPrefix) + { + f('0'); + f('x'); + } + + do + { + d = (n >> (x << 2)) & 0xf; + d = (d > 9) ? (d - 0xa + 'a') : (d + '0'); + f(d); + } while (x--); + return FR_S_OK; + } + return FR_E_FAIL; } diff --git a/FR_math.h b/FR_math.h index 9d88c48..1fe8061 100644 --- a/FR_math.h +++ b/FR_math.h @@ -1,6 +1,6 @@ /** * @FR_math.h - header definition file for fixed radix math routines - * + * * @copy Copyright (C) <2001-2012> * @author M A Chatterjee * @version 1.02 M. A. Chatterjee, cleaned up naming @@ -10,7 +10,7 @@ * naming cleaned up in 2012, but otherwise collected from random progs I've * written in the last 15 or so years. * - * @license: + * @license: * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. @@ -44,178 +44,172 @@ extern "C" #include "FR_defs.h" #endif - -/* Quick Note on MACRO param wrapping: +/* Quick Note on MACRO param wrapping: * All macro inputs are wrapped in paranthesis in this code. * eg: #define MACRO_X_SQUARED(x) ((x)*(x)) //<<-- note internal paranthesis - * this is done because macros use true source substitution in C/C++ so a if + * this is done because macros use true source substitution in C/C++ so a if * a macro internally uses many operators of mixed precedence e.g. >> and * together - * undesired behavior can result if the parameter "passed" in the the macro is a + * undesired behavior can result if the parameter "passed" in the the macro is a * a complex contruct e.g. instead of being a value or single variable is a * something like 3+4*5 --> all of this would gets substituted in to the MACRO - * expression and parans eliminate chances for odd behavior. - * For example: + * expression and parans eliminate chances for odd behavior. + * For example: * MACRO_X_SQUARED_BAD(x) (x*x) * will expand this way: * 3+4*5*3+4*5 ==> 3+60+20 == 83 // due to precedence operations whereas * MACRO_X_SQUARED(x) ((x)*(x)) * (3+4*5)*(3+4*5) ==> (3+20)*(3+20) == (23)*(23) == 529 */ - - -/*absolute value for integer and fixed radix types*/ -#define FR_ABS(x) (((x)<0)?(-(x)):(x)) + +/*absolute value for integer and fixed radix types*/ +#define FR_ABS(x) (((x) < 0) ? (-(x)) : (x)) /*sign of x. Not as good as The Sign of Four, but I digress */ -#define FR_SGN(x) ((x)>>((((signed)sizeof(x))<<3)-1)) +#define FR_SGN(x) ((x) >> ((((signed)sizeof(x)) << 3) - 1)) -/*=============================================== - * Simple Fixed Point Math Conversions +/*=============================================== + * Simple Fixed Point Math Conversions * r is radix precision in bits, converts to/from integer w truncation */ -#define I2FR(x,r) ((x)<<(r)) -#define FR2I(x,r) ((x)>>(r)) +#define I2FR(x, r) ((x) << (r)) +#define FR2I(x, r) ((x) >> (r)) -/*=============================================== +/*=============================================== * Make a fixed radix number from integer, fractional parts * r is radix precision in bits * FR_NUM(12,34,10) == 12.34 , radix 10 === (12.34)*(1<<10) = 12636 * fractional part = f * pow2(radix) / pow10( ceil( log10 (f) ) ) * = (f << r) / pow10 ( ceil ( log10 (f))) */ -#define FR_NUM(i,f,r) ((i)<<(r)) +#define FR_NUM(i, f, r) ((i) << (r)) /* -FR_INT(x,r) convert a fixed radix variable x of radix r to an integer +FR_INT(x,r) convert a fixed radix variable x of radix r to an integer */ -#define FR_INT(x,r) (((x)<0)?-((-(x))>>(r)):((x)>>(r))) - +#define FR_INT(x, r) (((x) < 0) ? -((-(x)) >> (r)) : ((x) >> (r))) -/* Change Radix (x,current_radix, new_radix) +/* Change Radix (x,current_radix, new_radix) * change number from its current fixed radix (can be 0 or integer) to a new fixed radix - * Useful when dealing with numbers with mixed radixes. This is a MACRO so + * Useful when dealing with numbers with mixed radixes. This is a MACRO so * this code expands in place and x is modified. */ -#define FR_CHRDX(x,r_cur,r_new) (((r_cur)-(r_new))>=0?((x)>>((r_cur)-(r_new))):((x)<<((r_new)-(r_cur)))) +#define FR_CHRDX(x, r_cur, r_new) (((r_cur) - (r_new)) >= 0 ? ((x) >> ((r_cur) - (r_new))) : ((x) << ((r_new) - (r_cur)))) /* return only the fractional part of x */ -#define FR_FRAC(x,r) ((FR_ABS(x))&(((1<<(r))-1))) +#define FR_FRAC(x, r) ((FR_ABS(x)) & (((1 << (r)) - 1))) /* #define FR_FRAC(x,r) ((x)&(((1<<(r))-1))) */ -/* return the fractional part of number x with radix xr scaled to radix nr bits */ -#define FR_FRACS(x,xr,nr) (FR_CHRDX(FR_FRAC((x),(xr)),(xr),(nr))) +/* return the fractional part of number x with radix xr scaled to radix nr bits */ +#define FR_FRACS(x, xr, nr) (FR_CHRDX(FR_FRAC((x), (xr)), (xr), (nr))) -/****************************************************** - Add (sub) to fixed point numbers by converting the second number to the +/****************************************************** + Add (sub) to fixed point numbers by converting the second number to the same radix as the first. If yr < xr then possibility of overflow is increased. Note: for two vars, i, j, of prec ir, jr, it is not necessarily true that - FR_ADD(i,ir,j,jr) == FR_ADD(j,jr,i,ir) + FR_ADD(i,ir,j,jr) == FR_ADD(j,jr,i,ir) */ -#define FR_ADD(x,xr,y,yr) ((x)+=FR_CHRDX(y,yr,xr)) -#define FR_SUB(x,xr,y,yr) ((x)-=FR_CHRDX(y,yr,xr)) +#define FR_ADD(x, xr, y, yr) ((x) += FR_CHRDX(y, yr, xr)) +#define FR_SUB(x, xr, y, yr) ((x) -= FR_CHRDX(y, yr, xr)) /* Check if x is a power of 2. */ -#define FR_ISPOW2(x) (!((x)&((x)-1))) +#define FR_ISPOW2(x) (!((x) & ((x) - 1))) -/* floor and ceiling in current radix, leaving current radix intact +/* floor and ceiling in current radix, leaving current radix intact * this means the lower radix number of bits are set to 0 */ -#define FR_FLOOR(x,r) ((x)&(~((1<>(prec))) +#define FR_INTERP(x0, x1, delta, prec) ((x0) + ((((x1) - (x0)) * (delta)) >> (prec))) -/****************************************************** +/****************************************************** FR_INTERPI is the same as FR_INTERP except that insures the range is btw [0<= delta < 1] --> ((0 ... (1<= 0 */ -#define FR_INTERPI(x0,x1,delta,prec) ((x0)+((((x1)-(x0))*((delta)&((1<<(prec))-1)))>>(prec))) +#define FR_INTERPI(x0, x1, delta, prec) ((x0) + ((((x1) - (x0)) * ((delta) & ((1 << (prec)) - 1))) >> (prec))) /****************************************************** Convert to double, this is for debug only and WILL NOT compile under many embedded systems. Fixed Radix to Floating Point Double Conversion - since this is a MACRO it will not be compiled or instantiated unless it is actually called in code. + since this is a MACRO it will not be compiled or instantiated unless it is actually called in code. */ -#define FR2D(x,r) ((double)(((double)(x))/((double)(1<<(r))))) -#define D2FR(d,r) ((s32)(d*(1<>16)*(y>>16))<<16)+ \ - ((x>>16)*(y&0xffff))+ \ - ((y>>16)*(x&0xffff))+ \ - ((((x&0xffff)*(y&0xffff)))>>16) \ - ) - -/* this version had overflow -- leaving here as a note until better test coverage is generated. -#define FR_FIXMUL32u(x,y) ( \ - (((x>>16)*(y>>16))<<16)+ \ - ((x>>16)*(y&0xffff))+ \ - ((y>>16)*(x&0xffff))+ \ - ((((x&0xffff)*(y&0xffff)))) \ - ) -*/ +#define FR_FIXMUL32u(x, y) ( \ + (((x >> 16) * (y >> 16)) << 16) + \ + ((x >> 16) * (y & 0xffff)) + \ + ((y >> 16) * (x & 0xffff)) + \ + ((((x & 0xffff) * (y & 0xffff))) >> 16)) + + /* this version had overflow -- leaving here as a note until better test coverage is generated. + #define FR_FIXMUL32u(x,y) ( \ + (((x>>16)*(y>>16))<<16)+ \ + ((x>>16)*(y&0xffff))+ \ + ((y>>16)*(x&0xffff))+ \ + ((((x&0xffff)*(y&0xffff)))) \ + ) + */ #define FR_SQUARE(x) (FR_FIXMUL32u((x), (x)) -/*=============================================== - * Arithmetic operations - */ -s32 FR_FixMuls(s32 x, s32 y); // mul signed/unsigned NOT Saturated -s32 FR_FixMulSat(s32 x, s32 y); // mul signed/unsigned AND Saturated -s32 FR_FixAddSat(s32 x, s32 y); // add signed/unsigned AND Saturated - + /*=============================================== + * Arithmetic operations + */ + s32 FR_FixMuls(s32 x, s32 y); // mul signed/unsigned NOT Saturated + s32 FR_FixMulSat(s32 x, s32 y); // mul signed/unsigned AND Saturated + s32 FR_FixAddSat(s32 x, s32 y); // add signed/unsigned AND Saturated /*================================================ * Constants used in Trig tables, definitions @@ -224,10 +218,10 @@ s32 FR_FixAddSat(s32 x, s32 y); // add signed/unsigned AND Saturated FR_TRIG_MAXVAL == maximum fixed pt num returned by trig operations (e.g. Tan(90 deg)) FR_TRIG_MINVAL == minumum fixed pt num returned by trig operations (e.g. Tan(270 deg)) */ -#define FR_TRIG_PREC (15) -#define FR_TRIG_MASK ((1<<(FR_TRIG_PREC))-1) -#define FR_TRIG_MAXVAL (FR_TRIG_MASK) -#define FR_TRIG_MINVAL (-FR_TRIG_MASK) +#define FR_TRIG_PREC (15) +#define FR_TRIG_MASK ((1 << (FR_TRIG_PREC)) - 1) +#define FR_TRIG_MAXVAL (FR_TRIG_MASK) +#define FR_TRIG_MINVAL (-FR_TRIG_MASK) /* Bit Shift Scaling macros. Useful on some platforms with poor MUL performance. * Also can be useful if you need to scale numbers with @@ -245,20 +239,20 @@ s32 FR_FixAddSat(s32 x, s32 y); // add signed/unsigned AND Saturated */ /* scale by 10s */ -#define FR_SMUL10(x) (((x)<<3)+(((x)<<1))) -#define FR_SDIV10(x) (((x)>>3)-((x)>>5)+((x)>>7)-((x)>>9)+((x)>>11)) +#define FR_SMUL10(x) (((x) << 3) + (((x) << 1))) +#define FR_SDIV10(x) (((x) >> 3) - ((x) >> 5) + ((x) >> 7) - ((x) >> 9) + ((x) >> 11)) /* scale by 1/log2(e) 0.693147180560 used for converting log2() to ln() */ -#define FR_SrLOG2E(x) (((x)>>1)+((x)>>2)-((x)>>3)+((x)>>4)+((x)>>7)-((x)>>9)-((x)>>12)+((x)>>15)) +#define FR_SrLOG2E(x) (((x) >> 1) + ((x) >> 2) - ((x) >> 3) + ((x) >> 4) + ((x) >> 7) - ((x) >> 9) - ((x) >> 12) + ((x) >> 15)) /* scale by log2(e) 1.442695040889 used for converting pow2() to exp() */ -#define FR_SLOG2E(x) ((x)+((x)>>1)-((x)>>4)+((x)>>8)+((x)>>10)+((x)>>12)+((x)>>14)) +#define FR_SLOG2E(x) ((x) + ((x) >> 1) - ((x) >> 4) + ((x) >> 8) + ((x) >> 10) + ((x) >> 12) + ((x) >> 14)) /* scale by 1/log2(10) 0.30102999566 used for converting log2() to log10 */ -#define FR_SrLOG2_10(x) (((x)>>2)+((x)>>4)-((x)>>6)+((x)>>7)-((x)>>8)+((x)>>12)) +#define FR_SrLOG2_10(x) (((x) >> 2) + ((x) >> 4) - ((x) >> 6) + ((x) >> 7) - ((x) >> 8) + ((x) >> 12)) /* scale by log2(10) 3.32192809489 used for converting pow2() to pow10 */ -#define FR_SLOG2_10(x) (((x)<<1)+(x)+((x)>>2)+((x)>>4)+((x)>>7)+((x)>>10)+((x)>>11)+((x)>>13)) +#define FR_SLOG2_10(x) (((x) << 1) + (x) + ((x) >> 2) + ((x) >> 4) + ((x) >> 7) + ((x) >> 10) + ((x) >> 11) + ((x) >> 13)) /* TRIG Conversion macros * Convert degrees <--> radians <--> quadrants <--> degrees @@ -269,57 +263,54 @@ s32 FR_FixAddSat(s32 x, s32 y); // add signed/unsigned AND Saturated * quadrants = 4 per revolution * freq = 1 per revolution */ -#define FR_DEG2RAD(x) (((x)<<6)-((x)<<3)+(x)+((x)>>2)+((x>>4)-((x)>>6))-((x)>>10)) -#define FR_RAD2DEG(x) (((x)>>6)+((x)>>9)-((x)>>13)) +#define FR_DEG2RAD(x) (((x) << 6) - ((x) << 3) + (x) + ((x) >> 2) + ((x >> 4) - ((x) >> 6)) - ((x) >> 10)) +#define FR_RAD2DEG(x) (((x) >> 6) + ((x) >> 9) - ((x) >> 13)) -#define FR_RAD2Q(x) (((x)>>1)+((x)>>3)+((x)>>7)+((x)>>8)-((x)>>14)) -#define FR_Q2RAD(x) ((x)+((x)>>1)+((x)>>4)+((x)>>7)+((x)>>11)) +#define FR_RAD2Q(x) (((x) >> 1) + ((x) >> 3) + ((x) >> 7) + ((x) >> 8) - ((x) >> 14)) +#define FR_Q2RAD(x) ((x) + ((x) >> 1) + ((x) >> 4) + ((x) >> 7) + ((x) >> 11)) -#define FR_DEG2Q(x) (((x)>>6)-((x)>>8)-((x)>>11)-((x)>>13)) -#define FR_Q2DEG(x) (((x)<<6)+((x)<<4)+((x)<<3)+((x)<<1)) +#define FR_DEG2Q(x) (((x) >> 6) - ((x) >> 8) - ((x) >> 11) - ((x) >> 13)) +#define FR_Q2DEG(x) (((x) << 6) + ((x) << 4) + ((x) << 3) + ((x) << 1)) -/* sin, cos with integer input (degrees), s.15 result */ -s16 FR_CosI(s16 deg); -s16 FR_SinI(s16 deg); + /* sin, cos with integer input (degrees), s.15 result */ + s16 FR_CosI(s16 deg); + s16 FR_SinI(s16 deg); -/* tan with integer input precision in degrees, returns, s15.16 result */ -s32 FR_TanI(s16 deg); + /* tan with integer input precision in degrees, returns, s15.16 result */ + s32 FR_TanI(s16 deg); -/* Fixed radix (interpolated) input (in degrees), s.15 result */ -s16 FR_Cos(s16 deg,u16 radix); -s16 FR_Sin(s16 deg,u16 radix); + /* Fixed radix (interpolated) input (in degrees), s.15 result */ + s16 FR_Cos(s16 deg, u16 radix); + s16 FR_Sin(s16 deg, u16 radix); -/* Fixed radix tan returns fixed s15.16 result result (interpolated) */ -s32 FR_Tan(s16 deg,u16 radix); + /* Fixed radix tan returns fixed s15.16 result result (interpolated) */ + s32 FR_Tan(s16 deg, u16 radix); -/* Inverse trig (output in degrees) */ -s16 FR_acos(s32 input, u16 radix); -s16 FR_asin(s32 input, u16 radix); -s16 FR_atan(s32 input, u16 radix); + /* Inverse trig (output in degrees) */ + s16 FR_acos(s32 input, u16 radix); + s16 FR_asin(s32 input, u16 radix); + s16 FR_atan(s32 input, u16 radix); /* Logarithms */ -#define FR_LOG2MIN (-(32767<<16)) /* returned instead of "negative infinity" */ - -s32 FR_log2( s32 input, u16 radix, u16 output_radix); -s32 FR_ln( s32 input, u16 radix, u16 output_radix); -s32 FR_log10(s32 input, u16 radix, u16 output_radix); - - -/* Power */ -s32 FR_pow2( s32 input, u16 radix); -#define FR_EXP(input,radix) (FR_pow2(FR_SLOG2E(input),radix)) -#define FR_POW10(input,radix) (FR_pow2(FR_SLOG2_10(input),radix)) -/* -s32 FR_exp( s32 input, u16 radix); -s32 FR_pow10(s32 input, u16 radix); -*/ - - -/* printing family of functions */ -int FR_printNumF( int (*f)(char), s32 n, int radix, int pad, int prec); /* print fixed radix num as floating point e.g. -12.34" */ -int FR_printNumD( int (*f)(char), int n, int pad ); /* print decimal number with optional padding e.g. " 12" */ -int FR_printNumH( int (*f)(char), int n, int showPrefix ); /* print num as a hexidecimal e.g. "0x12ab" */ - +#define FR_LOG2MIN (-(32767 << 16)) /* returned instead of "negative infinity" */ + + s32 FR_log2(s32 input, u16 radix, u16 output_radix); + s32 FR_ln(s32 input, u16 radix, u16 output_radix); + s32 FR_log10(s32 input, u16 radix, u16 output_radix); + + /* Power */ + s32 FR_pow2(s32 input, u16 radix); +#define FR_EXP(input, radix) (FR_pow2(FR_SLOG2E(input), radix)) +#define FR_POW10(input, radix) (FR_pow2(FR_SLOG2_10(input), radix)) + /* + s32 FR_exp( s32 input, u16 radix); + s32 FR_pow10(s32 input, u16 radix); + */ + + /* printing family of functions */ + int FR_printNumF(int (*f)(char), s32 n, int radix, int pad, int prec); /* print fixed radix num as floating point e.g. -12.34" */ + int FR_printNumD(int (*f)(char), int n, int pad); /* print decimal number with optional padding e.g. " 12" */ + int FR_printNumH(int (*f)(char), int n, int showPrefix); /* print num as a hexidecimal e.g. "0x12ab" */ #ifdef __cplusplus diff --git a/FR_math_2D.cpp b/FR_math_2D.cpp index 25f78ed..d633530 100644 --- a/FR_math_2D.cpp +++ b/FR_math_2D.cpp @@ -1,8 +1,8 @@ /** * - * @file FR_math_2D.cpp - c++ implementation file for fixed radix math + * @file FR_math_2D.cpp - c++ implementation file for fixed radix math * - 2D simple matrices - * + * * @copy Copyright (C) <2001-2014> * @author M A Chatterjee * @version 1.02 M. A. Chatterjee, cleaned up naming @@ -29,33 +29,32 @@ * distribution. * */ - #include "FR_math_2D.h" #include "FR_math.h" -//namespace FR_MATH { +// namespace FR_MATH { //======================================================= -// Matrix Functions +// Matrix Functions void FR_Matrix2D_CPT::ID() { - m00 = I2FR(1,radix); + m00 = I2FR(1, radix); m01 = 0; m02 = 0; m10 = 0; - m11 = I2FR(1,radix); + m11 = I2FR(1, radix); m12 = 0; fast = checkfast(); } -// det() computes the determinant based on the assumption that these are -// coordinate x-form matrices and not general purpose 3x3 matrices -s32 FR_Matrix2D_CPT ::det () +// det() computes the determinant based on the assumption that these are +// coordinate x-form matrices and not general purpose 3x3 matrices +s32 FR_Matrix2D_CPT ::det() { // det = m00 * m11 - m01 * m10 - return FR_FixAddSat(FR_FixMulSat(m00,m11), -FR_FixMulSat(m01,m10))>>radix; + return FR_FixAddSat(FR_FixMulSat(m00, m11), -FR_FixMulSat(m01, m10)) >> radix; } // inv() computes inverse of a coordinate transform matrix @@ -67,7 +66,7 @@ s32 FR_Matrix2D_CPT ::det () // when once each time the user changes the zoom or rotation etc. This fn is not called // in the rendering loop -FR_RESULT FR_Matrix2D_CPT ::inv ( FR_Matrix2D_CPT *npI) +FR_RESULT FR_Matrix2D_CPT ::inv(FR_Matrix2D_CPT *npI) { s32 d = det(); if (0 == d) @@ -77,49 +76,49 @@ FR_RESULT FR_Matrix2D_CPT ::inv ( FR_Matrix2D_CPT *npI) { return FR_E_BADARGUMENTS; } - npI->m00 = ( m11 <m01 = ((-m01)<m02 = (FR_FixAddSat( FR_FixMulSat(m01,m12), -FR_FixMulSat(m02,m11) ))/d; + npI->m00 = (m11 << radix) / d; + npI->m01 = ((-m01) << radix) / d; + npI->m02 = (FR_FixAddSat(FR_FixMulSat(m01, m12), -FR_FixMulSat(m02, m11))) / d; - npI->m10 = ((-m10)<m11 = ( m00 <m12 = (FR_FixAddSat( FR_FixMulSat(m02,m10), -FR_FixMulSat(m00,m12) ))/d; + npI->m10 = ((-m10) << radix) / d; + npI->m11 = (m00 << radix) / d; + npI->m12 = (FR_FixAddSat(FR_FixMulSat(m02, m10), -FR_FixMulSat(m00, m12))) / d; npI->fast = npI->checkfast(); - npI->radix= radix; - + npI->radix = radix; + return FR_S_OK; } //=============================================================== // compute inverse of self, note some matrices not invertable -FR_RESULT FR_Matrix2D_CPT ::inv () +FR_RESULT FR_Matrix2D_CPT ::inv() { FR_RESULT res = FR_S_OK; FR_Matrix2D_CPT n; inv(&n); - *this=n; + *this = n; return res; } //================================================================ -//set up an integer rotation in degrees. -FR_RESULT FR_Matrix2D_CPT ::setrotate (s16 deg) +// set up an integer rotation in degrees. +FR_RESULT FR_Matrix2D_CPT ::setrotate(s16 deg) { - m00 = (FR_CosI(deg))>>(FR_TRIG_PREC-radix); // the 15 comes from Cos/Sin precision - m01 = -(FR_SinI(deg))>>(FR_TRIG_PREC-radix); // see those fns for more details - m10 = (FR_SinI(deg))>>(FR_TRIG_PREC-radix); - m11 = (FR_CosI(deg))>>(FR_TRIG_PREC-radix); + m00 = (FR_CosI(deg)) >> (FR_TRIG_PREC - radix); // the 15 comes from Cos/Sin precision + m01 = -(FR_SinI(deg)) >> (FR_TRIG_PREC - radix); // see those fns for more details + m10 = (FR_SinI(deg)) >> (FR_TRIG_PREC - radix); + m11 = (FR_CosI(deg)) >> (FR_TRIG_PREC - radix); checkfast(); - return FR_S_OK; -; + return FR_S_OK; + ; } //================================================================ -//set up a rotation with fixed radix input precision -FR_RESULT FR_Matrix2D_CPT ::setrotate (s16 deg, u16 deg_radix) +// set up a rotation with fixed radix input precision +FR_RESULT FR_Matrix2D_CPT ::setrotate(s16 deg, u16 deg_radix) { - m00 = (FR_Cos(deg,deg_radix))>>(FR_TRIG_PREC-radix); // the 15 comes from Cos/Sin precision - m01 = -(FR_Sin(deg,deg_radix))>>(FR_TRIG_PREC-radix); // see those fns for more details - m10 = (FR_Sin(deg,deg_radix))>>(FR_TRIG_PREC-radix); - m11 = (FR_Cos(deg,deg_radix))>>(FR_TRIG_PREC-radix); + m00 = (FR_Cos(deg, deg_radix)) >> (FR_TRIG_PREC - radix); // the 15 comes from Cos/Sin precision + m01 = -(FR_Sin(deg, deg_radix)) >> (FR_TRIG_PREC - radix); // see those fns for more details + m10 = (FR_Sin(deg, deg_radix)) >> (FR_TRIG_PREC - radix); + m11 = (FR_Cos(deg, deg_radix)) >> (FR_TRIG_PREC - radix); checkfast(); return FR_S_OK; } @@ -128,12 +127,12 @@ FR_RESULT FR_Matrix2D_CPT ::setrotate (s16 deg, u16 deg_radix) FR_RESULT FR_Matrix2D_CPT ::add(const FR_Matrix2D_CPT *pAdd) { - m00 = FR_FixAddSat (m00,pAdd->m00); - m01 = FR_FixAddSat (m01,pAdd->m01); - m02 = FR_FixAddSat (m02,pAdd->m02); - m10 = FR_FixAddSat (m10,pAdd->m10); - m11 = FR_FixAddSat (m11,pAdd->m11); - m12 = FR_FixAddSat (m12,pAdd->m12); + m00 = FR_FixAddSat(m00, pAdd->m00); + m01 = FR_FixAddSat(m01, pAdd->m01); + m02 = FR_FixAddSat(m02, pAdd->m02); + m10 = FR_FixAddSat(m10, pAdd->m10); + m11 = FR_FixAddSat(m11, pAdd->m11); + m12 = FR_FixAddSat(m12, pAdd->m12); checkfast(); return FR_S_OK; } @@ -142,12 +141,12 @@ FR_RESULT FR_Matrix2D_CPT ::add(const FR_Matrix2D_CPT *pAdd) FR_RESULT FR_Matrix2D_CPT ::sub(const FR_Matrix2D_CPT *pSub) { - m00 = FR_FixAddSat (m00,-(pSub->m00)); - m01 = FR_FixAddSat (m01,-(pSub->m01)); - m02 = FR_FixAddSat (m02,-(pSub->m02)); - m10 = FR_FixAddSat (m10,-(pSub->m10)); - m11 = FR_FixAddSat (m11,-(pSub->m11)); - m12 = FR_FixAddSat (m12,-(pSub->m12)); + m00 = FR_FixAddSat(m00, -(pSub->m00)); + m01 = FR_FixAddSat(m01, -(pSub->m01)); + m02 = FR_FixAddSat(m02, -(pSub->m02)); + m10 = FR_FixAddSat(m10, -(pSub->m10)); + m11 = FR_FixAddSat(m11, -(pSub->m11)); + m12 = FR_FixAddSat(m12, -(pSub->m12)); checkfast(); return FR_S_OK; } @@ -155,54 +154,54 @@ FR_RESULT FR_Matrix2D_CPT ::sub(const FR_Matrix2D_CPT *pSub) //======================================================= // set a matrix with the provided data -void FR_Matrix2D_CPT ::set(s32 a00, s32 a01, s32 a02, s32 a10, s32 a11, s32 a12, u16 nRadix ) +void FR_Matrix2D_CPT ::set(s32 a00, s32 a01, s32 a02, s32 a10, s32 a11, s32 a12, u16 nRadix) { m00 = a00; m01 = a01; m02 = a02; m10 = a10; m11 = a11; - m12 = a12; + m12 = a12; radix = nRadix; checkfast(); } //======================================================= // standard matrix operators -FR_Matrix2D_CPT& FR_Matrix2D_CPT ::operator += (const FR_Matrix2D_CPT &nM) +FR_Matrix2D_CPT &FR_Matrix2D_CPT ::operator+=(const FR_Matrix2D_CPT &nM) { add(&nM); return *this; } -FR_Matrix2D_CPT& FR_Matrix2D_CPT ::operator -= (const FR_Matrix2D_CPT &nM) +FR_Matrix2D_CPT &FR_Matrix2D_CPT ::operator-=(const FR_Matrix2D_CPT &nM) { sub(&nM); return *this; } -FR_Matrix2D_CPT& FR_Matrix2D_CPT ::operator *= (const s32 &X) +FR_Matrix2D_CPT &FR_Matrix2D_CPT ::operator*=(const s32 &X) { - m00 = FR_FixMulSat(m00,X); - m01 = FR_FixMulSat(m01,X); - m02 = FR_FixMulSat(m02,X); - m10 = FR_FixMulSat(m10,X); - m11 = FR_FixMulSat(m11,X); - m12 = FR_FixMulSat(m12,X); + m00 = FR_FixMulSat(m00, X); + m01 = FR_FixMulSat(m01, X); + m02 = FR_FixMulSat(m02, X); + m10 = FR_FixMulSat(m10, X); + m11 = FR_FixMulSat(m11, X); + m12 = FR_FixMulSat(m12, X); checkfast(); return *this; } -FR_Matrix2D_CPT& FR_Matrix2D_CPT ::operator = (const FR_Matrix2D_CPT &nM) +FR_Matrix2D_CPT &FR_Matrix2D_CPT ::operator=(const FR_Matrix2D_CPT &nM) { m00 = nM.m00; m01 = nM.m01; m02 = nM.m02; m10 = nM.m10; m11 = nM.m11; - m12 = nM.m12; + m12 = nM.m12; radix = nM.radix; checkfast(); return *this; } -//} //end namespace FR_MATH +//} //end namespace FR_MATH diff --git a/FR_math_2D.h b/FR_math_2D.h index 3b11970..2a3edd6 100644 --- a/FR_math_2D.h +++ b/FR_math_2D.h @@ -1,6 +1,6 @@ /** * @FR_mathroutines.h - header definition file for fixed radix 2D coordinate transforms - * + * * @copy Copyright (C) <2001-2012> * @author M A Chatterjee * @version 1.01 M. A. Chatterjee, cleaned up naming @@ -10,7 +10,7 @@ * naming cleaned up in 2012, but otherwise collected from random progs I've * written in the last 15 or so years. * - * @license: + * @license: * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. @@ -36,7 +36,7 @@ #define __FR_math_2D_h__ #ifdef __cplusplus -//namespace FR_MATH { +// namespace FR_MATH { extern "C" { #endif @@ -56,104 +56,154 @@ extern "C" // precision/radix tradeoffs //================================================ // Simple class to handle coord transforms -#define FR_MAT_DEFPREC (8) // default precision radix +#define FR_MAT_DEFPREC (8) // default precision radix + + struct FR_Matrix2D_CPT + { + // note: if modifying these variables by hand be sure to call checkfast() afterwards + s32 m00; + s32 m01; + s32 m02; + s32 m10; + s32 m11; + s32 m12; + + // don't need m20 .. m22 because always 0 0 1 in coord-matrices + // wrote out m00 to make code easier to read in fix point (see the .cpp) + + u16 radix; // internal precision radix point + int fast; // for taking advantage of scale-only matrices + + //======================== + void ID(); // create Identity matrix + + //======================= + FR_Matrix2D_CPT(u16 nRadix = FR_MAT_DEFPREC) : radix(nRadix) { ID(); }; // constructor + + // matrix operators + s32 det(); // compute determinant, result is in same precision as radix (not 2*radix) + FR_RESULT inv(FR_Matrix2D_CPT *nInv); // compute matrix inverse and put in nInv + FR_RESULT inv(); // compute matrix inverse of this + FR_RESULT add(const FR_Matrix2D_CPT *pAdd); // matrix this = this+pAdd; + FR_RESULT sub(const FR_Matrix2D_CPT *pSub); // matrix this = this-pSub; + FR_RESULT setrotate(s16 deg); // set upr left 2x2 to rot matrix + FR_RESULT setrotate(s16 deg, u16 radix); // set upr left 2x2 to rot matrix + + FR_Matrix2D_CPT &operator=(const FR_Matrix2D_CPT &nM); + FR_Matrix2D_CPT &operator+=(const FR_Matrix2D_CPT &nM); + FR_Matrix2D_CPT &operator-=(const FR_Matrix2D_CPT &nM); + + // scalar operators + FR_Matrix2D_CPT &operator*=(const s32 &X); + + bool checkfast() + { + fast = ((m01 == 0) && (m10 == 0)) ? true : false; + return fast; + }; + void set(s32 a00, s32 a01, s32 a02, s32 a10, s32 a11, s32 a12, u16 nRadix = FR_MAT_DEFPREC); + + // coordinate transform fns + void XlateI(s32 x, s32 y) + { + m02 = x << radix; + m12 = y << radix; + } + void XlateI(s32 x, s32 y, u16 nRadix) + { + m02 = x << nRadix; + m12 = y << nRadix; + } + void XlateRelativeI(s32 x, s32 y) + { + m02 += x << radix; + m12 += y << radix; + } + void XlateRelativeI(s32 x, s32 y, u16 nRadix) + { + m02 += x << nRadix; + m12 += y << nRadix; + } -struct FR_Matrix2D_CPT -{ - //note: if modifying these variables by hand be sure to call checkfast() afterwards - s32 m00; - s32 m01; - s32 m02; - s32 m10; - s32 m11; - s32 m12; - - // don't need m20 .. m22 because always 0 0 1 in coord-matrices - // wrote out m00 to make code easier to read in fix point (see the .cpp) - - u16 radix; // internal precision radix point - int fast; // for taking advantage of scale-only matrices - - //======================== - void ID (); // create Identity matrix - - //======================= - FR_Matrix2D_CPT (u16 nRadix = FR_MAT_DEFPREC ) : radix(nRadix) {ID();}; // constructor - - //matrix operators - s32 det (); // compute determinant, result is in same precision as radix (not 2*radix) - FR_RESULT inv (FR_Matrix2D_CPT* nInv); // compute matrix inverse and put in nInv - FR_RESULT inv (); // compute matrix inverse of this - FR_RESULT add (const FR_Matrix2D_CPT* pAdd); // matrix this = this+pAdd; - FR_RESULT sub (const FR_Matrix2D_CPT* pSub); // matrix this = this-pSub; - FR_RESULT setrotate (s16 deg); // set upr left 2x2 to rot matrix - FR_RESULT setrotate (s16 deg, u16 radix); // set upr left 2x2 to rot matrix - - FR_Matrix2D_CPT& operator = (const FR_Matrix2D_CPT &nM); - FR_Matrix2D_CPT& operator += (const FR_Matrix2D_CPT &nM); - FR_Matrix2D_CPT& operator -= (const FR_Matrix2D_CPT &nM); - - //scalar operators - FR_Matrix2D_CPT& operator *= (const s32 &X); - - bool checkfast() {fast = ((m01==0) && (m10 ==0)) ? true:false; return fast;}; - void set(s32 a00, s32 a01, s32 a02, s32 a10, s32 a11, s32 a12, u16 nRadix = FR_MAT_DEFPREC ); - - //coordinate transform fns - void XlateI(s32 x, s32 y) {m02=x<>r; *yp = (y*m11 + m12)>>r;} + //======================== + // XFormPtI takes Integer input and produces fixed pt output for multiple Xforms + // user is responsible for watching location of radix point. For integer results use: + // MyMatrix.XFormPtI(x,y,&xp,&yp,MyMatrix.radix); + // note that all precision etc. has been precomputed in inv() + // take a point and XForm it to coords represented by this matrix + void inline XFormPtI(s32 x, s32 y, s32 *xp, s32 *yp, u16 r) + { + if (fast) + { + *xp = (x * m00 + m02) >> r; + *yp = (y * m11 + m12) >> r; + } else - {*xp = (x*m00 + y*m01 + m02)>>r; *yp = (x*m10 + y*m11 + m12)>>r;} + { + *xp = (x * m00 + y * m01 + m02) >> r; + *yp = (x * m10 + y * m11 + m12) >> r; + } } - void inline XFormPtI (s32 x, s32 y, s32* xp, s32* yp) - { if(fast) - {*xp = (x*m00 + m02)>>radix; *yp = (y*m11 + m12)>>radix;} + void inline XFormPtI(s32 x, s32 y, s32 *xp, s32 *yp) + { + if (fast) + { + *xp = (x * m00 + m02) >> radix; + *yp = (y * m11 + m12) >> radix; + } else - {*xp = (x*m00 + y*m01 + m02)>>radix; *yp = (x*m10 + y*m11 + m12)>>radix;} + { + *xp = (x * m00 + y * m01 + m02) >> radix; + *yp = (x * m10 + y * m11 + m12) >> radix; + } } - // take a point and XForm it to coords represented by this matrix w/o translation - void inline XFormPtINoTranslate (s32 x, s32 y, s32* xp, s32* yp, u16 r) - { if(fast) - {*xp = (x*m00 )>>r; *yp = (y*m11 )>>r;} + // take a point and XForm it to coords represented by this matrix w/o translation + void inline XFormPtINoTranslate(s32 x, s32 y, s32 *xp, s32 *yp, u16 r) + { + if (fast) + { + *xp = (x * m00) >> r; + *yp = (y * m11) >> r; + } else - {*xp = (x*m00 + y*m01)>>r; *yp = (x*m10 + y*m11)>>r;} + { + *xp = (x * m00 + y * m01) >> r; + *yp = (x * m10 + y * m11) >> r; + } } - //======================== - // XFormPtI16 takes Integer input and produces Integer output for quikr needs - // take a point and XForm it to coords represented by this matrix - void inline XFormPtI16 (s16 x, s16 y, s16* xp, s16* yp) - { if(fast) - {*xp = (s16)((((s32)x)*m00 + m02)>>radix); - *yp = (s16)((((s32)y)*m11 + m12)>>radix);} + //======================== + // XFormPtI16 takes Integer input and produces Integer output for quikr needs + // take a point and XForm it to coords represented by this matrix + void inline XFormPtI16(s16 x, s16 y, s16 *xp, s16 *yp) + { + if (fast) + { + *xp = (s16)((((s32)x) * m00 + m02) >> radix); + *yp = (s16)((((s32)y) * m11 + m12) >> radix); + } else - {*xp = (s16)((((s32)x)*m00 + ((s32)y)*m01 + m02)>>radix); - *yp = (s16)((((s32)x)*m10 + ((s32)y)*m11 + m12)>>radix);} + { + *xp = (s16)((((s32)x) * m00 + ((s32)y) * m01 + m02) >> radix); + *yp = (s16)((((s32)x) * m10 + ((s32)y) * m11 + m12) >> radix); + } } - // take a point and XForm it to coords represented by this matrix (no translate) - void inline XFormPtI16NoTranslate (s16 x, s16 y, s16* xp, s16* yp) - { if(fast) - {*xp = (s16)((((s32)x)*m00 )>>radix); *yp = (s16)((((s32)y)*m11 )>>radix);} + // take a point and XForm it to coords represented by this matrix (no translate) + void inline XFormPtI16NoTranslate(s16 x, s16 y, s16 *xp, s16 *yp) + { + if (fast) + { + *xp = (s16)((((s32)x) * m00) >> radix); + *yp = (s16)((((s32)y) * m11) >> radix); + } else - {*xp = (s16)((((s32)x)*m00 + ((s32)y)*m01 )>>radix); *yp = (s16)((((s32)x)*m10 + ((s32)y)*m11 )>>radix);} + { + *xp = (s16)((((s32)x) * m00 + ((s32)y) * m01) >> radix); + *yp = (s16)((((s32)x) * m10 + ((s32)y) * m11) >> radix); + } } -}; - - - + }; #ifdef __cplusplus } // extern "C" diff --git a/fr_math_test.c b/fr_math_test.c index 0c63615..4c095cc 100644 --- a/fr_math_test.c +++ b/fr_math_test.c @@ -4,10 +4,10 @@ @copy Copyright (C) @author M A Chatterjee - + This file contains header defintions for travis-ci code testing example. - @license: + @license: Copyright (c) 2011-2018, M. A. Chatterjee All rights reserved. @@ -40,63 +40,58 @@ #define S_OK (0) #define E_FAIL (-1) - /* ************************************************ - simple test cases for library functions + simple test cases for library functions ************************************************ */ - - /* test cases for op_and() function */ -int test_FR_CosI() { - +int test_FR_CosI() +{ + if (FR_CosI(90) != 0) return E_FAIL; - // un comment me to make this fail! - //if ((f_ib_and(2,7) == (2&7)) + // if ((f_ib_and(2,7) == (2&7)) // return E_FAIL;) return S_OK; } - /* ************************************************ - this is a simple test suite. - normally you would run cppUnit or some other + this is a simple test suite. + normally you would run cppUnit or some other more general purpose test framework. */ -int run_tests() { - if (E_FAIL == test_FR_CosI()) { +int run_tests() +{ + if (E_FAIL == test_FR_CosI()) + { printf("failed test_FR_CosI()\n"); return E_FAIL; } - return S_OK; } - -/* +/* This main function only runs all the test code. - If successful it returns S_OK which is equal to the numerical value of 0. - Any other value is considered a failure. - + If successful it returns S_OK which is equal to the numerical value of 0. + Any other value is considered a failure. + */ int main() { int result; - + printf("Running Example tests .. \n"); result = run_tests(); - if (result == S_OK) - printf ("tests passed.\n"); + if (result == S_OK) + printf("tests passed.\n"); else - printf ("tests failed.\n"); - - return result; /* remember the value 0 is considered passing in a travis-ci sense */ + printf("tests failed.\n"); + return result; /* remember the value 0 is considered passing in a travis-ci sense */ } \ No newline at end of file