├── .github └── funding.yml ├── README.md ├── uint32.h ├── .gitignore ├── fixmath.h ├── libfixmath_conf.h ├── uint32.c ├── .gitattributes ├── fract32.c ├── examples ├── Fix16_unittest │ ├── unittests.h │ └── Fix16_unittest.ino ├── Fix8_unittest │ ├── unittests.h │ └── Fix8_unittest.ino ├── Fix8_benchmark │ └── Fix8_benchmark.ino └── Fix16_benchmark │ └── Fix16_benchmark.ino ├── LICENSE ├── fract32.h ├── fix16_sqrt.c ├── fix16_str.c ├── fix16_exp.c ├── int64.h ├── fix16_trig.c ├── fix8.c ├── fix8.h ├── fix16.h ├── fix8.hpp ├── fix16.hpp └── fix16.c /.github/funding.yml: -------------------------------------------------------------------------------- 1 | buy_me_a_coffee: yveaux 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Arduino_fixpt 2 | ============= 3 | 4 | Fixed point (16.16 & 8.8) library for Arduino to replace resource hungry float/double. 5 | Implementation based on libfixmath (https://code.google.com/p/libfixmath). 6 | See also http://en.wikipedia.org/wiki/Libfixmath. 7 | 8 | Released under MIT License. 9 | -------------------------------------------------------------------------------- /uint32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_uint32_h__ 2 | #define __libfixmath_uint32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #include 10 | 11 | /*! Performs an unsigned log-base2 on the specified unsigned integer and returns the result. 12 | */ 13 | extern uint32_t uint32_log2(uint32_t inVal); 14 | 15 | #ifdef __cplusplus 16 | } 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | 11 | # Compiled Static libraries 12 | *.lai 13 | *.la 14 | *.a 15 | *.lib 16 | 17 | # Visual Studio 18 | *.ipch 19 | *.sdf 20 | *.suo 21 | *.user 22 | *.log 23 | *.opensdf 24 | Debug 25 | Release 26 | 27 | # Executables 28 | *.out 29 | *.app 30 | 31 | # SVN storage 32 | .svn -------------------------------------------------------------------------------- /fixmath.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fixmath_h__ 2 | #define __libfixmath_fixmath_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | /*! 10 | \file fixmath.h 11 | \brief Functions to perform fast accurate fixed-point math operations. 12 | */ 13 | 14 | #include "uint32.h" 15 | #include "int64.h" 16 | #include "fract32.h" 17 | #include "fix16.h" 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /libfixmath_conf.h: -------------------------------------------------------------------------------- 1 | #ifndef _LIBFIXMATH_CONF_H 2 | #define _LIBFIXMATH_CONF_H 3 | 4 | #ifdef ARDUINO_ARCH_AVR 5 | // ARDUINO_ARCH_AVR might not be the optimum define to check on, 6 | // but nothing else is available... 7 | // AVR Architecture is 8bit & limited in memory 8 | #define FIXMATH_NO_64BIT 9 | #define FIXMATH_OPTIMIZE_8BIT 10 | #define FIXMATH_NO_CACHE 11 | //#define FIXMATH_NO_OVERFLOW 12 | //#define FIXMATH_NO_ROUNDING 13 | #endif 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uint32.c: -------------------------------------------------------------------------------- 1 | #include "uint32.h" 2 | 3 | 4 | 5 | uint32_t uint32_log2(uint32_t inVal) { 6 | if(inVal == 0) 7 | return 0; 8 | uint32_t tempOut = 0; 9 | if(inVal >= (1 << 16)) { inVal >>= 16; tempOut += 16; } 10 | if(inVal >= (1 << 8)) { inVal >>= 8; tempOut += 8; } 11 | if(inVal >= (1 << 4)) { inVal >>= 4; tempOut += 4; } 12 | if(inVal >= (1 << 2)) { inVal >>= 2; tempOut += 2; } 13 | if(inVal >= (1 << 1)) { tempOut += 1; } 14 | return tempOut; 15 | } 16 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /fract32.c: -------------------------------------------------------------------------------- 1 | #include "fract32.h" 2 | 3 | 4 | 5 | fract32_t fract32_create(uint32_t inNumerator, uint32_t inDenominator) { 6 | if(inDenominator <= inNumerator) 7 | return 0xFFFFFFFF; 8 | uint32_t tempMod = (inNumerator % inDenominator); 9 | uint32_t tempDiv = (0xFFFFFFFF / (inDenominator - 1)); 10 | return (tempMod * tempDiv); 11 | } 12 | 13 | fract32_t fract32_invert(fract32_t inFract) { 14 | return (0xFFFFFFFF - inFract); 15 | } 16 | 17 | #ifndef FIXMATH_NO_64BIT 18 | uint32_t fract32_usmul(uint32_t inVal, fract32_t inFract) { 19 | return (uint32_t)(((uint64_t)inVal * (uint64_t)inFract) >> 32); 20 | } 21 | 22 | int32_t fract32_smul(int32_t inVal, fract32_t inFract) { 23 | if(inVal < 0) 24 | return -(int32_t)fract32_usmul(-inVal, inFract); 25 | return fract32_usmul(inVal, inFract); 26 | } 27 | #endif 28 | -------------------------------------------------------------------------------- /examples/Fix16_unittest/unittests.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define FLOAT_DECS (8) 4 | 5 | #define COMMENT(x) Serial.println(F("\n----" x "----")); 6 | #define STR(x) #x 7 | #define STR2(x) STR(x) 8 | #define TEST(x) \ 9 | if (!(x)) { \ 10 | Serial.println(F("FAILED: " __FILE__ ":" STR2(__LINE__) " " #x) ); \ 11 | status = 1; \ 12 | } else { \ 13 | Serial.println(F("OK: " #x)); \ 14 | } 15 | 16 | 17 | #define LOG_OVERFLOW_NOT_DETECTED(a, b, op) \ 18 | Serial.print(double(a), FLOAT_DECS); \ 19 | Serial.print(F(" " op " ")); \ 20 | Serial.print(double(b), FLOAT_DECS); \ 21 | Serial.println(F(" overflow not detected!")); 22 | 23 | #define LOG_NEWLINE \ 24 | Serial.println(F("")); 25 | 26 | #define LOG_EXPRESSION(a, b, res, op) \ 27 | Serial.print(double(a), FLOAT_DECS); \ 28 | Serial.print(F(" " op " ")); \ 29 | Serial.print(double(b), FLOAT_DECS); \ 30 | Serial.print(F(" = ")); \ 31 | Serial.println(double(res), FLOAT_DECS); 32 | 33 | -------------------------------------------------------------------------------- /examples/Fix8_unittest/unittests.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define FLOAT_DECS (8) 4 | 5 | #define COMMENT(x) Serial.println(F("\n----" x "----")); 6 | #define STR(x) #x 7 | #define STR2(x) STR(x) 8 | #define TEST(x) \ 9 | if (!(x)) { \ 10 | Serial.println(F("FAILED: " __FILE__ ":" STR2(__LINE__) " " #x) ); \ 11 | status = 1; \ 12 | } else { \ 13 | Serial.println(F("OK: " #x)); \ 14 | } 15 | 16 | 17 | #define LOG_OVERFLOW_NOT_DETECTED(a, b, op) \ 18 | Serial.print(double(a), FLOAT_DECS); \ 19 | Serial.print(F(" " op " ")); \ 20 | Serial.print(double(b), FLOAT_DECS); \ 21 | Serial.println(F(" overflow not detected!")); 22 | 23 | #define LOG_NEWLINE \ 24 | Serial.println(F("")); 25 | 26 | #define LOG_EXPRESSION(a, b, res, op) \ 27 | Serial.print(double(a), FLOAT_DECS); \ 28 | Serial.print(F(" " op " ")); \ 29 | Serial.print(double(b), FLOAT_DECS); \ 30 | Serial.print(F(" = ")); \ 31 | Serial.println(double(res), FLOAT_DECS); 32 | 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Flatmush@googlemail.com, i.pullens@emmission.nl 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /fract32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fract32_h__ 2 | #define __libfixmath_fract32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #include 10 | 11 | typedef uint32_t fract32_t; 12 | 13 | /*! Creates a fraction using unsigned integers. 14 | \param inNumerator the unsigned integer numerator 15 | \param inDenominator the unsigned integer denominator 16 | \return a fraction using the given numerator and denominator 17 | */ 18 | extern fract32_t fract32_create(uint32_t inNumerator, uint32_t inDenominator); 19 | 20 | /*! Inverts the given fraction, swapping the numerator and the denominator. 21 | */ 22 | extern fract32_t fract32_invert(fract32_t inFract); 23 | 24 | #ifndef FIXMATH_NO_64BIT 25 | /*! Performs unsigned saturated (overflow-protected) multiplication with the two given fractions and returns the result as an unsigned integer. 26 | */ 27 | extern uint32_t fract32_usmul(uint32_t inVal, fract32_t inFract); 28 | 29 | /*! Performs saturated (overflow-protected) multiplication with the two given fractions and returns the result as a signed integer. 30 | */ 31 | extern int32_t fract32_smul(int32_t inVal, fract32_t inFract); 32 | #endif 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /fix16_sqrt.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | 3 | /* The square root algorithm is quite directly from 4 | * http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29 5 | * An important difference is that it is split to two parts 6 | * in order to use only 32-bit operations. 7 | * 8 | * Note that for negative numbers we return -sqrt(-inValue). 9 | * Not sure if someone relies on this behaviour, but not going 10 | * to break it for now. It doesn't slow the code much overall. 11 | */ 12 | fix16_t fix16_sqrt(fix16_t inValue) 13 | { 14 | uint8_t neg = (inValue < 0); 15 | uint32_t num = (neg ? -inValue : inValue); 16 | uint32_t result = 0; 17 | uint32_t bit; 18 | uint8_t n; 19 | 20 | // Many numbers will be less than 15, so 21 | // this gives a good balance between time spent 22 | // in if vs. time spent in the while loop 23 | // when searching for the starting value. 24 | if (num & 0xFFF00000) 25 | bit = (uint32_t)1 << 30; 26 | else 27 | bit = (uint32_t)1 << 18; 28 | 29 | while (bit > num) bit >>= 2; 30 | 31 | // The main part is executed twice, in order to avoid 32 | // using 64 bit values in computations. 33 | for (n = 0; n < 2; n++) 34 | { 35 | // First we get the top 24 bits of the answer. 36 | while (bit) 37 | { 38 | if (num >= result + bit) 39 | { 40 | num -= result + bit; 41 | result = (result >> 1) + bit; 42 | } 43 | else 44 | { 45 | result = (result >> 1); 46 | } 47 | bit >>= 2; 48 | } 49 | 50 | if (n == 0) 51 | { 52 | // Then process it again to get the lowest 8 bits. 53 | if (num > 65535) 54 | { 55 | // The remainder 'num' is too large to be shifted left 56 | // by 16, so we have to add 1 to result manually and 57 | // adjust 'num' accordingly. 58 | // num = a - (result + 0.5)^2 59 | // = num + result^2 - (result + 0.5)^2 60 | // = num - result - 0.5 61 | num -= result; 62 | num = (num << 16) - 0x8000; 63 | result = (result << 16) + 0x8000; 64 | } 65 | else 66 | { 67 | num <<= 16; 68 | result <<= 16; 69 | } 70 | 71 | bit = 1 << 14; 72 | } 73 | } 74 | 75 | #ifndef FIXMATH_NO_ROUNDING 76 | // Finally, if next bit would have been 1, round the result upwards. 77 | if (num > result) 78 | { 79 | result++; 80 | } 81 | #endif 82 | 83 | return (neg ? -(fix16_t)result : (fix16_t)result); 84 | } 85 | -------------------------------------------------------------------------------- /fix16_str.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #include 3 | #include 4 | 5 | static const uint32_t scales[8] = { 6 | /* 5 decimals is enough for full fix16_t precision */ 7 | 1, 10, 100, 1000, 10000, 100000, 100000, 100000 8 | }; 9 | 10 | static char *itoa_loop(char *buf, uint32_t scale, uint32_t value, bool skip) 11 | { 12 | while (scale) 13 | { 14 | unsigned digit = (value / scale); 15 | 16 | if (!skip || digit || scale == 1) 17 | { 18 | skip = false; 19 | *buf++ = '0' + digit; 20 | value %= scale; 21 | } 22 | 23 | scale /= 10; 24 | } 25 | return buf; 26 | } 27 | 28 | void fix16_to_str(fix16_t value, char *buf, int decimals) 29 | { 30 | uint32_t uvalue = (value >= 0) ? value : -value; 31 | if (value < 0) 32 | *buf++ = '-'; 33 | 34 | /* Separate the integer and decimal parts of the value */ 35 | unsigned intpart = uvalue >> 16; 36 | uint32_t fracpart = uvalue & 0xFFFF; 37 | uint32_t scale = scales[decimals & 7]; 38 | fracpart = fix16_mul(fracpart, scale); 39 | 40 | if (fracpart >= scale) 41 | { 42 | /* Handle carry from decimal part */ 43 | intpart++; 44 | fracpart -= scale; 45 | } 46 | 47 | /* Format integer part */ 48 | buf = itoa_loop(buf, 10000, intpart, true); 49 | 50 | /* Format decimal part (if any) */ 51 | if (scale != 1) 52 | { 53 | *buf++ = '.'; 54 | buf = itoa_loop(buf, scale / 10, fracpart, false); 55 | } 56 | 57 | *buf = '\0'; 58 | } 59 | 60 | fix16_t fix16_from_str(const char *buf) 61 | { 62 | while (isspace(*buf)) 63 | buf++; 64 | 65 | /* Decode the sign */ 66 | bool negative = (*buf == '-'); 67 | if (*buf == '+' || *buf == '-') 68 | buf++; 69 | 70 | /* Decode the integer part */ 71 | uint32_t intpart = 0; 72 | int count = 0; 73 | while (isdigit(*buf)) 74 | { 75 | intpart *= 10; 76 | intpart += *buf++ - '0'; 77 | count++; 78 | } 79 | 80 | if (count == 0 || count > 5 81 | || intpart > 32768 || (!negative && intpart > 32767)) 82 | return fix16_overflow; 83 | 84 | fix16_t value = intpart << 16; 85 | 86 | /* Decode the decimal part */ 87 | if (*buf == '.' || *buf == ',') 88 | { 89 | buf++; 90 | 91 | uint32_t fracpart = 0; 92 | uint32_t scale = 1; 93 | while (isdigit(*buf) && scale < 100000) 94 | { 95 | scale *= 10; 96 | fracpart *= 10; 97 | fracpart += *buf++ - '0'; 98 | } 99 | 100 | value += fix16_div(fracpart, scale); 101 | } 102 | 103 | /* Verify that there is no garbage left over */ 104 | while (*buf != '\0') 105 | { 106 | if (!isdigit(*buf) && !isspace(*buf)) 107 | return fix16_overflow; 108 | 109 | buf++; 110 | } 111 | 112 | return negative ? -value : value; 113 | } 114 | 115 | -------------------------------------------------------------------------------- /fix16_exp.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #include 3 | 4 | #ifndef FIXMATH_NO_CACHE 5 | static fix16_t _fix16_exp_cache_index[4096] = { 0 }; 6 | static fix16_t _fix16_exp_cache_value[4096] = { 0 }; 7 | #endif 8 | 9 | 10 | 11 | fix16_t fix16_exp(fix16_t inValue) { 12 | if(inValue == 0 ) return fix16_one; 13 | if(inValue == fix16_one) return fix16_e; 14 | if(inValue >= 681391 ) return fix16_maximum; 15 | if(inValue <= -772243 ) return 0; 16 | 17 | #ifndef FIXMATH_NO_CACHE 18 | fix16_t tempIndex = (inValue ^ (inValue >> 16)); 19 | tempIndex = (inValue ^ (inValue >> 4)) & 0x0FFF; 20 | if(_fix16_exp_cache_index[tempIndex] == inValue) 21 | return _fix16_exp_cache_value[tempIndex]; 22 | #endif 23 | 24 | /* The algorithm is based on the power series for exp(x): 25 | * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition 26 | * 27 | * From term n, we get term n+1 by multiplying with x/n. 28 | * When the sum term drops to zero, we can stop summing. 29 | */ 30 | 31 | // The power-series converges much faster on positive values 32 | // and exp(-x) = 1/exp(x). 33 | bool neg = (inValue < 0); 34 | if (neg) inValue = -inValue; 35 | 36 | fix16_t result = inValue + fix16_one; 37 | fix16_t term = inValue; 38 | 39 | uint_fast8_t i; 40 | for (i = 2; i < 30; i++) 41 | { 42 | term = fix16_mul(term, fix16_div(inValue, fix16_from_int(i))); 43 | result += term; 44 | 45 | if ((term < 500) && ((i > 15) || (term < 20))) 46 | break; 47 | } 48 | 49 | if (neg) result = fix16_div(fix16_one, result); 50 | 51 | #ifndef FIXMATH_NO_CACHE 52 | _fix16_exp_cache_index[tempIndex] = inValue; 53 | _fix16_exp_cache_value[tempIndex] = result; 54 | #endif 55 | 56 | return result; 57 | } 58 | 59 | 60 | 61 | fix16_t fix16_log(fix16_t inValue) 62 | { 63 | fix16_t guess = fix16_from_int(2); 64 | fix16_t delta; 65 | int scaling = 0; 66 | int count = 0; 67 | 68 | if (inValue <= 0) 69 | return fix16_minimum; 70 | 71 | // Bring the value to the most accurate range (1 < x < 100) 72 | const fix16_t e_to_fourth = 3578144; 73 | while (inValue > fix16_from_int(100)) 74 | { 75 | inValue = fix16_div(inValue, e_to_fourth); 76 | scaling += 4; 77 | } 78 | 79 | while (inValue < fix16_one) 80 | { 81 | inValue = fix16_mul(inValue, e_to_fourth); 82 | scaling -= 4; 83 | } 84 | 85 | do 86 | { 87 | // Solving e(x) = y using Newton's method 88 | // f(x) = e(x) - y 89 | // f'(x) = e(x) 90 | fix16_t e = fix16_exp(guess); 91 | delta = fix16_div(inValue - e, e); 92 | 93 | // It's unlikely that logarithm is very large, so avoid overshooting. 94 | if (delta > fix16_from_int(3)) 95 | delta = fix16_from_int(3); 96 | 97 | guess += delta; 98 | } while ((count++ < 10) 99 | && ((delta > 1) || (delta < -1))); 100 | 101 | return guess + fix16_from_int(scaling); 102 | } 103 | 104 | 105 | 106 | static inline fix16_t fix16_rs(fix16_t x) 107 | { 108 | #ifdef FIXMATH_NO_ROUNDING 109 | return (x >> 1); 110 | #else 111 | fix16_t y = (x >> 1) + (x & 1); 112 | return y; 113 | #endif 114 | } 115 | 116 | /** 117 | * This assumes that the input value is >= 1. 118 | * 119 | * Note that this is only ever called with inValue >= 1 (because it has a wrapper to check. 120 | * As such, the result is always less than the input. 121 | */ 122 | static fix16_t fix16__log2_inner(fix16_t x) 123 | { 124 | fix16_t result = 0; 125 | 126 | while(x >= fix16_from_int(2)) 127 | { 128 | result++; 129 | x = fix16_rs(x); 130 | } 131 | 132 | if(x == 0) return (result << 16); 133 | 134 | uint_fast8_t i; 135 | for(i = 16; i > 0; i--) 136 | { 137 | x = fix16_mul(x, x); 138 | result <<= 1; 139 | if(x >= fix16_from_int(2)) 140 | { 141 | result |= 1; 142 | x = fix16_rs(x); 143 | } 144 | } 145 | #ifndef FIXMATH_NO_ROUNDING 146 | x = fix16_mul(x, x); 147 | if(x >= fix16_from_int(2)) result++; 148 | #endif 149 | 150 | return result; 151 | } 152 | 153 | 154 | 155 | /** 156 | * calculates the log base 2 of input. 157 | * Note that negative inputs are invalid! (will return fix16_overflow, since there are no exceptions) 158 | * 159 | * i.e. 2 to the power output = input. 160 | * It's equivalent to the log or ln functions, except it uses base 2 instead of base 10 or base e. 161 | * This is useful as binary things like this are easy for binary devices, like modern microprocessros, to calculate. 162 | * 163 | * This can be used as a helper function to calculate powers with non-integer powers and/or bases. 164 | */ 165 | fix16_t fix16_log2(fix16_t x) 166 | { 167 | // Note that a negative x gives a non-real result. 168 | // If x == 0, the limit of log2(x) as x -> 0 = -infinity. 169 | // log2(-ve) gives a complex result. 170 | if (x <= 0) return fix16_overflow; 171 | 172 | // If the input is less than one, the result is -log2(1.0 / in) 173 | if (x < fix16_one) 174 | { 175 | // Note that the inverse of this would overflow. 176 | // This is the exact answer for log2(1.0 / 65536) 177 | if (x == 1) return fix16_from_int(-16); 178 | 179 | fix16_t inverse = fix16_div(fix16_one, x); 180 | return -fix16__log2_inner(inverse); 181 | } 182 | 183 | // If input >= 1, just proceed as normal. 184 | // Note that x == fix16_one is a special case, where the answer is 0. 185 | return fix16__log2_inner(x); 186 | } 187 | 188 | /** 189 | * This is a wrapper for fix16_log2 which implements saturation arithmetic. 190 | */ 191 | fix16_t fix16_slog2(fix16_t x) 192 | { 193 | fix16_t retval = fix16_log2(x); 194 | // The only overflow possible is when the input is negative. 195 | if(retval == fix16_overflow) 196 | return fix16_minimum; 197 | return retval; 198 | } 199 | -------------------------------------------------------------------------------- /int64.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_int64_h__ 2 | #define __libfixmath_int64_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #ifndef FIXMATH_NO_64BIT 10 | static inline int64_t int64_const(int32_t hi, uint32_t lo) { return (((int64_t)hi << 32) | lo); } 11 | static inline int64_t int64_from_int32(int32_t x) { return (int64_t)x; } 12 | static inline int32_t int64_hi(int64_t x) { return (x >> 32); } 13 | static inline uint32_t int64_lo(int64_t x) { return (x & ((1ULL << 32) - 1)); } 14 | 15 | static inline int64_t int64_add(int64_t x, int64_t y) { return (x + y); } 16 | static inline int64_t int64_neg(int64_t x) { return (-x); } 17 | static inline int64_t int64_sub(int64_t x, int64_t y) { return (x - y); } 18 | static inline int64_t int64_shift(int64_t x, int8_t y) { return (y < 0 ? (x >> -y) : (x << y)); } 19 | 20 | static inline int64_t int64_mul_i32_i32(int32_t x, int32_t y) { return (x * y); } 21 | static inline int64_t int64_mul_i64_i32(int64_t x, int32_t y) { return (x * y); } 22 | 23 | static inline int64_t int64_div_i64_i32(int64_t x, int32_t y) { return (x / y); } 24 | 25 | static inline int int64_cmp_eq(int64_t x, int64_t y) { return (x == y); } 26 | static inline int int64_cmp_ne(int64_t x, int64_t y) { return (x != y); } 27 | static inline int int64_cmp_gt(int64_t x, int64_t y) { return (x > y); } 28 | static inline int int64_cmp_ge(int64_t x, int64_t y) { return (x >= y); } 29 | static inline int int64_cmp_lt(int64_t x, int64_t y) { return (x < y); } 30 | static inline int int64_cmp_le(int64_t x, int64_t y) { return (x <= y); } 31 | #else 32 | 33 | typedef struct { 34 | int32_t hi; 35 | uint32_t lo; 36 | } __int64_t; 37 | 38 | static inline __int64_t int64_const(int32_t hi, uint32_t lo) { return (__int64_t){ hi, lo }; } 39 | static inline __int64_t int64_from_int32(int32_t x) { return (__int64_t){ (x < 0 ? -1 : 0), x }; } 40 | static inline int32_t int64_hi(__int64_t x) { return x.hi; } 41 | static inline uint32_t int64_lo(__int64_t x) { return x.lo; } 42 | 43 | static inline int int64_cmp_eq(__int64_t x, __int64_t y) { return ((x.hi == y.hi) && (x.lo == y.lo)); } 44 | static inline int int64_cmp_ne(__int64_t x, __int64_t y) { return ((x.hi != y.hi) || (x.lo != y.lo)); } 45 | static inline int int64_cmp_gt(__int64_t x, __int64_t y) { return ((x.hi > y.hi) || ((x.hi == y.hi) && (x.lo > y.lo))); } 46 | static inline int int64_cmp_ge(__int64_t x, __int64_t y) { return ((x.hi > y.hi) || ((x.hi == y.hi) && (x.lo >= y.lo))); } 47 | static inline int int64_cmp_lt(__int64_t x, __int64_t y) { return ((x.hi < y.hi) || ((x.hi == y.hi) && (x.lo < y.lo))); } 48 | static inline int int64_cmp_le(__int64_t x, __int64_t y) { return ((x.hi < y.hi) || ((x.hi == y.hi) && (x.lo <= y.lo))); } 49 | 50 | static inline __int64_t int64_add(__int64_t x, __int64_t y) { 51 | __int64_t ret; 52 | ret.hi = x.hi + y.hi; 53 | ret.lo = x.lo + y.lo; 54 | if((ret.lo < x.lo) || (ret.hi < y.hi)) 55 | ret.hi++; 56 | return ret; 57 | } 58 | 59 | static inline __int64_t int64_neg(__int64_t x) { 60 | __int64_t ret; 61 | ret.hi = ~x.hi; 62 | ret.lo = ~x.lo + 1; 63 | if(ret.lo == 0) 64 | ret.hi++; 65 | return ret; 66 | } 67 | 68 | static inline __int64_t int64_sub(__int64_t x, __int64_t y) { 69 | return int64_add(x, int64_neg(y)); 70 | } 71 | 72 | static inline __int64_t int64_shift(__int64_t x, int8_t y) { 73 | __int64_t ret; 74 | if(y > 0) { 75 | if(y >= 32) 76 | return (__int64_t){ 0, 0 }; 77 | ret.hi = (x.hi << y) | (x.lo >> (32 - y)); 78 | ret.lo = (x.lo << y); 79 | } else { 80 | y = -y; 81 | if(y >= 32) 82 | return (__int64_t){ 0, 0 }; 83 | ret.lo = (x.lo >> y) | (x.hi << (32 - y)); 84 | ret.hi = (x.hi >> y); 85 | } 86 | return ret; 87 | } 88 | 89 | static inline __int64_t int64_mul_i32_i32(int32_t x, int32_t y) { 90 | int16_t hi[2] = { (x >> 16), (y >> 16) }; 91 | uint16_t lo[2] = { (x & 0xFFFF), (y & 0xFFFF) }; 92 | 93 | int32_t r_hi = hi[0] * hi[1]; 94 | int32_t r_md = (hi[0] * lo[1]) + (hi[1] * lo[0]); 95 | uint32_t r_lo = lo[0] * lo[1]; 96 | 97 | r_hi += (r_md >> 16); 98 | r_lo += (r_md << 16); 99 | 100 | return (__int64_t){ r_hi, r_lo }; 101 | } 102 | 103 | static inline __int64_t int64_mul_i64_i32(__int64_t x, int32_t y) { 104 | int neg = ((x.hi ^ y) < 0); 105 | if(x.hi < 0) 106 | x = int64_neg(x); 107 | if(y < 0) 108 | y = -y; 109 | 110 | uint32_t _x[4] = { (x.hi >> 16), (x.hi & 0xFFFF), (x.lo >> 16), (x.lo & 0xFFFF) }; 111 | uint32_t _y[2] = { (y >> 16), (y & 0xFFFF) }; 112 | 113 | uint32_t r[4]; 114 | r[0] = (_x[0] * _y[0]); 115 | r[1] = (_x[1] * _y[0]) + (_x[0] * _y[1]); 116 | r[2] = (_x[1] * _y[1]) + (_x[2] * _y[0]); 117 | r[3] = (_x[2] * _y[0]) + (_x[1] * _y[1]); 118 | 119 | __int64_t ret; 120 | ret.lo = r[0] + (r[1] << 16); 121 | ret.hi = (r[3] << 16) + r[2] + (r[1] >> 16); 122 | return (neg ? int64_neg(ret) : ret); 123 | } 124 | 125 | static inline __int64_t int64_div_i64_i32(__int64_t x, int32_t y) { 126 | int neg = ((x.hi ^ y) < 0); 127 | if(x.hi < 0) 128 | x = int64_neg(x); 129 | if(y < 0) 130 | y = -y; 131 | 132 | __int64_t ret = { (x.hi / y) , (x.lo / y) }; 133 | x.hi = x.hi % y; 134 | x.lo = x.lo % y; 135 | 136 | __int64_t _y = int64_from_int32(y); 137 | 138 | __int64_t i; 139 | for(i = int64_from_int32(1); int64_cmp_lt(_y, x); _y = int64_shift(_y, 1), i = int64_shift(i, 1)); 140 | 141 | while(x.hi) { 142 | _y = int64_shift(_y, -1); 143 | i = int64_shift(i, -1); 144 | if(int64_cmp_ge(x, _y)) { 145 | x = int64_sub(x, _y); 146 | ret = int64_add(ret, i); 147 | } 148 | } 149 | 150 | ret = int64_add(ret, int64_from_int32(x.lo / y)); 151 | return (neg ? int64_neg(ret) : ret); 152 | } 153 | 154 | #define int64_t __int64_t 155 | 156 | #endif 157 | 158 | #ifdef __cplusplus 159 | } 160 | #endif 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /fix16_trig.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "fix16.h" 3 | 4 | #if defined(FIXMATH_SIN_LUT) 5 | #include "fix16_trig_sin_lut.h" 6 | #elif !defined(FIXMATH_NO_CACHE) 7 | static fix16_t _fix16_sin_cache_index[4096] = { 0 }; 8 | static fix16_t _fix16_sin_cache_value[4096] = { 0 }; 9 | #endif 10 | 11 | #ifndef FIXMATH_NO_CACHE 12 | static fix16_t _fix16_atan_cache_index[2][4096] = { { 0 }, { 0 } }; 13 | static fix16_t _fix16_atan_cache_value[4096] = { 0 }; 14 | #endif 15 | 16 | 17 | fix16_t fix16_sin_parabola(fix16_t inAngle) 18 | { 19 | fix16_t abs_inAngle, abs_retval, retval; 20 | fix16_t mask; 21 | 22 | /* Absolute function */ 23 | mask = (inAngle >> (sizeof(fix16_t)*CHAR_BIT-1)); 24 | abs_inAngle = (inAngle + mask) ^ mask; 25 | 26 | /* On 0->PI, sin looks like x² that is : 27 | - centered on PI/2, 28 | - equals 1 on PI/2, 29 | - equals 0 on 0 and PI 30 | that means : 4/PI * x - 4/PI² * x² 31 | Use abs(x) to handle (-PI) -> 0 zone. 32 | */ 33 | retval = fix16_mul(FOUR_DIV_PI, inAngle) + fix16_mul( fix16_mul(_FOUR_DIV_PI2, inAngle), abs_inAngle ); 34 | /* At this point, retval equals sin(inAngle) on important points ( -PI, -PI/2, 0, PI/2, PI), 35 | but is not very precise between these points 36 | */ 37 | #ifndef FIXMATH_FAST_SIN 38 | /* Absolute value of retval */ 39 | mask = (retval >> (sizeof(fix16_t)*CHAR_BIT-1)); 40 | abs_retval = (retval + mask) ^ mask; 41 | /* So improve its precision by adding some x^4 component to retval */ 42 | retval += fix16_mul(X4_CORRECTION_COMPONENT, fix16_mul(retval, abs_retval) - retval ); 43 | #endif 44 | return retval; 45 | } 46 | 47 | fix16_t fix16_sin(fix16_t inAngle) 48 | { 49 | fix16_t tempAngle = inAngle % (fix16_pi << 1); 50 | 51 | #ifdef FIXMATH_SIN_LUT 52 | if(tempAngle < 0) 53 | tempAngle += (fix16_pi << 1); 54 | 55 | fix16_t tempOut; 56 | if(tempAngle >= fix16_pi) { 57 | tempAngle -= fix16_pi; 58 | if(tempAngle >= (fix16_pi >> 1)) 59 | tempAngle = fix16_pi - tempAngle; 60 | tempOut = -(tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 61 | } else { 62 | if(tempAngle >= (fix16_pi >> 1)) 63 | tempAngle = fix16_pi - tempAngle; 64 | tempOut = (tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 65 | } 66 | #else 67 | if(tempAngle > fix16_pi) 68 | tempAngle -= (fix16_pi << 1); 69 | else if(tempAngle < -fix16_pi) 70 | tempAngle += (fix16_pi << 1); 71 | 72 | #ifndef FIXMATH_NO_CACHE 73 | fix16_t tempIndex = ((inAngle >> 5) & 0x00000FFF); 74 | if(_fix16_sin_cache_index[tempIndex] == inAngle) 75 | return _fix16_sin_cache_value[tempIndex]; 76 | #endif 77 | 78 | fix16_t tempAngleSq = fix16_mul(tempAngle, tempAngle); 79 | 80 | #ifndef FIXMATH_FAST_SIN // Most accurate version, accurate to ~2.1% 81 | fix16_t tempOut = tempAngle; 82 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 83 | tempOut -= (tempAngle / 6); 84 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 85 | tempOut += (tempAngle / 120); 86 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 87 | tempOut -= (tempAngle / 5040); 88 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 89 | tempOut += (tempAngle / 362880); 90 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 91 | tempOut -= (tempAngle / 39916800); 92 | #else // Fast implementation, runs at 159% the speed of above 'accurate' version with an slightly lower accuracy of ~2.3% 93 | fix16_t tempOut; 94 | tempOut = fix16_mul(-13, tempAngleSq) + 546; 95 | tempOut = fix16_mul(tempOut, tempAngleSq) - 10923; 96 | tempOut = fix16_mul(tempOut, tempAngleSq) + 65536; 97 | tempOut = fix16_mul(tempOut, tempAngle); 98 | #endif 99 | 100 | #ifndef FIXMATH_NO_CACHE 101 | _fix16_sin_cache_index[tempIndex] = inAngle; 102 | _fix16_sin_cache_value[tempIndex] = tempOut; 103 | #endif 104 | #endif 105 | 106 | return tempOut; 107 | } 108 | 109 | fix16_t fix16_cos(fix16_t inAngle) 110 | { 111 | return fix16_sin(inAngle + (fix16_pi >> 1)); 112 | } 113 | 114 | fix16_t fix16_tan(fix16_t inAngle) 115 | { 116 | return fix16_sdiv(fix16_sin(inAngle), fix16_cos(inAngle)); 117 | } 118 | 119 | fix16_t fix16_asin(fix16_t x) 120 | { 121 | if((x > fix16_one) 122 | || (x < -fix16_one)) 123 | return 0; 124 | 125 | fix16_t out; 126 | out = (fix16_one - fix16_mul(x, x)); 127 | out = fix16_div(x, fix16_sqrt(out)); 128 | out = fix16_atan(out); 129 | return out; 130 | } 131 | 132 | fix16_t fix16_acos(fix16_t x) 133 | { 134 | return ((fix16_pi >> 1) - fix16_asin(x)); 135 | } 136 | 137 | fix16_t fix16_atan2(fix16_t inY , fix16_t inX) 138 | { 139 | fix16_t abs_inY, mask, angle, r, r_3; 140 | 141 | #ifndef FIXMATH_NO_CACHE 142 | uintptr_t hash = (inX ^ inY); 143 | hash ^= hash >> 20; 144 | hash &= 0x0FFF; 145 | if((_fix16_atan_cache_index[0][hash] == inX) && (_fix16_atan_cache_index[1][hash] == inY)) 146 | return _fix16_atan_cache_value[hash]; 147 | #endif 148 | 149 | /* Absolute inY */ 150 | mask = (inY >> (sizeof(fix16_t)*CHAR_BIT-1)); 151 | abs_inY = (inY + mask) ^ mask; 152 | 153 | if (inX >= 0) 154 | { 155 | r = fix16_div( (inX - abs_inY), (inX + abs_inY)); 156 | r_3 = fix16_mul(fix16_mul(r, r),r); 157 | angle = fix16_mul(0x00003240 , r_3) - fix16_mul(0x0000FB50,r) + PI_DIV_4; 158 | } else { 159 | r = fix16_div( (inX + abs_inY), (abs_inY - inX)); 160 | r_3 = fix16_mul(fix16_mul(r, r),r); 161 | angle = fix16_mul(0x00003240 , r_3) 162 | - fix16_mul(0x0000FB50,r) 163 | + THREE_PI_DIV_4; 164 | } 165 | if (inY < 0) 166 | { 167 | angle = -angle; 168 | } 169 | 170 | #ifndef FIXMATH_NO_CACHE 171 | _fix16_atan_cache_index[0][hash] = inX; 172 | _fix16_atan_cache_index[1][hash] = inY; 173 | _fix16_atan_cache_value[hash] = angle; 174 | #endif 175 | 176 | return angle; 177 | } 178 | 179 | fix16_t fix16_atan(fix16_t x) 180 | { 181 | return fix16_atan2(x, fix16_one); 182 | } 183 | -------------------------------------------------------------------------------- /examples/Fix8_benchmark/Fix8_benchmark.ino: -------------------------------------------------------------------------------- 1 | //#define NO_DOUBLE 2 | #define NO_FIX8 3 | 4 | #if !defined(NO_DOUBLE) && !defined(NO_FIX8) 5 | #include 6 | #endif 7 | 8 | #ifndef NO_FIX8 9 | #include 10 | #endif 11 | #ifndef NO_DOUBLE 12 | #include 13 | #endif 14 | 15 | #define NUM_RUNS (5) 16 | 17 | #define COMMENT(x) Serial.println(F("\n----" x "----")); 18 | 19 | const int16_t testcases[] = { 20 | // Small numbers 21 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 22 | -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 23 | 24 | // Integer numbers 25 | 0x100, -0x100, 0x200, -0x200, 0x300, -0x300, 26 | 0x400, -0x400, 0x500, -0x500, 0x600, -0x600, 27 | 28 | // Fractions (1/2, 1/4, 1/8) 29 | 0x80, -0x80, 0x40, -0x40, 0x20, -0x20, 30 | 31 | // Problematic carry 32 | 0xFF, -0xFF, 0x1FF, -0x1FF, 0x3FF, -0x3FF, 33 | 34 | // Smallest and largest values 35 | 0x7FFF, 0x8000, 36 | 37 | // Large random numbers 38 | 831858892/0x10000, 574794913/0x10000, 2147272293/0x10000, -469161054/0x10000, -961611615/0x10000, 39 | 1841960234/0x10000, 1992698389/0x10000, 520485404/0x10000, 560523116/0x10000, -2094993050/0x10000, 40 | -876897543/0x10000, -67813629/0x10000, 2146227091/0x10000, 509861939/0x10000, -1073573657/0x10000, 41 | 42 | // Small random numbers 43 | -14985/0x100, 30520/0x100, -83587/0x100, 41129/0x100, 42137/0x100, 58537/0x100, -2259/0x100, 84142/0x100, 44 | -28283/0x100, 90914/0x100, 19865/0x100, 33191/0x100, 81844/0x100, -66273/0x100, -63215/0x100, -44459/0x100, 45 | -11326/0x100, 84295/0x100, 47515/0x100, -39324/0x100, 46 | 47 | // Tiny random numbers 48 | -171/0x10, -359/0x10, 491/0x10, 844/0x10, 158/0x10, -413/0x10, -422/0x10, -737/0x10, -575/0x10, -330/0x10, 49 | -376/0x10, 435/0x10, -311/0x10, 116/0x10, 715/0x10, -1024/0x10, -487/0x10, 59/0x10, 724/0x10, 993/0x10 50 | }; 51 | 52 | #define TESTCASES_COUNT (sizeof(testcases)/sizeof(testcases[0])) 53 | 54 | //__attribute__((optimize("O2"))) 55 | 56 | // Volatile global variable which tests will assign values to, to assure compiler will not 57 | // remove calculation statement as it now has side-effects. 58 | static volatile bool f; 59 | 60 | template void test_multTestcases( void ) 61 | { 62 | unsigned int i, j; 63 | for (i = 0; i < TESTCASES_COUNT; i++) 64 | { 65 | for (j = 0; j < TESTCASES_COUNT; j++) 66 | { 67 | T a = testcases[i]; 68 | T b = testcases[j]; 69 | f = a * b; 70 | } 71 | } 72 | } 73 | 74 | template void test_divTestcases( void ) 75 | { 76 | unsigned int i, j; 77 | for (i = 0; i < TESTCASES_COUNT; i++) 78 | { 79 | for (j = 0; j < TESTCASES_COUNT; j++) 80 | { 81 | T a = testcases[i]; 82 | T b = testcases[j]; 83 | // We don't require a solution for /0 :) 84 | if (b == T(0)) continue; 85 | f = a / b; 86 | } 87 | } 88 | } 89 | 90 | template void test_addTestcases( void ) 91 | { 92 | unsigned int i, j; 93 | for (i = 0; i < TESTCASES_COUNT; i++) 94 | { 95 | for (j = 0; j < TESTCASES_COUNT; j++) 96 | { 97 | T a = testcases[i]; 98 | T b = testcases[j]; 99 | f = a + b; 100 | } 101 | } 102 | } 103 | 104 | template void test_subTestcases( void ) 105 | { 106 | unsigned int i, j; 107 | for (i = 0; i < TESTCASES_COUNT; i++) 108 | { 109 | for (j = 0; j < TESTCASES_COUNT; j++) 110 | { 111 | T a = testcases[i]; 112 | T b = testcases[j]; 113 | f = a - b; 114 | } 115 | } 116 | } 117 | 118 | #define TIMED_EXEC(func,delta,runs) \ 119 | { \ 120 | unsigned long t0 = micros(); \ 121 | for (uint8_t i = 0; i < runs; ++i) \ 122 | func(); \ 123 | delta = micros() - t0; \ 124 | } 125 | 126 | void setup() 127 | { 128 | #if !defined(NO_DOUBLE) && !defined(NO_FIX8) 129 | Serial.begin(115200); 130 | 131 | COMMENT("Running testcases for multiplication"); 132 | unsigned long time_Fix8Mult, time_doubleMult; 133 | TIMED_EXEC( test_multTestcases, time_Fix8Mult, NUM_RUNS ); 134 | TIMED_EXEC( test_multTestcases, time_doubleMult, NUM_RUNS ); 135 | 136 | COMMENT("Running testcases for division"); 137 | unsigned long time_Fix8Div, time_doubleDiv; 138 | TIMED_EXEC( test_divTestcases, time_Fix8Div, NUM_RUNS ); 139 | TIMED_EXEC( test_divTestcases, time_doubleDiv, NUM_RUNS ); 140 | 141 | COMMENT("Running testcases for addition"); 142 | unsigned long time_Fix8Add, time_doubleAdd; 143 | TIMED_EXEC( test_addTestcases, time_Fix8Add, NUM_RUNS ); 144 | TIMED_EXEC( test_addTestcases, time_doubleAdd, NUM_RUNS ); 145 | 146 | COMMENT("Running testcases for subtraction"); 147 | unsigned long time_Fix8Sub, time_doubleSub; 148 | TIMED_EXEC( test_subTestcases, time_Fix8Sub, NUM_RUNS ); 149 | TIMED_EXEC( test_subTestcases, time_doubleSub, NUM_RUNS ); 150 | 151 | double incr_Mult = 100.0*double(time_doubleMult)/double(time_Fix8Mult); 152 | double incr_Div = 100.0*double(time_doubleDiv)/double(time_Fix8Div); 153 | double incr_Add = 100.0*double(time_doubleAdd)/double(time_Fix8Add); 154 | double incr_Sub = 100.0*double(time_doubleSub)/double(time_Fix8Sub); 155 | 156 | Serial.println("Op double\tfixpt\tspeed improvement fixpt over double"); 157 | Serial.print("Mult "); Serial.print(time_doubleMult); Serial.print("\t"); Serial.print(time_Fix8Mult); Serial.print("\t"); Serial.print(incr_Mult); Serial.println("%"); 158 | Serial.print("Div "); Serial.print(time_doubleDiv); Serial.print("\t"); Serial.print(time_Fix8Div); Serial.print("\t"); Serial.print(incr_Div); Serial.println("%"); 159 | Serial.print("Add "); Serial.print(time_doubleAdd); Serial.print("\t"); Serial.print(time_Fix8Add); Serial.print("\t"); Serial.print(incr_Add); Serial.println("%"); 160 | Serial.print("Sub "); Serial.print(time_doubleSub); Serial.print("\t"); Serial.print(time_Fix8Sub); Serial.print("\t"); Serial.print(incr_Sub); Serial.println("%"); 161 | 162 | COMMENT("Test finished"); 163 | #endif 164 | 165 | #if defined(NO_DOUBLE) && !defined(NO_FIX8) 166 | test_multTestcases(); 167 | test_divTestcases(); 168 | test_addTestcases(); 169 | test_subTestcases(); 170 | #endif 171 | 172 | #if !defined(NO_DOUBLE) && defined(NO_FIX8) 173 | test_multTestcases(); 174 | test_divTestcases(); 175 | test_addTestcases(); 176 | test_subTestcases(); 177 | #endif 178 | 179 | while (1) {}; 180 | } 181 | 182 | void loop() 183 | { 184 | } 185 | -------------------------------------------------------------------------------- /examples/Fix16_benchmark/Fix16_benchmark.ino: -------------------------------------------------------------------------------- 1 | #define NO_DOUBLE 2 | #define NO_FIX16 3 | 4 | #if !defined(NO_DOUBLE) && !defined(NO_FIX16) 5 | #include 6 | #endif 7 | 8 | #ifndef NO_FIX16 9 | #include 10 | #endif 11 | #ifndef NO_DOUBLE 12 | #include 13 | #endif 14 | 15 | #define NUM_RUNS (5) 16 | 17 | #define COMMENT(x) Serial.println(F("\n----" x "----")); 18 | 19 | const int32_t testcases[] = { 20 | // Small numbers 21 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 22 | -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 23 | 24 | // Integer numbers 25 | 0x10000, -0x10000, 0x20000, -0x20000, 0x30000, -0x30000, 26 | 0x40000, -0x40000, 0x50000, -0x50000, 0x60000, -0x60000, 27 | 28 | // Fractions (1/2, 1/4, 1/8) 29 | 0x8000, -0x8000, 0x4000, -0x4000, 0x2000, -0x2000, 30 | 31 | // Problematic carry 32 | 0xFFFF, -0xFFFF, 0x1FFFF, -0x1FFFF, 0x3FFFF, -0x3FFFF, 33 | 34 | // Smallest and largest values 35 | 0x7FFFFFFF, 0x80000000, 36 | 37 | // Large random numbers 38 | 831858892, 574794913, 2147272293, -469161054, -961611615, 39 | 1841960234, 1992698389, 520485404, 560523116, -2094993050, 40 | -876897543, -67813629, 2146227091, 509861939, -1073573657, 41 | 42 | // Small random numbers 43 | -14985, 30520, -83587, 41129, 42137, 58537, -2259, 84142, 44 | -28283, 90914, 19865, 33191, 81844, -66273, -63215, -44459, 45 | -11326, 84295, 47515, -39324, 46 | 47 | // Tiny random numbers 48 | -171, -359, 491, 844, 158, -413, -422, -737, -575, -330, 49 | -376, 435, -311, 116, 715, -1024, -487, 59, 724, 993 50 | }; 51 | 52 | #define TESTCASES_COUNT (sizeof(testcases)/sizeof(testcases[0])) 53 | 54 | //__attribute__((optimize("O2"))) 55 | 56 | // Volatile global variable which tests will assign values to, to assure compiler will not 57 | // remove calculation statement as it now has side-effects. 58 | static volatile bool f; 59 | 60 | template void test_multTestcases( void ) 61 | { 62 | unsigned int i, j; 63 | for (i = 0; i < TESTCASES_COUNT; i++) 64 | { 65 | for (j = 0; j < TESTCASES_COUNT; j++) 66 | { 67 | T a = testcases[i]; 68 | T b = testcases[j]; 69 | f = a * b; 70 | } 71 | } 72 | } 73 | 74 | template void test_divTestcases( void ) 75 | { 76 | unsigned int i, j; 77 | for (i = 0; i < TESTCASES_COUNT; i++) 78 | { 79 | for (j = 0; j < TESTCASES_COUNT; j++) 80 | { 81 | T a = testcases[i]; 82 | T b = testcases[j]; 83 | // We don't require a solution for /0 :) 84 | if (b == T(0)) continue; 85 | f = a / b; 86 | } 87 | } 88 | } 89 | 90 | template void test_addTestcases( void ) 91 | { 92 | unsigned int i, j; 93 | for (i = 0; i < TESTCASES_COUNT; i++) 94 | { 95 | for (j = 0; j < TESTCASES_COUNT; j++) 96 | { 97 | T a = testcases[i]; 98 | T b = testcases[j]; 99 | f = a + b; 100 | } 101 | } 102 | } 103 | 104 | template void test_subTestcases( void ) 105 | { 106 | unsigned int i, j; 107 | for (i = 0; i < TESTCASES_COUNT; i++) 108 | { 109 | for (j = 0; j < TESTCASES_COUNT; j++) 110 | { 111 | T a = testcases[i]; 112 | T b = testcases[j]; 113 | f = a - b; 114 | } 115 | } 116 | } 117 | 118 | #ifndef NO_FIX16 119 | void test_sqrtTestcasesFix16( void ) 120 | { 121 | unsigned int i; 122 | for (i = 0; i < TESTCASES_COUNT; i++) 123 | { 124 | Fix16 a = testcases[i]; 125 | if (a < Fix16(0)) continue; 126 | f = a.sqrt(); 127 | } 128 | } 129 | #endif 130 | 131 | #ifndef NO_DOUBLE 132 | void test_sqrtTestcasesDouble( void ) 133 | { 134 | unsigned int i; 135 | for (i = 0; i < TESTCASES_COUNT; i++) 136 | { 137 | double a = testcases[i]; 138 | if (a < 0) continue; 139 | f = sqrt(a); 140 | } 141 | } 142 | #endif 143 | 144 | #define TIMED_EXEC(func,delta,runs) \ 145 | { \ 146 | unsigned long t0 = micros(); \ 147 | for (uint8_t i = 0; i < runs; ++i) \ 148 | func(); \ 149 | delta = micros() - t0; \ 150 | } 151 | 152 | void setup() 153 | { 154 | #if !defined(NO_DOUBLE) && !defined(NO_FIX16) 155 | Serial.begin(115200); 156 | 157 | COMMENT("Running testcases for multiplication"); 158 | unsigned long time_Fix16Mult, time_doubleMult; 159 | TIMED_EXEC( test_multTestcases, time_Fix16Mult, NUM_RUNS ); 160 | TIMED_EXEC( test_multTestcases, time_doubleMult, NUM_RUNS ); 161 | 162 | COMMENT("Running testcases for division"); 163 | unsigned long time_Fix16Div, time_doubleDiv; 164 | TIMED_EXEC( test_divTestcases, time_Fix16Div, NUM_RUNS ); 165 | TIMED_EXEC( test_divTestcases, time_doubleDiv, NUM_RUNS ); 166 | 167 | COMMENT("Running testcases for addition"); 168 | unsigned long time_Fix16Add, time_doubleAdd; 169 | TIMED_EXEC( test_addTestcases, time_Fix16Add, NUM_RUNS ); 170 | TIMED_EXEC( test_addTestcases, time_doubleAdd, NUM_RUNS ); 171 | 172 | COMMENT("Running testcases for subtraction"); 173 | unsigned long time_Fix16Sub, time_doubleSub; 174 | TIMED_EXEC( test_subTestcases, time_Fix16Sub, NUM_RUNS ); 175 | TIMED_EXEC( test_subTestcases, time_doubleSub, NUM_RUNS ); 176 | 177 | COMMENT("Running test cases for square root"); 178 | unsigned long time_Fix16Sqrt, time_doubleSqrt; 179 | TIMED_EXEC( test_sqrtTestcasesFix16, time_Fix16Sqrt, NUM_RUNS ); 180 | TIMED_EXEC( test_sqrtTestcasesDouble, time_doubleSqrt, NUM_RUNS ); 181 | 182 | double incr_Mult = 100.0*double(time_doubleMult)/double(time_Fix16Mult); 183 | double incr_Div = 100.0*double(time_doubleDiv)/double(time_Fix16Div); 184 | double incr_Add = 100.0*double(time_doubleAdd)/double(time_Fix16Add); 185 | double incr_Sub = 100.0*double(time_doubleSub)/double(time_Fix16Sub); 186 | double incr_Sqrt = 100.0*double(time_doubleSqrt)/double(time_Fix16Sqrt); 187 | 188 | Serial.println("Op double\tfixpt\tspeed improvement fixpt over double"); 189 | Serial.print("Mult "); Serial.print(time_doubleMult); Serial.print("\t"); Serial.print(time_Fix16Mult); Serial.print("\t"); Serial.print(incr_Mult); Serial.println("%"); 190 | Serial.print("Div "); Serial.print(time_doubleDiv); Serial.print("\t"); Serial.print(time_Fix16Div); Serial.print("\t"); Serial.print(incr_Div); Serial.println("%"); 191 | Serial.print("Add "); Serial.print(time_doubleAdd); Serial.print("\t"); Serial.print(time_Fix16Add); Serial.print("\t"); Serial.print(incr_Add); Serial.println("%"); 192 | Serial.print("Sub "); Serial.print(time_doubleSub); Serial.print("\t"); Serial.print(time_Fix16Sub); Serial.print("\t"); Serial.print(incr_Sub); Serial.println("%"); 193 | Serial.print("Sqrt "); Serial.print(time_doubleSqrt); Serial.print("\t"); Serial.print(time_Fix16Sqrt); Serial.print("\t"); Serial.print(incr_Sqrt); Serial.println("%"); 194 | 195 | COMMENT("Test finished"); 196 | #endif 197 | 198 | #if defined(NO_DOUBLE) && !defined(NO_FIX16) 199 | test_multTestcases(); 200 | test_divTestcases(); 201 | test_addTestcases(); 202 | test_subTestcases(); 203 | test_sqrtTestcasesFix16(); 204 | #endif 205 | 206 | #if !defined(NO_DOUBLE) && defined(NO_FIX16) 207 | test_multTestcases(); 208 | test_divTestcases(); 209 | test_addTestcases(); 210 | test_subTestcases(); 211 | test_sqrtTestcasesDouble(); 212 | #endif 213 | 214 | while (1) {}; 215 | } 216 | 217 | void loop() 218 | { 219 | } 220 | -------------------------------------------------------------------------------- /fix8.c: -------------------------------------------------------------------------------- 1 | #include "fix8.h" 2 | #if !defined(FIXMATH_NO_64BIT) 3 | #include "int64.h" 4 | #endif 5 | 6 | 7 | /* Subtraction and addition with overflow detection. 8 | * The versions without overflow detection are inlined in the header. 9 | */ 10 | #ifndef FIXMATH_NO_OVERFLOW 11 | fix8_t fix8_add(fix8_t a, fix8_t b) 12 | { 13 | // Use unsigned integers because overflow with signed integers is 14 | // an undefined operation (http://www.airs.com/blog/archives/120). 15 | uint16_t _a = a, _b = b; 16 | uint16_t sum = _a + _b; 17 | 18 | // Overflow can only happen if sign of a == sign of b, and then 19 | // it causes sign of sum != sign of a. 20 | if (!((_a ^ _b) & 0x8000) && ((_a ^ sum) & 0x8000)) 21 | return fix8_overflow; 22 | 23 | return sum; 24 | } 25 | 26 | fix8_t fix8_sub(fix8_t a, fix8_t b) 27 | { 28 | uint16_t _a = a, _b = b; 29 | uint16_t diff = _a - _b; 30 | 31 | // Overflow can only happen if sign of a != sign of b, and then 32 | // it causes sign of diff != sign of a. 33 | if (((_a ^ _b) & 0x8000) && ((_a ^ diff) & 0x8000)) 34 | return fix8_overflow; 35 | 36 | return diff; 37 | } 38 | 39 | /* Saturating arithmetic */ 40 | fix8_t fix8_sadd(fix8_t a, fix8_t b) 41 | { 42 | fix8_t result = fix8_add(a, b); 43 | 44 | if (result == fix8_overflow) 45 | return (a >= 0) ? fix8_maximum : fix8_minimum; 46 | 47 | return result; 48 | } 49 | 50 | fix8_t fix8_ssub(fix8_t a, fix8_t b) 51 | { 52 | fix8_t result = fix8_sub(a, b); 53 | 54 | if (result == fix8_overflow) 55 | return (a >= 0) ? fix8_maximum : fix8_minimum; 56 | 57 | return result; 58 | } 59 | #endif 60 | /* 61 | fix8_t fix8_mul(fix8_t inArg0, fix8_t inArg1) 62 | { 63 | int32_t product = (int32_t)inArg0 * inArg1; 64 | 65 | #ifndef FIXMATH_NO_OVERFLOW 66 | // The upper 9 bits should all be the same (the sign). 67 | uint16_t upper = (product >> 23); 68 | #endif 69 | 70 | if (product < 0) 71 | { 72 | #ifndef FIXMATH_NO_OVERFLOW 73 | if (~upper) 74 | return fix8_overflow; 75 | #endif 76 | 77 | #ifndef FIXMATH_NO_ROUNDING 78 | // This adjustment is required in order to round -1/2 correctly 79 | product--; 80 | #endif 81 | } 82 | else 83 | { 84 | #ifndef FIXMATH_NO_OVERFLOW 85 | if (upper) 86 | return fix8_overflow; 87 | #endif 88 | } 89 | 90 | #ifdef FIXMATH_NO_ROUNDING 91 | return product >> 8; 92 | #else 93 | fix8_t result = product >> 8; 94 | result += (product & 0x80) >> 7; 95 | 96 | return result; 97 | #endif 98 | } 99 | */ 100 | 101 | /* 8-bit implementation of fix16_mul. Fastest on e.g. Atmel AVR. 102 | * Uses 8*8->16bit multiplications, and also skips any bytes that 103 | * are zero. 104 | */ 105 | // TODO: See also http://mekonik.wordpress.com/2009/03/18/arduino-avr-gcc-multiplication/ 106 | #if defined(FIXMATH_OPTIMIZE_8BIT) 107 | fix8_t fix8_mul(fix8_t inArg0, fix8_t inArg1) 108 | { 109 | uint16_t _a = (inArg0 >= 0) ? inArg0 : (-inArg0); 110 | uint16_t _b = (inArg1 >= 0) ? inArg1 : (-inArg1); 111 | 112 | uint8_t va[2] = {_a, (_a >> 8)}; 113 | uint8_t vb[2] = {_b, (_b >> 8)}; 114 | 115 | uint16_t low = 0; 116 | 117 | // Result column i depends on va[0..i] and vb[i..0] 118 | 119 | #ifndef FIXMATH_NO_OVERFLOW 120 | // i = 6 121 | if (va[1] && vb[1]) return fix8_overflow; 122 | #endif 123 | 124 | // x * y = 65536 * x1 * y1 + 256 * x1 * y0 + 256 * x0 * y1 + x0 * y0 125 | // 0x0200 * 0x0300 = 0x0600 126 | 127 | // i = 1 128 | if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1]; 129 | if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0]; 130 | low <<= 8; 131 | 132 | // i = 0 133 | if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0]; 134 | 135 | #ifndef FIXMATH_NO_ROUNDING 136 | low += 0x80; 137 | #endif 138 | low >>= 8; 139 | 140 | #ifndef FIXMATH_NO_OVERFLOW 141 | if (low & 0x8000) 142 | return fix8_overflow; 143 | #endif 144 | 145 | fix8_t result = low; 146 | 147 | /* Figure out the sign of result */ 148 | if ((inArg0 >= 0) != (inArg1 >= 0)) 149 | { 150 | result = -result; 151 | } 152 | 153 | return result; 154 | } 155 | #endif 156 | 157 | 158 | #ifndef FIXMATH_NO_OVERFLOW 159 | /* Wrapper around fix8_mul to add saturating arithmetic. */ 160 | fix8_t fix8_smul(fix8_t inArg0, fix8_t inArg1) 161 | { 162 | fix8_t result = fix8_mul(inArg0, inArg1); 163 | 164 | if (result == fix8_overflow) 165 | { 166 | if ((inArg0 >= 0) == (inArg1 >= 0)) 167 | return fix8_maximum; 168 | else 169 | return fix8_minimum; 170 | } 171 | 172 | return result; 173 | } 174 | #endif 175 | 176 | /* Alternative 32-bit implementation of fix16_div. Fastest on e.g. Atmel AVR. 177 | * This does the division manually, and is therefore good for processors that 178 | * do not have hardware division. 179 | */ 180 | #if defined(FIXMATH_OPTIMIZE_8BIT) 181 | fix8_t fix8_div(fix8_t a, fix8_t b) 182 | { 183 | // This uses the basic binary restoring division algorithm. 184 | // It appears to be faster to do the whole division manually than 185 | // trying to compose a 64-bit divide out of 32-bit divisions on 186 | // platforms without hardware divide. 187 | 188 | if (b == 0) 189 | return fix8_minimum; 190 | 191 | uint16_t remainder = (a >= 0) ? a : (-a); 192 | uint16_t divider = (b >= 0) ? b : (-b); 193 | 194 | uint16_t quotient = 0; 195 | uint16_t bit = 0x100; 196 | 197 | /* The algorithm requires D >= R */ 198 | while (divider < remainder) 199 | { 200 | divider <<= 1; 201 | bit <<= 1; 202 | } 203 | 204 | #ifndef FIXMATH_NO_OVERFLOW 205 | if (!bit) 206 | return fix8_overflow; 207 | #endif 208 | 209 | if (divider & 0x8000) 210 | { 211 | // Perform one step manually to avoid overflows later. 212 | // We know that divider's bottom bit is 0 here. 213 | if (remainder >= divider) 214 | { 215 | quotient |= bit; 216 | remainder -= divider; 217 | } 218 | divider >>= 1; 219 | bit >>= 1; 220 | } 221 | 222 | /* Main division loop */ 223 | while (bit && remainder) 224 | { 225 | if (remainder >= divider) 226 | { 227 | quotient |= bit; 228 | remainder -= divider; 229 | } 230 | 231 | remainder <<= 1; 232 | bit >>= 1; 233 | } 234 | 235 | #ifndef FIXMATH_NO_ROUNDING 236 | if (remainder >= divider) 237 | { 238 | quotient++; 239 | } 240 | #endif 241 | 242 | fix8_t result = quotient; 243 | 244 | /* Figure out the sign of result */ 245 | if ((a ^ b) & 0x8000) 246 | { 247 | #ifndef FIXMATH_NO_OVERFLOW 248 | if (result == fix8_minimum) 249 | return fix8_overflow; 250 | #endif 251 | 252 | result = -result; 253 | } 254 | 255 | return result; 256 | } 257 | #endif 258 | 259 | #ifndef FIXMATH_NO_OVERFLOW 260 | /* Wrapper around fix8_div to add saturating arithmetic. */ 261 | fix8_t fix8_sdiv(fix8_t inArg0, fix8_t inArg1) 262 | { 263 | fix8_t result = fix8_div(inArg0, inArg1); 264 | 265 | if (result == fix8_overflow) 266 | { 267 | if ((inArg0 >= 0) == (inArg1 >= 0)) 268 | return fix8_maximum; 269 | else 270 | return fix8_minimum; 271 | } 272 | 273 | return result; 274 | } 275 | #endif 276 | 277 | fix8_t fix8_mod(fix8_t x, fix8_t y) 278 | { 279 | #ifdef FIXMATH_OPTIMIZE_8BIT 280 | /* The reason we do this, rather than use a modulo operator 281 | * is that if you don't have a hardware divider, this will result 282 | * in faster operations when the angles are close to the bounds. 283 | */ 284 | while(x >= y) x -= y; 285 | while(x <= -y) x += y; 286 | #else 287 | /* Note that in C90, the sign of result of the modulo operation is 288 | * undefined. in C99, it's the same as the dividend (aka numerator). 289 | */ 290 | x %= y; 291 | #endif 292 | 293 | return x; 294 | } 295 | 296 | 297 | #ifndef FIXMATH_NO_64BIT 298 | 299 | fix8_t fix8_lerp8(fix8_t inArg0, fix8_t inArg1, uint8_t inFract) 300 | { 301 | int64_t tempOut = int64_mul_i32_i32(inArg0, ((1 << 8) - inFract)); 302 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 303 | tempOut = int64_shift(tempOut, -8); 304 | return (fix8_t)int64_lo(tempOut); 305 | } 306 | 307 | fix8_t fix8_lerp16(fix8_t inArg0, fix8_t inArg1, uint16_t inFract) 308 | { 309 | int64_t tempOut = int64_mul_i32_i32(inArg0, (((int32_t)1 << 16) - inFract)); 310 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 311 | tempOut = int64_shift(tempOut, -16); 312 | return (fix8_t)int64_lo(tempOut); 313 | } 314 | 315 | fix8_t fix8_lerp32(fix8_t inArg0, fix8_t inArg1, uint32_t inFract) 316 | { 317 | int64_t tempOut; 318 | tempOut = ((int64_t)inArg0 * (0 - inFract)); 319 | tempOut += ((int64_t)inArg1 * inFract); 320 | tempOut >>= 32; 321 | return (fix8_t)tempOut; 322 | } 323 | #endif 324 | -------------------------------------------------------------------------------- /fix8.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fix8_h__ 2 | #define __libfixmath_fix8_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | /* These options may let the optimizer to remove some calls to the functions. 10 | * Refer to http://gcc.gnu.org/onlinedocs/gcc/Function-Attributes.html 11 | */ 12 | #ifndef FIXMATH_FUNC_ATTRS 13 | # ifdef __GNUC__ 14 | # if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 6) 15 | # define FIXMATH_FUNC_ATTRS __attribute__((leaf, nothrow, const)) 16 | # else 17 | # define FIXMATH_FUNC_ATTRS __attribute__((nothrow, const)) 18 | # endif 19 | # else 20 | # define FIXMATH_FUNC_ATTRS 21 | # endif 22 | #endif 23 | 24 | #include "libfixmath_conf.h" 25 | #include 26 | 27 | typedef int16_t fix8_t; 28 | #if 0 29 | static const fix8_t FOUR_DIV_PI = 0x145F3; /*!< Fix8 value of 4/PI */ 30 | static const fix8_t _FOUR_DIV_PI2 = 0xFFFF9840; /*!< Fix8 value of -4/PI² */ 31 | static const fix8_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix8 value of 0.225 */ 32 | static const fix8_t PI_DIV_4 = 0x0000C90F; /*!< Fix8 value of PI/4 */ 33 | static const fix8_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix8 value of 3PI/4 */ 34 | #endif 35 | 36 | static const fix8_t fix8_maximum = 0x7FFF; /*!< the maximum value of fix8_t */ 37 | static const fix8_t fix8_minimum = 0x8000; /*!< the minimum value of fix8_t */ 38 | static const fix8_t fix8_overflow = 0x8000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */ 39 | 40 | #if 0 41 | static const fix8_t fix8_pi = 205887; /*!< fix8_t value of pi */ 42 | static const fix8_t fix8_e = 178145; /*!< fix8_t value of e */ 43 | #endif 44 | static const fix8_t fix8_one = 0x0100; /*!< fix8_t value of 1 */ 45 | 46 | /* Conversion functions between fix8_t and float/integer. 47 | * These are inlined to allow compiler to optimize away constant numbers 48 | */ 49 | static inline fix8_t fix8_from_int(int8_t a) { return a * fix8_one; } 50 | static inline float fix8_to_float(fix8_t a) { return (float)a / fix8_one; } 51 | static inline double fix8_to_dbl(fix8_t a) { return (double)a / fix8_one; } 52 | 53 | static inline int8_t fix8_to_int(fix8_t a) 54 | { 55 | #ifdef FIXMATH_NO_ROUNDING 56 | return (a >> 8); 57 | #else 58 | if (a >= 0) 59 | return (a + (fix8_one >> 1)) / fix8_one; 60 | return (a - (fix8_one >> 1)) / fix8_one; 61 | #endif 62 | } 63 | 64 | static inline fix8_t fix8_from_float(float a) 65 | { 66 | float temp = a * fix8_one; 67 | #ifndef FIXMATH_NO_ROUNDING 68 | temp += (temp >= 0) ? 0.5f : -0.5f; 69 | #endif 70 | return (fix8_t)temp; 71 | } 72 | 73 | static inline fix8_t fix8_from_dbl(double a) 74 | { 75 | double temp = a * fix8_one; 76 | #ifndef FIXMATH_NO_ROUNDING 77 | temp += (temp >= 0) ? 0.5f : -0.5f; 78 | #endif 79 | return (fix8_t)temp; 80 | } 81 | 82 | /* Macro for defining fix8_t constant values. 83 | The functions above can't be used from e.g. global variable initializers, 84 | and their names are quite long also. This macro is useful for constants 85 | springled alongside code, e.g. F8(1.234). 86 | 87 | Note that the argument is evaluated multiple times, and also otherwise 88 | you should only use this for constant values. For runtime-conversions, 89 | use the functions above. 90 | */ 91 | #define F8(x) ((fix8_t)(((x) >= 0) ? ((x) * 256.0 + 0.5) : ((x) * 256.0 - 0.5))) 92 | 93 | static inline fix8_t fix8_abs(fix8_t x) 94 | { return (x < 0 ? -x : x); } 95 | static inline fix8_t fix8_floor(fix8_t x) 96 | { return (x & 0xFF00UL); } 97 | static inline fix8_t fix8_ceil(fix8_t x) 98 | { return (x & 0xFF00UL) + (x & 0x00FFUL ? fix8_one : 0); } 99 | static inline fix8_t fix8_min(fix8_t x, fix8_t y) 100 | { return (x < y ? x : y); } 101 | static inline fix8_t fix8_max(fix8_t x, fix8_t y) 102 | { return (x > y ? x : y); } 103 | static inline fix8_t fix8_clamp(fix8_t x, fix8_t lo, fix8_t hi) 104 | { return fix8_min(fix8_max(x, lo), hi); } 105 | 106 | /* Subtraction and addition with (optional) overflow detection. */ 107 | #ifdef FIXMATH_NO_OVERFLOW 108 | 109 | static inline fix8_t fix8_add(fix8_t inArg0, fix8_t inArg1) { return (inArg0 + inArg1); } 110 | static inline fix8_t fix8_sub(fix8_t inArg0, fix8_t inArg1) { return (inArg0 - inArg1); } 111 | 112 | #else 113 | 114 | extern fix8_t fix8_add(fix8_t a, fix8_t b) FIXMATH_FUNC_ATTRS; 115 | extern fix8_t fix8_sub(fix8_t a, fix8_t b) FIXMATH_FUNC_ATTRS; 116 | 117 | /* Saturating arithmetic */ 118 | extern fix8_t fix8_sadd(fix8_t a, fix8_t b) FIXMATH_FUNC_ATTRS; 119 | extern fix8_t fix8_ssub(fix8_t a, fix8_t b) FIXMATH_FUNC_ATTRS; 120 | 121 | #endif 122 | 123 | /*! Multiplies the two given fix8_t's and returns the result. 124 | */ 125 | extern fix8_t fix8_mul(fix8_t inArg0, fix8_t inArg1) FIXMATH_FUNC_ATTRS; 126 | 127 | /*! Divides the first given fix8_t by the second and returns the result. 128 | */ 129 | extern fix8_t fix8_div(fix8_t inArg0, fix8_t inArg1) FIXMATH_FUNC_ATTRS; 130 | 131 | #ifndef FIXMATH_NO_OVERFLOW 132 | /*! Performs a saturated multiplication (overflow-protected) of the two given fix8_t's and returns the result. 133 | */ 134 | extern fix8_t fix8_smul(fix8_t inArg0, fix8_t inArg1) FIXMATH_FUNC_ATTRS; 135 | 136 | /*! Performs a saturated division (overflow-protected) of the first fix8_t by the second and returns the result. 137 | */ 138 | extern fix8_t fix8_sdiv(fix8_t inArg0, fix8_t inArg1) FIXMATH_FUNC_ATTRS; 139 | #endif 140 | 141 | /*! Divides the first given fix8_t by the second and returns the result. 142 | */ 143 | extern fix8_t fix8_mod(fix8_t x, fix8_t y) FIXMATH_FUNC_ATTRS; 144 | 145 | 146 | 147 | /*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract) 148 | */ 149 | extern fix8_t fix8_lerp8(fix8_t inArg0, fix8_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS; 150 | extern fix8_t fix8_lerp16(fix8_t inArg0, fix8_t inArg1, uint16_t inFract) FIXMATH_FUNC_ATTRS; 151 | #ifndef FIXMATH_NO_64BIT 152 | extern fix8_t fix8_lerp32(fix8_t inArg0, fix8_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS; 153 | #endif 154 | 155 | 156 | 157 | /*! Returns the sine of the given fix8_t. 158 | */ 159 | extern fix8_t fix8_sin_parabola(fix8_t inAngle) FIXMATH_FUNC_ATTRS; 160 | 161 | /*! Returns the sine of the given fix8_t. 162 | */ 163 | extern fix8_t fix8_sin(fix8_t inAngle) FIXMATH_FUNC_ATTRS; 164 | 165 | /*! Returns the cosine of the given fix8_t. 166 | */ 167 | extern fix8_t fix8_cos(fix8_t inAngle) FIXMATH_FUNC_ATTRS; 168 | 169 | /*! Returns the tangent of the given fix8_t. 170 | */ 171 | extern fix8_t fix8_tan(fix8_t inAngle) FIXMATH_FUNC_ATTRS; 172 | 173 | /*! Returns the arcsine of the given fix8_t. 174 | */ 175 | extern fix8_t fix8_asin(fix8_t inValue) FIXMATH_FUNC_ATTRS; 176 | 177 | /*! Returns the arccosine of the given fix8_t. 178 | */ 179 | extern fix8_t fix8_acos(fix8_t inValue) FIXMATH_FUNC_ATTRS; 180 | 181 | /*! Returns the arctangent of the given fix8_t. 182 | */ 183 | extern fix8_t fix8_atan(fix8_t inValue) FIXMATH_FUNC_ATTRS; 184 | 185 | /*! Returns the arctangent of inY/inX. 186 | */ 187 | extern fix8_t fix8_atan2(fix8_t inY, fix8_t inX) FIXMATH_FUNC_ATTRS; 188 | 189 | static const fix8_t fix8_rad_to_deg_mult = 14668; /* 3754936 / 256 */ 190 | static inline fix8_t fix8_rad_to_deg(fix8_t radians) 191 | { return fix8_mul(radians, fix8_rad_to_deg_mult); } 192 | 193 | static const fix8_t fix8_deg_to_rad_mult = 4; /* 1144 / 256 */ 194 | static inline fix8_t fix8_deg_to_rad(fix8_t degrees) 195 | { return fix8_mul(degrees, fix8_deg_to_rad_mult); } 196 | 197 | 198 | 199 | /*! Returns the square root of the given fix8_t. 200 | */ 201 | extern fix8_t fix8_sqrt(fix8_t inValue) FIXMATH_FUNC_ATTRS; 202 | 203 | /*! Returns the square of the given fix8_t. 204 | */ 205 | static inline fix8_t fix8_sq(fix8_t x) 206 | { return fix8_mul(x, x); } 207 | 208 | /*! Returns the exponent (e^) of the given fix8_t. 209 | */ 210 | extern fix8_t fix8_exp(fix8_t inValue) FIXMATH_FUNC_ATTRS; 211 | 212 | /*! Returns the natural logarithm of the given fix8_t. 213 | */ 214 | extern fix8_t fix8_log(fix8_t inValue) FIXMATH_FUNC_ATTRS; 215 | 216 | /*! Returns the base 2 logarithm of the given fix8_t. 217 | */ 218 | extern fix8_t fix8_log2(fix8_t x) FIXMATH_FUNC_ATTRS; 219 | 220 | /*! Returns the saturated base 2 logarithm of the given fix8_t. 221 | */ 222 | extern fix8_t fix8_slog2(fix8_t x) FIXMATH_FUNC_ATTRS; 223 | 224 | /*! Convert fix8_t value to a string. 225 | * Required buffer length for largest values is 13 bytes. 226 | */ 227 | extern void fix8_to_str(fix8_t value, char *buf, int decimals); 228 | 229 | /*! Convert string to a fix8_t value 230 | * Ignores spaces at beginning and end. Returns fix8_overflow if 231 | * value is too large or there were garbage characters. 232 | */ 233 | extern fix8_t fix8_from_str(const char *buf); 234 | 235 | #ifdef __cplusplus 236 | } 237 | #include "fix8.hpp" 238 | #endif 239 | 240 | #endif 241 | -------------------------------------------------------------------------------- /fix16.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fix16_h__ 2 | #define __libfixmath_fix16_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | /* These options may let the optimizer to remove some calls to the functions. 10 | * Refer to http://gcc.gnu.org/onlinedocs/gcc/Function-Attributes.html 11 | */ 12 | #ifndef FIXMATH_FUNC_ATTRS 13 | # ifdef __GNUC__ 14 | # if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 6) 15 | # define FIXMATH_FUNC_ATTRS __attribute__((leaf, nothrow, const)) 16 | # else 17 | # define FIXMATH_FUNC_ATTRS __attribute__((nothrow, const)) 18 | # endif 19 | # else 20 | # define FIXMATH_FUNC_ATTRS 21 | # endif 22 | #endif 23 | 24 | #include "libfixmath_conf.h" 25 | #include 26 | 27 | typedef int32_t fix16_t; 28 | 29 | static const fix16_t FOUR_DIV_PI = 0x145F3; /*!< Fix16 value of 4/PI */ 30 | static const fix16_t _FOUR_DIV_PI2 = 0xFFFF9840; /*!< Fix16 value of -4/PI² */ 31 | static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.225 */ 32 | static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */ 33 | static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */ 34 | 35 | static const fix16_t fix16_maximum = 0x7FFFFFFF; /*!< the maximum value of fix16_t */ 36 | static const fix16_t fix16_minimum = 0x80000000; /*!< the minimum value of fix16_t */ 37 | static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */ 38 | 39 | static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */ 40 | static const fix16_t fix16_e = 178145; /*!< fix16_t value of e */ 41 | static const fix16_t fix16_one = 0x00010000; /*!< fix16_t value of 1 */ 42 | 43 | /* Conversion functions between fix16_t and float/integer. 44 | * These are inlined to allow compiler to optimize away constant numbers 45 | */ 46 | static inline fix16_t fix16_from_int(int a) { return a * fix16_one; } 47 | static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; } 48 | static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; } 49 | 50 | static inline int fix16_to_int(fix16_t a) 51 | { 52 | #ifdef FIXMATH_NO_ROUNDING 53 | return (a >> 16); 54 | #else 55 | if (a >= 0) 56 | return (a + (fix16_one >> 1)) / fix16_one; 57 | return (a - (fix16_one >> 1)) / fix16_one; 58 | #endif 59 | } 60 | 61 | static inline fix16_t fix16_from_float(float a) 62 | { 63 | float temp = a * fix16_one; 64 | #ifndef FIXMATH_NO_ROUNDING 65 | temp += (temp >= 0) ? 0.5f : -0.5f; 66 | #endif 67 | return (fix16_t)temp; 68 | } 69 | 70 | static inline fix16_t fix16_from_dbl(double a) 71 | { 72 | double temp = a * fix16_one; 73 | #ifndef FIXMATH_NO_ROUNDING 74 | temp += (temp >= 0) ? 0.5f : -0.5f; 75 | #endif 76 | return (fix16_t)temp; 77 | } 78 | 79 | /* Macro for defining fix16_t constant values. 80 | The functions above can't be used from e.g. global variable initializers, 81 | and their names are quite long also. This macro is useful for constants 82 | springled alongside code, e.g. F16(1.234). 83 | 84 | Note that the argument is evaluated multiple times, and also otherwise 85 | you should only use this for constant values. For runtime-conversions, 86 | use the functions above. 87 | */ 88 | #define F16(x) ((fix16_t)(((x) >= 0) ? ((x) * 65536.0 + 0.5) : ((x) * 65536.0 - 0.5))) 89 | 90 | static inline fix16_t fix16_abs(fix16_t x) 91 | { return (x < 0 ? -x : x); } 92 | static inline fix16_t fix16_floor(fix16_t x) 93 | { return (x & 0xFFFF0000UL); } 94 | static inline fix16_t fix16_ceil(fix16_t x) 95 | { return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? fix16_one : 0); } 96 | static inline fix16_t fix16_min(fix16_t x, fix16_t y) 97 | { return (x < y ? x : y); } 98 | static inline fix16_t fix16_max(fix16_t x, fix16_t y) 99 | { return (x > y ? x : y); } 100 | static inline fix16_t fix16_clamp(fix16_t x, fix16_t lo, fix16_t hi) 101 | { return fix16_min(fix16_max(x, lo), hi); } 102 | 103 | /* Subtraction and addition with (optional) overflow detection. */ 104 | #ifdef FIXMATH_NO_OVERFLOW 105 | 106 | static inline fix16_t fix16_add(fix16_t inArg0, fix16_t inArg1) { return (inArg0 + inArg1); } 107 | static inline fix16_t fix16_sub(fix16_t inArg0, fix16_t inArg1) { return (inArg0 - inArg1); } 108 | 109 | #else 110 | 111 | extern fix16_t fix16_add(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 112 | extern fix16_t fix16_sub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 113 | 114 | /* Saturating arithmetic */ 115 | extern fix16_t fix16_sadd(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 116 | extern fix16_t fix16_ssub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 117 | 118 | #endif 119 | 120 | /*! Multiplies the two given fix16_t's and returns the result. 121 | */ 122 | extern fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 123 | 124 | /*! Divides the first given fix16_t by the second and returns the result. 125 | */ 126 | extern fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 127 | 128 | #ifndef FIXMATH_NO_OVERFLOW 129 | /*! Performs a saturated multiplication (overflow-protected) of the two given fix16_t's and returns the result. 130 | */ 131 | extern fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 132 | 133 | /*! Performs a saturated division (overflow-protected) of the first fix16_t by the second and returns the result. 134 | */ 135 | extern fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 136 | #endif 137 | 138 | /*! Divides the first given fix16_t by the second and returns the result. 139 | */ 140 | extern fix16_t fix16_mod(fix16_t x, fix16_t y) FIXMATH_FUNC_ATTRS; 141 | 142 | 143 | 144 | /*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract) 145 | */ 146 | extern fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS; 147 | extern fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) FIXMATH_FUNC_ATTRS; 148 | #ifndef FIXMATH_NO_64BIT 149 | extern fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS; 150 | #endif 151 | 152 | 153 | 154 | /*! Returns the sine of the given fix16_t. 155 | */ 156 | extern fix16_t fix16_sin_parabola(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 157 | 158 | /*! Returns the sine of the given fix16_t. 159 | */ 160 | extern fix16_t fix16_sin(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 161 | 162 | /*! Returns the cosine of the given fix16_t. 163 | */ 164 | extern fix16_t fix16_cos(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 165 | 166 | /*! Returns the tangent of the given fix16_t. 167 | */ 168 | extern fix16_t fix16_tan(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 169 | 170 | /*! Returns the arcsine of the given fix16_t. 171 | */ 172 | extern fix16_t fix16_asin(fix16_t inValue) FIXMATH_FUNC_ATTRS; 173 | 174 | /*! Returns the arccosine of the given fix16_t. 175 | */ 176 | extern fix16_t fix16_acos(fix16_t inValue) FIXMATH_FUNC_ATTRS; 177 | 178 | /*! Returns the arctangent of the given fix16_t. 179 | */ 180 | extern fix16_t fix16_atan(fix16_t inValue) FIXMATH_FUNC_ATTRS; 181 | 182 | /*! Returns the arctangent of inY/inX. 183 | */ 184 | extern fix16_t fix16_atan2(fix16_t inY, fix16_t inX) FIXMATH_FUNC_ATTRS; 185 | 186 | static const fix16_t fix16_rad_to_deg_mult = 3754936; 187 | static inline fix16_t fix16_rad_to_deg(fix16_t radians) 188 | { return fix16_mul(radians, fix16_rad_to_deg_mult); } 189 | 190 | static const fix16_t fix16_deg_to_rad_mult = 1144; 191 | static inline fix16_t fix16_deg_to_rad(fix16_t degrees) 192 | { return fix16_mul(degrees, fix16_deg_to_rad_mult); } 193 | 194 | 195 | 196 | /*! Returns the square root of the given fix16_t. 197 | */ 198 | extern fix16_t fix16_sqrt(fix16_t inValue) FIXMATH_FUNC_ATTRS; 199 | 200 | /*! Returns the square of the given fix16_t. 201 | */ 202 | static inline fix16_t fix16_sq(fix16_t x) 203 | { return fix16_mul(x, x); } 204 | 205 | /*! Returns the exponent (e^) of the given fix16_t. 206 | */ 207 | extern fix16_t fix16_exp(fix16_t inValue) FIXMATH_FUNC_ATTRS; 208 | 209 | /*! Returns the natural logarithm of the given fix16_t. 210 | */ 211 | extern fix16_t fix16_log(fix16_t inValue) FIXMATH_FUNC_ATTRS; 212 | 213 | /*! Returns the base 2 logarithm of the given fix16_t. 214 | */ 215 | extern fix16_t fix16_log2(fix16_t x) FIXMATH_FUNC_ATTRS; 216 | 217 | /*! Returns the saturated base 2 logarithm of the given fix16_t. 218 | */ 219 | extern fix16_t fix16_slog2(fix16_t x) FIXMATH_FUNC_ATTRS; 220 | 221 | /*! Convert fix16_t value to a string. 222 | * Required buffer length for largest values is 13 bytes. 223 | */ 224 | extern void fix16_to_str(fix16_t value, char *buf, int decimals); 225 | 226 | /*! Convert string to a fix16_t value 227 | * Ignores spaces at beginning and end. Returns fix16_overflow if 228 | * value is too large or there were garbage characters. 229 | */ 230 | extern fix16_t fix16_from_str(const char *buf); 231 | 232 | #ifdef __cplusplus 233 | } 234 | #include "fix16.hpp" 235 | #endif 236 | 237 | #endif 238 | -------------------------------------------------------------------------------- /examples/Fix8_unittest/Fix8_unittest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "unittests.h" 6 | 7 | static int status = 0; 8 | 9 | const double f8max = Fix8(fix8_maximum); 10 | const double f8min = Fix8(fix8_minimum); 11 | 12 | const fix8_t testcases[] = { 13 | // Small numbers 14 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 15 | -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 16 | 17 | // Integer numbers 18 | 0x100, -0x100, 0x200, -0x200, 0x300, -0x300, 19 | 0x400, -0x400, 0x500, -0x500, 0x600, -0x600, 20 | 21 | // Fractions (1/2, 1/4, 1/8) 22 | 0x80, -0x80, 0x40, -0x40, 0x20, -0x20, 23 | 24 | // Problematic carry 25 | 0xFF, -0xFF, 0x1FF, -0x1FF, 0x3FF, -0x3FF, 26 | 27 | // Smallest and largest values 28 | 0x7FFF, 0x8000, 29 | 30 | // Large random numbers 31 | 831858892/0x10000, 574794913/0x10000, 2147272293/0x10000, -469161054/0x10000, -961611615/0x10000, 32 | 1841960234/0x10000, 1992698389/0x10000, 520485404/0x10000, 560523116/0x10000, -2094993050/0x10000, 33 | -876897543/0x10000, -67813629/0x10000, 2146227091/0x10000, 509861939/0x10000, -1073573657/0x10000, 34 | 35 | // Small random numbers 36 | -14985/0x100, 30520/0x100, -83587/0x100, 41129/0x100, 42137/0x100, 58537/0x100, -2259/0x100, 84142/0x100, 37 | -28283/0x100, 90914/0x100, 19865/0x100, 33191/0x100, 81844/0x100, -66273/0x100, -63215/0x100, -44459/0x100, 38 | -11326/0x100, 84295/0x100, 47515/0x100, -39324/0x100, 39 | 40 | // Tiny random numbers 41 | -171/0x10, -359/0x10, 491/0x10, 844/0x10, 158/0x10, -413/0x10, -422/0x10, -737/0x10, -575/0x10, -330/0x10, 42 | -376/0x10, 435/0x10, -311/0x10, 116/0x10, 715/0x10, -1024/0x10, -487/0x10, 59/0x10, 724/0x10, 993/0x10 43 | }; 44 | 45 | #define TESTCASES_COUNT (sizeof(testcases)/sizeof(testcases[0])) 46 | 47 | #define delta(a,b) (((a)>=(b)) ? (a)-(b) : (b)-(a)) 48 | 49 | #ifdef FIXMATH_NO_ROUNDING 50 | const Fix8 max_delta(0.3); 51 | #else 52 | const Fix8 max_delta(0.3); 53 | #endif 54 | 55 | void test_multBasic( void ) 56 | { 57 | COMMENT("Testing basic multiplication"); 58 | TEST(Fix8(int8_t(5)) * Fix8(int8_t(5)) == Fix8(int8_t(25))); 59 | TEST(Fix8(int8_t(-5)) * Fix8(int8_t(5)) == Fix8(int8_t(-25))); 60 | TEST(Fix8(int8_t(-5)) * Fix8(int8_t(-5)) == Fix8(int8_t(25))); 61 | TEST(Fix8(int8_t(5)) * Fix8(int8_t(-5)) == Fix8(int8_t(-25))); 62 | } 63 | 64 | void test_multRound( void ) 65 | { 66 | #ifndef FIXMATH_NO_ROUNDING 67 | COMMENT("Testing multiplication rounding corner cases"); 68 | TEST(fix8_mul(0, 10) == 0); 69 | TEST(fix8_mul(2, 0x80) == 1); 70 | TEST(fix8_mul(-2, 0x80) == -1); 71 | TEST(fix8_mul(3, 0x80) == 2); 72 | TEST(fix8_mul(-3, 0x80) == -2); 73 | TEST(fix8_mul(2, 0x7F) == 1); 74 | TEST(fix8_mul(-2, 0x7F) == -1); 75 | TEST(fix8_mul(2, 0x81) == 1); 76 | TEST(fix8_mul(-2, 0x81) == -1); 77 | #endif 78 | } 79 | 80 | void test_multTestcases( void ) 81 | { 82 | unsigned int i, j; 83 | int failures = 0; 84 | COMMENT("Running testcases for multiplication"); 85 | 86 | for (i = 0; i < TESTCASES_COUNT; i++) 87 | { 88 | for (j = 0; j < TESTCASES_COUNT; j++) 89 | { 90 | Fix8 a = testcases[i]; 91 | Fix8 b = testcases[j]; 92 | Fix8 result = a * b; 93 | 94 | double fa = a; 95 | double fb = b; 96 | double fresult = fa * fb; 97 | 98 | if (delta(Fix8(fresult), result) > max_delta) 99 | { 100 | if ((fa * fb > f8max) || (fa * fb < f8min)) 101 | { 102 | #ifndef FIXMATH_NO_OVERFLOW 103 | if (result != fix8_overflow) 104 | { 105 | LOG_OVERFLOW_NOT_DETECTED(a, b, "*"); 106 | failures++; 107 | } 108 | #endif 109 | // Legitimate overflow 110 | continue; 111 | } 112 | LOG_NEWLINE 113 | LOG_EXPRESSION(a, b, result, "*"); 114 | LOG_EXPRESSION(fa, fb, fresult, "*"); 115 | failures++; 116 | } 117 | } 118 | } 119 | 120 | TEST(failures == 0); 121 | } 122 | 123 | 124 | void test_divBasic( void ) 125 | { 126 | COMMENT("Testing basic division"); 127 | TEST(Fix8(int8_t(15)) / Fix8(int8_t(5)) == Fix8(int8_t(3))); 128 | TEST(Fix8(int8_t(-15)) / Fix8(int8_t(5)) == Fix8(int8_t(-3))); 129 | TEST(Fix8(int8_t(-15)) / Fix8(int8_t(-5)) == Fix8(int8_t(3))); 130 | TEST(Fix8(int8_t(15)) / Fix8(int8_t(-5)) == Fix8(int8_t(-3))); 131 | } 132 | 133 | void test_divRound( void ) 134 | { 135 | #ifndef FIXMATH_NO_ROUNDING 136 | COMMENT("Testing division rounding corner cases"); 137 | TEST(fix8_div(0, 10) == 0); 138 | TEST(fix8_div(1, fix8_from_int(2)) == 1); 139 | TEST(fix8_div(-1, fix8_from_int(2)) == -1); 140 | TEST(fix8_div(1, fix8_from_int(-2)) == -1); 141 | TEST(fix8_div(-1, fix8_from_int(-2)) == 1); 142 | TEST(fix8_div(3, fix8_from_int(2)) == 2); 143 | TEST(fix8_div(-3, fix8_from_int(2)) == -2); 144 | TEST(fix8_div(3, fix8_from_int(-2)) == -2); 145 | TEST(fix8_div(-3, fix8_from_int(-2)) == 2); 146 | TEST(fix8_div(2, 0x7F) == 4); 147 | TEST(fix8_div(-2, 0x7F) == -4); 148 | TEST(fix8_div(2, 0x81) == 4); 149 | TEST(fix8_div(-2, 0x81) == -4); 150 | #endif 151 | } 152 | 153 | void test_divTestcases( void ) 154 | { 155 | unsigned int i, j; 156 | int failures = 0; 157 | COMMENT("Running testcases for division"); 158 | 159 | for (i = 0; i < TESTCASES_COUNT; i++) 160 | { 161 | for (j = 0; j < TESTCASES_COUNT; j++) 162 | { 163 | Fix8 a = testcases[i]; 164 | Fix8 b = testcases[j]; 165 | 166 | // We don't require a solution for /0 :) 167 | if (b == 0) continue; 168 | 169 | Fix8 result = a / b; 170 | 171 | double fa = a; 172 | double fb = b; 173 | double fresult = fa / fb; 174 | 175 | if (delta(Fix8(fresult), result) > max_delta) 176 | { 177 | if (((fa / fb) > f8max) || ((fa / fb) < f8min)) 178 | { 179 | #ifndef FIXMATH_NO_OVERFLOW 180 | if (result != fix8_overflow) 181 | { 182 | LOG_OVERFLOW_NOT_DETECTED(a, b, "/"); 183 | failures++; 184 | } 185 | #endif 186 | // Legitimate overflow 187 | continue; 188 | } 189 | LOG_NEWLINE 190 | LOG_EXPRESSION(a, b, result, "/"); 191 | LOG_EXPRESSION(fa, fb, fresult, "/"); 192 | failures++; 193 | } 194 | } 195 | } 196 | 197 | TEST(failures == 0); 198 | } 199 | 200 | void test_addTestcases( void ) 201 | { 202 | unsigned int i, j; 203 | int failures = 0; 204 | COMMENT("Running testcases for addition"); 205 | 206 | for (i = 0; i < TESTCASES_COUNT; i++) 207 | { 208 | for (j = 0; j < TESTCASES_COUNT; j++) 209 | { 210 | Fix8 a = testcases[i]; 211 | Fix8 b = testcases[j]; 212 | Fix8 result = a + b; 213 | 214 | double fa = a; 215 | double fb = b; 216 | double fresult = fa + fb; 217 | 218 | if (delta(Fix8(fresult), result) > max_delta) 219 | { 220 | if ((fa + fb > f8max) || (fa + fb < f8min)) 221 | { 222 | #ifndef FIXMATH_NO_OVERFLOW 223 | if (result != fix8_overflow) 224 | { 225 | Serial.println(double(result), FLOAT_DECS); 226 | LOG_OVERFLOW_NOT_DETECTED(a, b, "+"); 227 | failures++; 228 | } 229 | #endif 230 | // Legitimate overflow 231 | continue; 232 | } 233 | LOG_NEWLINE 234 | LOG_EXPRESSION(a, b, result, "+"); 235 | LOG_EXPRESSION(fa, fb, fresult, "+"); 236 | failures++; 237 | } 238 | } 239 | } 240 | 241 | TEST(failures == 0); 242 | } 243 | 244 | void test_subTestcases( void ) 245 | { 246 | unsigned int i, j; 247 | int failures = 0; 248 | COMMENT("Running testcases for subtraction"); 249 | 250 | for (i = 0; i < TESTCASES_COUNT; i++) 251 | { 252 | for (j = 0; j < TESTCASES_COUNT; j++) 253 | { 254 | Fix8 a = testcases[i]; 255 | Fix8 b = testcases[j]; 256 | Fix8 result = a - b; 257 | 258 | double fa = a; 259 | double fb = b; 260 | double fresult = fa - fb; 261 | 262 | if (delta(Fix8(fresult), result) > max_delta) 263 | { 264 | if ((fa - fb > f8max) || (fa - fb < f8min)) 265 | { 266 | #ifndef FIXMATH_NO_OVERFLOW 267 | if (result != fix8_overflow) 268 | { 269 | LOG_OVERFLOW_NOT_DETECTED(a, b, "-"); 270 | failures++; 271 | } 272 | #endif 273 | // Legitimate overflow 274 | continue; 275 | } 276 | LOG_NEWLINE 277 | LOG_EXPRESSION(a, b, result, "-"); 278 | LOG_EXPRESSION(fa, fb, fresult, "-"); 279 | failures++; 280 | } 281 | } 282 | } 283 | 284 | TEST(failures == 0); 285 | } 286 | 287 | void setup() 288 | { 289 | Serial.begin(115200); 290 | 291 | test_multBasic(); 292 | test_multRound(); 293 | test_multTestcases(); 294 | test_divBasic(); 295 | test_divRound(); 296 | test_divTestcases(); 297 | test_addTestcases(); 298 | test_subTestcases(); 299 | 300 | if (status != 0) 301 | Serial.println("\n\nSome tests FAILED!"); 302 | 303 | COMMENT("Test finished"); 304 | while (1) {}; 305 | } 306 | 307 | void loop() 308 | { 309 | } 310 | -------------------------------------------------------------------------------- /examples/Fix16_unittest/Fix16_unittest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "unittests.h" 6 | 7 | static int status = 0; 8 | 9 | const double f16max = Fix16(fix16_maximum); 10 | const double f16min = Fix16(fix16_minimum); 11 | 12 | const fix16_t testcases[] = { 13 | // Small numbers 14 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 15 | -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 16 | 17 | // Integer numbers 18 | 0x10000, -0x10000, 0x20000, -0x20000, 0x30000, -0x30000, 19 | 0x40000, -0x40000, 0x50000, -0x50000, 0x60000, -0x60000, 20 | 21 | // Fractions (1/2, 1/4, 1/8) 22 | 0x8000, -0x8000, 0x4000, -0x4000, 0x2000, -0x2000, 23 | 24 | // Problematic carry 25 | 0xFFFF, -0xFFFF, 0x1FFFF, -0x1FFFF, 0x3FFFF, -0x3FFFF, 26 | 27 | // Smallest and largest values 28 | 0x7FFFFFFF, 0x80000000, 29 | 30 | // Large random numbers 31 | 831858892, 574794913, 2147272293, -469161054, -961611615, 32 | 1841960234, 1992698389, 520485404, 560523116, -2094993050, 33 | -876897543, -67813629, 2146227091, 509861939, -1073573657, 34 | 35 | // Small random numbers 36 | -14985, 30520, -83587, 41129, 42137, 58537, -2259, 84142, 37 | -28283, 90914, 19865, 33191, 81844, -66273, -63215, -44459, 38 | -11326, 84295, 47515, -39324, 39 | 40 | // Tiny random numbers 41 | -171, -359, 491, 844, 158, -413, -422, -737, -575, -330, 42 | -376, 435, -311, 116, 715, -1024, -487, 59, 724, 993 43 | }; 44 | 45 | #define TESTCASES_COUNT (sizeof(testcases)/sizeof(testcases[0])) 46 | 47 | #define delta(a,b) (((a)>=(b)) ? (a)-(b) : (b)-(a)) 48 | 49 | #ifdef FIXMATH_NO_ROUNDING 50 | const Fix16 max_delta(0.003); 51 | #else 52 | const Fix16 max_delta(0.003); 53 | #endif 54 | 55 | void test_multBasic( void ) 56 | { 57 | COMMENT("Testing basic multiplication"); 58 | TEST(Fix16(5) * Fix16(5) == Fix16(25)); 59 | TEST(Fix16(-5) * Fix16(5) == Fix16(-25)); 60 | TEST(Fix16(-5) * Fix16(-5) == Fix16(25)); 61 | TEST(Fix16(5) * Fix16(-5) == Fix16(-25)); 62 | } 63 | 64 | void test_multRound( void ) 65 | { 66 | #ifndef FIXMATH_NO_ROUNDING 67 | COMMENT("Testing multiplication rounding corner cases"); 68 | TEST(fix16_mul(0, 10) == 0); 69 | TEST(fix16_mul(2, 0x8000) == 1); 70 | TEST(fix16_mul(-2, 0x8000) == -1); 71 | TEST(fix16_mul(3, 0x8000) == 2); 72 | TEST(fix16_mul(-3, 0x8000) == -2); 73 | TEST(fix16_mul(2, 0x7FFF) == 1); 74 | TEST(fix16_mul(-2, 0x7FFF) == -1); 75 | TEST(fix16_mul(2, 0x8001) == 1); 76 | TEST(fix16_mul(-2, 0x8001) == -1); 77 | #endif 78 | } 79 | 80 | void test_multTestcases( void ) 81 | { 82 | unsigned int i, j; 83 | int failures = 0; 84 | COMMENT("Running testcases for multiplication"); 85 | 86 | for (i = 0; i < TESTCASES_COUNT; i++) 87 | { 88 | for (j = 0; j < TESTCASES_COUNT; j++) 89 | { 90 | Fix16 a = testcases[i]; 91 | Fix16 b = testcases[j]; 92 | Fix16 result = a * b; 93 | 94 | double fa = a; 95 | double fb = b; 96 | double fresult = fa * fb; 97 | 98 | if (delta(Fix16(fresult), result) > max_delta) 99 | { 100 | if ((fa * fb > f16max) || (fa * fb < f16min)) 101 | { 102 | #ifndef FIXMATH_NO_OVERFLOW 103 | if (result != fix16_overflow) 104 | { 105 | LOG_OVERFLOW_NOT_DETECTED(a, b, "*"); 106 | failures++; 107 | } 108 | #endif 109 | // Legitimate overflow 110 | continue; 111 | } 112 | LOG_NEWLINE 113 | LOG_EXPRESSION(a, b, result, "*"); 114 | LOG_EXPRESSION(fa, fb, fresult, "*"); 115 | failures++; 116 | } 117 | } 118 | } 119 | 120 | TEST(failures == 0); 121 | } 122 | 123 | 124 | void test_divBasic( void ) 125 | { 126 | COMMENT("Testing basic division"); 127 | TEST(Fix16(15) / Fix16(5) == Fix16(3)); 128 | TEST(Fix16(-15) / Fix16(5) == Fix16(-3)); 129 | TEST(Fix16(-15) / Fix16(-5) == Fix16(3)); 130 | TEST(Fix16(15) / Fix16(-5) == Fix16(-3)); 131 | } 132 | 133 | void test_divRound( void ) 134 | { 135 | #ifndef FIXMATH_NO_ROUNDING 136 | COMMENT("Testing division rounding corner cases"); 137 | TEST(fix16_div(0, 10) == 0); 138 | TEST(fix16_div(1, fix16_from_int(2)) == 1); 139 | TEST(fix16_div(-1, fix16_from_int(2)) == -1); 140 | TEST(fix16_div(1, fix16_from_int(-2)) == -1); 141 | TEST(fix16_div(-1, fix16_from_int(-2)) == 1); 142 | TEST(fix16_div(3, fix16_from_int(2)) == 2); 143 | TEST(fix16_div(-3, fix16_from_int(2)) == -2); 144 | TEST(fix16_div(3, fix16_from_int(-2)) == -2); 145 | TEST(fix16_div(-3, fix16_from_int(-2)) == 2); 146 | TEST(fix16_div(2, 0x7FFF) == 4); 147 | TEST(fix16_div(-2, 0x7FFF) == -4); 148 | TEST(fix16_div(2, 0x8001) == 4); 149 | TEST(fix16_div(-2, 0x8001) == -4); 150 | #endif 151 | } 152 | 153 | void test_divTestcases( void ) 154 | { 155 | unsigned int i, j; 156 | int failures = 0; 157 | COMMENT("Running testcases for division"); 158 | 159 | for (i = 0; i < TESTCASES_COUNT; i++) 160 | { 161 | for (j = 0; j < TESTCASES_COUNT; j++) 162 | { 163 | Fix16 a = testcases[i]; 164 | Fix16 b = testcases[j]; 165 | 166 | // We don't require a solution for /0 :) 167 | if (b == 0) continue; 168 | 169 | Fix16 result = a / b; 170 | 171 | double fa = a; 172 | double fb = b; 173 | double fresult = fa / fb; 174 | 175 | if (delta(Fix16(fresult), result) > max_delta) 176 | { 177 | if (((fa / fb) > f16max) || ((fa / fb) < f16min)) 178 | { 179 | #ifndef FIXMATH_NO_OVERFLOW 180 | if (result != fix16_overflow) 181 | { 182 | LOG_OVERFLOW_NOT_DETECTED(a, b, "/"); 183 | failures++; 184 | } 185 | #endif 186 | // Legitimate overflow 187 | continue; 188 | } 189 | LOG_NEWLINE 190 | LOG_EXPRESSION(a, b, result, "/"); 191 | LOG_EXPRESSION(fa, fb, fresult, "/"); 192 | failures++; 193 | } 194 | } 195 | } 196 | 197 | TEST(failures == 0); 198 | } 199 | 200 | void test_addTestcases( void ) 201 | { 202 | unsigned int i, j; 203 | int failures = 0; 204 | COMMENT("Running testcases for addition"); 205 | 206 | for (i = 0; i < TESTCASES_COUNT; i++) 207 | { 208 | for (j = 0; j < TESTCASES_COUNT; j++) 209 | { 210 | Fix16 a = testcases[i]; 211 | Fix16 b = testcases[j]; 212 | Fix16 result = a + b; 213 | 214 | double fa = a; 215 | double fb = b; 216 | double fresult = fa + fb; 217 | 218 | if (delta(Fix16(fresult), result) > max_delta) 219 | { 220 | if ((fa + fb > f16max) || (fa + fb < f16min)) 221 | { 222 | #ifndef FIXMATH_NO_OVERFLOW 223 | if (result != fix16_overflow) 224 | { 225 | Serial.println(double(result), FLOAT_DECS); 226 | LOG_OVERFLOW_NOT_DETECTED(a, b, "+"); 227 | failures++; 228 | } 229 | #endif 230 | // Legitimate overflow 231 | continue; 232 | } 233 | LOG_NEWLINE 234 | LOG_EXPRESSION(a, b, result, "+"); 235 | LOG_EXPRESSION(fa, fb, fresult, "+"); 236 | failures++; 237 | } 238 | } 239 | } 240 | 241 | TEST(failures == 0); 242 | } 243 | 244 | void test_subTestcases( void ) 245 | { 246 | unsigned int i, j; 247 | int failures = 0; 248 | COMMENT("Running testcases for subtraction"); 249 | 250 | for (i = 0; i < TESTCASES_COUNT; i++) 251 | { 252 | for (j = 0; j < TESTCASES_COUNT; j++) 253 | { 254 | Fix16 a = testcases[i]; 255 | Fix16 b = testcases[j]; 256 | Fix16 result = a - b; 257 | 258 | double fa = a; 259 | double fb = b; 260 | double fresult = fa - fb; 261 | 262 | if (delta(Fix16(fresult), result) > max_delta) 263 | { 264 | if ((fa - fb > f16max) || (fa - fb < f16min)) 265 | { 266 | #ifndef FIXMATH_NO_OVERFLOW 267 | if (result != fix16_overflow) 268 | { 269 | LOG_OVERFLOW_NOT_DETECTED(a, b, "-"); 270 | failures++; 271 | } 272 | #endif 273 | // Legitimate overflow 274 | continue; 275 | } 276 | LOG_NEWLINE 277 | LOG_EXPRESSION(a, b, result, "-"); 278 | LOG_EXPRESSION(fa, fb, fresult, "-"); 279 | failures++; 280 | } 281 | } 282 | } 283 | 284 | TEST(failures == 0); 285 | } 286 | 287 | 288 | void test_sqrtBasic( void ) 289 | { 290 | COMMENT("Testing basic square roots"); 291 | TEST(Fix16(16).sqrt() == Fix16(4)); 292 | TEST(Fix16(100).sqrt() == Fix16(10)); 293 | TEST(Fix16(1).sqrt() == Fix16(1)); 294 | } 295 | 296 | void test_sqrtRound( void ) 297 | { 298 | #ifndef FIXMATH_NO_ROUNDING 299 | COMMENT("Testing square root rounding corner cases"); 300 | TEST(fix16_sqrt(214748302) == 3751499); 301 | TEST(fix16_sqrt(214748303) == 3751499); 302 | TEST(fix16_sqrt(214748359) == 3751499); 303 | TEST(fix16_sqrt(214748360) == 3751500); 304 | #endif 305 | } 306 | 307 | void test_sqrtTestcases( void ) 308 | { 309 | unsigned int i; 310 | int failures = 0; 311 | COMMENT("Running test cases for square root"); 312 | 313 | for (i = 0; i < TESTCASES_COUNT; i++) 314 | { 315 | Fix16 a = testcases[i]; 316 | if (a < Fix16(0)) continue; 317 | Fix16 result = a.sqrt(); 318 | 319 | double fa = double(a); 320 | double fresult = sqrt(fa); 321 | 322 | if (delta(Fix16(fresult), result) > max_delta) 323 | { 324 | LOG_NEWLINE 325 | // Serial.print(F("Delta ")); 326 | // Serial.println(double(delta(Fix16(fresult), result)), FLOAT_DECS); 327 | 328 | Serial.print(F("Fix16.sqrt ")); 329 | Serial.print(double(a), FLOAT_DECS); 330 | Serial.print(F(" = ")); 331 | Serial.println(double(result), FLOAT_DECS); 332 | 333 | Serial.print(F("sqrt ")); 334 | Serial.print(fa, FLOAT_DECS); 335 | Serial.print(F(" = ")); 336 | Serial.println(fresult, FLOAT_DECS); 337 | failures++; 338 | } 339 | } 340 | 341 | TEST(failures == 0); 342 | } 343 | 344 | 345 | void setup() 346 | { 347 | Serial.begin(115200); 348 | 349 | test_multBasic(); 350 | test_multRound(); 351 | test_multTestcases(); 352 | test_divBasic(); 353 | test_divRound(); 354 | test_divTestcases(); 355 | test_addTestcases(); 356 | test_subTestcases(); 357 | test_sqrtBasic(); 358 | test_sqrtRound(); 359 | test_sqrtTestcases(); 360 | 361 | if (status != 0) 362 | Serial.println("\n\nSome tests FAILED!"); 363 | 364 | COMMENT("Test finished"); 365 | while (1) {}; 366 | } 367 | 368 | void loop() 369 | { 370 | } 371 | -------------------------------------------------------------------------------- /fix8.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fix8_hpp__ 2 | #define __libfixmath_fix8_hpp__ 3 | 4 | #include "fix8.h" 5 | 6 | class Fix8 { 7 | public: 8 | fix8_t value; 9 | 10 | Fix8() { value = 0; } 11 | Fix8(const Fix8 &inValue) { value = inValue.value; } 12 | Fix8(const fix8_t inValue) { value = inValue; } 13 | Fix8(const float inValue) { value = fix8_from_float(inValue); } 14 | Fix8(const double inValue) { value = fix8_from_dbl(inValue); } 15 | Fix8(const int8_t inValue) { value = fix8_from_int(inValue); } 16 | 17 | operator fix8_t() const { return value; } 18 | operator double() const { return fix8_to_dbl(value); } 19 | operator float() const { return fix8_to_float(value); } 20 | operator int8_t() const { return fix8_to_int(value); } 21 | operator bool() const { return bool(value); } 22 | 23 | Fix8 & operator=(const Fix8 &rhs) { value = rhs.value; return *this; } 24 | Fix8 & operator=(const fix8_t rhs) { value = rhs; return *this; } 25 | Fix8 & operator=(const double rhs) { value = fix8_from_dbl(rhs); return *this; } 26 | Fix8 & operator=(const float rhs) { value = fix8_from_float(rhs); return *this; } 27 | Fix8 & operator=(const int8_t rhs) { value = fix8_from_int(rhs); return *this; } 28 | 29 | Fix8 & operator+=(const Fix8 &rhs) { value = fix8_add(value, rhs.value); return *this; } 30 | Fix8 & operator+=(const fix8_t rhs) { value = fix8_add(value, rhs); return *this; } 31 | Fix8 & operator+=(const double rhs) { value = fix8_add(value, fix8_from_dbl(rhs)); return *this; } 32 | Fix8 & operator+=(const float rhs) { value = fix8_add(value, fix8_from_float(rhs)); return *this; } 33 | Fix8 & operator+=(const int8_t rhs) { value = fix8_add(value, fix8_from_int(rhs)); return *this; } 34 | 35 | Fix8 & operator-=(const Fix8 &rhs) { value = fix8_sub(value, rhs.value); return *this; } 36 | Fix8 & operator-=(const fix8_t rhs) { value = fix8_sub(value, rhs); return *this; } 37 | Fix8 & operator-=(const double rhs) { value = fix8_sub(value, fix8_from_dbl(rhs)); return *this; } 38 | Fix8 & operator-=(const float rhs) { value = fix8_sub(value, fix8_from_float(rhs)); return *this; } 39 | Fix8 & operator-=(const int8_t rhs) { value = fix8_sub(value, fix8_from_int(rhs)); return *this; } 40 | 41 | Fix8 & operator*=(const Fix8 &rhs) { value = fix8_mul(value, rhs.value); return *this; } 42 | Fix8 & operator*=(const fix8_t rhs) { value = fix8_mul(value, rhs); return *this; } 43 | Fix8 & operator*=(const double rhs) { value = fix8_mul(value, fix8_from_dbl(rhs)); return *this; } 44 | Fix8 & operator*=(const float rhs) { value = fix8_mul(value, fix8_from_float(rhs)); return *this; } 45 | Fix8 & operator*=(const int8_t rhs) { value = fix8_mul(value, fix8_from_int(rhs)); return *this; } 46 | 47 | Fix8 & operator/=(const Fix8 &rhs) { value = fix8_div(value, rhs.value); return *this; } 48 | Fix8 & operator/=(const fix8_t rhs) { value = fix8_div(value, rhs); return *this; } 49 | Fix8 & operator/=(const double rhs) { value = fix8_div(value, fix8_from_dbl(rhs)); return *this; } 50 | Fix8 & operator/=(const float rhs) { value = fix8_div(value, fix8_from_float(rhs)); return *this; } 51 | Fix8 & operator/=(const int8_t rhs) { value = fix8_div(value, fix8_from_int(rhs)); return *this; } 52 | 53 | const Fix8 operator+(const Fix8 &other) const { Fix8 ret = *this; ret += other; return ret; } 54 | const Fix8 operator+(const fix8_t other) const { Fix8 ret = *this; ret += other; return ret; } 55 | const Fix8 operator+(const double other) const { Fix8 ret = *this; ret += other; return ret; } 56 | const Fix8 operator+(const float other) const { Fix8 ret = *this; ret += other; return ret; } 57 | const Fix8 operator+(const int8_t other) const { Fix8 ret = *this; ret += other; return ret; } 58 | 59 | #ifndef FIXMATH_NO_OVERFLOW 60 | const Fix8 sadd(const Fix8 &other) const { Fix8 ret = fix8_sadd(value, other.value); return ret; } 61 | const Fix8 sadd(const fix8_t other) const { Fix8 ret = fix8_sadd(value, other); return ret; } 62 | const Fix8 sadd(const double other) const { Fix8 ret = fix8_sadd(value, fix8_from_dbl(other)); return ret; } 63 | const Fix8 sadd(const float other) const { Fix8 ret = fix8_sadd(value, fix8_from_float(other)); return ret; } 64 | const Fix8 sadd(const int8_t other) const { Fix8 ret = fix8_sadd(value, fix8_from_int(other)); return ret; } 65 | #endif 66 | 67 | const Fix8 operator-(const Fix8 &other) const { Fix8 ret = *this; ret -= other; return ret; } 68 | const Fix8 operator-(const fix8_t other) const { Fix8 ret = *this; ret -= other; return ret; } 69 | const Fix8 operator-(const double other) const { Fix8 ret = *this; ret -= other; return ret; } 70 | const Fix8 operator-(const float other) const { Fix8 ret = *this; ret -= other; return ret; } 71 | const Fix8 operator-(const int8_t other) const { Fix8 ret = *this; ret -= other; return ret; } 72 | 73 | #ifndef FIXMATH_NO_OVERFLOW 74 | const Fix8 ssub(const Fix8 &other) const { Fix8 ret = fix8_sadd(value, -other.value); return ret; } 75 | const Fix8 ssub(const fix8_t other) const { Fix8 ret = fix8_sadd(value, -other); return ret; } 76 | const Fix8 ssub(const double other) const { Fix8 ret = fix8_sadd(value, -fix8_from_dbl(other)); return ret; } 77 | const Fix8 ssub(const float other) const { Fix8 ret = fix8_sadd(value, -fix8_from_float(other)); return ret; } 78 | const Fix8 ssub(const int8_t other) const { Fix8 ret = fix8_sadd(value, -fix8_from_int(other)); return ret; } 79 | #endif 80 | 81 | const Fix8 operator*(const Fix8 &other) const { Fix8 ret = *this; ret *= other; return ret; } 82 | const Fix8 operator*(const fix8_t other) const { Fix8 ret = *this; ret *= other; return ret; } 83 | const Fix8 operator*(const double other) const { Fix8 ret = *this; ret *= other; return ret; } 84 | const Fix8 operator*(const float other) const { Fix8 ret = *this; ret *= other; return ret; } 85 | const Fix8 operator*(const int8_t other) const { Fix8 ret = *this; ret *= other; return ret; } 86 | 87 | #ifndef FIXMATH_NO_OVERFLOW 88 | const Fix8 smul(const Fix8 &other) const { Fix8 ret = fix8_smul(value, other.value); return ret; } 89 | const Fix8 smul(const fix8_t other) const { Fix8 ret = fix8_smul(value, other); return ret; } 90 | const Fix8 smul(const double other) const { Fix8 ret = fix8_smul(value, fix8_from_dbl(other)); return ret; } 91 | const Fix8 smul(const float other) const { Fix8 ret = fix8_smul(value, fix8_from_float(other)); return ret; } 92 | const Fix8 smul(const int8_t other) const { Fix8 ret = fix8_smul(value, fix8_from_int(other)); return ret; } 93 | #endif 94 | 95 | const Fix8 operator/(const Fix8 &other) const { Fix8 ret = *this; ret /= other; return ret; } 96 | const Fix8 operator/(const fix8_t other) const { Fix8 ret = *this; ret /= other; return ret; } 97 | const Fix8 operator/(const double other) const { Fix8 ret = *this; ret /= other; return ret; } 98 | const Fix8 operator/(const float other) const { Fix8 ret = *this; ret /= other; return ret; } 99 | const Fix8 operator/(const int8_t other) const { Fix8 ret = *this; ret /= other; return ret; } 100 | 101 | #ifndef FIXMATH_NO_OVERFLOW 102 | const Fix8 sdiv(const Fix8 &other) const { Fix8 ret = fix8_sdiv(value, other.value); return ret; } 103 | const Fix8 sdiv(const fix8_t other) const { Fix8 ret = fix8_sdiv(value, other); return ret; } 104 | const Fix8 sdiv(const double other) const { Fix8 ret = fix8_sdiv(value, fix8_from_dbl(other)); return ret; } 105 | const Fix8 sdiv(const float other) const { Fix8 ret = fix8_sdiv(value, fix8_from_float(other)); return ret; } 106 | const Fix8 sdiv(const int8_t other) const { Fix8 ret = fix8_sdiv(value, fix8_from_int(other)); return ret; } 107 | #endif 108 | 109 | const int operator==(const Fix8 &other) const { return (value == other.value); } 110 | const int operator==(const fix8_t other) const { return (value == other); } 111 | const int operator==(const double other) const { return (value == fix8_from_dbl(other)); } 112 | const int operator==(const float other) const { return (value == fix8_from_float(other)); } 113 | const int operator==(const int8_t other) const { return (value == fix8_from_int(other)); } 114 | 115 | const int operator!=(const Fix8 &other) const { return (value != other.value); } 116 | const int operator!=(const fix8_t other) const { return (value != other); } 117 | const int operator!=(const double other) const { return (value != fix8_from_dbl(other)); } 118 | const int operator!=(const float other) const { return (value != fix8_from_float(other)); } 119 | const int operator!=(const int8_t other) const { return (value != fix8_from_int(other)); } 120 | 121 | const int operator<=(const Fix8 &other) const { return (value <= other.value); } 122 | const int operator<=(const fix8_t other) const { return (value <= other); } 123 | const int operator<=(const double other) const { return (value <= fix8_from_dbl(other)); } 124 | const int operator<=(const float other) const { return (value <= fix8_from_float(other)); } 125 | const int operator<=(const int8_t other) const { return (value <= fix8_from_int(other)); } 126 | 127 | const int operator>=(const Fix8 &other) const { return (value >= other.value); } 128 | const int operator>=(const fix8_t other) const { return (value >= other); } 129 | const int operator>=(const double other) const { return (value >= fix8_from_dbl(other)); } 130 | const int operator>=(const float other) const { return (value >= fix8_from_float(other)); } 131 | const int operator>=(const int8_t other) const { return (value >= fix8_from_int(other)); } 132 | 133 | const int operator< (const Fix8 &other) const { return (value < other.value); } 134 | const int operator< (const fix8_t other) const { return (value < other); } 135 | const int operator< (const double other) const { return (value < fix8_from_dbl(other)); } 136 | const int operator< (const float other) const { return (value < fix8_from_float(other)); } 137 | const int operator< (const int8_t other) const { return (value < fix8_from_int(other)); } 138 | 139 | const int operator> (const Fix8 &other) const { return (value > other.value); } 140 | const int operator> (const fix8_t other) const { return (value > other); } 141 | const int operator> (const double other) const { return (value > fix8_from_dbl(other)); } 142 | const int operator> (const float other) const { return (value > fix8_from_float(other)); } 143 | const int operator> (const int8_t other) const { return (value > fix8_from_int(other)); } 144 | /* 145 | Fix8 sin() { return Fix8(fix8_sin(value)); } 146 | Fix8 cos() { return Fix8(fix8_cos(value)); } 147 | Fix8 tan() { return Fix8(fix8_tan(value)); } 148 | Fix8 asin() { return Fix8(fix8_asin(value)); } 149 | Fix8 acos() { return Fix8(fix8_acos(value)); } 150 | Fix8 atan() { return Fix8(fix8_atan(value)); } 151 | Fix8 atan2(const Fix8 &inY) { return Fix8(fix8_atan2(value, inY.value)); } 152 | Fix8 sqrt() { return Fix8(fix8_sqrt(value)); } 153 | */ 154 | }; 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /fix16.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fix16_hpp__ 2 | #define __libfixmath_fix16_hpp__ 3 | 4 | #include "fix16.h" 5 | 6 | class Fix16 { 7 | public: 8 | fix16_t value; 9 | 10 | Fix16() { value = 0; } 11 | Fix16(const Fix16 &inValue) { value = inValue.value; } 12 | Fix16(const fix16_t inValue) { value = inValue; } 13 | Fix16(const float inValue) { value = fix16_from_float(inValue); } 14 | Fix16(const double inValue) { value = fix16_from_dbl(inValue); } 15 | Fix16(const int16_t inValue) { value = fix16_from_int(inValue); } 16 | 17 | operator fix16_t() const { return value; } 18 | operator double() const { return fix16_to_dbl(value); } 19 | operator float() const { return fix16_to_float(value); } 20 | operator int16_t() const { return fix16_to_int(value); } 21 | operator bool() const { return bool(value); } 22 | 23 | Fix16 & operator=(const Fix16 &rhs) { value = rhs.value; return *this; } 24 | Fix16 & operator=(const fix16_t rhs) { value = rhs; return *this; } 25 | Fix16 & operator=(const double rhs) { value = fix16_from_dbl(rhs); return *this; } 26 | Fix16 & operator=(const float rhs) { value = fix16_from_float(rhs); return *this; } 27 | Fix16 & operator=(const int16_t rhs) { value = fix16_from_int(rhs); return *this; } 28 | 29 | Fix16 & operator+=(const Fix16 &rhs) { value = fix16_add(value, rhs.value); return *this; } 30 | Fix16 & operator+=(const fix16_t rhs) { value = fix16_add(value, rhs); return *this; } 31 | Fix16 & operator+=(const double rhs) { value = fix16_add(value, fix16_from_dbl(rhs)); return *this; } 32 | Fix16 & operator+=(const float rhs) { value = fix16_add(value, fix16_from_float(rhs)); return *this; } 33 | Fix16 & operator+=(const int16_t rhs) { value = fix16_add(value, fix16_from_int(rhs)); return *this; } 34 | 35 | Fix16 & operator-=(const Fix16 &rhs) { value = fix16_sub(value, rhs.value); return *this; } 36 | Fix16 & operator-=(const fix16_t rhs) { value = fix16_sub(value, rhs); return *this; } 37 | Fix16 & operator-=(const double rhs) { value = fix16_sub(value, fix16_from_dbl(rhs)); return *this; } 38 | Fix16 & operator-=(const float rhs) { value = fix16_sub(value, fix16_from_float(rhs)); return *this; } 39 | Fix16 & operator-=(const int16_t rhs) { value = fix16_sub(value, fix16_from_int(rhs)); return *this; } 40 | 41 | Fix16 & operator*=(const Fix16 &rhs) { value = fix16_mul(value, rhs.value); return *this; } 42 | Fix16 & operator*=(const fix16_t rhs) { value = fix16_mul(value, rhs); return *this; } 43 | Fix16 & operator*=(const double rhs) { value = fix16_mul(value, fix16_from_dbl(rhs)); return *this; } 44 | Fix16 & operator*=(const float rhs) { value = fix16_mul(value, fix16_from_float(rhs)); return *this; } 45 | Fix16 & operator*=(const int16_t rhs) { value = fix16_mul(value, fix16_from_int(rhs)); return *this; } 46 | 47 | Fix16 & operator/=(const Fix16 &rhs) { value = fix16_div(value, rhs.value); return *this; } 48 | Fix16 & operator/=(const fix16_t rhs) { value = fix16_div(value, rhs); return *this; } 49 | Fix16 & operator/=(const double rhs) { value = fix16_div(value, fix16_from_dbl(rhs)); return *this; } 50 | Fix16 & operator/=(const float rhs) { value = fix16_div(value, fix16_from_float(rhs)); return *this; } 51 | Fix16 & operator/=(const int16_t rhs) { value = fix16_div(value, fix16_from_int(rhs)); return *this; } 52 | 53 | const Fix16 operator+(const Fix16 &other) const { Fix16 ret = *this; ret += other; return ret; } 54 | const Fix16 operator+(const fix16_t other) const { Fix16 ret = *this; ret += other; return ret; } 55 | const Fix16 operator+(const double other) const { Fix16 ret = *this; ret += other; return ret; } 56 | const Fix16 operator+(const float other) const { Fix16 ret = *this; ret += other; return ret; } 57 | const Fix16 operator+(const int16_t other) const { Fix16 ret = *this; ret += other; return ret; } 58 | 59 | #ifndef FIXMATH_NO_OVERFLOW 60 | const Fix16 sadd(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, other.value); return ret; } 61 | const Fix16 sadd(const fix16_t other) const { Fix16 ret = fix16_sadd(value, other); return ret; } 62 | const Fix16 sadd(const double other) const { Fix16 ret = fix16_sadd(value, fix16_from_dbl(other)); return ret; } 63 | const Fix16 sadd(const float other) const { Fix16 ret = fix16_sadd(value, fix16_from_float(other)); return ret; } 64 | const Fix16 sadd(const int16_t other) const { Fix16 ret = fix16_sadd(value, fix16_from_int(other)); return ret; } 65 | #endif 66 | 67 | const Fix16 operator-(const Fix16 &other) const { Fix16 ret = *this; ret -= other; return ret; } 68 | const Fix16 operator-(const fix16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 69 | const Fix16 operator-(const double other) const { Fix16 ret = *this; ret -= other; return ret; } 70 | const Fix16 operator-(const float other) const { Fix16 ret = *this; ret -= other; return ret; } 71 | const Fix16 operator-(const int16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 72 | 73 | #ifndef FIXMATH_NO_OVERFLOW 74 | const Fix16 ssub(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, -other.value); return ret; } 75 | const Fix16 ssub(const fix16_t other) const { Fix16 ret = fix16_sadd(value, -other); return ret; } 76 | const Fix16 ssub(const double other) const { Fix16 ret = fix16_sadd(value, -fix16_from_dbl(other)); return ret; } 77 | const Fix16 ssub(const float other) const { Fix16 ret = fix16_sadd(value, -fix16_from_float(other)); return ret; } 78 | const Fix16 ssub(const int16_t other) const { Fix16 ret = fix16_sadd(value, -fix16_from_int(other)); return ret; } 79 | #endif 80 | 81 | const Fix16 operator*(const Fix16 &other) const { Fix16 ret = *this; ret *= other; return ret; } 82 | const Fix16 operator*(const fix16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 83 | const Fix16 operator*(const double other) const { Fix16 ret = *this; ret *= other; return ret; } 84 | const Fix16 operator*(const float other) const { Fix16 ret = *this; ret *= other; return ret; } 85 | const Fix16 operator*(const int16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 86 | 87 | #ifndef FIXMATH_NO_OVERFLOW 88 | const Fix16 smul(const Fix16 &other) const { Fix16 ret = fix16_smul(value, other.value); return ret; } 89 | const Fix16 smul(const fix16_t other) const { Fix16 ret = fix16_smul(value, other); return ret; } 90 | const Fix16 smul(const double other) const { Fix16 ret = fix16_smul(value, fix16_from_dbl(other)); return ret; } 91 | const Fix16 smul(const float other) const { Fix16 ret = fix16_smul(value, fix16_from_float(other)); return ret; } 92 | const Fix16 smul(const int16_t other) const { Fix16 ret = fix16_smul(value, fix16_from_int(other)); return ret; } 93 | #endif 94 | 95 | const Fix16 operator/(const Fix16 &other) const { Fix16 ret = *this; ret /= other; return ret; } 96 | const Fix16 operator/(const fix16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 97 | const Fix16 operator/(const double other) const { Fix16 ret = *this; ret /= other; return ret; } 98 | const Fix16 operator/(const float other) const { Fix16 ret = *this; ret /= other; return ret; } 99 | const Fix16 operator/(const int16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 100 | 101 | #ifndef FIXMATH_NO_OVERFLOW 102 | const Fix16 sdiv(const Fix16 &other) const { Fix16 ret = fix16_sdiv(value, other.value); return ret; } 103 | const Fix16 sdiv(const fix16_t other) const { Fix16 ret = fix16_sdiv(value, other); return ret; } 104 | const Fix16 sdiv(const double other) const { Fix16 ret = fix16_sdiv(value, fix16_from_dbl(other)); return ret; } 105 | const Fix16 sdiv(const float other) const { Fix16 ret = fix16_sdiv(value, fix16_from_float(other)); return ret; } 106 | const Fix16 sdiv(const int16_t other) const { Fix16 ret = fix16_sdiv(value, fix16_from_int(other)); return ret; } 107 | #endif 108 | 109 | const int operator==(const Fix16 &other) const { return (value == other.value); } 110 | const int operator==(const fix16_t other) const { return (value == other); } 111 | const int operator==(const double other) const { return (value == fix16_from_dbl(other)); } 112 | const int operator==(const float other) const { return (value == fix16_from_float(other)); } 113 | const int operator==(const int16_t other) const { return (value == fix16_from_int(other)); } 114 | 115 | const int operator!=(const Fix16 &other) const { return (value != other.value); } 116 | const int operator!=(const fix16_t other) const { return (value != other); } 117 | const int operator!=(const double other) const { return (value != fix16_from_dbl(other)); } 118 | const int operator!=(const float other) const { return (value != fix16_from_float(other)); } 119 | const int operator!=(const int16_t other) const { return (value != fix16_from_int(other)); } 120 | 121 | const int operator<=(const Fix16 &other) const { return (value <= other.value); } 122 | const int operator<=(const fix16_t other) const { return (value <= other); } 123 | const int operator<=(const double other) const { return (value <= fix16_from_dbl(other)); } 124 | const int operator<=(const float other) const { return (value <= fix16_from_float(other)); } 125 | const int operator<=(const int16_t other) const { return (value <= fix16_from_int(other)); } 126 | 127 | const int operator>=(const Fix16 &other) const { return (value >= other.value); } 128 | const int operator>=(const fix16_t other) const { return (value >= other); } 129 | const int operator>=(const double other) const { return (value >= fix16_from_dbl(other)); } 130 | const int operator>=(const float other) const { return (value >= fix16_from_float(other)); } 131 | const int operator>=(const int16_t other) const { return (value >= fix16_from_int(other)); } 132 | 133 | const int operator< (const Fix16 &other) const { return (value < other.value); } 134 | const int operator< (const fix16_t other) const { return (value < other); } 135 | const int operator< (const double other) const { return (value < fix16_from_dbl(other)); } 136 | const int operator< (const float other) const { return (value < fix16_from_float(other)); } 137 | const int operator< (const int16_t other) const { return (value < fix16_from_int(other)); } 138 | 139 | const int operator> (const Fix16 &other) const { return (value > other.value); } 140 | const int operator> (const fix16_t other) const { return (value > other); } 141 | const int operator> (const double other) const { return (value > fix16_from_dbl(other)); } 142 | const int operator> (const float other) const { return (value > fix16_from_float(other)); } 143 | const int operator> (const int16_t other) const { return (value > fix16_from_int(other)); } 144 | 145 | Fix16 sin() { return Fix16(fix16_sin(value)); } 146 | Fix16 cos() { return Fix16(fix16_cos(value)); } 147 | Fix16 tan() { return Fix16(fix16_tan(value)); } 148 | Fix16 asin() { return Fix16(fix16_asin(value)); } 149 | Fix16 acos() { return Fix16(fix16_acos(value)); } 150 | Fix16 atan() { return Fix16(fix16_atan(value)); } 151 | Fix16 atan2(const Fix16 &inY) { return Fix16(fix16_atan2(value, inY.value)); } 152 | Fix16 sqrt() { return Fix16(fix16_sqrt(value)); } 153 | }; 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /fix16.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #if !defined(FIXMATH_NO_64BIT) 3 | #include "int64.h" 4 | #endif 5 | 6 | /* Subtraction and addition with overflow detection. 7 | * The versions without overflow detection are inlined in the header. 8 | */ 9 | #ifndef FIXMATH_NO_OVERFLOW 10 | fix16_t fix16_add(fix16_t a, fix16_t b) 11 | { 12 | // Use unsigned integers because overflow with signed integers is 13 | // an undefined operation (http://www.airs.com/blog/archives/120). 14 | uint32_t _a = a, _b = b; 15 | uint32_t sum = _a + _b; 16 | 17 | // Overflow can only happen if sign of a == sign of b, and then 18 | // it causes sign of sum != sign of a. 19 | if (!((_a ^ _b) & 0x80000000) && ((_a ^ sum) & 0x80000000)) 20 | return fix16_overflow; 21 | 22 | return sum; 23 | } 24 | 25 | fix16_t fix16_sub(fix16_t a, fix16_t b) 26 | { 27 | uint32_t _a = a, _b = b; 28 | uint32_t diff = _a - _b; 29 | 30 | // Overflow can only happen if sign of a != sign of b, and then 31 | // it causes sign of diff != sign of a. 32 | if (((_a ^ _b) & 0x80000000) && ((_a ^ diff) & 0x80000000)) 33 | return fix16_overflow; 34 | 35 | return diff; 36 | } 37 | 38 | /* Saturating arithmetic */ 39 | fix16_t fix16_sadd(fix16_t a, fix16_t b) 40 | { 41 | fix16_t result = fix16_add(a, b); 42 | 43 | if (result == fix16_overflow) 44 | return (a >= 0) ? fix16_maximum : fix16_minimum; 45 | 46 | return result; 47 | } 48 | 49 | fix16_t fix16_ssub(fix16_t a, fix16_t b) 50 | { 51 | fix16_t result = fix16_sub(a, b); 52 | 53 | if (result == fix16_overflow) 54 | return (a >= 0) ? fix16_maximum : fix16_minimum; 55 | 56 | return result; 57 | } 58 | #endif 59 | 60 | 61 | 62 | /* 64-bit implementation for fix16_mul. Fastest version for e.g. ARM Cortex M3. 63 | * Performs a 32*32 -> 64bit multiplication. The middle 32 bits are the result, 64 | * bottom 16 bits are used for rounding, and upper 16 bits are used for overflow 65 | * detection. 66 | */ 67 | 68 | #if !defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 69 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 70 | { 71 | int64_t product = (int64_t)inArg0 * inArg1; 72 | 73 | #ifndef FIXMATH_NO_OVERFLOW 74 | // The upper 17 bits should all be the same (the sign). 75 | uint32_t upper = (product >> 47); 76 | #endif 77 | 78 | if (product < 0) 79 | { 80 | #ifndef FIXMATH_NO_OVERFLOW 81 | if (~upper) 82 | return fix16_overflow; 83 | #endif 84 | 85 | #ifndef FIXMATH_NO_ROUNDING 86 | // This adjustment is required in order to round -1/2 correctly 87 | product--; 88 | #endif 89 | } 90 | else 91 | { 92 | #ifndef FIXMATH_NO_OVERFLOW 93 | if (upper) 94 | return fix16_overflow; 95 | #endif 96 | } 97 | 98 | #ifdef FIXMATH_NO_ROUNDING 99 | return product >> 16; 100 | #else 101 | fix16_t result = product >> 16; 102 | result += (product & 0x8000) >> 15; 103 | 104 | return result; 105 | #endif 106 | } 107 | #endif 108 | 109 | /* 32-bit implementation of fix16_mul. Potentially fast on 16-bit processors, 110 | * and this is a relatively good compromise for compilers that do not support 111 | * uint64_t. Uses 16*16->32bit multiplications. 112 | */ 113 | #if defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 114 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 115 | { 116 | // Each argument is divided to 16-bit parts. 117 | // AB 118 | // * CD 119 | // ----------- 120 | // BD 16 * 16 -> 32 bit products 121 | // CB 122 | // AD 123 | // AC 124 | // |----| 64 bit product 125 | int32_t A = (inArg0 >> 16), C = (inArg1 >> 16); 126 | uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF); 127 | 128 | int32_t AC = A*C; 129 | int32_t AD_CB = A*D + C*B; 130 | uint32_t BD = B*D; 131 | 132 | int32_t product_hi = AC + (AD_CB >> 16); 133 | 134 | // Handle carry from lower 32 bits to upper part of result. 135 | uint32_t ad_cb_temp = AD_CB << 16; 136 | uint32_t product_lo = BD + ad_cb_temp; 137 | if (product_lo < BD) 138 | product_hi++; 139 | 140 | #ifndef FIXMATH_NO_OVERFLOW 141 | // The upper 17 bits should all be the same (the sign). 142 | if (product_hi >> 31 != product_hi >> 15) 143 | return fix16_overflow; 144 | #endif 145 | 146 | #ifdef FIXMATH_NO_ROUNDING 147 | return (product_hi << 16) | (product_lo >> 16); 148 | #else 149 | // Subtracting 0x8000 (= 0.5) and then using signed right shift 150 | // achieves proper rounding to result-1, except in the corner 151 | // case of negative numbers and lowest word = 0x8000. 152 | // To handle that, we also have to subtract 1 for negative numbers. 153 | uint32_t product_lo_tmp = product_lo; 154 | product_lo -= 0x8000; 155 | product_lo -= (uint32_t)product_hi >> 31; 156 | if (product_lo > product_lo_tmp) 157 | product_hi--; 158 | 159 | // Discard the lowest 16 bits. Note that this is not exactly the same 160 | // as dividing by 0x10000. For example if product = -1, result will 161 | // also be -1 and not 0. This is compensated by adding +1 to the result 162 | // and compensating this in turn in the rounding above. 163 | fix16_t result = (product_hi << 16) | (product_lo >> 16); 164 | result += 1; 165 | return result; 166 | #endif 167 | } 168 | #endif 169 | 170 | /* 8-bit implementation of fix16_mul. Fastest on e.g. Atmel AVR. 171 | * Uses 8*8->16bit multiplications, and also skips any bytes that 172 | * are zero. 173 | */ 174 | #if defined(FIXMATH_OPTIMIZE_8BIT) 175 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 176 | { 177 | uint32_t _a = (inArg0 >= 0) ? inArg0 : (-inArg0); 178 | uint32_t _b = (inArg1 >= 0) ? inArg1 : (-inArg1); 179 | 180 | uint8_t va[4] = {_a, (_a >> 8), (_a >> 16), (_a >> 24)}; 181 | uint8_t vb[4] = {_b, (_b >> 8), (_b >> 16), (_b >> 24)}; 182 | 183 | uint32_t low = 0; 184 | uint32_t mid = 0; 185 | 186 | // Result column i depends on va[0..i] and vb[i..0] 187 | 188 | #ifndef FIXMATH_NO_OVERFLOW 189 | // i = 6 190 | if (va[3] && vb[3]) return fix16_overflow; 191 | #endif 192 | 193 | // i = 5 194 | if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3]; 195 | if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2]; 196 | mid <<= 8; 197 | 198 | // i = 4 199 | if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3]; 200 | if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2]; 201 | if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1]; 202 | 203 | #ifndef FIXMATH_NO_OVERFLOW 204 | if (mid & 0xFF000000) return fix16_overflow; 205 | #endif 206 | mid <<= 8; 207 | 208 | // i = 3 209 | if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3]; 210 | if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2]; 211 | if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1]; 212 | if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0]; 213 | 214 | #ifndef FIXMATH_NO_OVERFLOW 215 | if (mid & 0xFF000000) return fix16_overflow; 216 | #endif 217 | mid <<= 8; 218 | 219 | // i = 2 220 | if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2]; 221 | if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1]; 222 | if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0]; 223 | 224 | // i = 1 225 | if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1]; 226 | if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0]; 227 | low <<= 8; 228 | 229 | // i = 0 230 | if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0]; 231 | 232 | #ifndef FIXMATH_NO_ROUNDING 233 | low += 0x8000; 234 | #endif 235 | mid += (low >> 16); 236 | 237 | #ifndef FIXMATH_NO_OVERFLOW 238 | if (mid & 0x80000000) 239 | return fix16_overflow; 240 | #endif 241 | 242 | fix16_t result = mid; 243 | 244 | /* Figure out the sign of result */ 245 | if ((inArg0 >= 0) != (inArg1 >= 0)) 246 | { 247 | result = -result; 248 | } 249 | 250 | return result; 251 | } 252 | #endif 253 | 254 | #ifndef FIXMATH_NO_OVERFLOW 255 | /* Wrapper around fix16_mul to add saturating arithmetic. */ 256 | fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) 257 | { 258 | fix16_t result = fix16_mul(inArg0, inArg1); 259 | 260 | if (result == fix16_overflow) 261 | { 262 | if ((inArg0 >= 0) == (inArg1 >= 0)) 263 | return fix16_maximum; 264 | else 265 | return fix16_minimum; 266 | } 267 | 268 | return result; 269 | } 270 | #endif 271 | 272 | /* 32-bit implementation of fix16_div. Fastest version for e.g. ARM Cortex M3. 273 | * Performs 32-bit divisions repeatedly to reduce the remainder. For this to 274 | * be efficient, the processor has to have 32-bit hardware division. 275 | */ 276 | #if !defined(FIXMATH_OPTIMIZE_8BIT) 277 | #ifdef __GNUC__ 278 | // Count leading zeros, using processor-specific instruction if available. 279 | #define clz(x) (__builtin_clzl(x) - (8 * sizeof(long) - 32)) 280 | #else 281 | static uint8_t clz(uint32_t x) 282 | { 283 | uint8_t result = 0; 284 | if (x == 0) return 32; 285 | while (!(x & 0xF0000000)) { result += 4; x <<= 4; } 286 | while (!(x & 0x80000000)) { result += 1; x <<= 1; } 287 | return result; 288 | } 289 | #endif 290 | 291 | fix16_t fix16_div(fix16_t a, fix16_t b) 292 | { 293 | // This uses a hardware 32/32 bit division multiple times, until we have 294 | // computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations. 295 | 296 | if (b == 0) 297 | return fix16_minimum; 298 | 299 | uint32_t remainder = (a >= 0) ? a : (-a); 300 | uint32_t divider = (b >= 0) ? b : (-b); 301 | uint32_t quotient = 0; 302 | int bit_pos = 17; 303 | 304 | // Kick-start the division a bit. 305 | // This improves speed in the worst-case scenarios where N and D are large 306 | // It gets a lower estimate for the result by N/(D >> 17 + 1). 307 | if (divider & 0xFFF00000) 308 | { 309 | uint32_t shifted_div = ((divider >> 17) + 1); 310 | quotient = remainder / shifted_div; 311 | remainder -= ((uint64_t)quotient * divider) >> 17; 312 | } 313 | 314 | // If the divider is divisible by 2^n, take advantage of it. 315 | while (!(divider & 0xF) && bit_pos >= 4) 316 | { 317 | divider >>= 4; 318 | bit_pos -= 4; 319 | } 320 | 321 | while (remainder && bit_pos >= 0) 322 | { 323 | // Shift remainder as much as we can without overflowing 324 | int shift = clz(remainder); 325 | if (shift > bit_pos) shift = bit_pos; 326 | remainder <<= shift; 327 | bit_pos -= shift; 328 | 329 | uint32_t div = remainder / divider; 330 | remainder = remainder % divider; 331 | quotient += div << bit_pos; 332 | 333 | #ifndef FIXMATH_NO_OVERFLOW 334 | if (div & ~(0xFFFFFFFF >> bit_pos)) 335 | return fix16_overflow; 336 | #endif 337 | 338 | remainder <<= 1; 339 | bit_pos--; 340 | } 341 | 342 | #ifndef FIXMATH_NO_ROUNDING 343 | // Quotient is always positive so rounding is easy 344 | quotient++; 345 | #endif 346 | 347 | fix16_t result = quotient >> 1; 348 | 349 | // Figure out the sign of the result 350 | if ((a ^ b) & 0x80000000) 351 | { 352 | #ifndef FIXMATH_NO_OVERFLOW 353 | if (result == fix16_minimum) 354 | return fix16_overflow; 355 | #endif 356 | 357 | result = -result; 358 | } 359 | 360 | return result; 361 | } 362 | #endif 363 | 364 | /* Alternative 32-bit implementation of fix16_div. Fastest on e.g. Atmel AVR. 365 | * This does the division manually, and is therefore good for processors that 366 | * do not have hardware division. 367 | */ 368 | #if defined(FIXMATH_OPTIMIZE_8BIT) 369 | fix16_t fix16_div(fix16_t a, fix16_t b) 370 | { 371 | // This uses the basic binary restoring division algorithm. 372 | // It appears to be faster to do the whole division manually than 373 | // trying to compose a 64-bit divide out of 32-bit divisions on 374 | // platforms without hardware divide. 375 | 376 | if (b == 0) 377 | return fix16_minimum; 378 | 379 | uint32_t remainder = (a >= 0) ? a : (-a); 380 | uint32_t divider = (b >= 0) ? b : (-b); 381 | 382 | uint32_t quotient = 0; 383 | uint32_t bit = 0x10000; 384 | 385 | /* The algorithm requires D >= R */ 386 | while (divider < remainder) 387 | { 388 | divider <<= 1; 389 | bit <<= 1; 390 | } 391 | 392 | #ifndef FIXMATH_NO_OVERFLOW 393 | if (!bit) 394 | return fix16_overflow; 395 | #endif 396 | 397 | if (divider & 0x80000000) 398 | { 399 | // Perform one step manually to avoid overflows later. 400 | // We know that divider's bottom bit is 0 here. 401 | if (remainder >= divider) 402 | { 403 | quotient |= bit; 404 | remainder -= divider; 405 | } 406 | divider >>= 1; 407 | bit >>= 1; 408 | } 409 | 410 | /* Main division loop */ 411 | while (bit && remainder) 412 | { 413 | if (remainder >= divider) 414 | { 415 | quotient |= bit; 416 | remainder -= divider; 417 | } 418 | 419 | remainder <<= 1; 420 | bit >>= 1; 421 | } 422 | 423 | #ifndef FIXMATH_NO_ROUNDING 424 | if (remainder >= divider) 425 | { 426 | quotient++; 427 | } 428 | #endif 429 | 430 | fix16_t result = quotient; 431 | 432 | /* Figure out the sign of result */ 433 | if ((a ^ b) & 0x80000000) 434 | { 435 | #ifndef FIXMATH_NO_OVERFLOW 436 | if (result == fix16_minimum) 437 | return fix16_overflow; 438 | #endif 439 | 440 | result = -result; 441 | } 442 | 443 | return result; 444 | } 445 | #endif 446 | 447 | #ifndef FIXMATH_NO_OVERFLOW 448 | /* Wrapper around fix16_div to add saturating arithmetic. */ 449 | fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) 450 | { 451 | fix16_t result = fix16_div(inArg0, inArg1); 452 | 453 | if (result == fix16_overflow) 454 | { 455 | if ((inArg0 >= 0) == (inArg1 >= 0)) 456 | return fix16_maximum; 457 | else 458 | return fix16_minimum; 459 | } 460 | 461 | return result; 462 | } 463 | #endif 464 | 465 | fix16_t fix16_mod(fix16_t x, fix16_t y) 466 | { 467 | #ifdef FIXMATH_OPTIMIZE_8BIT 468 | /* The reason we do this, rather than use a modulo operator 469 | * is that if you don't have a hardware divider, this will result 470 | * in faster operations when the angles are close to the bounds. 471 | */ 472 | while(x >= y) x -= y; 473 | while(x <= -y) x += y; 474 | #else 475 | /* Note that in C90, the sign of result of the modulo operation is 476 | * undefined. in C99, it's the same as the dividend (aka numerator). 477 | */ 478 | x %= y; 479 | #endif 480 | 481 | return x; 482 | } 483 | 484 | 485 | #ifndef FIXMATH_NO_64BIT 486 | 487 | fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) 488 | { 489 | int64_t tempOut = int64_mul_i32_i32(inArg0, ((1 << 8) - inFract)); 490 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 491 | tempOut = int64_shift(tempOut, -8); 492 | return (fix16_t)int64_lo(tempOut); 493 | } 494 | 495 | fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) 496 | { 497 | int64_t tempOut = int64_mul_i32_i32(inArg0, (((int32_t)1 << 16) - inFract)); 498 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 499 | tempOut = int64_shift(tempOut, -16); 500 | return (fix16_t)int64_lo(tempOut); 501 | } 502 | 503 | fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) 504 | { 505 | int64_t tempOut; 506 | tempOut = ((int64_t)inArg0 * (0 - inFract)); 507 | tempOut += ((int64_t)inArg1 * inFract); 508 | tempOut >>= 32; 509 | return (fix16_t)tempOut; 510 | } 511 | #endif 512 | --------------------------------------------------------------------------------