From fb944f6adc525cd5d5cd2e501e26f43d6a3d163c Mon Sep 17 00:00:00 2001 From: Flatmush Date: Wed, 25 Jul 2012 10:38:03 +0000 Subject: Added abs, floor, ceil, min, max, clamp. More code cleanup. --- libfixmath/fix16.c | 20 ++++---- libfixmath/fix16.h | 19 ++++++- libfixmath/fix16_exp.c | 136 +++++++++++++++++++++++-------------------------- 3 files changed, 92 insertions(+), 83 deletions(-) diff --git a/libfixmath/fix16.c b/libfixmath/fix16.c index 810efe6..0299151 100644 --- a/libfixmath/fix16.c +++ b/libfixmath/fix16.c @@ -40,7 +40,7 @@ fix16_t fix16_sadd(fix16_t a, fix16_t b) fix16_t result = fix16_add(a, b); if (result == fix16_overflow) - return (a > 0) ? fix16_max : fix16_min; + return (a > 0) ? fix16_maximum : fix16_minimum; return result; } @@ -50,7 +50,7 @@ fix16_t fix16_ssub(fix16_t a, fix16_t b) fix16_t result = fix16_sub(a, b); if (result == fix16_overflow) - return (a > 0) ? fix16_max : fix16_min; + return (a > 0) ? fix16_maximum : fix16_minimum; return result; } @@ -259,9 +259,9 @@ fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) if (result == fix16_overflow) { if ((inArg0 >= 0) == (inArg1 >= 0)) - return fix16_max; + return fix16_maximum; else - return fix16_min; + return fix16_minimum; } return result; @@ -293,7 +293,7 @@ fix16_t fix16_div(fix16_t a, fix16_t b) // computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations. if (b == 0) - return fix16_min; + return fix16_minimum; uint32_t remainder = (a >= 0) ? a : (-a); uint32_t divider = (b >= 0) ? b : (-b); @@ -349,7 +349,7 @@ fix16_t fix16_div(fix16_t a, fix16_t b) if ((a ^ b) & 0x80000000) { #ifndef FIXMATH_NO_OVERFLOW - if (result == fix16_min) + if (result == fix16_minimum) return fix16_overflow; #endif @@ -373,7 +373,7 @@ fix16_t fix16_div(fix16_t a, fix16_t b) // platforms without hardware divide. if (b == 0) - return fix16_min; + return fix16_minimum; uint32_t remainder = (a >= 0) ? a : (-a); uint32_t divider = (b >= 0) ? b : (-b); @@ -432,7 +432,7 @@ fix16_t fix16_div(fix16_t a, fix16_t b) if ((a ^ b) & 0x80000000) { #ifndef FIXMATH_NO_OVERFLOW - if (result == fix16_min) + if (result == fix16_minimum) return fix16_overflow; #endif @@ -452,9 +452,9 @@ fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) if (result == fix16_overflow) { if ((inArg0 >= 0) == (inArg1 >= 0)) - return fix16_max; + return fix16_maximum; else - return fix16_min; + return fix16_minimum; } return result; diff --git a/libfixmath/fix16.h b/libfixmath/fix16.h index f5359d3..0767627 100644 --- a/libfixmath/fix16.h +++ b/libfixmath/fix16.h @@ -31,8 +31,8 @@ static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.22 static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */ static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */ -static const fix16_t fix16_max = 0x7FFFFFFF; /*!< the maximum value of fix16_t */ -static const fix16_t fix16_min = 0x80000000; /*!< the minimum value of fix16_t */ +static const fix16_t fix16_maximum = 0x7FFFFFFF; /*!< the maximum value of fix16_t */ +static const fix16_t fix16_minimum = 0x80000000; /*!< the minimum value of fix16_t */ static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */ static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */ @@ -75,6 +75,21 @@ static inline fix16_t fix16_from_dbl(double a) return (fix16_t)temp; } + + +static inline fix16_t fix16_abs(fix16_t x) + { return (x < 0 ? -x : x); } +static inline fix16_t fix16_floor(fix16_t x) + { return (x & 0xFFFF0000UL); } +static inline fix16_t fix16_ceil(fix16_t x) + { return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? fix16_one : 0); } +static inline fix16_t fix16_min(fix16_t x, fix16_t y) + { return (x < y ? x : y); } +static inline fix16_t fix16_max(fix16_t x, fix16_t y) + { return (x > y ? x : y); } +static inline fix16_t fix16_clamp(fix16_t x, fix16_t lo, fix16_t hi) + { return fix16_min(fix16_max(x, lo), hi); } + /* Subtraction and addition with (optional) overflow detection. */ #ifdef FIXMATH_NO_OVERFLOW diff --git a/libfixmath/fix16_exp.c b/libfixmath/fix16_exp.c index 625e02b..747e77d 100644 --- a/libfixmath/fix16_exp.c +++ b/libfixmath/fix16_exp.c @@ -9,14 +9,10 @@ static fix16_t _fix16_exp_cache_value[4096] = { 0 }; fix16_t fix16_exp(fix16_t inValue) { - if(inValue == 0) - return fix16_one; - if(inValue == fix16_one) - return fix16_e; - if(inValue >= 681391) - return fix16_max; - if(inValue <= -772243) - return 0; + if(inValue == 0 ) return fix16_one; + if(inValue == fix16_one) return fix16_e; + if(inValue >= 681391 ) return fix16_maximum; + if(inValue <= -772243 ) return 0; #ifndef FIXMATH_NO_CACHE fix16_t tempIndex = (inValue ^ (inValue >> 16)); @@ -25,34 +21,32 @@ fix16_t fix16_exp(fix16_t inValue) { return _fix16_exp_cache_value[tempIndex]; #endif - /* The algorithm is based on the power series for exp(x): - * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition - * - * From term n, we get term n+1 by multiplying with x/n. - * When the sum term drops to zero, we can stop summing. - */ + /* The algorithm is based on the power series for exp(x): + * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition + * + * From term n, we get term n+1 by multiplying with x/n. + * When the sum term drops to zero, we can stop summing. + */ - // The power-series converges much faster on positive values - // and exp(-x) = 1/exp(x). - bool neg = (inValue < 0); - if (neg) - inValue = -inValue; + // The power-series converges much faster on positive values + // and exp(-x) = 1/exp(x). + bool neg = (inValue < 0); + if (neg) inValue = -inValue; - fix16_t result = inValue + fix16_one; - fix16_t term = inValue; - int i; - - for (i = 2; i < 30; i++) - { - term = fix16_mul(term, fix16_div(inValue, fix16_from_int(i))); - result += term; + fix16_t result = inValue + fix16_one; + fix16_t term = inValue; + + uint_fast8_t i; + for (i = 2; i < 30; i++) + { + term = fix16_mul(term, fix16_div(inValue, fix16_from_int(i))); + result += term; - if (term < 500 && (i > 15 || term < 20)) - break; - } + if ((term < 500) && ((i > 15) || (term < 20))) + break; + } - if (neg) - result = fix16_div(fix16_one, result); + if (neg) result = fix16_div(fix16_one, result); #ifndef FIXMATH_NO_CACHE _fix16_exp_cache_index[tempIndex] = inValue; @@ -66,45 +60,45 @@ fix16_t fix16_exp(fix16_t inValue) { fix16_t fix16_log(fix16_t inValue) { - fix16_t guess = fix16_from_int(2); - fix16_t delta; - int scaling = 0; - int count = 0; - - if (inValue <= 0) - return fix16_min; - - // Bring the value to the most accurate range (1 < x < 100) - const fix16_t e_to_fourth = 3578144; - while (inValue > fix16_from_int(100)) - { - inValue = fix16_div(inValue, e_to_fourth); - scaling += 4; - } - - while (inValue < fix16_one) - { - inValue = fix16_mul(inValue, e_to_fourth); - scaling -= 4; - } - - do - { - // Solving e(x) = y using Newton's method - // f(x) = e(x) - y - // f'(x) = e(x) - fix16_t e = fix16_exp(guess); - delta = fix16_div(inValue - e, e); - - // It's unlikely that logarithm is very large, so avoid overshooting. - if (delta > fix16_from_int(3)) - delta = fix16_from_int(3); - - guess += delta; - } while ((count++ < 10) + fix16_t guess = fix16_from_int(2); + fix16_t delta; + int scaling = 0; + int count = 0; + + if (inValue <= 0) + return fix16_minimum; + + // Bring the value to the most accurate range (1 < x < 100) + const fix16_t e_to_fourth = 3578144; + while (inValue > fix16_from_int(100)) + { + inValue = fix16_div(inValue, e_to_fourth); + scaling += 4; + } + + while (inValue < fix16_one) + { + inValue = fix16_mul(inValue, e_to_fourth); + scaling -= 4; + } + + do + { + // Solving e(x) = y using Newton's method + // f(x) = e(x) - y + // f'(x) = e(x) + fix16_t e = fix16_exp(guess); + delta = fix16_div(inValue - e, e); + + // It's unlikely that logarithm is very large, so avoid overshooting. + if (delta > fix16_from_int(3)) + delta = fix16_from_int(3); + + guess += delta; + } while ((count++ < 10) && ((delta > 1) || (delta < -1))); - - return guess + fix16_from_int(scaling); + + return guess + fix16_from_int(scaling); } @@ -199,6 +193,6 @@ fix16_t fix16_slog2(fix16_t x) fix16_t retval = fix16_log2(x); // The only overflow possible is when the input is negative. if(retval == fix16_overflow) - return fix16_min; + return fix16_minimum; return retval; } -- cgit v1.2.3