├── uint32.h ├── fixmath.h ├── uint32.c ├── README.md ├── fract32.c ├── LICENSE ├── Makefile ├── fract32.h ├── libfixmath.cbp ├── fix16_sqrt.c ├── fix16_str.c ├── int64.h ├── fix16_exp.c ├── fix16_trig.c ├── code_style.txt ├── fix16.hpp ├── fix16.h └── fix16.c /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # libfixmath # 2 | ## Cross Platform Fixed Point Maths Library ## 3 | 4 | This repository is a mirror of the [libfixmath](https://code.google.com/p/libfixmath/) repository at [https://code.google.com/p/libfixmath/](https://code.google.com/p/libfixmath/), which is released under an MIT license. 5 | 6 | ### Branch information### 7 | 8 | This is a clone of the [trunk/libfixmath](http://libfixmath.googlecode.com/svn/trunk/libfixmath) branch. The purpose of this mirror is to provide an easy way to use libfixmath as a git submodule in other projects. 9 | 10 | ### Documentation ### 11 | 12 | For the documentation of libfixmath, please see [https://code.google.com/p/libfixmath/w/list](https://code.google.com/p/libfixmath/w/list). -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) the authors of libfixmath as seen on https://code.google.com/p/libfixmath/ 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. -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | #Project settings 2 | PROJECT = libfixmath 3 | LIB = 4 | SRC = . 5 | INC = 6 | 7 | #Compiler settings 8 | CPP = gcc 9 | CC = gcc 10 | AS = gcc 11 | LD = gcc 12 | AR = ar 13 | CPP_FLAGS = -O2 $(INC) -Wall -Wextra -c 14 | CC_FLAGS = -O2 $(INC) -Wall -Wextra -c 15 | AS_FLAGS = $(CC_FLAGS) -D_ASSEMBLER_ 16 | LD_FLAGS = -Wall 17 | 18 | # Find all source files 19 | SRC_CPP = $(foreach dir, $(SRC), $(wildcard $(dir)/*.cpp)) 20 | SRC_C = $(foreach dir, $(SRC), $(wildcard $(dir)/*.c)) 21 | SRC_S = $(foreach dir, $(SRC), $(wildcard $(dir)/*.S)) 22 | OBJ_CPP = $(patsubst %.cpp, %.o, $(SRC_CPP)) 23 | OBJ_C = $(patsubst %.c, %.o, $(SRC_C)) 24 | OBJ_S = $(patsubst %.S, %.o, $(SRC_S)) 25 | OBJ = $(OBJ_CPP) $(OBJ_C) $(OBJ_S) 26 | 27 | # Compile rules. 28 | .PHONY : all 29 | all: $(PROJECT).a 30 | 31 | $(PROJECT).a: $(OBJ) 32 | $(AR) rcs $(PROJECT).a $(OBJ) 33 | 34 | $(OBJ_CPP) : %.o : %.cpp 35 | $(CPP) $(CPP_FLAGS) -o $@ $< 36 | 37 | $(OBJ_C) : %.o : %.c 38 | $(CC) $(CC_FLAGS) -o $@ $< 39 | 40 | $(OBJ_S) : %.o : %.S 41 | $(AS) $(AS_FLAGS) -o $@ $< 42 | 43 | 44 | 45 | # Clean rules 46 | .PHONY : clean 47 | clean: 48 | rm -f $(PROJECT).a $(OBJ) 49 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /libfixmath.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 71 | 72 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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_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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /code_style.txt: -------------------------------------------------------------------------------- 1 | 1.0 - BRACE CONVENTION 2 | 3 | Braces always occupy a new line exactly below the opening statement. 4 | Braces are not expected when the body is a single statement. 5 | If one part of an if-else construct uses braces then all should, 6 | even if they're single statements. 7 | 8 | Example: 9 | 0: 10 | if (condition) 11 | { 12 | // body 13 | } 14 | else 15 | { 16 | // else-body 17 | } 18 | 19 | 1: 20 | unsigned max(unsigned x, unsigned y) 21 | { 22 | if (x > y) return x; 23 | return y; 24 | } 25 | 26 | 2: 27 | unsigned array[] = 28 | { 29 | 0, 1, 30 | 2, 3, 31 | }; 32 | 33 | This follows for every use of a brace. 34 | 35 | Exceptions: 36 | 0: 37 | static inline max(unsigned x, unsigned y) 38 | { return (x > y ? x : y); } 39 | 40 | 1: 41 | unsigned array[] = { 0, 1, 2, 3 }; 42 | 43 | 44 | 45 | 2.0 - BRACKET SPACING 46 | 47 | All control keywords should have a space between the keyword and the opening bracket. 48 | Function calls and definitions will never have a space between the name and bracket. 49 | 50 | Example: 51 | 0: if (condition) 52 | 1: x = min(a, b); 53 | 54 | Counter-example: 55 | 0: if(condition) 56 | 1: x = min (a, b); 57 | 58 | 59 | 60 | 3.0 - OPERATOR SPACING 61 | 62 | Binary operators always have a single space either side. 63 | Unary operators never have a space between them and their value. 64 | Assignments are treated as binary operators. 65 | 66 | Example: 67 | 0: a = b + 5; 68 | 1: a++; 69 | 70 | Counter-example: 71 | 0: a=b+5; 72 | 1: a ++; 73 | 74 | 75 | 76 | 4.0 - OPERATOR PRECIDENCE 77 | 78 | Where operators with different precidence are used together you must always 79 | use brackets to make the precidence explicit. 80 | Never rely on the compilers precidence rules as readers will forget this. 81 | 82 | Example: 83 | 0: y = (m * x) + a; 84 | 1: if ((a || b) && c) 85 | 86 | Counter-example: 87 | 0: y = m * x + a; 88 | 1: if (a || b && c) 89 | 90 | 91 | 92 | 5.0 - INDENTATION AND WHITESPACE 93 | 94 | Tabs must ALWAYS be used for nesting/indentation. 95 | Spaces must ALWAYS be used for alignment. 96 | 97 | Example: 98 | 0: 99 | if (condition) 100 | return value; 101 | 1: 102 | int mins(int x, int y); 103 | unsigned minu(unsigned x, unsigned y); 104 | 105 | 106 | 107 | 6.0 - ADVANCED CONTROL FLOW 108 | 109 | If the body of an if statement causes the function to return 110 | or control structure to break then there musn't be an else clause. 111 | 112 | Example: 113 | 0: 114 | if (condition) 115 | return a; 116 | return b; 117 | 118 | Counter-example: 119 | 0: 120 | if (condition) 121 | return a; 122 | else 123 | return b; 124 | 125 | 126 | 127 | 7.0 - LINE WIDTH 128 | 129 | A line must not exceed 80 characters wide, this applies universally including 130 | comments defines, etc. 131 | 132 | There are multiple valid points to break a line. 133 | - On a binary operator, in which case the binary operator follows on the 134 | next line with an indent. 135 | - On a function definition/implementation the parameters may go on another 136 | line, in which case they are indented. In this case the params should 137 | be split sensibly and are aligned by the fact that they're all 138 | indented to the same point. 139 | 140 | Examples: 141 | 0: 142 | some_really_long_variable_name 143 | = quite_a_long_value; 144 | 1: 145 | if (really_really_really_long_clause_a 146 | && (really_really_really_long_clause_b 147 | || really_really_really_long_clause_c) 148 | && really_really_really_long_clause_d 149 | && really_really_really_long_clause_e) 150 | 2: 151 | static inline const unsigned long int really_long_function( 152 | unsigned param_a, 153 | int param_b, 154 | bool param_c); 155 | 156 | 157 | 158 | 8.0 - COMMENTS 159 | 160 | All comments must use the classic C comment styles and preceed what they're 161 | commenting on. 162 | All comments must be in plain english starting with a capital letter 163 | and finishing with a full stop. 164 | 165 | Example: 166 | 0: 167 | /* Single line comment about a. */ 168 | a = 0; 169 | 1: 170 | /* Multi-line comment 171 | * about the code below. */ 172 | a = 0; 173 | 174 | 175 | 176 | 9.0 - LABELS AND GOTO 177 | 178 | It is advised quite heavily that you don't use the GOTO keyword and prefer other 179 | more readable control structures. 180 | 181 | 182 | 183 | 10.0 - STRUCTURES, UNIONS, ENUMS, BITFIELDS and TYPEDEFS 184 | 185 | Suffixes must be used for the various types: 186 | _s - struct names 187 | _u - union names 188 | _t - typedef'd struct/union names 189 | _e - enums (and typedef'd enums) "_e" 190 | Enums must always be typedef'd. 191 | Enum values must be prefixed by the enum name. 192 | Structs which rely on ordering must use __attribute__((__packed__)). 193 | Unions must use __attribute__((__packed__)). 194 | Bitfields: 195 | - must always use __attribute__((__packed__)). 196 | - must always follow the GCC rules regarding bitfields. 197 | i.e. first field occupies low bits. 198 | - field size must be described for every member of a bit-field. 199 | - members must never be larger than 32 bits. 200 | 201 | Examples: 202 | 0: 203 | typedef enum 204 | { 205 | fuzzy_no = 0, 206 | fuzzy_yes, 207 | fuzzy_maybe, 208 | } fuzzy_e; 209 | 1: 210 | struct vector_s 211 | { float x, y, z, w; }; 212 | typedef struct vector_s vector_t; 213 | 2: 214 | union types_u 215 | __attribute__((__packed__)) 216 | { 217 | int i; 218 | unsigned u; 219 | float f; 220 | bool b; 221 | }; 222 | typedef union types_u types_t; 223 | 3: 224 | typedef struct 225 | { 226 | bool vflip : 1; 227 | bool hflip : 1; 228 | unsigned reserved : 2; 229 | unsigned alpha : 4; 230 | } img_desc_t; 231 | 232 | 233 | 234 | 11.0 - MACROS 235 | 236 | Macros should only be used for condtional compilation and header protection 237 | where this is possible. 238 | For function like macros static inline functions should be defined. 239 | For variable like macros static const variables should be defined. 240 | For enums the enum type should always be used. 241 | 242 | Examples: 243 | 0: 244 | static const unsigned max(unsigned x, unsigned y) 245 | { return (x > y ? x : y); } 246 | 1: 247 | static const float pi = 3.14152f; 248 | 2: 249 | typedef enum 250 | { 251 | fuzzy_no = 0, 252 | fuzzy_yes, 253 | fuzzy_maybe, 254 | } fuzzy_e; 255 | 256 | Counter-examples: 257 | 0: 258 | #define MAX(x, y) ((x) > (y) ? (x) : (y)) 259 | 1: 260 | #define PI 3.14152 261 | 2: 262 | #define FUZZY_NO 0 263 | #define FUZZY_YES 1 264 | #define FUZZY_MAYBE 2 265 | -------------------------------------------------------------------------------- /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 | 22 | Fix16 & operator=(const Fix16 &rhs) { value = rhs.value; return *this; } 23 | Fix16 & operator=(const fix16_t rhs) { value = rhs; return *this; } 24 | Fix16 & operator=(const double rhs) { value = fix16_from_dbl(rhs); return *this; } 25 | Fix16 & operator=(const float rhs) { value = fix16_from_float(rhs); return *this; } 26 | Fix16 & operator=(const int16_t rhs) { value = fix16_from_int(rhs); return *this; } 27 | 28 | Fix16 & operator+=(const Fix16 &rhs) { value += rhs.value; return *this; } 29 | Fix16 & operator+=(const fix16_t rhs) { value += rhs; return *this; } 30 | Fix16 & operator+=(const double rhs) { value += fix16_from_dbl(rhs); return *this; } 31 | Fix16 & operator+=(const float rhs) { value += fix16_from_float(rhs); return *this; } 32 | Fix16 & operator+=(const int16_t rhs) { value += fix16_from_int(rhs); return *this; } 33 | 34 | Fix16 & operator-=(const Fix16 &rhs) { value -= rhs.value; return *this; } 35 | Fix16 & operator-=(const fix16_t rhs) { value -= rhs; return *this; } 36 | Fix16 & operator-=(const double rhs) { value -= fix16_from_dbl(rhs); return *this; } 37 | Fix16 & operator-=(const float rhs) { value -= fix16_from_float(rhs); return *this; } 38 | Fix16 & operator-=(const int16_t rhs) { value -= fix16_from_int(rhs); return *this; } 39 | 40 | Fix16 & operator*=(const Fix16 &rhs) { value = fix16_mul(value, rhs.value); return *this; } 41 | Fix16 & operator*=(const fix16_t rhs) { value = fix16_mul(value, rhs); return *this; } 42 | Fix16 & operator*=(const double rhs) { value = fix16_mul(value, fix16_from_dbl(rhs)); return *this; } 43 | Fix16 & operator*=(const float rhs) { value = fix16_mul(value, fix16_from_float(rhs)); return *this; } 44 | Fix16 & operator*=(const int16_t rhs) { value *= rhs; return *this; } 45 | 46 | Fix16 & operator/=(const Fix16 &rhs) { value = fix16_div(value, rhs.value); return *this; } 47 | Fix16 & operator/=(const fix16_t rhs) { value = fix16_div(value, rhs); return *this; } 48 | Fix16 & operator/=(const double rhs) { value = fix16_div(value, fix16_from_dbl(rhs)); return *this; } 49 | Fix16 & operator/=(const float rhs) { value = fix16_div(value, fix16_from_float(rhs)); return *this; } 50 | Fix16 & operator/=(const int16_t rhs) { value /= rhs; return *this; } 51 | 52 | const Fix16 operator+(const Fix16 &other) const { Fix16 ret = *this; ret += other; return ret; } 53 | const Fix16 operator+(const fix16_t other) const { Fix16 ret = *this; ret += other; return ret; } 54 | const Fix16 operator+(const double other) const { Fix16 ret = *this; ret += other; return ret; } 55 | const Fix16 operator+(const float other) const { Fix16 ret = *this; ret += other; return ret; } 56 | const Fix16 operator+(const int16_t other) const { Fix16 ret = *this; ret += other; return ret; } 57 | 58 | #ifndef FIXMATH_NO_OVERFLOW 59 | const Fix16 sadd(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, other.value); return ret; } 60 | const Fix16 sadd(const fix16_t other) const { Fix16 ret = fix16_sadd(value, other); return ret; } 61 | const Fix16 sadd(const double other) const { Fix16 ret = fix16_sadd(value, fix16_from_dbl(other)); return ret; } 62 | const Fix16 sadd(const float other) const { Fix16 ret = fix16_sadd(value, fix16_from_float(other)); return ret; } 63 | const Fix16 sadd(const int16_t other) const { Fix16 ret = fix16_sadd(value, fix16_from_int(other)); return ret; } 64 | #endif 65 | 66 | const Fix16 operator-(const Fix16 &other) const { Fix16 ret = *this; ret -= other; return ret; } 67 | const Fix16 operator-(const fix16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 68 | const Fix16 operator-(const double other) const { Fix16 ret = *this; ret -= other; return ret; } 69 | const Fix16 operator-(const float other) const { Fix16 ret = *this; ret -= other; return ret; } 70 | const Fix16 operator-(const int16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 71 | 72 | #ifndef FIXMATH_NO_OVERFLOW 73 | const Fix16 ssub(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, -other.value); return ret; } 74 | const Fix16 ssub(const fix16_t other) const { Fix16 ret = fix16_sadd(value, -other); return ret; } 75 | const Fix16 ssub(const double other) const { Fix16 ret = fix16_sadd(value, -fix16_from_dbl(other)); return ret; } 76 | const Fix16 ssub(const float other) const { Fix16 ret = fix16_sadd(value, -fix16_from_float(other)); return ret; } 77 | const Fix16 ssub(const int16_t other) const { Fix16 ret = fix16_sadd(value, -fix16_from_int(other)); return ret; } 78 | #endif 79 | 80 | const Fix16 operator*(const Fix16 &other) const { Fix16 ret = *this; ret *= other; return ret; } 81 | const Fix16 operator*(const fix16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 82 | const Fix16 operator*(const double other) const { Fix16 ret = *this; ret *= other; return ret; } 83 | const Fix16 operator*(const float other) const { Fix16 ret = *this; ret *= other; return ret; } 84 | const Fix16 operator*(const int16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 85 | 86 | #ifndef FIXMATH_NO_OVERFLOW 87 | const Fix16 smul(const Fix16 &other) const { Fix16 ret = fix16_smul(value, other.value); return ret; } 88 | const Fix16 smul(const fix16_t other) const { Fix16 ret = fix16_smul(value, other); return ret; } 89 | const Fix16 smul(const double other) const { Fix16 ret = fix16_smul(value, fix16_from_dbl(other)); return ret; } 90 | const Fix16 smul(const float other) const { Fix16 ret = fix16_smul(value, fix16_from_float(other)); return ret; } 91 | const Fix16 smul(const int16_t other) const { Fix16 ret = fix16_smul(value, fix16_from_int(other)); return ret; } 92 | #endif 93 | 94 | const Fix16 operator/(const Fix16 &other) const { Fix16 ret = *this; ret /= other; return ret; } 95 | const Fix16 operator/(const fix16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 96 | const Fix16 operator/(const double other) const { Fix16 ret = *this; ret /= other; return ret; } 97 | const Fix16 operator/(const float other) const { Fix16 ret = *this; ret /= other; return ret; } 98 | const Fix16 operator/(const int16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 99 | 100 | #ifndef FIXMATH_NO_OVERFLOW 101 | const Fix16 sdiv(const Fix16 &other) const { Fix16 ret = fix16_sdiv(value, other.value); return ret; } 102 | const Fix16 sdiv(const fix16_t other) const { Fix16 ret = fix16_sdiv(value, other); return ret; } 103 | const Fix16 sdiv(const double other) const { Fix16 ret = fix16_sdiv(value, fix16_from_dbl(other)); return ret; } 104 | const Fix16 sdiv(const float other) const { Fix16 ret = fix16_sdiv(value, fix16_from_float(other)); return ret; } 105 | const Fix16 sdiv(const int16_t other) const { Fix16 ret = fix16_sdiv(value, fix16_from_int(other)); return ret; } 106 | #endif 107 | 108 | const int operator==(const Fix16 &other) const { return (value == other.value); } 109 | const int operator==(const fix16_t other) const { return (value == other); } 110 | const int operator==(const double other) const { return (value == fix16_from_dbl(other)); } 111 | const int operator==(const float other) const { return (value == fix16_from_float(other)); } 112 | const int operator==(const int16_t other) const { return (value == fix16_from_int(other)); } 113 | 114 | const int operator!=(const Fix16 &other) const { return (value != other.value); } 115 | const int operator!=(const fix16_t other) const { return (value != other); } 116 | const int operator!=(const double other) const { return (value != fix16_from_dbl(other)); } 117 | const int operator!=(const float other) const { return (value != fix16_from_float(other)); } 118 | const int operator!=(const int16_t other) const { return (value != fix16_from_int(other)); } 119 | 120 | const int operator<=(const Fix16 &other) const { return (value <= other.value); } 121 | const int operator<=(const fix16_t other) const { return (value <= other); } 122 | const int operator<=(const double other) const { return (value <= fix16_from_dbl(other)); } 123 | const int operator<=(const float other) const { return (value <= fix16_from_float(other)); } 124 | const int operator<=(const int16_t other) const { return (value <= fix16_from_int(other)); } 125 | 126 | const int operator>=(const Fix16 &other) const { return (value >= other.value); } 127 | const int operator>=(const fix16_t other) const { return (value >= other); } 128 | const int operator>=(const double other) const { return (value >= fix16_from_dbl(other)); } 129 | const int operator>=(const float other) const { return (value >= fix16_from_float(other)); } 130 | const int operator>=(const int16_t other) const { return (value >= fix16_from_int(other)); } 131 | 132 | const int operator< (const Fix16 &other) const { return (value < other.value); } 133 | const int operator< (const fix16_t other) const { return (value < other); } 134 | const int operator< (const double other) const { return (value < fix16_from_dbl(other)); } 135 | const int operator< (const float other) const { return (value < fix16_from_float(other)); } 136 | const int operator< (const int16_t other) const { return (value < fix16_from_int(other)); } 137 | 138 | const int operator> (const Fix16 &other) const { return (value > other.value); } 139 | const int operator> (const fix16_t other) const { return (value > other); } 140 | const int operator> (const double other) const { return (value > fix16_from_dbl(other)); } 141 | const int operator> (const float other) const { return (value > fix16_from_float(other)); } 142 | const int operator> (const int16_t other) const { return (value > fix16_from_int(other)); } 143 | 144 | Fix16 sin() { return Fix16(fix16_sin(value)); } 145 | Fix16 cos() { return Fix16(fix16_cos(value)); } 146 | Fix16 tan() { return Fix16(fix16_tan(value)); } 147 | Fix16 asin() { return Fix16(fix16_asin(value)); } 148 | Fix16 acos() { return Fix16(fix16_acos(value)); } 149 | Fix16 atan() { return Fix16(fix16_atan(value)); } 150 | Fix16 atan2(const Fix16 &inY) { return Fix16(fix16_atan2(value, inY.value)); } 151 | Fix16 sqrt() { return Fix16(fix16_sqrt(value)); } 152 | }; 153 | 154 | #endif 155 | -------------------------------------------------------------------------------- /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 25 | 26 | typedef int32_t fix16_t; 27 | 28 | static const fix16_t FOUR_DIV_PI = 0x145F3; /*!< Fix16 value of 4/PI */ 29 | static const fix16_t _FOUR_DIV_PI2 = 0xFFFF9840; /*!< Fix16 value of -4/PI² */ 30 | static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.225 */ 31 | static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */ 32 | static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */ 33 | 34 | static const fix16_t fix16_maximum = 0x7FFFFFFF; /*!< the maximum value of fix16_t */ 35 | static const fix16_t fix16_minimum = 0x80000000; /*!< the minimum value of fix16_t */ 36 | static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */ 37 | 38 | static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */ 39 | static const fix16_t fix16_e = 178145; /*!< fix16_t value of e */ 40 | static const fix16_t fix16_one = 0x00010000; /*!< fix16_t value of 1 */ 41 | 42 | /* Conversion functions between fix16_t and float/integer. 43 | * These are inlined to allow compiler to optimize away constant numbers 44 | */ 45 | static inline fix16_t fix16_from_int(int a) { return a * fix16_one; } 46 | static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; } 47 | static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; } 48 | 49 | static inline int fix16_to_int(fix16_t a) 50 | { 51 | #ifdef FIXMATH_NO_ROUNDING 52 | return (a >> 16); 53 | #else 54 | if (a >= 0) 55 | return (a + (fix16_one >> 1)) / fix16_one; 56 | return (a - (fix16_one >> 1)) / fix16_one; 57 | #endif 58 | } 59 | 60 | static inline fix16_t fix16_from_float(float a) 61 | { 62 | float temp = a * fix16_one; 63 | #ifndef FIXMATH_NO_ROUNDING 64 | temp += (temp >= 0) ? 0.5f : -0.5f; 65 | #endif 66 | return (fix16_t)temp; 67 | } 68 | 69 | static inline fix16_t fix16_from_dbl(double a) 70 | { 71 | double temp = a * fix16_one; 72 | #ifndef FIXMATH_NO_ROUNDING 73 | temp += (temp >= 0) ? 0.5f : -0.5f; 74 | #endif 75 | return (fix16_t)temp; 76 | } 77 | 78 | /* Macro for defining fix16_t constant values. 79 | The functions above can't be used from e.g. global variable initializers, 80 | and their names are quite long also. This macro is useful for constants 81 | springled alongside code, e.g. F16(1.234). 82 | 83 | Note that the argument is evaluated multiple times, and also otherwise 84 | you should only use this for constant values. For runtime-conversions, 85 | use the functions above. 86 | */ 87 | #define F16(x) ((fix16_t)(((x) >= 0) ? ((x) * 65536.0 + 0.5) : ((x) * 65536.0 - 0.5))) 88 | 89 | static inline fix16_t fix16_abs(fix16_t x) 90 | { return (x < 0 ? -x : x); } 91 | static inline fix16_t fix16_floor(fix16_t x) 92 | { return (x & 0xFFFF0000UL); } 93 | static inline fix16_t fix16_ceil(fix16_t x) 94 | { return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? fix16_one : 0); } 95 | static inline fix16_t fix16_min(fix16_t x, fix16_t y) 96 | { return (x < y ? x : y); } 97 | static inline fix16_t fix16_max(fix16_t x, fix16_t y) 98 | { return (x > y ? x : y); } 99 | static inline fix16_t fix16_clamp(fix16_t x, fix16_t lo, fix16_t hi) 100 | { return fix16_min(fix16_max(x, lo), hi); } 101 | 102 | /* Subtraction and addition with (optional) overflow detection. */ 103 | #ifdef FIXMATH_NO_OVERFLOW 104 | 105 | static inline fix16_t fix16_add(fix16_t inArg0, fix16_t inArg1) { return (inArg0 + inArg1); } 106 | static inline fix16_t fix16_sub(fix16_t inArg0, fix16_t inArg1) { return (inArg0 - inArg1); } 107 | 108 | #else 109 | 110 | extern fix16_t fix16_add(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 111 | extern fix16_t fix16_sub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 112 | 113 | /* Saturating arithmetic */ 114 | extern fix16_t fix16_sadd(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 115 | extern fix16_t fix16_ssub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 116 | 117 | #endif 118 | 119 | /*! Multiplies the two given fix16_t's and returns the result. 120 | */ 121 | extern fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 122 | 123 | /*! Divides the first given fix16_t by the second and returns the result. 124 | */ 125 | extern fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 126 | 127 | #ifndef FIXMATH_NO_OVERFLOW 128 | /*! Performs a saturated multiplication (overflow-protected) of the two given fix16_t's and returns the result. 129 | */ 130 | extern fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 131 | 132 | /*! Performs a saturated division (overflow-protected) of the first fix16_t by the second and returns the result. 133 | */ 134 | extern fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 135 | #endif 136 | 137 | /*! Divides the first given fix16_t by the second and returns the result. 138 | */ 139 | extern fix16_t fix16_mod(fix16_t x, fix16_t y) FIXMATH_FUNC_ATTRS; 140 | 141 | 142 | 143 | /*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract) 144 | */ 145 | extern fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS; 146 | extern fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) FIXMATH_FUNC_ATTRS; 147 | #ifndef FIXMATH_NO_64BIT 148 | extern fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS; 149 | #endif 150 | 151 | 152 | 153 | /*! Returns the sine of the given fix16_t. 154 | */ 155 | extern fix16_t fix16_sin_parabola(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 156 | 157 | /*! Returns the sine of the given fix16_t. 158 | */ 159 | extern fix16_t fix16_sin(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 160 | 161 | /*! Returns the cosine of the given fix16_t. 162 | */ 163 | extern fix16_t fix16_cos(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 164 | 165 | /*! Returns the tangent of the given fix16_t. 166 | */ 167 | extern fix16_t fix16_tan(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 168 | 169 | /*! Returns the arcsine of the given fix16_t. 170 | */ 171 | extern fix16_t fix16_asin(fix16_t inValue) FIXMATH_FUNC_ATTRS; 172 | 173 | /*! Returns the arccosine of the given fix16_t. 174 | */ 175 | extern fix16_t fix16_acos(fix16_t inValue) FIXMATH_FUNC_ATTRS; 176 | 177 | /*! Returns the arctangent of the given fix16_t. 178 | */ 179 | extern fix16_t fix16_atan(fix16_t inValue) FIXMATH_FUNC_ATTRS; 180 | 181 | /*! Returns the arctangent of inY/inX. 182 | */ 183 | extern fix16_t fix16_atan2(fix16_t inY, fix16_t inX) FIXMATH_FUNC_ATTRS; 184 | 185 | static const fix16_t fix16_rad_to_deg_mult = 3754936; 186 | static inline fix16_t fix16_rad_to_deg(fix16_t radians) 187 | { return fix16_mul(radians, fix16_rad_to_deg_mult); } 188 | 189 | static const fix16_t fix16_deg_to_rad_mult = 1144; 190 | static inline fix16_t fix16_deg_to_rad(fix16_t degrees) 191 | { return fix16_mul(degrees, fix16_deg_to_rad_mult); } 192 | 193 | 194 | 195 | /*! Returns the square root of the given fix16_t. 196 | */ 197 | extern fix16_t fix16_sqrt(fix16_t inValue) FIXMATH_FUNC_ATTRS; 198 | 199 | /*! Returns the square of the given fix16_t. 200 | */ 201 | static inline fix16_t fix16_sq(fix16_t x) 202 | { return fix16_mul(x, x); } 203 | 204 | /*! Returns the exponent (e^) of the given fix16_t. 205 | */ 206 | extern fix16_t fix16_exp(fix16_t inValue) FIXMATH_FUNC_ATTRS; 207 | 208 | /*! Returns the natural logarithm of the given fix16_t. 209 | */ 210 | extern fix16_t fix16_log(fix16_t inValue) FIXMATH_FUNC_ATTRS; 211 | 212 | /*! Returns the base 2 logarithm of the given fix16_t. 213 | */ 214 | extern fix16_t fix16_log2(fix16_t x) FIXMATH_FUNC_ATTRS; 215 | 216 | /*! Returns the saturated base 2 logarithm of the given fix16_t. 217 | */ 218 | extern fix16_t fix16_slog2(fix16_t x) FIXMATH_FUNC_ATTRS; 219 | 220 | /*! Convert fix16_t value to a string. 221 | * Required buffer length for largest values is 13 bytes. 222 | */ 223 | extern void fix16_to_str(fix16_t value, char *buf, int decimals); 224 | 225 | /*! Convert string to a fix16_t value 226 | * Ignores spaces at beginning and end. Returns fix16_overflow if 227 | * value is too large or there were garbage characters. 228 | */ 229 | extern fix16_t fix16_from_str(const char *buf); 230 | 231 | /** Helper macro for F16C. Replace token with its number of characters/digits. */ 232 | #define FIXMATH_TOKLEN(token) ( sizeof( #token ) - 1 ) 233 | 234 | /** Helper macro for F16C. Handles pow(10, n) for n from 0 to 8. */ 235 | #define FIXMATH_CONSTANT_POW10(times) ( \ 236 | (times == 0) ? 1ULL \ 237 | : (times == 1) ? 10ULL \ 238 | : (times == 2) ? 100ULL \ 239 | : (times == 3) ? 1000ULL \ 240 | : (times == 4) ? 10000ULL \ 241 | : (times == 5) ? 100000ULL \ 242 | : (times == 6) ? 1000000ULL \ 243 | : (times == 7) ? 10000000ULL \ 244 | : 100000000ULL \ 245 | ) 246 | 247 | 248 | /** Helper macro for F16C, the type uint64_t is only used at compile time and 249 | * shouldn't be visible in the generated code. 250 | * 251 | * @note We do not use fix16_one instead of 65536ULL, because the 252 | * "use of a const variable in a constant expression is nonstandard in C". 253 | */ 254 | #define FIXMATH_CONVERT_MANTISSA(m) \ 255 | ( (unsigned) \ 256 | ( \ 257 | ( \ 258 | ( \ 259 | (uint64_t)( ( ( 1 ## m ## ULL ) - FIXMATH_CONSTANT_POW10(FIXMATH_TOKLEN(m)) ) * FIXMATH_CONSTANT_POW10(5 - FIXMATH_TOKLEN(m)) ) \ 260 | * 100000ULL * 65536ULL \ 261 | ) \ 262 | + 5000000000ULL /* rounding: + 0.5 */ \ 263 | ) \ 264 | / \ 265 | 10000000000LL \ 266 | ) \ 267 | ) 268 | 269 | 270 | #define FIXMATH_COMBINE_I_M(i, m) \ 271 | ( \ 272 | ( \ 273 | ( i ) \ 274 | << 16 \ 275 | ) \ 276 | | \ 277 | ( \ 278 | FIXMATH_CONVERT_MANTISSA(m) \ 279 | & 0xFFFF \ 280 | ) \ 281 | ) 282 | 283 | 284 | /** Create int16_t (Q16.16) constant from separate integer and mantissa part. 285 | * 286 | * Only tested on 32-bit ARM Cortex-M0 / x86 Intel. 287 | * 288 | * This macro is needed when compiling with options like "--fpu=none", 289 | * which forbid all and every use of float and related types and 290 | * would thus make it impossible to have fix16_t constants. 291 | * 292 | * Just replace uses of F16() with F16C() like this: 293 | * F16(123.1234) becomes F16C(123,1234) 294 | * 295 | * @warning Specification of any value outside the mentioned intervals 296 | * WILL result in undefined behavior! 297 | * 298 | * @note Regardless of the specified minimum and maximum values for i and m below, 299 | * the total value of the number represented by i and m MUST be in the interval 300 | * ]-32768.00000:32767.99999[ else usage with this macro will yield undefined behavior. 301 | * 302 | * @param i Signed integer constant with a value in the interval ]-32768:32767[. 303 | * @param m Positive integer constant in the interval ]0:99999[ (fractional part/mantissa). 304 | */ 305 | #define F16C(i, m) \ 306 | ( (fix16_t) \ 307 | ( \ 308 | (( #i[0] ) == '-') \ 309 | ? -FIXMATH_COMBINE_I_M((unsigned)( ( (i) * -1) ), m) \ 310 | : FIXMATH_COMBINE_I_M(i, m) \ 311 | ) \ 312 | ) 313 | 314 | #ifdef __cplusplus 315 | } 316 | #include "fix16.hpp" 317 | #endif 318 | 319 | #endif 320 | -------------------------------------------------------------------------------- /fix16.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #include "int64.h" 3 | 4 | 5 | /* Subtraction and addition with overflow detection. 6 | * The versions without overflow detection are inlined in the header. 7 | */ 8 | #ifndef FIXMATH_NO_OVERFLOW 9 | fix16_t fix16_add(fix16_t a, fix16_t b) 10 | { 11 | // Use unsigned integers because overflow with signed integers is 12 | // an undefined operation (http://www.airs.com/blog/archives/120). 13 | uint32_t _a = a, _b = b; 14 | uint32_t sum = _a + _b; 15 | 16 | // Overflow can only happen if sign of a == sign of b, and then 17 | // it causes sign of sum != sign of a. 18 | if (!((_a ^ _b) & 0x80000000) && ((_a ^ sum) & 0x80000000)) 19 | return fix16_overflow; 20 | 21 | return sum; 22 | } 23 | 24 | fix16_t fix16_sub(fix16_t a, fix16_t b) 25 | { 26 | uint32_t _a = a, _b = b; 27 | uint32_t diff = _a - _b; 28 | 29 | // Overflow can only happen if sign of a != sign of b, and then 30 | // it causes sign of diff != sign of a. 31 | if (((_a ^ _b) & 0x80000000) && ((_a ^ diff) & 0x80000000)) 32 | return fix16_overflow; 33 | 34 | return diff; 35 | } 36 | 37 | /* Saturating arithmetic */ 38 | fix16_t fix16_sadd(fix16_t a, fix16_t b) 39 | { 40 | fix16_t result = fix16_add(a, b); 41 | 42 | if (result == fix16_overflow) 43 | return (a >= 0) ? fix16_maximum : fix16_minimum; 44 | 45 | return result; 46 | } 47 | 48 | fix16_t fix16_ssub(fix16_t a, fix16_t b) 49 | { 50 | fix16_t result = fix16_sub(a, b); 51 | 52 | if (result == fix16_overflow) 53 | return (a >= 0) ? fix16_maximum : fix16_minimum; 54 | 55 | return result; 56 | } 57 | #endif 58 | 59 | 60 | 61 | /* 64-bit implementation for fix16_mul. Fastest version for e.g. ARM Cortex M3. 62 | * Performs a 32*32 -> 64bit multiplication. The middle 32 bits are the result, 63 | * bottom 16 bits are used for rounding, and upper 16 bits are used for overflow 64 | * detection. 65 | */ 66 | 67 | #if !defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 68 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 69 | { 70 | int64_t product = (int64_t)inArg0 * inArg1; 71 | 72 | #ifndef FIXMATH_NO_OVERFLOW 73 | // The upper 17 bits should all be the same (the sign). 74 | uint32_t upper = (product >> 47); 75 | #endif 76 | 77 | if (product < 0) 78 | { 79 | #ifndef FIXMATH_NO_OVERFLOW 80 | if (~upper) 81 | return fix16_overflow; 82 | #endif 83 | 84 | #ifndef FIXMATH_NO_ROUNDING 85 | // This adjustment is required in order to round -1/2 correctly 86 | product--; 87 | #endif 88 | } 89 | else 90 | { 91 | #ifndef FIXMATH_NO_OVERFLOW 92 | if (upper) 93 | return fix16_overflow; 94 | #endif 95 | } 96 | 97 | #ifdef FIXMATH_NO_ROUNDING 98 | return product >> 16; 99 | #else 100 | fix16_t result = product >> 16; 101 | result += (product & 0x8000) >> 15; 102 | 103 | return result; 104 | #endif 105 | } 106 | #endif 107 | 108 | /* 32-bit implementation of fix16_mul. Potentially fast on 16-bit processors, 109 | * and this is a relatively good compromise for compilers that do not support 110 | * uint64_t. Uses 16*16->32bit multiplications. 111 | */ 112 | #if defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 113 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 114 | { 115 | // Each argument is divided to 16-bit parts. 116 | // AB 117 | // * CD 118 | // ----------- 119 | // BD 16 * 16 -> 32 bit products 120 | // CB 121 | // AD 122 | // AC 123 | // |----| 64 bit product 124 | int32_t A = (inArg0 >> 16), C = (inArg1 >> 16); 125 | uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF); 126 | 127 | int32_t AC = A*C; 128 | int32_t AD_CB = A*D + C*B; 129 | uint32_t BD = B*D; 130 | 131 | int32_t product_hi = AC + (AD_CB >> 16); 132 | 133 | // Handle carry from lower 32 bits to upper part of result. 134 | uint32_t ad_cb_temp = AD_CB << 16; 135 | uint32_t product_lo = BD + ad_cb_temp; 136 | if (product_lo < BD) 137 | product_hi++; 138 | 139 | #ifndef FIXMATH_NO_OVERFLOW 140 | // The upper 17 bits should all be the same (the sign). 141 | if (product_hi >> 31 != product_hi >> 15) 142 | return fix16_overflow; 143 | #endif 144 | 145 | #ifdef FIXMATH_NO_ROUNDING 146 | return (product_hi << 16) | (product_lo >> 16); 147 | #else 148 | // Subtracting 0x8000 (= 0.5) and then using signed right shift 149 | // achieves proper rounding to result-1, except in the corner 150 | // case of negative numbers and lowest word = 0x8000. 151 | // To handle that, we also have to subtract 1 for negative numbers. 152 | uint32_t product_lo_tmp = product_lo; 153 | product_lo -= 0x8000; 154 | product_lo -= (uint32_t)product_hi >> 31; 155 | if (product_lo > product_lo_tmp) 156 | product_hi--; 157 | 158 | // Discard the lowest 16 bits. Note that this is not exactly the same 159 | // as dividing by 0x10000. For example if product = -1, result will 160 | // also be -1 and not 0. This is compensated by adding +1 to the result 161 | // and compensating this in turn in the rounding above. 162 | fix16_t result = (product_hi << 16) | (product_lo >> 16); 163 | result += 1; 164 | return result; 165 | #endif 166 | } 167 | #endif 168 | 169 | /* 8-bit implementation of fix16_mul. Fastest on e.g. Atmel AVR. 170 | * Uses 8*8->16bit multiplications, and also skips any bytes that 171 | * are zero. 172 | */ 173 | #if defined(FIXMATH_OPTIMIZE_8BIT) 174 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 175 | { 176 | uint32_t _a = (inArg0 >= 0) ? inArg0 : (-inArg0); 177 | uint32_t _b = (inArg1 >= 0) ? inArg1 : (-inArg1); 178 | 179 | uint8_t va[4] = {_a, (_a >> 8), (_a >> 16), (_a >> 24)}; 180 | uint8_t vb[4] = {_b, (_b >> 8), (_b >> 16), (_b >> 24)}; 181 | 182 | uint32_t low = 0; 183 | uint32_t mid = 0; 184 | 185 | // Result column i depends on va[0..i] and vb[i..0] 186 | 187 | #ifndef FIXMATH_NO_OVERFLOW 188 | // i = 6 189 | if (va[3] && vb[3]) return fix16_overflow; 190 | #endif 191 | 192 | // i = 5 193 | if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3]; 194 | if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2]; 195 | mid <<= 8; 196 | 197 | // i = 4 198 | if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3]; 199 | if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2]; 200 | if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1]; 201 | 202 | #ifndef FIXMATH_NO_OVERFLOW 203 | if (mid & 0xFF000000) return fix16_overflow; 204 | #endif 205 | mid <<= 8; 206 | 207 | // i = 3 208 | if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3]; 209 | if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2]; 210 | if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1]; 211 | if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0]; 212 | 213 | #ifndef FIXMATH_NO_OVERFLOW 214 | if (mid & 0xFF000000) return fix16_overflow; 215 | #endif 216 | mid <<= 8; 217 | 218 | // i = 2 219 | if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2]; 220 | if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1]; 221 | if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0]; 222 | 223 | // i = 1 224 | if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1]; 225 | if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0]; 226 | low <<= 8; 227 | 228 | // i = 0 229 | if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0]; 230 | 231 | #ifndef FIXMATH_NO_ROUNDING 232 | low += 0x8000; 233 | #endif 234 | mid += (low >> 16); 235 | 236 | #ifndef FIXMATH_NO_OVERFLOW 237 | if (mid & 0x80000000) 238 | return fix16_overflow; 239 | #endif 240 | 241 | fix16_t result = mid; 242 | 243 | /* Figure out the sign of result */ 244 | if ((inArg0 >= 0) != (inArg1 >= 0)) 245 | { 246 | result = -result; 247 | } 248 | 249 | return result; 250 | } 251 | #endif 252 | 253 | #ifndef FIXMATH_NO_OVERFLOW 254 | /* Wrapper around fix16_mul to add saturating arithmetic. */ 255 | fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) 256 | { 257 | fix16_t result = fix16_mul(inArg0, inArg1); 258 | 259 | if (result == fix16_overflow) 260 | { 261 | if ((inArg0 >= 0) == (inArg1 >= 0)) 262 | return fix16_maximum; 263 | else 264 | return fix16_minimum; 265 | } 266 | 267 | return result; 268 | } 269 | #endif 270 | 271 | /* 32-bit implementation of fix16_div. Fastest version for e.g. ARM Cortex M3. 272 | * Performs 32-bit divisions repeatedly to reduce the remainder. For this to 273 | * be efficient, the processor has to have 32-bit hardware division. 274 | */ 275 | #if !defined(FIXMATH_OPTIMIZE_8BIT) 276 | #ifdef __GNUC__ 277 | // Count leading zeros, using processor-specific instruction if available. 278 | #define clz(x) (__builtin_clzl(x) - (8 * sizeof(long) - 32)) 279 | #else 280 | static uint8_t clz(uint32_t x) 281 | { 282 | uint8_t result = 0; 283 | if (x == 0) return 32; 284 | while (!(x & 0xF0000000)) { result += 4; x <<= 4; } 285 | while (!(x & 0x80000000)) { result += 1; x <<= 1; } 286 | return result; 287 | } 288 | #endif 289 | 290 | fix16_t fix16_div(fix16_t a, fix16_t b) 291 | { 292 | // This uses a hardware 32/32 bit division multiple times, until we have 293 | // computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations. 294 | 295 | if (b == 0) 296 | return fix16_minimum; 297 | 298 | uint32_t remainder = (a >= 0) ? a : (-a); 299 | uint32_t divider = (b >= 0) ? b : (-b); 300 | uint32_t quotient = 0; 301 | int bit_pos = 17; 302 | 303 | // Kick-start the division a bit. 304 | // This improves speed in the worst-case scenarios where N and D are large 305 | // It gets a lower estimate for the result by N/(D >> 17 + 1). 306 | if (divider & 0xFFF00000) 307 | { 308 | uint32_t shifted_div = ((divider >> 17) + 1); 309 | quotient = remainder / shifted_div; 310 | remainder -= ((uint64_t)quotient * divider) >> 17; 311 | } 312 | 313 | // If the divider is divisible by 2^n, take advantage of it. 314 | while (!(divider & 0xF) && bit_pos >= 4) 315 | { 316 | divider >>= 4; 317 | bit_pos -= 4; 318 | } 319 | 320 | while (remainder && bit_pos >= 0) 321 | { 322 | // Shift remainder as much as we can without overflowing 323 | int shift = clz(remainder); 324 | if (shift > bit_pos) shift = bit_pos; 325 | remainder <<= shift; 326 | bit_pos -= shift; 327 | 328 | uint32_t div = remainder / divider; 329 | remainder = remainder % divider; 330 | quotient += div << bit_pos; 331 | 332 | #ifndef FIXMATH_NO_OVERFLOW 333 | if (div & ~(0xFFFFFFFF >> bit_pos)) 334 | return fix16_overflow; 335 | #endif 336 | 337 | remainder <<= 1; 338 | bit_pos--; 339 | } 340 | 341 | #ifndef FIXMATH_NO_ROUNDING 342 | // Quotient is always positive so rounding is easy 343 | quotient++; 344 | #endif 345 | 346 | fix16_t result = quotient >> 1; 347 | 348 | // Figure out the sign of the result 349 | if ((a ^ b) & 0x80000000) 350 | { 351 | #ifndef FIXMATH_NO_OVERFLOW 352 | if (result == fix16_minimum) 353 | return fix16_overflow; 354 | #endif 355 | 356 | result = -result; 357 | } 358 | 359 | return result; 360 | } 361 | #endif 362 | 363 | /* Alternative 32-bit implementation of fix16_div. Fastest on e.g. Atmel AVR. 364 | * This does the division manually, and is therefore good for processors that 365 | * do not have hardware division. 366 | */ 367 | #if defined(FIXMATH_OPTIMIZE_8BIT) 368 | fix16_t fix16_div(fix16_t a, fix16_t b) 369 | { 370 | // This uses the basic binary restoring division algorithm. 371 | // It appears to be faster to do the whole division manually than 372 | // trying to compose a 64-bit divide out of 32-bit divisions on 373 | // platforms without hardware divide. 374 | 375 | if (b == 0) 376 | return fix16_minimum; 377 | 378 | uint32_t remainder = (a >= 0) ? a : (-a); 379 | uint32_t divider = (b >= 0) ? b : (-b); 380 | 381 | uint32_t quotient = 0; 382 | uint32_t bit = 0x10000; 383 | 384 | /* The algorithm requires D >= R */ 385 | while (divider < remainder) 386 | { 387 | divider <<= 1; 388 | bit <<= 1; 389 | } 390 | 391 | #ifndef FIXMATH_NO_OVERFLOW 392 | if (!bit) 393 | return fix16_overflow; 394 | #endif 395 | 396 | if (divider & 0x80000000) 397 | { 398 | // Perform one step manually to avoid overflows later. 399 | // We know that divider's bottom bit is 0 here. 400 | if (remainder >= divider) 401 | { 402 | quotient |= bit; 403 | remainder -= divider; 404 | } 405 | divider >>= 1; 406 | bit >>= 1; 407 | } 408 | 409 | /* Main division loop */ 410 | while (bit && remainder) 411 | { 412 | if (remainder >= divider) 413 | { 414 | quotient |= bit; 415 | remainder -= divider; 416 | } 417 | 418 | remainder <<= 1; 419 | bit >>= 1; 420 | } 421 | 422 | #ifndef FIXMATH_NO_ROUNDING 423 | if (remainder >= divider) 424 | { 425 | quotient++; 426 | } 427 | #endif 428 | 429 | fix16_t result = quotient; 430 | 431 | /* Figure out the sign of result */ 432 | if ((a ^ b) & 0x80000000) 433 | { 434 | #ifndef FIXMATH_NO_OVERFLOW 435 | if (result == fix16_minimum) 436 | return fix16_overflow; 437 | #endif 438 | 439 | result = -result; 440 | } 441 | 442 | return result; 443 | } 444 | #endif 445 | 446 | #ifndef FIXMATH_NO_OVERFLOW 447 | /* Wrapper around fix16_div to add saturating arithmetic. */ 448 | fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) 449 | { 450 | fix16_t result = fix16_div(inArg0, inArg1); 451 | 452 | if (result == fix16_overflow) 453 | { 454 | if ((inArg0 >= 0) == (inArg1 >= 0)) 455 | return fix16_maximum; 456 | else 457 | return fix16_minimum; 458 | } 459 | 460 | return result; 461 | } 462 | #endif 463 | 464 | fix16_t fix16_mod(fix16_t x, fix16_t y) 465 | { 466 | #ifdef FIXMATH_OPTIMIZE_8BIT 467 | /* The reason we do this, rather than use a modulo operator 468 | * is that if you don't have a hardware divider, this will result 469 | * in faster operations when the angles are close to the bounds. 470 | */ 471 | while(x >= y) x -= y; 472 | while(x <= -y) x += y; 473 | #else 474 | /* Note that in C90, the sign of result of the modulo operation is 475 | * undefined. in C99, it's the same as the dividend (aka numerator). 476 | */ 477 | x %= y; 478 | #endif 479 | 480 | return x; 481 | } 482 | 483 | 484 | #ifndef FIXMATH_NO_64BIT 485 | 486 | fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) 487 | { 488 | int64_t tempOut = int64_mul_i32_i32(inArg0, ((1 << 8) - inFract)); 489 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 490 | tempOut = int64_shift(tempOut, -8); 491 | return (fix16_t)int64_lo(tempOut); 492 | } 493 | 494 | fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) 495 | { 496 | int64_t tempOut = int64_mul_i32_i32(inArg0, (((int32_t)1 << 16) - inFract)); 497 | tempOut = int64_add(tempOut, int64_mul_i32_i32(inArg1, inFract)); 498 | tempOut = int64_shift(tempOut, -16); 499 | return (fix16_t)int64_lo(tempOut); 500 | } 501 | 502 | fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) 503 | { 504 | int64_t tempOut; 505 | tempOut = ((int64_t)inArg0 * (0 - inFract)); 506 | tempOut += ((int64_t)inArg1 * inFract); 507 | tempOut >>= 32; 508 | return (fix16_t)tempOut; 509 | } 510 | #endif 511 | --------------------------------------------------------------------------------