├── .travis.yml ├── benchmarks ├── results-avr.ods ├── interface.c ├── interface-arm.c ├── interface-avr.c ├── interface.h ├── Makefile ├── generate_testcases.py └── benchmark.c ├── images ├── error_analysis.ods └── sinf-fix16_sin.png ├── tests ├── tests_str.h ├── tests_lerp.h ├── tests_sqrt.h ├── tests_macros.h ├── tests_basic.h ├── tests_sqrt.c ├── tests_lerp.c ├── tests.c ├── tests_macros.c ├── tests.cmake ├── tests_str.c ├── tests.h └── tests_basic.c ├── libfixmath ├── libfixmath.cmake ├── fixmath.h ├── uint32.h ├── uint32.c ├── fract32.c ├── Makefile ├── fract32.h ├── libfixmath.cbp ├── fix16_sqrt.c ├── fix16_str.c ├── fix16_fft.c ├── fix16_exp.c ├── fix16_trig.c ├── int64.h ├── code_style.txt ├── fix16.h ├── fix16.hpp └── fix16.c ├── library.json ├── fixtest ├── fixtest.layout ├── hiclock.c ├── hiclock.h ├── fixtest.cbp ├── fixtest.depend └── main.c ├── AUTHORS ├── fixsingen ├── fixsingen.depend ├── fixsingen.cbp └── main.c ├── CMakeLists.txt ├── LICENSE ├── .github └── workflows │ └── cmake.yml ├── .gitignore └── README.md /.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | dist: focal 3 | 4 | script: 5 | - pushd unittests && make && popd 6 | 7 | 8 | -------------------------------------------------------------------------------- /benchmarks/results-avr.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PetteriAimonen/libfixmath/HEAD/benchmarks/results-avr.ods -------------------------------------------------------------------------------- /images/error_analysis.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PetteriAimonen/libfixmath/HEAD/images/error_analysis.ods -------------------------------------------------------------------------------- /images/sinf-fix16_sin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PetteriAimonen/libfixmath/HEAD/images/sinf-fix16_sin.png -------------------------------------------------------------------------------- /tests/tests_str.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_STR_H 2 | #define TESTS_STR_H 3 | 4 | int test_str(); 5 | 6 | #endif // TESTS_STR_H 7 | -------------------------------------------------------------------------------- /tests/tests_lerp.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_LERP_H 2 | #define TESTS_LERP_H 3 | 4 | int test_lerp(); 5 | 6 | #endif // TESTS_LERP_H 7 | -------------------------------------------------------------------------------- /tests/tests_sqrt.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_SQRT_H 2 | #define TESTS_SQRT_H 3 | 4 | int test_sqrt(); 5 | 6 | #endif // TESTS_SQRT_H 7 | -------------------------------------------------------------------------------- /tests/tests_macros.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_MACROS_H 2 | #define TESTS_MACROS_H 3 | 4 | int test_macros(); 5 | 6 | #endif // TESTS_MACROS_H 7 | -------------------------------------------------------------------------------- /libfixmath/libfixmath.cmake: -------------------------------------------------------------------------------- 1 | file(GLOB libfixmath-srcs libfixmath/*.c) 2 | 3 | add_library(libfixmath STATIC ${libfixmath-srcs}) 4 | 5 | target_include_directories(libfixmath INTERFACE 6 | ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /tests/tests_basic.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_BASIC_H 2 | #define TESTS_BASIC_H 3 | 4 | int test_abs(void); 5 | int test_add(void); 6 | int test_mul(void); 7 | int test_div(void); 8 | int test_sub(); 9 | 10 | #endif // TESTS_BASIC_H 11 | -------------------------------------------------------------------------------- /benchmarks/interface.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "interface.h" 3 | 4 | void print_value(const char *label, int32_t value) 5 | { 6 | printf("%-20s %6ld\n", label, value); 7 | } 8 | 9 | void print(const char *label, cyclecount_t *count) 10 | { 11 | printf("%-20s %6ld %6ld %6ld\n",label,count->min, count->sum / count->count, count->max); 12 | } 13 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "libfixmath", 3 | "description": "Cross Platform Fixed Point Maths Library", 4 | "keywords": [ 5 | "fixed", 6 | "point", 7 | "math" 8 | ], 9 | "repository": { 10 | "type": "git", 11 | "url": "https://github.com/PetteriAimonen/libfixmath.git" 12 | }, 13 | "license": "MIT", 14 | "build": { 15 | "srcFilter": "+" 16 | }, 17 | "frameworks": "*", 18 | "platforms": "*" 19 | } 20 | -------------------------------------------------------------------------------- /libfixmath/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/uint32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_uint32_h__ 2 | #define __libfixmath_uint32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #ifdef __KERNEL__ 10 | #include 11 | #else 12 | #include 13 | #endif 14 | 15 | /*! Performs an unsigned log-base2 on the specified unsigned integer and returns the result. 16 | */ 17 | extern uint32_t uint32_log2(uint32_t inVal); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /fixtest/fixtest.layout: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /libfixmath/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 >= (1UL << 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 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | Chris Hammond 2 | David Lechner 3 | Flatmush 4 | Gaëtan Harter 5 | Joe Schaack 6 | Martin Larralde 7 | Petteri Aimonen 8 | Stargirl Flowers 9 | Vincent del Medico 10 | Vitaly Puzrin 11 | Xin Li 12 | J.P. Hutchins 13 | -------------------------------------------------------------------------------- /fixsingen/fixsingen.depend: -------------------------------------------------------------------------------- 1 | # depslib dependency file v1.0 2 | 1298649568 source:g:\vrfx\libfixmath\fixsingen\main.c 3 | 4 | 5 | 6 | 7 | 8 | 9 | 1298453688 g:\vrfx\libfixmath\\libfixmath\fixmath.h 10 | "uint32.h" 11 | "fract32.h" 12 | "fix16.h" 13 | 14 | 1298453688 g:\vrfx\libfixmath\\libfixmath\uint32.h 15 | 16 | 17 | 1298453688 g:\vrfx\libfixmath\\libfixmath\fract32.h 18 | 19 | 20 | 1298648502 g:\vrfx\libfixmath\\libfixmath\fix16.h 21 | 22 | 23 | -------------------------------------------------------------------------------- /benchmarks/interface-arm.c: -------------------------------------------------------------------------------- 1 | #include "interface.h" 2 | #include 3 | #include 4 | 5 | // This targets an ARM Cortex M3 core using QEmu LM3S6965 emulation. 6 | #define STBASE 0xE000E000 7 | #define STCTRL (*(volatile uint32_t*)(0x010 + STBASE)) 8 | #define STRELOAD (*(volatile uint32_t*)(0x014 + STBASE)) 9 | #define STCURRENT (*(volatile uint32_t*)(0x018 + STBASE)) 10 | 11 | 12 | void interface_init() 13 | { 14 | STRELOAD = 0x00FFFFFF; 15 | STCTRL = 5; 16 | } 17 | 18 | void start_timing() 19 | { 20 | STCURRENT = 0; 21 | } 22 | 23 | uint16_t end_timing() 24 | { 25 | return 0x00FFFFFF - STCURRENT - 4; 26 | } 27 | 28 | 29 | -------------------------------------------------------------------------------- /fixtest/hiclock.c: -------------------------------------------------------------------------------- 1 | #include "hiclock.h" 2 | #include 3 | 4 | #if defined(__WIN32) || defined(__WIN64) 5 | LONGLONG HICLOCKS_PER_SEC = 0; 6 | 7 | void hiclock_init() 8 | { 9 | LARGE_INTEGER freq; 10 | QueryPerformanceFrequency(&freq); 11 | HICLOCKS_PER_SEC = freq.QuadPart; 12 | } 13 | #endif 14 | 15 | hiclock_t hiclock() 16 | { 17 | #if defined(__unix__) 18 | struct timeval clocks; 19 | gettimeofday(&clocks, NULL); 20 | return ((uint64_t)clocks.tv_sec * 1000000ULL) + clocks.tv_usec; 21 | #elif defined(__WIN32) || defined(__WIN64) 22 | LARGE_INTEGER clocks; 23 | QueryPerformanceCounter(&clocks); 24 | return clocks.QuadPart; 25 | #else 26 | return clock(); 27 | #endif 28 | } 29 | -------------------------------------------------------------------------------- /benchmarks/interface-avr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "interface.h" 4 | #include 5 | 6 | #define special_output_port (*((volatile char *)0x20)) 7 | static int output_char(char c, FILE *stream) 8 | { 9 | special_output_port = c; 10 | return 0; 11 | } 12 | 13 | static FILE mystdout = FDEV_SETUP_STREAM(output_char, NULL, _FDEV_SETUP_WRITE); 14 | 15 | void interface_init() 16 | { 17 | // Set timer 1 to count cycles 18 | TCCR1B = 1; 19 | 20 | // Set output to simulator 21 | stdout = &mystdout; 22 | stderr = &mystdout; 23 | } 24 | 25 | 26 | void start_timing() 27 | { 28 | TCNT1 = 0; 29 | } 30 | 31 | uint16_t end_timing() 32 | { 33 | return TCNT1 - 9; 34 | } 35 | -------------------------------------------------------------------------------- /benchmarks/interface.h: -------------------------------------------------------------------------------- 1 | // This file defines the hardware or simulator interface that will be used to 2 | // measure timings and report results. 3 | 4 | #include 5 | 6 | /* Tools for profiling */ 7 | 8 | typedef struct { 9 | uint32_t min; 10 | uint32_t max; 11 | uint32_t sum; 12 | uint32_t count; 13 | } cyclecount_t; 14 | 15 | // Initialize 16 | void interface_init(); 17 | 18 | // Reset timer/counter/something 19 | void start_timing(); 20 | 21 | // Return the number of clock cycles passed since start_timing(); 22 | uint16_t end_timing(); 23 | 24 | // Print a value to console, along with a descriptive label 25 | void print_value(const char *label, int32_t value); 26 | 27 | void print(const char *label, cyclecount_t *count); 28 | -------------------------------------------------------------------------------- /libfixmath/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 | -------------------------------------------------------------------------------- /fixtest/hiclock.h: -------------------------------------------------------------------------------- 1 | #ifndef __hiclock_h__ 2 | #define __hiclock_h__ 3 | 4 | #include 5 | #include 6 | 7 | #if defined(__unix__) 8 | #include 9 | #define PRIuHICLOCK PRIu64 10 | #define PRIiHICLOCK PRIi64 11 | typedef uint64_t hiclock_t; 12 | #define HICLOCKS_PER_SEC 1000000 13 | #define hiclock_init() 14 | #elif defined(__WIN32) || defined(__WIN64) 15 | #include 16 | #define PRIuHICLOCK PRIu64 17 | #define PRIiHICLOCK PRIi64 18 | typedef LONGLONG hiclock_t; 19 | extern LONGLONG HICLOCKS_PER_SEC; 20 | extern void hiclock_init(); 21 | #else 22 | #include 23 | #define PRIuHICLOCK PRIu32 24 | #define PRIiHICLOCK PRIi32 25 | typedef clock_t hiclock_t; 26 | #define HICLOCKS_PER_SEC CLOCKS_PER_SEC 27 | #define hiclock_init() 28 | #endif 29 | 30 | extern hiclock_t hiclock(); 31 | 32 | #endif 33 | 34 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.13) 2 | 3 | project(libfixmath LANGUAGES CXX C) 4 | 5 | set(CMAKE_C_STANDARD 11) 6 | set(CMAKE_C_STANDARD_REQUIRED ON) 7 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -pedantic -Wextra -Werror=return-type") 8 | 9 | set(CMAKE_CXX_STANDARD 11) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic -Wextra -Werror=return-type") 12 | 13 | include(libfixmath/libfixmath.cmake) 14 | 15 | if(CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) 16 | # We're in the root, define additional targets for developers. 17 | include(tests/tests.cmake) 18 | 19 | file(GLOB fixsingen-srcs fixsingen/*.c) 20 | file(GLOB fixtest-srcs fixtest/*.c fixtest/*.h) 21 | 22 | add_executable(fixtest ${fixtest-srcs}) 23 | target_link_libraries(fixtest PRIVATE libfixmath m) 24 | target_include_directories(fixtest PRIVATE ${CMAKE_SOURCE_DIR}) 25 | 26 | add_executable(fixsingen ${fixsingen-srcs}) 27 | target_link_libraries(fixsingen PRIVATE libfixmath m) 28 | target_include_directories(fixsingen PRIVATE ${CMAKE_SOURCE_DIR}) 29 | endif() 30 | -------------------------------------------------------------------------------- /tests/tests_sqrt.c: -------------------------------------------------------------------------------- 1 | #include "tests_sqrt.h" 2 | #include "tests.h" 3 | 4 | int test_sqrt_specific() 5 | { 6 | ASSERT_EQ_INT(fix16_sqrt(fix16_from_int(16)), fix16_from_int(4)); 7 | ASSERT_EQ_INT(fix16_sqrt(fix16_from_int(100)), fix16_from_int(10)); 8 | ASSERT_EQ_INT(fix16_sqrt(fix16_from_int(1)), fix16_from_int(1)); 9 | #ifndef FIXMATH_NO_ROUNDING 10 | ASSERT_EQ_INT(fix16_sqrt(214748302), 3751499); 11 | ASSERT_EQ_INT(fix16_sqrt(214748303), 3751499); 12 | ASSERT_EQ_INT(fix16_sqrt(214748359), 3751499); 13 | ASSERT_EQ_INT(fix16_sqrt(214748360), 3751500); 14 | #endif 15 | return 0; 16 | } 17 | 18 | int test_sqrt_short() 19 | { 20 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 21 | { 22 | fix16_t a = testcases[i]; 23 | double fa = fix16_to_dbl(a); 24 | fix16_t result = fix16_sqrt(a); 25 | double fresult = sqrt(fa); 26 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), fix16_to_dbl(1), 27 | "in: %f", fa); 28 | } 29 | return 0; 30 | } 31 | 32 | int test_sqrt() 33 | { 34 | TEST(test_sqrt_specific()); 35 | TEST(test_sqrt_short()); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /libfixmath/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 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | libfixmath is Copyright (c) 2011-2021 Flatmush , 2 | Petteri Aimonen , & libfixmath AUTHORS 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | -------------------------------------------------------------------------------- /benchmarks/Makefile: -------------------------------------------------------------------------------- 1 | # These are testcases & benchmarks for the library on the target processors 2 | # (currently ARM Cortex M3 and AVR). They are a bit tricky to run, as they 3 | # depend on specific simulator versions. 4 | 5 | FILES = benchmark.c interface.c ../libfixmath/fix16.c ../libfixmath/fix16_sqrt.c ../libfixmath/fix16_exp.c 6 | 7 | CFLAGS = -std=gnu11 -I../libfixmath 8 | 9 | .PHONY clean: 10 | rm -f *.elf 11 | 12 | testcases.c: generate_testcases.py 13 | python $< 14 | 15 | benchmark-arm.elf: $(FILES) interface-arm.c testcases.c 16 | # Note: this needs hacked QEmu that "makes no sense": 17 | # https://bugs.launchpad.net/qemu/+bug/696094 18 | arm-none-eabi-gcc -mcpu=cortex-m3 -mthumb -T generic-m-hosted.ld \ 19 | -Wall -O2 $(CFLAGS) \ 20 | -o $@ -I .. $(FILES) interface-arm.c -lm 21 | 22 | run-benchmark-arm: benchmark-arm.elf 23 | qemu-system-arm -cpu cortex-m3 -icount 0 -device armv7m_nvic \ 24 | -nographic -monitor null -serial null \ 25 | -semihosting -kernel $< 26 | 27 | benchmark-avr.elf: $(FILES) interface-avr.c testcases.c 28 | avr-gcc -Wall -mmcu=atmega128 $(CFLAGS) \ 29 | -O2 -DFIXMATH_OPTIMIZE_8BIT \ 30 | -o $@ -I .. $(FILES) interface-avr.c 31 | 32 | run-benchmark-avr: benchmark-avr.elf 33 | # Note: this needs simulavrxx 1.0rc0 or newer 34 | simulavr -d atmega128 -f $< -W 0x20,- -T exit 35 | 36 | 37 | -------------------------------------------------------------------------------- /libfixmath/fract32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fract32_h__ 2 | #define __libfixmath_fract32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #ifdef __KERNEL__ 10 | #include 11 | #else 12 | #include 13 | #endif 14 | 15 | typedef uint32_t fract32_t; 16 | 17 | /*! Creates a fraction using unsigned integers. 18 | \param inNumerator the unsigned integer numerator 19 | \param inDenominator the unsigned integer denominator 20 | \return a fraction using the given numerator and denominator 21 | */ 22 | extern fract32_t fract32_create(uint32_t inNumerator, uint32_t inDenominator); 23 | 24 | /*! Inverts the given fraction, swapping the numerator and the denominator. 25 | */ 26 | extern fract32_t fract32_invert(fract32_t inFract); 27 | 28 | #ifndef FIXMATH_NO_64BIT 29 | /*! Performs unsigned saturated (overflow-protected) multiplication with the two given fractions and returns the result as an unsigned integer. 30 | */ 31 | extern uint32_t fract32_usmul(uint32_t inVal, fract32_t inFract); 32 | 33 | /*! Performs saturated (overflow-protected) multiplication with the two given fractions and returns the result as a signed integer. 34 | */ 35 | extern int32_t fract32_smul(int32_t inVal, fract32_t inFract); 36 | #endif 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /fixsingen/fixsingen.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 48 | 49 | -------------------------------------------------------------------------------- /fixsingen/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char** argv) { 8 | FILE* fp = fopen("fix16_trig_sin_lut.h", "wb"); 9 | if(fp == NULL) { 10 | fprintf(stderr, "Error: Unable to open file for writing.\n"); 11 | return EXIT_FAILURE; 12 | } 13 | 14 | // TODO - Store as uint16_t with a count to determine the end and return 1. 15 | 16 | fprintf(fp, "#ifndef __fix16_trig_sin_lut_h__\n"); 17 | fprintf(fp, "#define __fix16_trig_sin_lut_h__\n"); 18 | fprintf(fp, "\n"); 19 | 20 | fix16_t fix16_sin_lut_count = (fix16_pi >> 1); 21 | fix16_t fix16_sin_lut[fix16_sin_lut_count]; 22 | 23 | uintptr_t i; 24 | for(i = 0; i < fix16_sin_lut_count; i++) 25 | fix16_sin_lut[i] = fix16_from_dbl(sin(fix16_to_dbl(i))); 26 | for(i--; fix16_sin_lut[i] == fix16_one; i--, fix16_sin_lut_count--); 27 | 28 | fprintf(fp, "static const uint32_t _fix16_sin_lut_count = %"PRIi32";\n", fix16_sin_lut_count); 29 | fprintf(fp, "static uint16_t _fix16_sin_lut[%"PRIi32"] = {", fix16_sin_lut_count); 30 | 31 | for(i = 0; i < fix16_sin_lut_count; i++) { 32 | if((i & 7) == 0) 33 | fprintf(fp, "\n\t"); 34 | fprintf(fp, "%"PRIi32", ", fix16_sin_lut[i]); 35 | } 36 | fprintf(fp, "\n\t};\n"); 37 | 38 | fprintf(fp, "\n"); 39 | fprintf(fp, "#endif\n"); 40 | 41 | fclose(fp); 42 | 43 | return EXIT_SUCCESS; 44 | } 45 | -------------------------------------------------------------------------------- /fixtest/fixtest.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 52 | 53 | -------------------------------------------------------------------------------- /.github/workflows/cmake.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: 4 | - push 5 | - pull_request 6 | 7 | env: 8 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 9 | BUILD_TYPE: Release 10 | 11 | jobs: 12 | build: 13 | # The CMake configure and build commands are platform agnostic and should work equally 14 | # well on Windows or Mac. You can convert this to a matrix build if you need 15 | # cross-platform coverage. 16 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 17 | runs-on: ubuntu-latest 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | 22 | - name: Configure CMake 23 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 24 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 25 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} 26 | 27 | - name: Build 28 | # Build your program with the given configuration 29 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target make_tests 30 | 31 | - name: Test 32 | working-directory: ${{github.workspace}}/build 33 | # Execute tests defined by the CMake configuration. 34 | # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 35 | run: ctest -C ${{env.BUILD_TYPE}} --output-on-failure 36 | 37 | 38 | -------------------------------------------------------------------------------- /tests/tests_lerp.c: -------------------------------------------------------------------------------- 1 | #include "tests_lerp.h" 2 | #include "tests.h" 3 | 4 | int test_lerp() 5 | { 6 | ASSERT_EQ_INT(fix16_lerp8(0, 2, 0), 0); 7 | ASSERT_EQ_INT(fix16_lerp8(0, 2, 127), 0); 8 | ASSERT_EQ_INT(fix16_lerp8(0, 2, 128), 1); 9 | ASSERT_EQ_INT(fix16_lerp8(0, 2, 255), 1); 10 | ASSERT_EQ_INT(fix16_lerp8(fix16_minimum, fix16_maximum, 0), fix16_minimum); 11 | ASSERT_EQ_INT(fix16_lerp8(fix16_minimum, fix16_maximum, 255), 12 | (fix16_maximum - (1 << 24))); 13 | ASSERT_EQ_INT(fix16_lerp8(-fix16_maximum, fix16_maximum, 128), 0); 14 | 15 | ASSERT_EQ_INT(fix16_lerp16(0, 2, 0), 0); 16 | ASSERT_EQ_INT(fix16_lerp16(0, 2, 0x7fff), 0); 17 | ASSERT_EQ_INT(fix16_lerp16(0, 2, 0x8000), 1); 18 | ASSERT_EQ_INT(fix16_lerp16(0, 2, 0xffff), 1); 19 | ASSERT_EQ_INT(fix16_lerp16(fix16_minimum, fix16_maximum, 0), fix16_minimum); 20 | ASSERT_EQ_INT(fix16_lerp16(fix16_minimum, fix16_maximum, 0xffff), 21 | (fix16_maximum - (1UL << 16))); 22 | ASSERT_EQ_INT(fix16_lerp16(-fix16_maximum, fix16_maximum, 0x8000), 0); 23 | 24 | ASSERT_EQ_INT(fix16_lerp32(0, 2, 0), 0); 25 | ASSERT_EQ_INT(fix16_lerp32(0, 2, 0x7fffffff), 0); 26 | ASSERT_EQ_INT(fix16_lerp32(0, 2, 0x80000000), 1); 27 | ASSERT_EQ_INT(fix16_lerp32(0, 2, 0xffffffff), 1); 28 | ASSERT_EQ_INT(fix16_lerp32(fix16_minimum, fix16_maximum, 0), fix16_minimum); 29 | ASSERT_EQ_INT(fix16_lerp32(fix16_minimum, fix16_maximum, 0xffffffff), 30 | (fix16_maximum - 1)); 31 | ASSERT_EQ_INT(fix16_lerp32(-fix16_maximum, fix16_maximum, 0x80000000), 0); 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /fixtest/fixtest.depend: -------------------------------------------------------------------------------- 1 | # depslib dependency file v1.0 2 | 1298636296 source:g:\vrfx\libfixmath\fixtest\main.c 3 | 4 | 5 | 6 | 7 | 8 | 9 | "hiclock.h" 10 | 11 | 1298453688 g:\vrfx\libfixmath\\libfixmath\fixmath.h 12 | "uint32.h" 13 | "fract32.h" 14 | "fix16.h" 15 | 16 | 1298453688 g:\vrfx\libfixmath\\libfixmath\uint32.h 17 | 18 | 19 | 1298453688 g:\vrfx\libfixmath\\libfixmath\fract32.h 20 | 21 | 22 | 1298648502 g:\vrfx\libfixmath\\libfixmath\fix16.h 23 | 24 | 25 | 1298562746 source:g:\vrfx\libfixmath\fixtest\hiclock.c 26 | "hiclock.h" 27 | 28 | 1298564182 g:\vrfx\libfixmath\fixtest\hiclock.h 29 | 30 | 31 | 32 | 33 | 34 | 35 | 1298567472 source:i:\vrfx\libfixmath\fixtest\main.c 36 | 37 | 38 | 39 | 40 | 41 | 42 | "hiclock.h" 43 | 44 | 1298453688 i:\vrfx\libfixmath\\libfixmath\fixmath.h 45 | "uint32.h" 46 | "fract32.h" 47 | "fix16.h" 48 | 49 | 1298453688 i:\vrfx\libfixmath\\libfixmath\uint32.h 50 | 51 | 52 | 1298453688 i:\vrfx\libfixmath\\libfixmath\fract32.h 53 | 54 | 55 | 1298577432 i:\vrfx\libfixmath\\libfixmath\fix16.h 56 | 57 | 58 | 1298564182 i:\vrfx\libfixmath\fixtest\hiclock.h 59 | 60 | 61 | 62 | 63 | 64 | 65 | 1298562746 source:i:\vrfx\libfixmath\fixtest\hiclock.c 66 | "hiclock.h" 67 | 68 | -------------------------------------------------------------------------------- /libfixmath/libfixmath.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 71 | 72 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # This file is used to ignore files which are generated 2 | # ---------------------------------------------------------------------------- 3 | 4 | *~ 5 | *.autosave 6 | *.a 7 | *.core 8 | *.moc 9 | *.o 10 | *.obj 11 | *.orig 12 | *.rej 13 | *.so 14 | *.so.* 15 | *_pch.h.cpp 16 | *_resource.rc 17 | *.qm 18 | .#* 19 | *.*# 20 | core 21 | !core/ 22 | tags 23 | .DS_Store 24 | .directory 25 | *.debug 26 | Makefile* 27 | *.prl 28 | *.app 29 | moc_*.cpp 30 | ui_*.h 31 | qrc_*.cpp 32 | Thumbs.db 33 | *.res 34 | *.rc 35 | /.qmake.cache 36 | /.qmake.stash 37 | 38 | # qtcreator generated files 39 | *.pro.user* 40 | 41 | # xemacs temporary files 42 | *.flc 43 | 44 | # Vim temporary files 45 | .*.swp 46 | 47 | # Visual Studio generated files 48 | *.ib_pdb_index 49 | *.idb 50 | *.ilk 51 | *.pdb 52 | *.sln 53 | *.suo 54 | *.vcproj 55 | *vcproj.*.*.user 56 | *.ncb 57 | *.sdf 58 | *.opensdf 59 | *.vcxproj 60 | *vcxproj.* 61 | 62 | # MinGW generated files 63 | *.Debug 64 | *.Release 65 | 66 | # Python byte code 67 | *.pyc 68 | 69 | # Binaries 70 | # -------- 71 | *.dll 72 | *.exe 73 | 74 | # Prerequisites 75 | *.d 76 | 77 | # Compiled Object files 78 | *.slo 79 | *.lo 80 | *.o 81 | *.obj 82 | 83 | # Precompiled Headers 84 | *.gch 85 | *.pch 86 | 87 | # Compiled Dynamic libraries 88 | *.so 89 | *.dylib 90 | *.dll 91 | 92 | # Fortran module files 93 | *.mod 94 | *.smod 95 | 96 | # Compiled Static libraries 97 | *.lai 98 | *.la 99 | *.a 100 | *.lib 101 | 102 | # Executables 103 | *.exe 104 | *.out 105 | *.app 106 | 107 | # C++ objects and libs 108 | *.slo 109 | *.lo 110 | *.o 111 | *.a 112 | *.la 113 | *.lai 114 | *.so 115 | *.so.* 116 | *.dll 117 | *.dylib 118 | 119 | # Qt-es 120 | object_script.*.Release 121 | object_script.*.Debug 122 | *_plugin_import.cpp 123 | /.qmake.cache 124 | /.qmake.stash 125 | *.pro.user 126 | *.pro.user.* 127 | *.qbs.user 128 | *.qbs.user.* 129 | *.moc 130 | moc_*.cpp 131 | moc_*.h 132 | qrc_*.cpp 133 | ui_*.h 134 | *.qmlc 135 | *.jsc 136 | Makefile* 137 | *build-* 138 | *.qm 139 | *.prl 140 | 141 | # Qt unit tests 142 | target_wrapper.* 143 | 144 | # QtCreator 145 | *.autosave 146 | 147 | # QtCreator Qml 148 | *.qmlproject.user 149 | *.qmlproject.user.* 150 | 151 | # QtCreator CMake 152 | CMakeLists.txt.user* 153 | 154 | # QtCreator 4.8< compilation database 155 | compile_commands.json 156 | 157 | # QtCreator local machine specific files for imported projects 158 | *creator.user* 159 | 160 | *_qmlcache.qrc 161 | 162 | benchmarks/testcases.c 163 | 164 | *.elf 165 | -------------------------------------------------------------------------------- /tests/tests.c: -------------------------------------------------------------------------------- 1 | #include "tests.h" 2 | #include "tests_basic.h" 3 | #include "tests_lerp.h" 4 | #include "tests_macros.h" 5 | #include "tests_sqrt.h" 6 | #include "tests_str.h" 7 | #include 8 | 9 | const fix16_t testcases[] = { 10 | // Small numbers 11 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 12 | 13 | // Integer numbers 14 | 0x10000, -0x10000, 0x20000, -0x20000, 0x30000, -0x30000, 0x40000, -0x40000, 15 | 0x50000, -0x50000, 0x60000, -0x60000, 16 | 17 | // Fractions (1/2, 1/4, 1/8) 18 | 0x8000, -0x8000, 0x4000, -0x4000, 0x2000, -0x2000, 19 | 20 | // Problematic carry 21 | 0xFFFF, -0xFFFF, 0x1FFFF, -0x1FFFF, 0x3FFFF, -0x3FFFF, 22 | 23 | // Smallest and largest values 24 | 0x7FFFFFFF, 0x80000000, 25 | 26 | // Large random numbers 27 | 831858892, 574794913, 2147272293, -469161054, -961611615, 1841960234, 28 | 1992698389, 520485404, 560523116, -2094993050, -876897543, -67813629, 29 | 2146227091, 509861939, -1073573657, 30 | 31 | // Small random numbers 32 | -14985, 30520, -83587, 41129, 42137, 58537, -2259, 84142, -28283, 90914, 33 | 19865, 33191, 81844, -66273, -63215, -44459, -11326, 84295, 47515, -39324, 34 | 35 | // Tiny random numbers 36 | -171, -359, 491, 844, 158, -413, -422, -737, -575, -330, -376, 435, -311, 37 | 116, 715, -1024, -487, 59, 724, 993}; 38 | 39 | unsigned stack_depth = 0; 40 | 41 | int main() 42 | { 43 | printf("\033[1;34m\nVARIANT: \033[39m" STR2(PREFIX) "\033[0m\n"); 44 | #if 0 45 | fix16_t a = 65536; 46 | fix16_t b = -2147483648; 47 | fix16_t result = fix16_div(a, b); 48 | 49 | double fa = fix16_to_dbl(a); 50 | double fb = fix16_to_dbl(b); 51 | double fresult = fa / fb; 52 | 53 | double max = fix16_to_dbl(fix16_maximum); 54 | double min = fix16_to_dbl(fix16_minimum); 55 | 56 | printf("result %i, %.20f\n", result, fix16_to_dbl(result)); 57 | printf("fresult %i, %.20f\n", fix16_from_dbl(fresult), fresult); 58 | 59 | if ((fa / fb) > max || (fa / fb) < min) 60 | { 61 | #ifndef FIXMATH_NO_OVERFLOW 62 | ASSERT_EQ_INT(result, fix16_overflow); 63 | #endif 64 | } 65 | else 66 | { 67 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 68 | fix16_to_dbl(fix16_eps), "%i / %i \n", a, b); 69 | } 70 | 71 | #else 72 | TEST(test_abs()); 73 | TEST(test_add()); 74 | TEST(test_mul()); 75 | TEST(test_div()); 76 | TEST(test_sub()); 77 | TEST(test_sqrt()); 78 | TEST(test_lerp()); 79 | TEST(test_macros()); 80 | //TEST(test_str()); 81 | #endif 82 | return 0; 83 | } 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # libfixmath 2 | 3 | This is a mirror of the libfixmath's original SVN repository on Google Code. 4 | 5 | **Not actively maintained, pull requests welcome.** 6 | 7 | Libfixmath implements Q16.16 format fixed point operations in C. 8 | 9 | License: MIT 10 | 11 | # Options 12 | 13 | Configuration options are compile definitions that are checked by the preprocessor with `#ifdef` and `#ifndef`. All of these are undefined by default. 14 | 15 | #### `FIXMATH_FAST_SIN` 16 | 17 | - `#ifndef`: Most accurate version, accurate to ~2.1%. 18 | - `#ifdef`: Fast implementation, runs at 159% the speed of above 'accurate' version with a slightly lower accuracy of ~2.3%. 19 | 20 | #### `FIXMATH_NO_64BIT` 21 | 22 | - `#ifndef`: For compilers/platforms that have `uint64_t`. 23 | - `#ifdef`: For compilers/platforms that do not have `uint64_t`. 24 | 25 | #### `FIXMATH_NO_CACHE` 26 | 27 | - `#ifndef`: Use static memory caches for exponents (32KB) and trigonometry (80KB). 28 | - `#ifdef`: Do not use caches. 29 | 30 | #### `FIXMATH_NO_HARD_DIVISION` 31 | 32 | Note: will be automatically defined if `FIXMATH_OPTIMIZE_8BIT` is defined. 33 | 34 | - `#ifndef`: For platforms that have hardware integer division. 35 | - `#ifdef`: For platforms that do not have hardware integer division. 36 | 37 | #### `FIXMATH_NO_OVERFLOW` 38 | 39 | - `#ifndef`: Check for overflow and return the overflow constants. 40 | - `#ifdef`: Do not check for overflow. 41 | 42 | #### `FIXMATH_NO_ROUNDING` 43 | 44 | - `#ifndef`: Use rounding. 45 | - `#ifdef`: Do not use rounding. 46 | 47 | #### `FIXMATH_OPTIMIZE_8BIT` 48 | 49 | - `#ifndef`: Do not optimize for processors with 8-bit multiplication like Atmel AVR. 50 | - `#ifdef`: Optimize for processors like Atmel AVR. Also defines `FIXMATH_NO_HARD_DIVISION` automatically in `fix16.h`. 51 | 52 | # Include the `libfixmath` library in your CMake Project 53 | 54 | The simplest way to use `libfixmath` as a dependency is with CMake's [FetchContent API](https://cmake.org/cmake/help/latest/module/FetchContent.html). 55 | 56 | ```cmake 57 | include(FetchContent) 58 | FetchContent_Declare( 59 | libfixmath 60 | GIT_REPOSITORY https://github.com/PetteriAimonen/libfixmath.git 61 | GIT_TAG 62 | ) 63 | FetchContent_MakeAvailable(libfixmath) 64 | 65 | target_compile_definitions(libfixmath PRIVATE 66 | # FIXMATH_FAST_SIN 67 | # FIXMATH_NO_64BIT 68 | # FIXMATH_NO_CACHE 69 | # FIXMATH_NO_HARD_DIVISION 70 | # FIXMATH_NO_OVERFLOW 71 | # FIXMATH_NO_ROUNDING 72 | # FIXMATH_OPTIMIZE_8BIT 73 | ) 74 | 75 | target_link_libraries(my_cmake_project PRIVATE libfixmath) 76 | ``` -------------------------------------------------------------------------------- /fixtest/main.c: -------------------------------------------------------------------------------- 1 | #include "hiclock.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define EXP 10 | //#define ATAN 11 | //#define SIN 12 | //#define SQRT 13 | 14 | #ifdef EXP 15 | #define fix_func fix16_exp 16 | #define fix_func_str "fix16_exp" 17 | #define flt_func expf 18 | #define flt_func_str "expf" 19 | #endif 20 | 21 | #ifdef ATAN 22 | #define fix_func fix16_atan 23 | #define fix_func_str "fix16_atan" 24 | #define flt_func atanf 25 | #define flt_func_str "atanf" 26 | #endif 27 | 28 | #ifdef SIN 29 | #define fix_func fix16_sin 30 | #define fix_func_str "fix16_sin" 31 | #define flt_func sinf 32 | #define flt_func_str "sinf" 33 | #endif 34 | 35 | #ifdef SQRT 36 | #define fix_func fix16_sqrt 37 | #define fix_func_str "fix16_sqrt" 38 | #define flt_func sqrtf 39 | #define flt_func_str "sqrtf" 40 | #endif 41 | 42 | int main(int argc, char **argv) 43 | { 44 | printf("libfixmath test tool\n"); 45 | 46 | hiclock_init(); 47 | 48 | uintptr_t args = (1 << 8); 49 | uintptr_t iter = (1 << 8); 50 | uintptr_t pass = (1 << 8); 51 | 52 | uintptr_t i; 53 | srand(time(NULL)); 54 | 55 | hiclock_t fix_duration = 0; 56 | hiclock_t flt_duration = 0; 57 | fix16_t fix_error = 0; 58 | 59 | uintptr_t k; 60 | for (k = 0; k < pass; k++) 61 | { 62 | fix16_t fix_args[args]; 63 | for (i = 0; i < args; i++) fix_args[i] = (rand() ^ (rand() << 16)); 64 | fix16_t fix_result[args]; 65 | hiclock_t fix_start = hiclock(); 66 | for (i = 0; i < iter; i++) 67 | { 68 | uintptr_t j; 69 | for (j = 0; j < args; j++) fix_result[j] = fix_func(fix_args[j]); 70 | } 71 | hiclock_t fix_end = hiclock(); 72 | 73 | float flt_args[args]; 74 | for (i = 0; i < args; i++) flt_args[i] = fix16_to_float(fix_args[i]); 75 | float flt_result[args]; 76 | hiclock_t flt_start = hiclock(); 77 | for (i = 0; i < iter; i++) 78 | { 79 | uintptr_t j; 80 | for (j = 0; j < args; j++) flt_result[j] = flt_func(flt_args[j]); 81 | } 82 | hiclock_t flt_end = hiclock(); 83 | 84 | for (i = 0; i < args; i++) 85 | fix_error += abs(fix16_from_float(flt_result[i]) - fix_result[i]); 86 | flt_duration += (flt_end - flt_start); 87 | fix_duration += (fix_end - fix_start); 88 | } 89 | 90 | printf("%16s: %08" PRIuHICLOCK " @ %" PRIu32 "Hz\n", flt_func_str, flt_duration, HICLOCKS_PER_SEC); 91 | printf("%16s: %08" PRIuHICLOCK " @ %" PRIu32 "Hz\n", fix_func_str, fix_duration, HICLOCKS_PER_SEC); 92 | printf(" Difference: %08" PRIiHICLOCK " (% 3.2f%%)\n", (flt_duration - fix_duration), ((fix_duration * 100.0) / flt_duration)); 93 | printf(" Error: %f%%\n", ((fix16_to_dbl(fix_error) * 100.0) / (args * pass))); 94 | 95 | return EXIT_SUCCESS; 96 | } 97 | -------------------------------------------------------------------------------- /libfixmath/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 = fix_abs(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) 30 | bit >>= 2; 31 | 32 | // The main part is executed twice, in order to avoid 33 | // using 64 bit values in computations. 34 | for (n = 0; n < 2; n++) 35 | { 36 | // First we get the top 24 bits of the answer. 37 | while (bit) 38 | { 39 | if (num >= result + bit) 40 | { 41 | num -= result + bit; 42 | result = (result >> 1) + bit; 43 | } 44 | else 45 | { 46 | result = (result >> 1); 47 | } 48 | bit >>= 2; 49 | } 50 | 51 | if (n == 0) 52 | { 53 | // Then process it again to get the lowest 8 bits. 54 | if (num > 65535) 55 | { 56 | // The remainder 'num' is too large to be shifted left 57 | // by 16, so we have to add 1 to result manually and 58 | // adjust 'num' accordingly. 59 | // num = a - (result + 0.5)^2 60 | // = num + result^2 - (result + 0.5)^2 61 | // = num - result - 0.5 62 | num -= result; 63 | num = (num << 16) - 0x8000; 64 | result = (result << 16) + 0x8000; 65 | } 66 | else 67 | { 68 | num <<= 16; 69 | result <<= 16; 70 | } 71 | 72 | bit = 1 << 14; 73 | } 74 | } 75 | 76 | #ifndef FIXMATH_NO_ROUNDING 77 | // Finally, if next bit would have been 1, round the result upwards. 78 | if (num > result) 79 | { 80 | result++; 81 | } 82 | #endif 83 | 84 | return (neg ? -(fix16_t)result : (fix16_t)result); 85 | } 86 | -------------------------------------------------------------------------------- /tests/tests_macros.c: -------------------------------------------------------------------------------- 1 | #include "tests_macros.h" 2 | #include "tests.h" 3 | 4 | #define DO_TEST(i, m) \ 5 | do \ 6 | { \ 7 | ASSERT_EQ_INT(F16(i##.##m), F16C(i, m)); \ 8 | ASSERT_EQ_INT(F16(i##.##m), fix16_from_dbl(i##.##m)); \ 9 | } while (0) 10 | 11 | int test_macros() 12 | { 13 | DO_TEST(1, 234); 14 | DO_TEST(0, 0); 15 | DO_TEST(1, 0); 16 | DO_TEST(-1, 0); 17 | DO_TEST(1, 5); 18 | DO_TEST(-1, 5); 19 | DO_TEST(000000, 00000); 20 | DO_TEST(0, 00001); 21 | DO_TEST(0, 00010); 22 | DO_TEST(0, 1); 23 | DO_TEST(0, 10001); 24 | DO_TEST(0, 11000); 25 | DO_TEST(25, 133); 26 | DO_TEST(32767, 00000); 27 | DO_TEST(32767, 00001); 28 | DO_TEST(32767, 99999); 29 | DO_TEST(0, 25); 30 | DO_TEST(0, 99555); 31 | DO_TEST(0, 99998); 32 | DO_TEST(0, 99999); 33 | DO_TEST(-1, 1); 34 | DO_TEST(-25, 133); 35 | DO_TEST(-32767, 00001); 36 | DO_TEST(-32768, 00000); 37 | 38 | /* Random values */ 39 | DO_TEST(0, 02267); 40 | DO_TEST(1, 49887); 41 | DO_TEST(0, 27589); 42 | DO_TEST(0, 38393); 43 | DO_TEST(0, 08934); 44 | DO_TEST(0, 95820); 45 | DO_TEST(0, 95596); 46 | DO_TEST(72, 10642); 47 | DO_TEST(0, 48939); 48 | DO_TEST(3, 37797); 49 | DO_TEST(1, 09194); 50 | DO_TEST(0, 08605); 51 | DO_TEST(3, 04349); 52 | DO_TEST(3, 95401); 53 | DO_TEST(15, 36292); 54 | DO_TEST(56, 09242); 55 | DO_TEST(0, 54071); 56 | DO_TEST(27, 08953); 57 | DO_TEST(0, 03913); 58 | DO_TEST(1, 32707); 59 | DO_TEST(4, 50117); 60 | DO_TEST(0, 24990); 61 | DO_TEST(44, 77319); 62 | DO_TEST(2, 59139); 63 | DO_TEST(0, 16279); 64 | DO_TEST(17, 14712); 65 | DO_TEST(11, 54281); 66 | DO_TEST(0, 02768); 67 | DO_TEST(0, 39278); 68 | DO_TEST(0, 19369); 69 | DO_TEST(-0, 04534); 70 | DO_TEST(-0, 00349); 71 | DO_TEST(-2, 30380); 72 | DO_TEST(-0, 03061); 73 | DO_TEST(-7, 50065); 74 | DO_TEST(-3, 97050); 75 | DO_TEST(-0, 43898); 76 | DO_TEST(-3, 49876); 77 | DO_TEST(-1, 35942); 78 | DO_TEST(-10, 81154); 79 | DO_TEST(-0, 26676); 80 | DO_TEST(-9, 52134); 81 | DO_TEST(-0, 42592); 82 | DO_TEST(-0, 05424); 83 | DO_TEST(-0, 62461); 84 | DO_TEST(-0, 21562); 85 | DO_TEST(-0, 22366); 86 | DO_TEST(-0, 09074); 87 | DO_TEST(-1, 29527); 88 | DO_TEST(-4, 98427); 89 | DO_TEST(-0, 10721); 90 | DO_TEST(-11, 39446); 91 | DO_TEST(-451, 53916); 92 | DO_TEST(-0, 04279); 93 | DO_TEST(-3, 36543); 94 | DO_TEST(-0, 01003); 95 | DO_TEST(-12, 08326); 96 | DO_TEST(-1, 07143); 97 | DO_TEST(-1, 07737); 98 | DO_TEST(-0, 22957); 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /tests/tests.cmake: -------------------------------------------------------------------------------- 1 | file(GLOB tests-srcs tests/*.c tests/*.h) 2 | 3 | enable_testing() 4 | 5 | # -fno-sanitize-recover 6 | if(APPLE AND CMAKE_OSX_ARCHITECTURES MATCHES "ppc|ppc64") 7 | # Darwin PPC does not support ubsan presently. 8 | set(sanitizer_opts "") 9 | else() 10 | set(sanitizer_opts -fsanitize=undefined) 11 | endif() 12 | 13 | add_custom_target(make_tests) 14 | 15 | function(create_variant name) 16 | add_library(libfixmath_${name} STATIC ${libfixmath-srcs}) 17 | target_include_directories(libfixmath_${name} INTERFACE 18 | ${CMAKE_CURRENT_SOURCE_DIR}) 19 | target_compile_definitions(libfixmath_${name} PRIVATE PREFIX=${name} ${ARGN}) 20 | target_compile_options(libfixmath_${name} PRIVATE ${sanitizer_opts}) 21 | target_link_options(libfixmath_${name} PRIVATE ${sanitizer_opts}) 22 | add_executable(tests_${name} ${tests-srcs}) 23 | target_link_libraries(tests_${name} PRIVATE libfixmath_${name} m) 24 | target_include_directories(tests_${name} PRIVATE ${CMAKE_SOURCE_DIR}) 25 | target_compile_definitions(tests_${name} PRIVATE PREFIX=${name} ${ARGN}) 26 | target_compile_options(tests_${name} PRIVATE ${sanitizer_opts}) 27 | target_link_options(tests_${name} PRIVATE ${sanitizer_opts}) 28 | add_dependencies(make_tests tests_${name}) 29 | add_test(NAME tests_${name} COMMAND tests_${name}) 30 | endfunction() 31 | 32 | create_variant(ro64) 33 | create_variant(no64 FIXMATH_NO_ROUNDING) 34 | create_variant(rn64 FIXMATH_NO_OVERFLOW) 35 | create_variant(nn64 FIXMATH_NO_ROUNDING FIXMATH_NO_OVERFLOW) 36 | create_variant(ro32 FIXMATH_NO_64BIT) 37 | create_variant(no32 FIXMATH_NO_ROUNDING FIXMATH_NO_64BIT) 38 | create_variant(rn32 FIXMATH_NO_OVERFLOW FIXMATH_NO_64BIT) 39 | create_variant(nn32 FIXMATH_NO_OVERFLOW FIXMATH_NO_ROUNDING FIXMATH_NO_64BIT) 40 | create_variant(ro08 FIXMATH_OPTIMIZE_8BIT) 41 | create_variant(no08 FIXMATH_NO_ROUNDING FIXMATH_OPTIMIZE_8BIT) 42 | create_variant(rn08 FIXMATH_NO_OVERFLOW FIXMATH_OPTIMIZE_8BIT) 43 | create_variant(nn08 FIXMATH_NO_OVERFLOW FIXMATH_NO_ROUNDING FIXMATH_OPTIMIZE_8BIT) 44 | 45 | create_variant(ro64div FIXMATH_NO_HARD_DIV) 46 | create_variant(no64div FIXMATH_NO_HARD_DIV FIXMATH_NO_ROUNDING) 47 | create_variant(rn64div FIXMATH_NO_HARD_DIV FIXMATH_NO_OVERFLOW) 48 | create_variant(nn64div FIXMATH_NO_HARD_DIV FIXMATH_NO_ROUNDING FIXMATH_NO_OVERFLOW) 49 | create_variant(ro32div FIXMATH_NO_HARD_DIV FIXMATH_NO_64BIT) 50 | create_variant(no32div FIXMATH_NO_HARD_DIV FIXMATH_NO_ROUNDING FIXMATH_NO_64BIT) 51 | create_variant(rn32div FIXMATH_NO_HARD_DIV FIXMATH_NO_OVERFLOW FIXMATH_NO_64BIT) 52 | create_variant(nn32div FIXMATH_NO_HARD_DIV FIXMATH_NO_OVERFLOW FIXMATH_NO_ROUNDING FIXMATH_NO_64BIT) 53 | create_variant(ro08div FIXMATH_NO_HARD_DIV FIXMATH_OPTIMIZE_8BIT) 54 | create_variant(no08div FIXMATH_NO_HARD_DIV FIXMATH_NO_ROUNDING FIXMATH_OPTIMIZE_8BIT) 55 | create_variant(rn08div FIXMATH_NO_HARD_DIV FIXMATH_NO_OVERFLOW FIXMATH_OPTIMIZE_8BIT) 56 | create_variant(nn08div FIXMATH_NO_HARD_DIV FIXMATH_NO_OVERFLOW FIXMATH_NO_ROUNDING FIXMATH_OPTIMIZE_8BIT) 57 | -------------------------------------------------------------------------------- /tests/tests_str.c: -------------------------------------------------------------------------------- 1 | #include "tests_str.h" 2 | #include "tests.h" 3 | 4 | int test_str_to() 5 | { 6 | char buf[13]; 7 | fix16_to_str(fix16_from_dbl(1234.5678), buf, 4); 8 | ASSERT_EQ_STR(buf, "1234.5678"); 9 | 10 | fix16_to_str(fix16_from_dbl(-1234.5678), buf, 4); 11 | ASSERT_EQ_STR(buf, "-1234.5678"); 12 | 13 | fix16_to_str(0, buf, 0); 14 | ASSERT_EQ_STR(buf, "0"); 15 | 16 | fix16_to_str(fix16_from_dbl(0.9), buf, 0); 17 | ASSERT_EQ_STR(buf, "1"); 18 | 19 | fix16_to_str(1, buf, 5); 20 | ASSERT_EQ_STR(buf, "0.00002"); 21 | 22 | fix16_to_str(-1, buf, 5); 23 | ASSERT_EQ_STR(buf, "-0.00002"); 24 | 25 | fix16_to_str(65535, buf, 5); 26 | ASSERT_EQ_STR(buf, "0.99998"); 27 | 28 | fix16_to_str(65535, buf, 4); 29 | ASSERT_EQ_STR(buf, "1.0000"); 30 | 31 | fix16_to_str(fix16_maximum, buf, 5); 32 | ASSERT_EQ_STR(buf, "32767.99998"); 33 | 34 | fix16_to_str(fix16_minimum, buf, 5); 35 | ASSERT_EQ_STR(buf, "-32768.00000"); 36 | 37 | return 0; 38 | } 39 | 40 | int test_str_from() 41 | { 42 | ASSERT_EQ_INT(fix16_from_str("1234.5678"), fix16_from_dbl(1234.5678)); 43 | ASSERT_EQ_INT(fix16_from_str("-1234.5678"), fix16_from_dbl(-1234.5678)); 44 | ASSERT_EQ_INT(fix16_from_str(" +1234,56780 "), 45 | fix16_from_dbl(1234.5678)); 46 | 47 | ASSERT_EQ_INT(fix16_from_str("0"), 0); 48 | ASSERT_EQ_INT(fix16_from_str("1"), fix16_one); 49 | ASSERT_EQ_INT(fix16_from_str("1.0"), fix16_one); 50 | ASSERT_EQ_INT(fix16_from_str("1.0000000000"), fix16_one); 51 | 52 | ASSERT_EQ_INT(fix16_from_str("0.00002"), 1); 53 | ASSERT_EQ_INT(fix16_from_str("0.99998"), 65535); 54 | 55 | ASSERT_EQ_INT(fix16_from_str("32767.99998"), fix16_maximum); 56 | ASSERT_EQ_INT(fix16_from_str("-32768.00000"), fix16_minimum); 57 | 58 | return 0; 59 | } 60 | 61 | int test_str_extended() 62 | { 63 | 64 | fix16_t value = fix16_minimum; 65 | char testbuf[13]; 66 | char goodbuf[13]; 67 | 68 | while (value < fix16_maximum) 69 | { 70 | double fvalue = fix16_to_dbl(value); 71 | 72 | /* Turns out we have to jump through some hoops to round 73 | doubles perfectly for printing: 74 | http://stackoverflow.com/questions/994764/rounding-doubles-5-sprintf 75 | */ 76 | fvalue = round(fvalue * 100000.) / 100000.; 77 | 78 | snprintf(goodbuf, 13, "%0.5f", fvalue); 79 | fix16_to_str(value, testbuf, 5); 80 | 81 | if (strcmp(goodbuf, testbuf) != 0) 82 | { 83 | printf("Value (fix16_t)%d gave %s, should be %s\n", value, testbuf, 84 | goodbuf); 85 | return 1; 86 | } 87 | 88 | fix16_t roundtrip = fix16_from_str(testbuf); 89 | if (roundtrip != value) 90 | { 91 | printf("Roundtrip failed: (fix16_t)%d -> %s -> (fix16_t)%d\n", 92 | value, testbuf, roundtrip); 93 | return 1; 94 | } 95 | 96 | value += 0x10001; 97 | } 98 | 99 | return 0; 100 | } 101 | 102 | int test_str() 103 | { 104 | TEST(test_str_to()); 105 | TEST(test_str_from()); 106 | TEST(test_str_extended()); 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /libfixmath/fix16_str.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #ifdef __KERNEL__ 3 | #include 4 | #else 5 | #include 6 | #endif 7 | #if defined(FIXMATH_NO_CTYPE) || defined(__KERNEL__) 8 | static inline int isdigit(int c) 9 | { 10 | return c >= '0' && c <= '9'; 11 | } 12 | 13 | static inline int isspace(int c) 14 | { 15 | return c == ' ' || c == '\r' || c == '\n' || c == '\t' || c == '\v' || c == '\f'; 16 | } 17 | #else 18 | #include 19 | #endif 20 | 21 | static const uint32_t scales[8] = { 22 | /* 5 decimals is enough for full fix16_t precision */ 23 | 1, 10, 100, 1000, 10000, 100000, 100000, 100000 24 | }; 25 | 26 | static char *itoa_loop(char *buf, uint32_t scale, uint32_t value, bool skip) 27 | { 28 | while (scale) 29 | { 30 | unsigned digit = (value / scale); 31 | 32 | if (!skip || digit || scale == 1) 33 | { 34 | skip = false; 35 | *buf++ = '0' + digit; 36 | value %= scale; 37 | } 38 | 39 | scale /= 10; 40 | } 41 | return buf; 42 | } 43 | 44 | void fix16_to_str(fix16_t value, char *buf, int decimals) 45 | { 46 | uint32_t uvalue = (value >= 0) ? value : -value; 47 | if (value < 0) 48 | *buf++ = '-'; 49 | 50 | /* Separate the integer and decimal parts of the value */ 51 | unsigned intpart = uvalue >> 16; 52 | uint32_t fracpart = uvalue & 0xFFFF; 53 | uint32_t scale = scales[decimals & 7]; 54 | fracpart = fix16_mul(fracpart, scale); 55 | 56 | if (fracpart >= scale) 57 | { 58 | /* Handle carry from decimal part */ 59 | intpart++; 60 | fracpart -= scale; 61 | } 62 | 63 | /* Format integer part */ 64 | buf = itoa_loop(buf, 10000, intpart, true); 65 | 66 | /* Format decimal part (if any) */ 67 | if (scale != 1) 68 | { 69 | *buf++ = '.'; 70 | buf = itoa_loop(buf, scale / 10, fracpart, false); 71 | } 72 | 73 | *buf = '\0'; 74 | } 75 | 76 | fix16_t fix16_from_str(const char *buf) 77 | { 78 | while (isspace((unsigned char) *buf)) 79 | buf++; 80 | 81 | /* Decode the sign */ 82 | bool negative = (*buf == '-'); 83 | if (*buf == '+' || *buf == '-') 84 | buf++; 85 | 86 | /* Decode the integer part */ 87 | uint32_t intpart = 0; 88 | int count = 0; 89 | while (isdigit((unsigned char) *buf)) 90 | { 91 | intpart *= 10; 92 | intpart += *buf++ - '0'; 93 | count++; 94 | } 95 | 96 | #ifdef FIXMATH_NO_OVERFLOW 97 | if (count == 0) 98 | return fix16_overflow; 99 | #else 100 | if (count == 0 || count > 5 101 | || intpart > 32768 || (!negative && intpart > 32767)) 102 | return fix16_overflow; 103 | #endif 104 | 105 | fix16_t value = intpart << 16; 106 | 107 | /* Decode the decimal part */ 108 | if (*buf == '.' || *buf == ',') 109 | { 110 | buf++; 111 | 112 | uint32_t fracpart = 0; 113 | uint32_t scale = 1; 114 | while (isdigit((unsigned char) *buf) && scale < 100000) 115 | { 116 | scale *= 10; 117 | fracpart *= 10; 118 | fracpart += *buf++ - '0'; 119 | } 120 | 121 | value += fix16_div(fracpart, scale); 122 | } 123 | 124 | /* Verify that there is no garbage left over */ 125 | while (*buf != '\0') 126 | { 127 | if (!isdigit((unsigned char) *buf) && !isspace((unsigned char) *buf)) 128 | return fix16_overflow; 129 | 130 | buf++; 131 | } 132 | 133 | return negative ? -value : value; 134 | } 135 | 136 | -------------------------------------------------------------------------------- /benchmarks/generate_testcases.py: -------------------------------------------------------------------------------- 1 | '''This script precalculates the correct solutions for a set of test numbers, 2 | and writes them to testcases.c. This is aimed for running the tests on-target, 3 | therefore it doesn't test all the cases or use floating point math, but 4 | instead generates a ~10k binary. 5 | 6 | The tests are chosen randomly, so there is quite good chance to eventually 7 | catch most errors. Because the list is not regenerated automatically, the 8 | functioning of the benchmark application is still deterministic and easy 9 | to debug. 10 | ''' 11 | 12 | import math 13 | import random 14 | import struct 15 | 16 | # Fix16 scaling factor 17 | scale = 65536. 18 | 19 | # Fix16 overflow indicator 20 | overflow = -2**31 21 | 22 | def f16_to_float(val): 23 | return val / scale 24 | 25 | def float_to_f16(val): 26 | val = int(round(val * scale)) 27 | if val >= 2**31 or val < -2**31: 28 | val = overflow 29 | return val 30 | 31 | def to_ui32(val): 32 | return struct.unpack('I', struct.pack('i', val))[0] 33 | 34 | testcases = [ 35 | # Small numbers 36 | 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 37 | -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, 38 | 39 | # Integer numbers 40 | 0x10000, -0x10000, 0x20000, -0x20000, 0x30000, -0x30000, 41 | 0x40000, -0x40000, 0x50000, -0x50000, 0x60000, -0x60000, 42 | 43 | # Fractions (1/2, 1/4, 1/8) 44 | 0x8000, -0x8000, 0x4000, -0x4000, 0x2000, -0x2000, 45 | 46 | # Problematic carry 47 | 0xFFFF, -0xFFFF, 0x1FFFF, -0x1FFFF, 0x3FFFF, -0x3FFFF, 48 | 49 | # Smallest and largest values 50 | 0x7FFFFFFF, -0x80000000 51 | ] 52 | 53 | for i in range(10): 54 | # Large random numbers 55 | testcases.append(random.randint(-0x80000000, 0x7FFFFFFF)) 56 | 57 | # Small random numbers 58 | testcases.append(random.randint(-100000, 100000)) 59 | 60 | # Tiny random numbers 61 | testcases.append(random.randint(-200, 200)) 62 | 63 | out = open("testcases.c", "w") 64 | 65 | out.write(''' 66 | /* Automatically generated testcases for fix16 operations 67 | * See generate_testcases.py for the generator. 68 | */ 69 | 70 | #include 71 | 72 | typedef struct { 73 | // Input 74 | fix16_t a; 75 | 76 | // Correct output 77 | fix16_t sqrt; 78 | fix16_t exp; 79 | } fix16_1op_testcase; 80 | 81 | typedef struct { 82 | // Inputs 83 | fix16_t a; 84 | fix16_t b; 85 | 86 | // Correct output 87 | fix16_t add; 88 | fix16_t sub; 89 | fix16_t mul; 90 | fix16_t div; 91 | } fix16_2op_testcase; 92 | 93 | #define TESTCASES1_COUNT (sizeof(testcases1)/sizeof(testcases1[0])) 94 | #define TESTCASES2_COUNT (sizeof(testcases2)/sizeof(testcases2[0])) 95 | 96 | ''') 97 | 98 | # Write testcases for 1-operand functions 99 | 100 | out.write('static const fix16_1op_testcase testcases1[] = {\n') 101 | 102 | for i in range(10): 103 | a = random.choice(testcases) 104 | if a >= 0: 105 | sqrt = float_to_f16(math.sqrt(f16_to_float(a))) 106 | else: 107 | sqrt = 0 108 | 109 | try: 110 | exp = float_to_f16(math.exp(f16_to_float(a))) 111 | except OverflowError: 112 | exp = 0x7FFFFFFF 113 | 114 | out.write(' {0x%08x, 0x%08x, 0x%08x}, // %d\n' 115 | % (to_ui32(a), to_ui32(sqrt), to_ui32(exp), i)) 116 | 117 | out.write('};\n\n') 118 | 119 | # Write testcases for 2-operand functions 120 | 121 | out.write('static const fix16_2op_testcase testcases2[] = {\n') 122 | 123 | for i in range(50): 124 | a = random.choice(testcases) 125 | b = random.choice(testcases) 126 | 127 | add = float_to_f16(f16_to_float(a) + f16_to_float(b)) 128 | sub = float_to_f16(f16_to_float(a) - f16_to_float(b)) 129 | mul = float_to_f16(f16_to_float(a) * f16_to_float(b)) 130 | if b != 0: 131 | div = float_to_f16(f16_to_float(a) / f16_to_float(b)) 132 | else: 133 | div = 0 134 | out.write(' {0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x, 0x%08x}, // %d\n' 135 | % (to_ui32(a), to_ui32(b), to_ui32(add), to_ui32(sub), to_ui32(mul), to_ui32(div), i)) 136 | 137 | out.write('};\n\n') 138 | 139 | out.close() 140 | 141 | -------------------------------------------------------------------------------- /libfixmath/fix16_fft.c: -------------------------------------------------------------------------------- 1 | /* Real-input FFT implementation using the libfixmath fix16_t datatype. 2 | * Not the fastest implementation ever, but has a small code size. 3 | * 4 | * Refer to http://www.dspguide.com/ch12/2.htm for information on the 5 | * algorithm. 6 | * 7 | * (c) 2012 Petteri Aimonen 8 | * This file is released to public domain. 9 | */ 10 | 11 | #ifdef __KERNEL__ 12 | #include 13 | #else 14 | #include 15 | #endif 16 | #include "fix16.h" 17 | 18 | // You can change the input datatype and intermediate scaling here. 19 | // By default, the output is divided by the transform length to get a normalized FFT. 20 | // Input_convert determines the scaling of intermediate values. Multiplication by 256 21 | // gives a nice compromise between precision and numeric range. 22 | #ifndef INPUT_TYPE 23 | #define INPUT_TYPE uint8_t 24 | #endif 25 | 26 | #ifndef INPUT_CONVERT 27 | #define INPUT_CONVERT(x) ((x) << 8) 28 | #endif 29 | 30 | #ifndef INPUT_INDEX 31 | #define INPUT_INDEX(x) (x) 32 | #endif 33 | 34 | #ifndef OUTPUT_SCALE 35 | #define OUTPUT_SCALE(transform_size) (fix16_one * 256 / transform_size) 36 | #endif 37 | 38 | // Fast calculation of DFT for a 4-point signal. Based on the simplicity 39 | // of 4-point sinewave. 40 | static void four_point_dft(INPUT_TYPE *input, unsigned input_stride, 41 | fix16_t *real, fix16_t *imag) 42 | { 43 | fix16_t x0 = INPUT_CONVERT(input[0 * input_stride]); 44 | fix16_t x1 = INPUT_CONVERT(input[1 * input_stride]); 45 | fix16_t x2 = INPUT_CONVERT(input[2 * input_stride]); 46 | fix16_t x3 = INPUT_CONVERT(input[3 * input_stride]); 47 | 48 | real[0] = x0 + x1 + x2 + x3; 49 | imag[0] = 0; 50 | real[1] = x0 - x2; 51 | imag[1] = -x1 + x3; 52 | real[2] = x0 - x1 + x2 - x3; 53 | imag[2] = 0; 54 | real[3] = x0 - x2; 55 | imag[3] = x1 - x3; 56 | } 57 | 58 | // Mix N blocksize-sized transforms pairwise together to get N/2 2*blocksize-sized transforms. 59 | static void butterfly(fix16_t *real, fix16_t *imag, unsigned blocksize, unsigned blockpairs) 60 | { 61 | unsigned i, j; 62 | for (i = 0; i < blocksize; i++) 63 | { 64 | fix16_t angle = fix16_pi * i / blocksize; 65 | fix16_t c = fix16_cos(angle); 66 | fix16_t s = -fix16_sin(angle); 67 | 68 | fix16_t *rp = real + i; 69 | fix16_t *ip = imag + i; 70 | for (j = 0; j < blockpairs; j++) 71 | { 72 | // Get the odd-indexed tranform and multiply by sine 73 | fix16_t re = fix16_mul(rp[blocksize], c) - fix16_mul(ip[blocksize], s); 74 | fix16_t im = fix16_mul(ip[blocksize], c) + fix16_mul(rp[blocksize], s); 75 | 76 | // Update the transforms 77 | rp[blocksize] = rp[0] - re; 78 | ip[blocksize] = ip[0] - im; 79 | rp[0] += re; 80 | ip[0] += im; 81 | 82 | rp += blocksize * 2; 83 | ip += blocksize * 2; 84 | } 85 | } 86 | } 87 | 88 | // Reverse bits in a 32-bit number 89 | static uint32_t rbit_32(uint32_t x) 90 | { 91 | #if defined(__GNUC__) && defined(__ARM_ARCH_7M__) 92 | __asm__("rbit %0,%0" : "=r"(x) : "0"(x)); 93 | return x; 94 | #else 95 | x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1)); 96 | x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2)); 97 | x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4)); 98 | x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8)); 99 | return((x >> 16) | (x << 16)); 100 | #endif 101 | } 102 | 103 | // Reverse bits in an n-bit number. 104 | static uint32_t rbit_n(uint32_t x, unsigned n) 105 | { 106 | return rbit_32(x << (32 - n)); 107 | } 108 | 109 | // Base-2 integer logarithm 110 | static int ilog2(unsigned x) 111 | { 112 | int result = -1; 113 | while (x) 114 | { 115 | x >>= 1; 116 | result++; 117 | } 118 | return result; 119 | } 120 | 121 | // Compute a transform of the real-valued input array, and store results in two arrays. 122 | // Size of each array is the same as transform_length. 123 | // Transform length must be a power of two and atleast 4. 124 | void fix16_fft(INPUT_TYPE *input, fix16_t *real, fix16_t *imag, unsigned transform_length) 125 | { 126 | int log_length = ilog2(transform_length); 127 | transform_length = 1 << log_length; 128 | 129 | unsigned i; 130 | for (i = 0; i < transform_length / 4; i++) 131 | { 132 | four_point_dft(input + INPUT_INDEX(rbit_n(i, log_length - 2)), transform_length / 4, real + 4*i, imag + 4*i); 133 | } 134 | 135 | for (i = 2; i < (unsigned) log_length; i++) 136 | { 137 | butterfly(real, imag, 1 << i, transform_length / (2 << i)); 138 | } 139 | 140 | #ifdef OUTPUT_SCALE 141 | fix16_t scale = OUTPUT_SCALE(transform_length); 142 | for (i = 0; i < transform_length; i++) 143 | { 144 | real[i] = fix16_mul(real[i], scale); 145 | imag[i] = fix16_mul(imag[i], scale); 146 | } 147 | #endif 148 | } 149 | 150 | /* Just some test code 151 | #include 152 | int main() 153 | { 154 | INPUT_TYPE input[16] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; 155 | fix16_t real[16], imag[16]; 156 | 157 | fix16_fft(input, real, imag, 16); 158 | 159 | int count = 16; 160 | int i; 161 | for (i = 0; i < count; i++) 162 | { 163 | printf("%d: %0.4f, %0.4f\n", i, fix16_to_float(real[i]), fix16_to_float(imag[i])); 164 | } 165 | return 0; 166 | } 167 | */ 168 | -------------------------------------------------------------------------------- /libfixmath/fix16_exp.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #ifdef __KERNEL__ 3 | #include 4 | #define uint_fast8_t uint8_t 5 | #else 6 | #include 7 | #endif 8 | 9 | 10 | #ifndef FIXMATH_NO_CACHE 11 | static fix16_t _fix16_exp_cache_index[4096] = { 0 }; 12 | static fix16_t _fix16_exp_cache_value[4096] = { 0 }; 13 | #endif 14 | 15 | 16 | 17 | fix16_t fix16_exp(fix16_t inValue) { 18 | if(inValue == 0 ) return fix16_one; 19 | if(inValue == fix16_one) return fix16_e; 20 | if(inValue >= 681391 ) return fix16_maximum; 21 | if(inValue <= -772243 ) return 0; 22 | 23 | #ifndef FIXMATH_NO_CACHE 24 | fix16_t tempIndex = (inValue ^ (inValue >> 4)) & 0x0FFF; 25 | if(_fix16_exp_cache_index[tempIndex] == inValue) 26 | return _fix16_exp_cache_value[tempIndex]; 27 | #endif 28 | 29 | /* The algorithm is based on the power series for exp(x): 30 | * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition 31 | * 32 | * From term n, we get term n+1 by multiplying with x/n. 33 | * When the sum term drops to zero, we can stop summing. 34 | */ 35 | 36 | // The power-series converges much faster on positive values 37 | // and exp(-x) = 1/exp(x). 38 | bool neg = (inValue < 0); 39 | if (neg) inValue = -inValue; 40 | 41 | fix16_t result = inValue + fix16_one; 42 | fix16_t term = inValue; 43 | 44 | uint_fast8_t i; 45 | for (i = 2; i < 30; i++) 46 | { 47 | term = fix16_mul(term, fix16_div(inValue, fix16_from_int(i))); 48 | result += term; 49 | 50 | if ((term < 500) && ((i > 15) || (term < 20))) 51 | break; 52 | } 53 | 54 | if (neg) result = fix16_div(fix16_one, result); 55 | 56 | #ifndef FIXMATH_NO_CACHE 57 | _fix16_exp_cache_index[tempIndex] = inValue; 58 | _fix16_exp_cache_value[tempIndex] = result; 59 | #endif 60 | 61 | return result; 62 | } 63 | 64 | 65 | 66 | fix16_t fix16_log(fix16_t inValue) 67 | { 68 | fix16_t guess = fix16_from_int(2); 69 | fix16_t delta; 70 | int scaling = 0; 71 | int count = 0; 72 | 73 | if (inValue <= 0) 74 | return fix16_minimum; 75 | 76 | // Bring the value to the most accurate range (1 < x < 100) 77 | const fix16_t e_to_fourth = 3578144; 78 | while (inValue > fix16_from_int(100)) 79 | { 80 | inValue = fix16_div(inValue, e_to_fourth); 81 | scaling += 4; 82 | } 83 | 84 | while (inValue < fix16_one) 85 | { 86 | inValue = fix16_mul(inValue, e_to_fourth); 87 | scaling -= 4; 88 | } 89 | 90 | do 91 | { 92 | // Solving e(x) = y using Newton's method 93 | // f(x) = e(x) - y 94 | // f'(x) = e(x) 95 | fix16_t e = fix16_exp(guess); 96 | delta = fix16_div(inValue - e, e); 97 | 98 | // It's unlikely that logarithm is very large, so avoid overshooting. 99 | if (delta > fix16_from_int(3)) 100 | delta = fix16_from_int(3); 101 | 102 | guess += delta; 103 | } while ((count++ < 10) 104 | && ((delta > 1) || (delta < -1))); 105 | 106 | return guess + fix16_from_int(scaling); 107 | } 108 | 109 | 110 | 111 | static inline fix16_t fix16_rs(fix16_t x) 112 | { 113 | #ifdef FIXMATH_NO_ROUNDING 114 | return (x >> 1); 115 | #else 116 | fix16_t y = (x >> 1) + (x & 1); 117 | return y; 118 | #endif 119 | } 120 | 121 | /** 122 | * This assumes that the input value is >= 1. 123 | * 124 | * Note that this is only ever called with inValue >= 1 (because it has a wrapper to check. 125 | * As such, the result is always less than the input. 126 | */ 127 | static fix16_t fix16__log2_inner(fix16_t x) 128 | { 129 | fix16_t result = 0; 130 | 131 | while(x >= fix16_from_int(2)) 132 | { 133 | result++; 134 | x = fix16_rs(x); 135 | } 136 | 137 | if(x == 0) return (result << 16); 138 | 139 | uint_fast8_t i; 140 | for(i = 16; i > 0; i--) 141 | { 142 | x = fix16_mul(x, x); 143 | result <<= 1; 144 | if(x >= fix16_from_int(2)) 145 | { 146 | result |= 1; 147 | x = fix16_rs(x); 148 | } 149 | } 150 | #ifndef FIXMATH_NO_ROUNDING 151 | x = fix16_mul(x, x); 152 | if(x >= fix16_from_int(2)) result++; 153 | #endif 154 | 155 | return result; 156 | } 157 | 158 | 159 | 160 | /** 161 | * calculates the log base 2 of input. 162 | * Note that negative inputs are invalid! (will return fix16_overflow, since there are no exceptions) 163 | * 164 | * i.e. 2 to the power output = input. 165 | * It's equivalent to the log or ln functions, except it uses base 2 instead of base 10 or base e. 166 | * This is useful as binary things like this are easy for binary devices, like modern microprocessros, to calculate. 167 | * 168 | * This can be used as a helper function to calculate powers with non-integer powers and/or bases. 169 | */ 170 | fix16_t fix16_log2(fix16_t x) 171 | { 172 | // Note that a negative x gives a non-real result. 173 | // If x == 0, the limit of log2(x) as x -> 0 = -infinity. 174 | // log2(-ve) gives a complex result. 175 | if (x <= 0) return fix16_overflow; 176 | 177 | // If the input is less than one, the result is -log2(1.0 / in) 178 | if (x < fix16_one) 179 | { 180 | // Note that the inverse of this would overflow. 181 | // This is the exact answer for log2(1.0 / 65536) 182 | if (x == 1) return fix16_from_int(-16); 183 | 184 | fix16_t inverse = fix16_div(fix16_one, x); 185 | return -fix16__log2_inner(inverse); 186 | } 187 | 188 | // If input >= 1, just proceed as normal. 189 | // Note that x == fix16_one is a special case, where the answer is 0. 190 | return fix16__log2_inner(x); 191 | } 192 | 193 | /** 194 | * This is a wrapper for fix16_log2 which implements saturation arithmetic. 195 | */ 196 | fix16_t fix16_slog2(fix16_t x) 197 | { 198 | fix16_t retval = fix16_log2(x); 199 | // The only overflow possible is when the input is negative. 200 | if(retval == fix16_overflow) 201 | return fix16_minimum; 202 | return retval; 203 | } 204 | -------------------------------------------------------------------------------- /libfixmath/fix16_trig.c: -------------------------------------------------------------------------------- 1 | #ifdef __KERNEL__ 2 | #ifndef CHAR_BIT 3 | #define CHAR_BIT 8 /* Normally in */ 4 | #endif 5 | #else 6 | #include 7 | #endif 8 | #include "fix16.h" 9 | 10 | #if defined(FIXMATH_SIN_LUT) 11 | #include "fix16_trig_sin_lut.h" 12 | #elif !defined(FIXMATH_NO_CACHE) 13 | static fix16_t _fix16_sin_cache_index[4096] = { 0 }; 14 | static fix16_t _fix16_sin_cache_value[4096] = { 0 }; 15 | #endif 16 | 17 | #ifndef FIXMATH_NO_CACHE 18 | static fix16_t _fix16_atan_cache_index[2][4096] = { { 0 }, { 0 } }; 19 | static fix16_t _fix16_atan_cache_value[4096] = { 0 }; 20 | #endif 21 | 22 | 23 | fix16_t fix16_sin_parabola(fix16_t inAngle) 24 | { 25 | fix16_t abs_inAngle, retval; 26 | fix16_t mask; 27 | #ifndef FIXMATH_FAST_SIN 28 | fix16_t abs_retval; 29 | #endif 30 | 31 | /* Absolute function */ 32 | mask = (inAngle >> (sizeof(fix16_t)*CHAR_BIT-1)); 33 | abs_inAngle = (inAngle + mask) ^ mask; 34 | 35 | /* On 0->PI, sin looks like x² that is : 36 | - centered on PI/2, 37 | - equals 1 on PI/2, 38 | - equals 0 on 0 and PI 39 | that means : 4/PI * x - 4/PI² * x² 40 | Use abs(x) to handle (-PI) -> 0 zone. 41 | */ 42 | retval = fix16_mul(FOUR_DIV_PI, inAngle) + fix16_mul( fix16_mul(_FOUR_DIV_PI2, inAngle), abs_inAngle ); 43 | /* At this point, retval equals sin(inAngle) on important points ( -PI, -PI/2, 0, PI/2, PI), 44 | but is not very precise between these points 45 | */ 46 | #ifndef FIXMATH_FAST_SIN 47 | /* Absolute value of retval */ 48 | mask = (retval >> (sizeof(fix16_t)*CHAR_BIT-1)); 49 | abs_retval = (retval + mask) ^ mask; 50 | /* So improve its precision by adding some x^4 component to retval */ 51 | retval += fix16_mul(X4_CORRECTION_COMPONENT, fix16_mul(retval, abs_retval) - retval ); 52 | #endif 53 | return retval; 54 | } 55 | 56 | fix16_t fix16_sin(fix16_t inAngle) 57 | { 58 | fix16_t tempAngle = inAngle % (fix16_pi << 1); 59 | 60 | #ifdef FIXMATH_SIN_LUT 61 | if(tempAngle < 0) 62 | tempAngle += (fix16_pi << 1); 63 | 64 | fix16_t tempOut; 65 | if(tempAngle >= fix16_pi) { 66 | tempAngle -= fix16_pi; 67 | if(tempAngle >= (fix16_pi >> 1)) 68 | tempAngle = fix16_pi - tempAngle; 69 | tempOut = -(tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 70 | } else { 71 | if(tempAngle >= (fix16_pi >> 1)) 72 | tempAngle = fix16_pi - tempAngle; 73 | tempOut = (tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 74 | } 75 | #else 76 | if(tempAngle > fix16_pi) 77 | tempAngle -= (fix16_pi << 1); 78 | else if(tempAngle < -fix16_pi) 79 | tempAngle += (fix16_pi << 1); 80 | 81 | #ifndef FIXMATH_NO_CACHE 82 | fix16_t tempIndex = ((inAngle >> 5) & 0x00000FFF); 83 | if(_fix16_sin_cache_index[tempIndex] == inAngle) 84 | return _fix16_sin_cache_value[tempIndex]; 85 | #endif 86 | 87 | fix16_t tempAngleSq = fix16_mul(tempAngle, tempAngle); 88 | 89 | #ifndef FIXMATH_FAST_SIN // Most accurate version, accurate to ~2.1% 90 | fix16_t tempOut = tempAngle; 91 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 92 | tempOut -= (tempAngle / 6); 93 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 94 | tempOut += (tempAngle / 120); 95 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 96 | tempOut -= (tempAngle / 5040); 97 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 98 | tempOut += (tempAngle / 362880); 99 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 100 | tempOut -= (tempAngle / 39916800); 101 | #else // Fast implementation, runs at 159% the speed of above 'accurate' version with an slightly lower accuracy of ~2.3% 102 | fix16_t tempOut; 103 | tempOut = fix16_mul(-13, tempAngleSq) + 546; 104 | tempOut = fix16_mul(tempOut, tempAngleSq) - 10923; 105 | tempOut = fix16_mul(tempOut, tempAngleSq) + 65536; 106 | tempOut = fix16_mul(tempOut, tempAngle); 107 | #endif 108 | 109 | #ifndef FIXMATH_NO_CACHE 110 | _fix16_sin_cache_index[tempIndex] = inAngle; 111 | _fix16_sin_cache_value[tempIndex] = tempOut; 112 | #endif 113 | #endif 114 | 115 | return tempOut; 116 | } 117 | 118 | fix16_t fix16_cos(fix16_t inAngle) 119 | { 120 | return fix16_sin(inAngle + (fix16_pi >> 1)); 121 | } 122 | 123 | fix16_t fix16_tan(fix16_t inAngle) 124 | { 125 | #ifndef FIXMATH_NO_OVERFLOW 126 | return fix16_sdiv(fix16_sin(inAngle), fix16_cos(inAngle)); 127 | #else 128 | return fix16_div(fix16_sin(inAngle), fix16_cos(inAngle)); 129 | #endif 130 | } 131 | 132 | fix16_t fix16_asin(fix16_t x) 133 | { 134 | if((x > fix16_one) 135 | || (x < -fix16_one)) 136 | return 0; 137 | 138 | fix16_t out; 139 | out = (fix16_one - fix16_mul(x, x)); 140 | out = fix16_div(x, fix16_sqrt(out)); 141 | out = fix16_atan(out); 142 | return out; 143 | } 144 | 145 | fix16_t fix16_acos(fix16_t x) 146 | { 147 | return ((fix16_pi >> 1) - fix16_asin(x)); 148 | } 149 | 150 | fix16_t fix16_atan2(fix16_t inY , fix16_t inX) 151 | { 152 | fix16_t abs_inY, mask, angle, r, r_3; 153 | 154 | #ifndef FIXMATH_NO_CACHE 155 | uintptr_t hash = (inX ^ inY); 156 | hash ^= hash >> 20; 157 | hash &= 0x0FFF; 158 | if((_fix16_atan_cache_index[0][hash] == inX) && (_fix16_atan_cache_index[1][hash] == inY)) 159 | return _fix16_atan_cache_value[hash]; 160 | #endif 161 | 162 | /* Absolute inY */ 163 | mask = (inY >> (sizeof(fix16_t)*CHAR_BIT-1)); 164 | abs_inY = (inY + mask) ^ mask; 165 | 166 | if (inX >= 0) 167 | { 168 | r = fix16_div( (inX - abs_inY), (inX + abs_inY)); 169 | r_3 = fix16_mul(fix16_mul(r, r),r); 170 | angle = fix16_mul(0x00003240 , r_3) - fix16_mul(0x0000FB50,r) + PI_DIV_4; 171 | } else { 172 | r = fix16_div( (inX + abs_inY), (abs_inY - inX)); 173 | r_3 = fix16_mul(fix16_mul(r, r),r); 174 | angle = fix16_mul(0x00003240 , r_3) 175 | - fix16_mul(0x0000FB50,r) 176 | + THREE_PI_DIV_4; 177 | } 178 | if (inY < 0) 179 | { 180 | angle = -angle; 181 | } 182 | 183 | #ifndef FIXMATH_NO_CACHE 184 | _fix16_atan_cache_index[0][hash] = inX; 185 | _fix16_atan_cache_index[1][hash] = inY; 186 | _fix16_atan_cache_value[hash] = angle; 187 | #endif 188 | 189 | return angle; 190 | } 191 | 192 | fix16_t fix16_atan(fix16_t x) 193 | { 194 | return fix16_atan2(x, fix16_one); 195 | } 196 | -------------------------------------------------------------------------------- /libfixmath/int64.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_int64_h__ 2 | #define __libfixmath_int64_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #ifdef __KERNEL__ 10 | #include 11 | #else 12 | #include 13 | #endif 14 | 15 | #ifndef FIXMATH_NO_64BIT 16 | static inline int64_t int64_const(int32_t hi, uint32_t lo) { return (((int64_t)hi << 32) | lo); } 17 | static inline int64_t int64_from_int32(int32_t x) { return (int64_t)x; } 18 | static inline int32_t int64_hi(int64_t x) { return (x >> 32); } 19 | static inline uint32_t int64_lo(int64_t x) { return (x & ((1ULL << 32) - 1)); } 20 | 21 | static inline int64_t int64_add(int64_t x, int64_t y) { return (x + y); } 22 | static inline int64_t int64_neg(int64_t x) { return (-x); } 23 | static inline int64_t int64_sub(int64_t x, int64_t y) { return (x - y); } 24 | static inline int64_t int64_shift(int64_t x, int8_t y) { return (y < 0 ? (x >> -y) : (x << y)); } 25 | 26 | static inline int64_t int64_mul_i32_i32(int32_t x, int32_t y) { return ((int64_t)x * y); } 27 | static inline int64_t int64_mul_i64_i32(int64_t x, int32_t y) { return (x * y); } 28 | 29 | static inline int64_t int64_div_i64_i32(int64_t x, int32_t y) { return (x / y); } 30 | 31 | static inline int int64_cmp_eq(int64_t x, int64_t y) { return (x == y); } 32 | static inline int int64_cmp_ne(int64_t x, int64_t y) { return (x != y); } 33 | static inline int int64_cmp_gt(int64_t x, int64_t y) { return (x > y); } 34 | static inline int int64_cmp_ge(int64_t x, int64_t y) { return (x >= y); } 35 | static inline int int64_cmp_lt(int64_t x, int64_t y) { return (x < y); } 36 | static inline int int64_cmp_le(int64_t x, int64_t y) { return (x <= y); } 37 | #else 38 | 39 | typedef struct { 40 | int32_t hi; 41 | uint32_t lo; 42 | } _int64_t; 43 | 44 | static inline _int64_t int64_const(int32_t hi, uint32_t lo) { return (_int64_t){ hi, lo }; } 45 | static inline _int64_t int64_from_int32(int32_t x) { return (_int64_t){ (x < 0 ? -1 : 0), x }; } 46 | static inline int32_t int64_hi(_int64_t x) { return x.hi; } 47 | static inline uint32_t int64_lo(_int64_t x) { return x.lo; } 48 | 49 | static inline int int64_cmp_eq(_int64_t x, _int64_t y) { return ((x.hi == y.hi) && (x.lo == y.lo)); } 50 | static inline int int64_cmp_ne(_int64_t x, _int64_t y) { return ((x.hi != y.hi) || (x.lo != y.lo)); } 51 | 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))); } 52 | 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))); } 53 | 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))); } 54 | 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))); } 55 | 56 | static inline _int64_t int64_add(_int64_t x, _int64_t y) { 57 | _int64_t ret; 58 | ret.hi = x.hi + y.hi; 59 | ret.lo = x.lo + y.lo; 60 | if((ret.lo < x.lo) || (ret.lo < y.lo)) 61 | ret.hi++; 62 | return ret; 63 | } 64 | 65 | static inline _int64_t int64_neg(_int64_t x) { 66 | _int64_t ret; 67 | ret.hi = ~x.hi; 68 | ret.lo = ~x.lo + 1; 69 | if(ret.lo == 0) 70 | ret.hi++; 71 | return ret; 72 | } 73 | 74 | static inline _int64_t int64_sub(_int64_t x, _int64_t y) { 75 | return int64_add(x, int64_neg(y)); 76 | } 77 | 78 | static inline _int64_t int64_shift(_int64_t x, int8_t y) { 79 | _int64_t ret = {0,0}; 80 | if(y >= 64 || y <= -64) 81 | return (_int64_t){ 0, 0 }; 82 | if(y >= 32) { 83 | ret.hi = (x.lo << (y - 32)); 84 | } 85 | else if(y > 0) { 86 | ret.hi = (x.hi << y) | (x.lo >> (32 - y)); 87 | ret.lo = (x.lo << y); 88 | } 89 | else { 90 | y = -y; 91 | if(y >= 32){ 92 | ret.lo = (x.hi >> (y - 32)); 93 | ret.hi = (x.hi < 0) ? -1 : 0; 94 | } else { 95 | ret.lo = (x.lo >> y) | (x.hi << (32 - y)); 96 | ret.hi = (x.hi >> y); 97 | } 98 | } 99 | return ret; 100 | } 101 | 102 | static inline _int64_t int64_mul_i32_i32(int32_t x, int32_t y) { 103 | int16_t hi[2] = { (x >> 16), (y >> 16) }; 104 | uint16_t lo[2] = { (x & 0xFFFF), (y & 0xFFFF) }; 105 | 106 | int32_t r_hi = hi[0] * hi[1]; 107 | int32_t r_md = (hi[0] * lo[1]) + (hi[1] * lo[0]); 108 | uint32_t r_lo = lo[0] * lo[1]; 109 | 110 | _int64_t r_hilo64 = (_int64_t){ r_hi, r_lo }; 111 | _int64_t r_md64 = int64_shift(int64_from_int32(r_md), 16); 112 | 113 | return int64_add(r_hilo64, r_md64); 114 | } 115 | 116 | static inline _int64_t int64_mul_i64_i32(_int64_t x, int32_t y) { 117 | int neg = ((x.hi ^ y) < 0); 118 | if(x.hi < 0) 119 | x = int64_neg(x); 120 | uint32_t ypos = (y < 0)? (-y) : (y); 121 | 122 | uint32_t _x[4] = { (x.lo & 0xFFFF), (x.lo >> 16), (x.hi & 0xFFFF), (x.hi >> 16) }; 123 | uint32_t _y[2] = { (ypos & 0xFFFF), (ypos >> 16) }; 124 | 125 | uint32_t r[4]; 126 | r[0] = (_x[0] * _y[0]); 127 | r[1] = (_x[1] * _y[0]); 128 | uint32_t temp_r1 = r[1]; 129 | r[1] += (_x[0] * _y[1]); 130 | r[2] = (_x[2] * _y[0]) + (_x[1] * _y[1]); 131 | r[3] = (_x[3] * _y[0]) + (_x[2] * _y[1]); 132 | // Detect carry bit in r[1]. r[0] can't carry, and r[2]/r[3] don't matter. 133 | if(r[1] < temp_r1) 134 | r[3] ++; 135 | 136 | _int64_t middle = int64_shift(int64_const(0, r[1]), 16); 137 | _int64_t ret; 138 | ret.lo = r[0]; 139 | ret.hi = (r[3] << 16) + r[2]; 140 | ret = int64_add(ret, middle); 141 | return (neg ? int64_neg(ret) : ret); 142 | } 143 | 144 | static inline _int64_t int64_div_i64_i32(_int64_t x, int32_t y) { 145 | int neg = ((x.hi ^ y) < 0); 146 | if(x.hi < 0) 147 | x = int64_neg(x); 148 | if(y < 0) 149 | y = -y; 150 | 151 | _int64_t ret = { (x.hi / y) , (x.lo / y) }; 152 | x.hi = x.hi % y; 153 | x.lo = x.lo % y; 154 | 155 | _int64_t _y = int64_from_int32(y); 156 | 157 | _int64_t i; 158 | for(i = int64_from_int32(1); int64_cmp_lt(_y, x); _y = int64_shift(_y, 1), i = int64_shift(i, 1)); 159 | 160 | while(x.hi) { 161 | _y = int64_shift(_y, -1); 162 | i = int64_shift(i, -1); 163 | if(int64_cmp_ge(x, _y)) { 164 | x = int64_sub(x, _y); 165 | ret = int64_add(ret, i); 166 | } 167 | } 168 | 169 | ret = int64_add(ret, int64_from_int32(x.lo / y)); 170 | return (neg ? int64_neg(ret) : ret); 171 | } 172 | 173 | #define int64_t _int64_t 174 | 175 | #endif 176 | 177 | #ifdef __cplusplus 178 | } 179 | #endif 180 | 181 | #endif 182 | -------------------------------------------------------------------------------- /libfixmath/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 | -------------------------------------------------------------------------------- /tests/tests.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTS_H 2 | #define TESTS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | extern unsigned stack_depth; 11 | 12 | #define delta(a, b) (((a) >= (b)) ? (a) - (b) : (b) - (a)) 13 | 14 | #define COMMENT(x) printf("\n----" x "----\n"); 15 | #define STR(x) #x 16 | #define STR2(x) STR(x) 17 | #define TEST(x) \ 18 | do \ 19 | { \ 20 | ++stack_depth; \ 21 | if ((x)) \ 22 | { \ 23 | fflush(stdout); \ 24 | fflush(stderr); \ 25 | fprintf(stderr, \ 26 | "\033[31;1m FAILED:\033[22;39m%*s" #x \ 27 | " \033[0mat: " __FILE__ ":" STR2(__LINE__) " \n", \ 28 | stack_depth, ""); \ 29 | --stack_depth; \ 30 | return 1; \ 31 | } \ 32 | else \ 33 | { \ 34 | fflush(stdout); \ 35 | fflush(stderr); \ 36 | printf("\033[32;1m OK:\033[22;39m%*s" #x "\n\033[0m", \ 37 | stack_depth, ""); \ 38 | } \ 39 | --stack_depth; \ 40 | } while (0) 41 | 42 | #define ASSERT_NEAR_DOUBLE(a, b, eps, ...) \ 43 | do \ 44 | { \ 45 | if ((delta((a), (b)) >= (eps))) \ 46 | { \ 47 | fflush(stdout); \ 48 | fflush(stderr); \ 49 | fprintf(stderr, \ 50 | "\033[31;1m FAILED:\033[22;39m%*sASSERT_NEAR a: %f, b: " \ 51 | "%f, eps: %f\033[0m at: %s(), " __FILE__ \ 52 | ":" STR2(__LINE__) "\n", \ 53 | stack_depth, "", (a), (b), (eps), __func__); \ 54 | fprintf(stdout, " %*s", stack_depth, ""); \ 55 | fprintf(stdout, __VA_ARGS__); \ 56 | return 1; \ 57 | } \ 58 | } while (0) 59 | 60 | #define ASSERT_EQ_INT(a, b) \ 61 | do \ 62 | { \ 63 | if ((a) != (b)) \ 64 | { \ 65 | fflush(stdout); \ 66 | fflush(stderr); \ 67 | fprintf(stderr, \ 68 | "\033[31;1m FAILED:\033[22;39m%*sASSERT_EQ a: %i, b: " \ 69 | "%i\033[0m at: %s(), " __FILE__ ":" STR2(__LINE__) "\n", \ 70 | stack_depth, "", (a), (b), __func__); \ 71 | return 1; \ 72 | } \ 73 | } while (0) 74 | 75 | #define ASSERT_EQ_STR(a, b) \ 76 | do \ 77 | { \ 78 | size_t la = strlen(a); \ 79 | size_t lb = strlen(b); \ 80 | if (la != lb) \ 81 | { \ 82 | fflush(stdout); \ 83 | fflush(stderr); \ 84 | fprintf(stderr, \ 85 | "\033[31;1m FAILED:\033[22;39m%*sASSERT_EQ a: %s, b: " \ 86 | "%s\033[0m at: %s(), " __FILE__ ":" STR2(__LINE__) "\n", \ 87 | stack_depth, "", (a), (b), __func__); \ 88 | return 1; \ 89 | } \ 90 | int res = strncmp((a), (b), la); \ 91 | if (res != 0) \ 92 | { \ 93 | fflush(stdout); \ 94 | fflush(stderr); \ 95 | fprintf(stderr, \ 96 | "\033[31;1m FAILED:\033[22;39m%*sASSERT_EQ a: %s, b: " \ 97 | "%s\033[0m at: %s(), " __FILE__ ":" STR2(__LINE__) "\n", \ 98 | stack_depth, "", (a), (b), __func__); \ 99 | return 1; \ 100 | } \ 101 | } while (0) 102 | 103 | extern const fix16_t testcases[102]; 104 | 105 | #define TESTCASES_COUNT (sizeof(testcases) / sizeof(testcases[0])) 106 | 107 | #define delta(a, b) (((a) >= (b)) ? (a) - (b) : (b) - (a)) 108 | 109 | #endif // TESTS_H 110 | -------------------------------------------------------------------------------- /benchmarks/benchmark.c: -------------------------------------------------------------------------------- 1 | #ifndef NO_FLOAT 2 | #include 3 | #endif 4 | 5 | #include 6 | #include "interface.h" 7 | #include 8 | 9 | /* Autogenerated testcases */ 10 | #include "testcases.c" 11 | 12 | // Initializer for cyclecount_t structure. 13 | // Max is initialized to 0 and min is 2^32-1 so that first call to cyclecount_update will set them. 14 | #define CYCLECOUNT_INIT {0xFFFFFFFF, 0, 0, 0} 15 | 16 | // Update cyclecount_t structure after a single measurement has been made. 17 | static void cyclecount_update(cyclecount_t *data, uint32_t cycles) 18 | { 19 | if (cycles < data->min) 20 | data->min = cycles; 21 | if (cycles > data->max) 22 | data->max = cycles; 23 | 24 | data->sum += cycles; 25 | data->count++; 26 | } 27 | 28 | #define MEASURE(variable, statement) { \ 29 | start_timing(); \ 30 | statement; \ 31 | cyclecount_update(&variable, end_timing()); \ 32 | } 33 | 34 | static cyclecount_t exp_cycles = CYCLECOUNT_INIT; 35 | static cyclecount_t sqrt_cycles = CYCLECOUNT_INIT; 36 | static cyclecount_t add_cycles = CYCLECOUNT_INIT; 37 | static cyclecount_t sub_cycles = CYCLECOUNT_INIT; 38 | static cyclecount_t div_cycles = CYCLECOUNT_INIT; 39 | static cyclecount_t mul_cycles = CYCLECOUNT_INIT; 40 | 41 | static cyclecount_t float_sqrtf_cycles = CYCLECOUNT_INIT; 42 | static cyclecount_t float_expf_cycles = CYCLECOUNT_INIT; 43 | static cyclecount_t float_add_cycles = CYCLECOUNT_INIT; 44 | static cyclecount_t float_sub_cycles = CYCLECOUNT_INIT; 45 | static cyclecount_t float_div_cycles = CYCLECOUNT_INIT; 46 | static cyclecount_t float_mul_cycles = CYCLECOUNT_INIT; 47 | 48 | static fix16_t delta(fix16_t result, fix16_t expected) 49 | { 50 | #ifdef FIXMATH_NO_OVERFLOW 51 | // Ignore overflow errors when the detection is turned off 52 | if (expected == fix16_minimum) 53 | return 0; 54 | #endif 55 | 56 | if (result >= expected) 57 | { 58 | return result - expected; 59 | } 60 | else 61 | { 62 | return expected - result; 63 | } 64 | } 65 | 66 | #ifdef FIXMATH_NO_ROUNDING 67 | const fix16_t max_delta = 1; 68 | #else 69 | const fix16_t max_delta = 0; 70 | #endif 71 | 72 | int main() 73 | { 74 | int i; 75 | interface_init(); 76 | 77 | start_timing(); 78 | print_value("Timestamp bias", end_timing()); 79 | 80 | for (i = 0; i < TESTCASES1_COUNT; i++) 81 | { 82 | fix16_t input = testcases1[i].a; 83 | fix16_t result; 84 | fix16_t expected = testcases1[i].sqrt; 85 | MEASURE(sqrt_cycles, result = fix16_sqrt(input)); 86 | 87 | if (input > 0 && delta(result, expected) > max_delta) 88 | { 89 | print_value("Failed SQRT, i", i); 90 | print_value("Failed SQRT, input", input); 91 | print_value("Failed SQRT, output", result); 92 | print_value("Failed SQRT, expected", expected); 93 | } 94 | 95 | expected = testcases1[i].exp; 96 | MEASURE(exp_cycles, result = fix16_exp(input)); 97 | 98 | if (delta(result, expected) > 400) 99 | { 100 | print_value("Failed EXP, i", i); 101 | print_value("Failed EXP, input", input); 102 | print_value("Failed EXP, output", result); 103 | print_value("Failed EXP, expected", expected); 104 | } 105 | } 106 | 107 | for (i = 0; i < TESTCASES2_COUNT; i++) 108 | { 109 | fix16_t a = testcases2[i].a; 110 | fix16_t b = testcases2[i].b; 111 | volatile fix16_t result; 112 | 113 | fix16_t expected = testcases2[i].add; 114 | MEASURE(add_cycles, result = fix16_add(a, b)); 115 | if (delta(result, expected) > max_delta) 116 | { 117 | print_value("Failed ADD, i", i); 118 | print_value("Failed ADD, a", a); 119 | print_value("Failed ADD, b", b); 120 | print_value("Failed ADD, output", result); 121 | print_value("Failed ADD, expected", expected); 122 | } 123 | 124 | expected = testcases2[i].sub; 125 | MEASURE(sub_cycles, result = fix16_sub(a, b)); 126 | if (delta(result, expected) > max_delta) 127 | { 128 | print_value("Failed SUB, i", i); 129 | print_value("Failed SUB, a", a); 130 | print_value("Failed SUB, b", b); 131 | print_value("Failed SUB, output", result); 132 | print_value("Failed SUB, expected", expected); 133 | } 134 | 135 | expected = testcases2[i].mul; 136 | MEASURE(mul_cycles, result = fix16_mul(a, b)); 137 | if (delta(result, expected) > max_delta) 138 | { 139 | print_value("Failed MUL, i", i); 140 | print_value("Failed MUL, a", a); 141 | print_value("Failed MUL, b", b); 142 | print_value("Failed MUL, output", result); 143 | print_value("Failed MUL, expected", expected); 144 | } 145 | 146 | if (b != 0) 147 | { 148 | expected = testcases2[i].div; 149 | MEASURE(div_cycles, result = fix16_div(a, b)); 150 | if (delta(result, expected) > max_delta) 151 | { 152 | print_value("Failed DIV, i", i); 153 | print_value("Failed DIV, a", a); 154 | print_value("Failed DIV, b", b); 155 | print_value("Failed DIV, output", result); 156 | print_value("Failed DIV, expected", expected); 157 | } 158 | } 159 | } 160 | 161 | /* Compare with floating point performance */ 162 | #ifndef NO_FLOAT 163 | for (i = 0; i < TESTCASES1_COUNT; i++) 164 | { 165 | float input = fix16_to_float(testcases1[i].a); 166 | volatile float result; 167 | MEASURE(float_sqrtf_cycles, result = sqrtf(input)); 168 | MEASURE(float_expf_cycles, result = expf(input)); 169 | } 170 | 171 | for (i = 0; i < TESTCASES2_COUNT; i++) 172 | { 173 | float a = fix16_to_float(testcases2[i].a); 174 | float b = fix16_to_float(testcases2[i].b); 175 | volatile float result; 176 | MEASURE(float_add_cycles, result = a + b); 177 | MEASURE(float_sub_cycles, result = a - b); 178 | MEASURE(float_mul_cycles, result = a * b); 179 | 180 | if (b != 0) 181 | { 182 | MEASURE(float_div_cycles, result = a / b); 183 | } 184 | } 185 | #endif 186 | 187 | print("fix16_sqrt", &sqrt_cycles); 188 | print("float sqrtf", &float_sqrtf_cycles); 189 | print("fix16_exp", &exp_cycles); 190 | print("float expf", &float_expf_cycles); 191 | print("fix16_add", &add_cycles); 192 | print("float add", &float_add_cycles); 193 | print("fix16_sub", &sub_cycles); 194 | print("float sub", &float_sub_cycles); 195 | print("fix16_mul", &mul_cycles); 196 | print("float mul", &float_mul_cycles); 197 | print("fix16_div", &div_cycles); 198 | print("float div", &float_div_cycles); 199 | 200 | return 0; 201 | } 202 | -------------------------------------------------------------------------------- /tests/tests_basic.c: -------------------------------------------------------------------------------- 1 | #include "tests_basic.h" 2 | #include "tests.h" 3 | 4 | int test_abs_short(void) 5 | { 6 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 7 | { 8 | fix16_t a = testcases[i]; 9 | double fa = fix16_to_dbl(a); 10 | fix16_t result = fix16_abs(a); 11 | double fresult = fabs(fa); 12 | double min = fix16_to_dbl(fix16_minimum); 13 | if (fa <= min) 14 | { 15 | #ifndef FIXMATH_NO_OVERFLOW 16 | ASSERT_EQ_INT(result, fix16_overflow); 17 | #endif 18 | } 19 | else 20 | { 21 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 22 | fix16_to_dbl(fix16_eps), "in: %f", fa); 23 | } 24 | } 25 | return 0; 26 | } 27 | 28 | int test_abs(void) 29 | { 30 | TEST(test_abs_short()); 31 | return 0; 32 | } 33 | 34 | int test_add_short(void) 35 | { 36 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 37 | { 38 | for (unsigned j = 0; j < TESTCASES_COUNT; ++j) 39 | { 40 | fix16_t a = testcases[i]; 41 | fix16_t b = testcases[j]; 42 | fix16_t result = fix16_add(a, b); 43 | 44 | double fa = fix16_to_dbl(a); 45 | double fb = fix16_to_dbl(b); 46 | double fresult = fa + fb; 47 | 48 | double max = fix16_to_dbl(fix16_maximum); 49 | double min = fix16_to_dbl(fix16_minimum); 50 | if ((fa + fb > max) || (fa + fb < min)) 51 | { 52 | #ifndef FIXMATH_NO_OVERFLOW 53 | ASSERT_EQ_INT(result, fix16_overflow); 54 | #endif 55 | } 56 | else 57 | { 58 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 59 | fix16_to_dbl(fix16_eps), "%f + %f", fa, fb); 60 | } 61 | } 62 | } 63 | return 0; 64 | } 65 | 66 | int test_add(void) 67 | { 68 | TEST(test_add_short()); 69 | return 0; 70 | } 71 | 72 | int test_mul_specific(void) 73 | { 74 | ASSERT_EQ_INT(fix16_mul(fix16_from_int(5), fix16_from_int(5)), 75 | fix16_from_int(25)); 76 | ASSERT_EQ_INT(fix16_mul(fix16_from_int(-5), fix16_from_int(5)), 77 | fix16_from_int(-25)); 78 | ASSERT_EQ_INT(fix16_mul(fix16_from_int(-5), fix16_from_int(-5)), 79 | fix16_from_int(25)); 80 | ASSERT_EQ_INT(fix16_mul(fix16_from_int(5), fix16_from_int(-5)), 81 | fix16_from_int(-25)); 82 | 83 | ASSERT_EQ_INT(fix16_mul(0, 10), 0); 84 | ASSERT_EQ_INT(fix16_mul(2, 0x8000), 1); 85 | ASSERT_EQ_INT(fix16_mul(-2, 0x8000), -1); 86 | #ifndef FIXMATH_NO_ROUNDING 87 | ASSERT_EQ_INT(fix16_mul(3, 0x8000), 2); 88 | ASSERT_EQ_INT(fix16_mul(2, 0x7FFF), 1); 89 | ASSERT_EQ_INT(fix16_mul(-2, 0x8001), -1); 90 | ASSERT_EQ_INT(fix16_mul(-3, 0x8000), -2); 91 | ASSERT_EQ_INT(fix16_mul(-2, 0x7FFF), -1); 92 | ASSERT_EQ_INT(fix16_mul(2, 0x8001), 1); 93 | #endif 94 | return 0; 95 | } 96 | 97 | int test_mul_short() 98 | { 99 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 100 | { 101 | for (unsigned j = 0; j < TESTCASES_COUNT; ++j) 102 | { 103 | fix16_t a = testcases[i]; 104 | fix16_t b = testcases[j]; 105 | fix16_t result = fix16_mul(a, b); 106 | 107 | double fa = fix16_to_dbl(a); 108 | double fb = fix16_to_dbl(b); 109 | double fresult = fa * fb; 110 | 111 | double max = fix16_to_dbl(fix16_maximum); 112 | double min = fix16_to_dbl(fix16_minimum); 113 | 114 | if (fa * fb > max || fa * fb < min) 115 | { 116 | #ifndef FIXMATH_NO_OVERFLOW 117 | ASSERT_EQ_INT(result, fix16_overflow); 118 | #endif 119 | } 120 | else 121 | { 122 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 123 | fix16_to_dbl(fix16_eps), "%f * %f", fa, fb); 124 | } 125 | } 126 | } 127 | return 0; 128 | } 129 | 130 | int test_mul(void) 131 | { 132 | TEST(test_mul_specific()); 133 | TEST(test_mul_short()); 134 | return 0; 135 | } 136 | 137 | int test_div_specific() 138 | { 139 | ASSERT_EQ_INT(fix16_div(fix16_from_int(15), fix16_from_int(5)), 140 | fix16_from_int(3)); 141 | ASSERT_EQ_INT(fix16_div(fix16_from_int(-15), fix16_from_int(5)), 142 | fix16_from_int(-3)); 143 | ASSERT_EQ_INT(fix16_div(fix16_from_int(-15), fix16_from_int(-5)), 144 | fix16_from_int(3)); 145 | ASSERT_EQ_INT(fix16_div(fix16_from_int(15), fix16_from_int(-5)), 146 | fix16_from_int(-3)); 147 | 148 | #ifndef FIXMATH_NO_ROUNDING 149 | ASSERT_EQ_INT(fix16_div(0, 10), 0); 150 | ASSERT_EQ_INT(fix16_div(1, fix16_from_int(2)), 1); 151 | ASSERT_EQ_INT(fix16_div(-1, fix16_from_int(2)), -1); 152 | ASSERT_EQ_INT(fix16_div(1, fix16_from_int(-2)), -1); 153 | ASSERT_EQ_INT(fix16_div(-1, fix16_from_int(-2)), 1); 154 | ASSERT_EQ_INT(fix16_div(3, fix16_from_int(2)), 2); 155 | ASSERT_EQ_INT(fix16_div(-3, fix16_from_int(2)), -2); 156 | ASSERT_EQ_INT(fix16_div(3, fix16_from_int(-2)), -2); 157 | ASSERT_EQ_INT(fix16_div(-3, fix16_from_int(-2)), 2); 158 | ASSERT_EQ_INT(fix16_div(2, 0x7FFF), 4); 159 | ASSERT_EQ_INT(fix16_div(-2, 0x7FFF), -4); 160 | ASSERT_EQ_INT(fix16_div(2, 0x8001), 4); 161 | ASSERT_EQ_INT(fix16_div(-2, 0x8001), -4); 162 | #endif 163 | 164 | return 0; 165 | } 166 | 167 | int test_div_short() 168 | { 169 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 170 | { 171 | for (unsigned j = 0; j < TESTCASES_COUNT; ++j) 172 | { 173 | fix16_t a = testcases[i]; 174 | fix16_t b = testcases[j]; 175 | // We don't require a solution for /0 :) 176 | if (b == 0) 177 | continue; 178 | fix16_t result = fix16_div(a, b); 179 | 180 | double fa = fix16_to_dbl(a); 181 | double fb = fix16_to_dbl(b); 182 | double fresult = fa / fb; 183 | 184 | double max = fix16_to_dbl(fix16_maximum); 185 | double min = fix16_to_dbl(fix16_minimum); 186 | 187 | if ((fa / fb) > max || (fa / fb) < min) 188 | { 189 | #ifndef FIXMATH_NO_OVERFLOW 190 | ASSERT_EQ_INT(result, fix16_overflow); 191 | #endif 192 | } 193 | else 194 | { 195 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 196 | fix16_to_dbl(fix16_eps), "%i / %i \n", a, b); 197 | } 198 | } 199 | } 200 | return 0; 201 | } 202 | 203 | int test_div(void) 204 | { 205 | TEST(test_div_specific()); 206 | TEST(test_div_short()); 207 | return 0; 208 | } 209 | 210 | int test_sub_short() 211 | { 212 | for (unsigned i = 0; i < TESTCASES_COUNT; ++i) 213 | { 214 | for (unsigned j = 0; j < TESTCASES_COUNT; ++j) 215 | { 216 | fix16_t a = testcases[i]; 217 | fix16_t b = testcases[j]; 218 | fix16_t result = fix16_sub(a, b); 219 | 220 | double fa = fix16_to_dbl(a); 221 | double fb = fix16_to_dbl(b); 222 | double fresult = fa - fb; 223 | 224 | double max = fix16_to_dbl(fix16_maximum); 225 | double min = fix16_to_dbl(fix16_minimum); 226 | if ((fa - fb > max) || (fa - fb < min)) 227 | { 228 | #ifndef FIXMATH_NO_OVERFLOW 229 | ASSERT_EQ_INT(result, fix16_overflow); 230 | #endif 231 | } 232 | else 233 | { 234 | ASSERT_NEAR_DOUBLE(fresult, fix16_to_dbl(result), 235 | fix16_to_dbl(fix16_eps), "%f - %f", fa, fb); 236 | } 237 | } 238 | } 239 | return 0; 240 | } 241 | 242 | int test_sub() 243 | { 244 | TEST(test_sub_short()); 245 | return 0; 246 | } 247 | -------------------------------------------------------------------------------- /libfixmath/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 | /* Automatically define FIXMATH_NO_HARD_DIVISION to maintain backwards 25 | * compatibility with usage of FIXMATH_OPTIMIZE_8BIT. 26 | */ 27 | #if defined(FIXMATH_OPTIMIZE_8BIT) 28 | # define FIXMATH_NO_HARD_DIVISION 29 | #endif 30 | 31 | #ifdef __KERNEL__ 32 | #include 33 | #else 34 | #include 35 | #endif 36 | 37 | typedef int32_t fix16_t; 38 | 39 | static const fix16_t FOUR_DIV_PI = 0x145F3; /*!< Fix16 value of 4/PI */ 40 | static const fix16_t _FOUR_DIV_PI2 = 0xFFFF9840; /*!< Fix16 value of -4/PI² */ 41 | static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.225 */ 42 | static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */ 43 | static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */ 44 | 45 | static const fix16_t fix16_maximum = 0x7FFFFFFF; /*!< the maximum value of fix16_t */ 46 | static const fix16_t fix16_minimum = 0x80000000; /*!< the minimum value of fix16_t */ 47 | static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */ 48 | 49 | static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */ 50 | static const fix16_t fix16_e = 178145; /*!< fix16_t value of e */ 51 | static const fix16_t fix16_one = 0x00010000; /*!< fix16_t value of 1 */ 52 | static const fix16_t fix16_eps = 1; /*!< fix16_t epsilon */ 53 | 54 | /* Conversion functions between fix16_t and float/integer. 55 | * These are inlined to allow compiler to optimize away constant numbers 56 | */ 57 | static inline fix16_t fix16_from_int(int a) { return a * fix16_one; } 58 | static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; } 59 | static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; } 60 | 61 | static inline int fix16_to_int(fix16_t a) 62 | { 63 | #ifdef FIXMATH_NO_ROUNDING 64 | return (a >> 16); 65 | #else 66 | if (a >= 0) 67 | return (a + (fix16_one >> 1)) / fix16_one; 68 | return (a - (fix16_one >> 1)) / fix16_one; 69 | #endif 70 | } 71 | 72 | static inline fix16_t fix16_from_float(float a) 73 | { 74 | float temp = a * fix16_one; 75 | #ifndef FIXMATH_NO_ROUNDING 76 | temp += (temp >= 0) ? 0.5f : -0.5f; 77 | #endif 78 | return (fix16_t)temp; 79 | } 80 | 81 | static inline fix16_t fix16_from_dbl(double a) 82 | { 83 | double temp = a * fix16_one; 84 | /* F16() and F16C() are both rounding allways, so this should as well */ 85 | //#ifndef FIXMATH_NO_ROUNDING 86 | temp += (double)((temp >= 0) ? 0.5f : -0.5f); 87 | //#endif 88 | return (fix16_t)temp; 89 | } 90 | 91 | /* Macro for defining fix16_t constant values. 92 | The functions above can't be used from e.g. global variable initializers, 93 | and their names are quite long also. This macro is useful for constants 94 | springled alongside code, e.g. F16(1.234). 95 | 96 | Note that the argument is evaluated multiple times, and also otherwise 97 | you should only use this for constant values. For runtime-conversions, 98 | use the functions above. 99 | */ 100 | #define F16(x) ((fix16_t)(((x) >= 0) ? ((x) * 65536.0 + 0.5) : ((x) * 65536.0 - 0.5))) 101 | 102 | static inline fix16_t fix16_abs(fix16_t x) 103 | { return (fix16_t)(x < 0 ? -(uint32_t)x : (uint32_t)x); } 104 | static inline fix16_t fix16_floor(fix16_t x) 105 | { return (x & 0xFFFF0000UL); } 106 | static inline fix16_t fix16_ceil(fix16_t x) 107 | { return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? fix16_one : 0); } 108 | static inline fix16_t fix16_min(fix16_t x, fix16_t y) 109 | { return (x < y ? x : y); } 110 | static inline fix16_t fix16_max(fix16_t x, fix16_t y) 111 | { return (x > y ? x : y); } 112 | static inline fix16_t fix16_clamp(fix16_t x, fix16_t lo, fix16_t hi) 113 | { return fix16_min(fix16_max(x, lo), hi); } 114 | 115 | /* Subtraction and addition with (optional) overflow detection. */ 116 | #ifdef FIXMATH_NO_OVERFLOW 117 | 118 | static inline fix16_t fix16_add(fix16_t inArg0, fix16_t inArg1) { return (inArg0 + inArg1); } 119 | static inline fix16_t fix16_sub(fix16_t inArg0, fix16_t inArg1) { return (inArg0 - inArg1); } 120 | 121 | #else 122 | 123 | extern fix16_t fix16_add(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 124 | extern fix16_t fix16_sub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 125 | 126 | /* Saturating arithmetic */ 127 | extern fix16_t fix16_sadd(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 128 | extern fix16_t fix16_ssub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS; 129 | 130 | #endif 131 | 132 | /*! Multiplies the two given fix16_t's and returns the result. 133 | */ 134 | extern fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 135 | 136 | /*! Divides the first given fix16_t by the second and returns the result. 137 | */ 138 | extern fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 139 | 140 | #ifndef FIXMATH_NO_OVERFLOW 141 | /*! Performs a saturated multiplication (overflow-protected) of the two given fix16_t's and returns the result. 142 | */ 143 | extern fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 144 | 145 | /*! Performs a saturated division (overflow-protected) of the first fix16_t by the second and returns the result. 146 | */ 147 | extern fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS; 148 | #endif 149 | 150 | /*! Divides the first given fix16_t by the second and returns the result. 151 | */ 152 | extern fix16_t fix16_mod(fix16_t x, fix16_t y) FIXMATH_FUNC_ATTRS; 153 | 154 | 155 | 156 | /*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract) 157 | */ 158 | extern fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS; 159 | extern fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) FIXMATH_FUNC_ATTRS; 160 | extern fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS; 161 | 162 | 163 | 164 | /*! Returns the sine of the given fix16_t. 165 | */ 166 | extern fix16_t fix16_sin_parabola(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 167 | 168 | /*! Returns the sine of the given fix16_t. 169 | */ 170 | extern fix16_t fix16_sin(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 171 | 172 | /*! Returns the cosine of the given fix16_t. 173 | */ 174 | extern fix16_t fix16_cos(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 175 | 176 | /*! Returns the tangent of the given fix16_t. 177 | */ 178 | extern fix16_t fix16_tan(fix16_t inAngle) FIXMATH_FUNC_ATTRS; 179 | 180 | /*! Returns the arcsine of the given fix16_t. 181 | */ 182 | extern fix16_t fix16_asin(fix16_t inValue) FIXMATH_FUNC_ATTRS; 183 | 184 | /*! Returns the arccosine of the given fix16_t. 185 | */ 186 | extern fix16_t fix16_acos(fix16_t inValue) FIXMATH_FUNC_ATTRS; 187 | 188 | /*! Returns the arctangent of the given fix16_t. 189 | */ 190 | extern fix16_t fix16_atan(fix16_t inValue) FIXMATH_FUNC_ATTRS; 191 | 192 | /*! Returns the arctangent of inY/inX. 193 | */ 194 | extern fix16_t fix16_atan2(fix16_t inY, fix16_t inX) FIXMATH_FUNC_ATTRS; 195 | 196 | static const fix16_t fix16_rad_to_deg_mult = 3754936; 197 | static inline fix16_t fix16_rad_to_deg(fix16_t radians) 198 | { return fix16_mul(radians, fix16_rad_to_deg_mult); } 199 | 200 | static const fix16_t fix16_deg_to_rad_mult = 1144; 201 | static inline fix16_t fix16_deg_to_rad(fix16_t degrees) 202 | { return fix16_mul(degrees, fix16_deg_to_rad_mult); } 203 | 204 | 205 | 206 | /*! Returns the square root of the given fix16_t. 207 | */ 208 | extern fix16_t fix16_sqrt(fix16_t inValue) FIXMATH_FUNC_ATTRS; 209 | 210 | /*! Returns the square of the given fix16_t. 211 | */ 212 | static inline fix16_t fix16_sq(fix16_t x) 213 | { return fix16_mul(x, x); } 214 | 215 | /*! Returns the exponent (e^) of the given fix16_t. 216 | */ 217 | extern fix16_t fix16_exp(fix16_t inValue) FIXMATH_FUNC_ATTRS; 218 | 219 | /*! Returns the natural logarithm of the given fix16_t. 220 | */ 221 | extern fix16_t fix16_log(fix16_t inValue) FIXMATH_FUNC_ATTRS; 222 | 223 | /*! Returns the base 2 logarithm of the given fix16_t. 224 | */ 225 | extern fix16_t fix16_log2(fix16_t x) FIXMATH_FUNC_ATTRS; 226 | 227 | /*! Returns the saturated base 2 logarithm of the given fix16_t. 228 | */ 229 | extern fix16_t fix16_slog2(fix16_t x) FIXMATH_FUNC_ATTRS; 230 | 231 | /*! Convert fix16_t value to a string. 232 | * Required buffer length for largest values is 13 bytes. 233 | */ 234 | extern void fix16_to_str(fix16_t value, char *buf, int decimals); 235 | 236 | /*! Convert string to a fix16_t value 237 | * Ignores spaces at beginning and end. Returns fix16_overflow if 238 | * value is too large or there were garbage characters. 239 | */ 240 | extern fix16_t fix16_from_str(const char *buf); 241 | 242 | static inline uint32_t fix_abs(fix16_t in) 243 | { 244 | if(in == fix16_minimum) 245 | { 246 | // minimum negative number has same representation as 247 | // its absolute value in unsigned 248 | return 0x80000000; 249 | } 250 | else 251 | { 252 | return ((in >= 0)?(in):(-in)); 253 | } 254 | } 255 | 256 | 257 | /** Helper macro for F16C. Replace token with its number of characters/digits. */ 258 | #define FIXMATH_TOKLEN(token) ( sizeof( #token ) - 1 ) 259 | 260 | /** Helper macro for F16C. Handles pow(10, n) for n from 0 to 8. */ 261 | #define FIXMATH_CONSTANT_POW10(times) ( \ 262 | (times == 0) ? 1ULL \ 263 | : (times == 1) ? 10ULL \ 264 | : (times == 2) ? 100ULL \ 265 | : (times == 3) ? 1000ULL \ 266 | : (times == 4) ? 10000ULL \ 267 | : (times == 5) ? 100000ULL \ 268 | : (times == 6) ? 1000000ULL \ 269 | : (times == 7) ? 10000000ULL \ 270 | : 100000000ULL \ 271 | ) 272 | 273 | 274 | /** Helper macro for F16C, the type uint64_t is only used at compile time and 275 | * shouldn't be visible in the generated code. 276 | * 277 | * @note We do not use fix16_one instead of 65536ULL, because the 278 | * "use of a const variable in a constant expression is nonstandard in C". 279 | */ 280 | #define FIXMATH_CONVERT_MANTISSA(m) \ 281 | ( (unsigned) \ 282 | ( \ 283 | ( \ 284 | ( \ 285 | (uint64_t)( ( ( 1 ## m ## ULL ) - FIXMATH_CONSTANT_POW10(FIXMATH_TOKLEN(m)) ) * FIXMATH_CONSTANT_POW10(5 - FIXMATH_TOKLEN(m)) ) \ 286 | * 100000ULL * 65536ULL \ 287 | ) \ 288 | + 5000000000ULL /* rounding: + 0.5 */ \ 289 | ) \ 290 | / \ 291 | 10000000000LL \ 292 | ) \ 293 | ) 294 | 295 | 296 | #define FIXMATH_COMBINE_I_M(i, m) \ 297 | ( \ 298 | ( \ 299 | ( i ) \ 300 | << 16 \ 301 | ) \ 302 | | \ 303 | ( \ 304 | FIXMATH_CONVERT_MANTISSA(m) \ 305 | & 0xFFFF \ 306 | ) \ 307 | ) 308 | 309 | 310 | /** Create int16_t (Q16.16) constant from separate integer and mantissa part. 311 | * 312 | * Only tested on 32-bit ARM Cortex-M0 / x86 Intel. 313 | * 314 | * This macro is needed when compiling with options like "--fpu=none", 315 | * which forbid all and every use of float and related types and 316 | * would thus make it impossible to have fix16_t constants. 317 | * 318 | * Just replace uses of F16() with F16C() like this: 319 | * F16(123.1234) becomes F16C(123,1234) 320 | * 321 | * @warning Specification of any value outside the mentioned intervals 322 | * WILL result in undefined behavior! 323 | * 324 | * @note Regardless of the specified minimum and maximum values for i and m below, 325 | * the total value of the number represented by i and m MUST be in the interval 326 | * ]-32768.00000:32767.99999[ else usage with this macro will yield undefined behavior. 327 | * 328 | * @param i Signed integer constant with a value in the interval ]-32768:32767[. 329 | * @param m Positive integer constant in the interval ]0:99999[ (fractional part/mantissa). 330 | */ 331 | #define F16C(i, m) \ 332 | ( (fix16_t) \ 333 | ( \ 334 | (( #i[0] ) == '-') \ 335 | ? -FIXMATH_COMBINE_I_M((unsigned)( ( (i) * -1) ), m) \ 336 | : FIXMATH_COMBINE_I_M((unsigned)i, m) \ 337 | ) \ 338 | ) 339 | 340 | #ifdef __cplusplus 341 | } 342 | #include "fix16.hpp" 343 | #endif 344 | 345 | #endif 346 | -------------------------------------------------------------------------------- /libfixmath/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 (int16_t)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 | #if defined(FIXMATH_SATURATING_ARITHMETIC) && !defined(FIXMATH_NO_OVERFLOW) 29 | Fix16 & operator+=(const Fix16 &rhs) { value = fix16_sadd(value, rhs.value); return *this; } 30 | Fix16 & operator+=(const fix16_t rhs) { value = fix16_sadd(value, rhs); return *this; } 31 | Fix16 & operator+=(const double rhs) { value = fix16_sadd(value, fix16_from_dbl(rhs)); return *this; } 32 | Fix16 & operator+=(const float rhs) { value = fix16_sadd(value, fix16_from_float(rhs)); return *this; } 33 | Fix16 & operator+=(const int16_t rhs) { value = fix16_sadd(value, fix16_from_int(rhs)); return *this; } 34 | 35 | Fix16 & operator-=(const Fix16 &rhs) { value = fix16_ssub(value, rhs.value); return *this; } 36 | Fix16 & operator-=(const fix16_t rhs) { value = fix16_ssub(value, rhs); return *this; } 37 | Fix16 & operator-=(const double rhs) { value = fix16_ssub(value, fix16_from_dbl(rhs)); return *this; } 38 | Fix16 & operator-=(const float rhs) { value = fix16_ssub(value, fix16_from_float(rhs)); return *this; } 39 | Fix16 & operator-=(const int16_t rhs) { value = fix16_ssub(value, fix16_from_int(rhs)); return *this; } 40 | 41 | Fix16 & operator*=(const Fix16 &rhs) { value = fix16_smul(value, rhs.value); return *this; } 42 | Fix16 & operator*=(const fix16_t rhs) { value = fix16_smul(value, rhs); return *this; } 43 | Fix16 & operator*=(const double rhs) { value = fix16_smul(value, fix16_from_dbl(rhs)); return *this; } 44 | Fix16 & operator*=(const float rhs) { value = fix16_smul(value, fix16_from_float(rhs)); return *this; } 45 | Fix16 & operator*=(const int16_t rhs) { value *= rhs; return *this; } 46 | 47 | Fix16 & operator/=(const Fix16 &rhs) { value = fix16_sdiv(value, rhs.value); return *this; } 48 | Fix16 & operator/=(const fix16_t rhs) { value = fix16_sdiv(value, rhs); return *this; } 49 | Fix16 & operator/=(const double rhs) { value = fix16_sdiv(value, fix16_from_dbl(rhs)); return *this; } 50 | Fix16 & operator/=(const float rhs) { value = fix16_sdiv(value, fix16_from_float(rhs)); return *this; } 51 | Fix16 & operator/=(const int16_t rhs) { value /= rhs; return *this; } 52 | #else 53 | Fix16 & operator+=(const Fix16 &rhs) { value += rhs.value; return *this; } 54 | Fix16 & operator+=(const fix16_t rhs) { value += rhs; return *this; } 55 | Fix16 & operator+=(const double rhs) { value += fix16_from_dbl(rhs); return *this; } 56 | Fix16 & operator+=(const float rhs) { value += fix16_from_float(rhs); return *this; } 57 | Fix16 & operator+=(const int16_t rhs) { value += fix16_from_int(rhs); return *this; } 58 | 59 | Fix16 & operator-=(const Fix16 &rhs) { value -= rhs.value; return *this; } 60 | Fix16 & operator-=(const fix16_t rhs) { value -= rhs; return *this; } 61 | Fix16 & operator-=(const double rhs) { value -= fix16_from_dbl(rhs); return *this; } 62 | Fix16 & operator-=(const float rhs) { value -= fix16_from_float(rhs); return *this; } 63 | Fix16 & operator-=(const int16_t rhs) { value -= fix16_from_int(rhs); return *this; } 64 | 65 | Fix16 & operator*=(const Fix16 &rhs) { value = fix16_mul(value, rhs.value); return *this; } 66 | Fix16 & operator*=(const fix16_t rhs) { value = fix16_mul(value, rhs); return *this; } 67 | Fix16 & operator*=(const double rhs) { value = fix16_mul(value, fix16_from_dbl(rhs)); return *this; } 68 | Fix16 & operator*=(const float rhs) { value = fix16_mul(value, fix16_from_float(rhs)); return *this; } 69 | Fix16 & operator*=(const int16_t rhs) { value *= rhs; return *this; } 70 | 71 | Fix16 & operator/=(const Fix16 &rhs) { value = fix16_div(value, rhs.value); return *this; } 72 | Fix16 & operator/=(const fix16_t rhs) { value = fix16_div(value, rhs); return *this; } 73 | Fix16 & operator/=(const double rhs) { value = fix16_div(value, fix16_from_dbl(rhs)); return *this; } 74 | Fix16 & operator/=(const float rhs) { value = fix16_div(value, fix16_from_float(rhs)); return *this; } 75 | Fix16 & operator/=(const int16_t rhs) { value /= rhs; return *this; } 76 | #endif 77 | 78 | const Fix16 operator+(const Fix16 &other) const { Fix16 ret = *this; ret += other; return ret; } 79 | const Fix16 operator+(const fix16_t other) const { Fix16 ret = *this; ret += other; return ret; } 80 | const Fix16 operator+(const double other) const { Fix16 ret = *this; ret += other; return ret; } 81 | const Fix16 operator+(const float other) const { Fix16 ret = *this; ret += other; return ret; } 82 | const Fix16 operator+(const int16_t other) const { Fix16 ret = *this; ret += other; return ret; } 83 | 84 | #ifndef FIXMATH_NO_OVERFLOW 85 | const Fix16 sadd(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, other.value); return ret; } 86 | const Fix16 sadd(const fix16_t other) const { Fix16 ret = fix16_sadd(value, other); return ret; } 87 | const Fix16 sadd(const double other) const { Fix16 ret = fix16_sadd(value, fix16_from_dbl(other)); return ret; } 88 | const Fix16 sadd(const float other) const { Fix16 ret = fix16_sadd(value, fix16_from_float(other)); return ret; } 89 | const Fix16 sadd(const int16_t other) const { Fix16 ret = fix16_sadd(value, fix16_from_int(other)); return ret; } 90 | #endif 91 | 92 | const Fix16 operator-(const Fix16 &other) const { Fix16 ret = *this; ret -= other; return ret; } 93 | const Fix16 operator-(const fix16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 94 | const Fix16 operator-(const double other) const { Fix16 ret = *this; ret -= other; return ret; } 95 | const Fix16 operator-(const float other) const { Fix16 ret = *this; ret -= other; return ret; } 96 | const Fix16 operator-(const int16_t other) const { Fix16 ret = *this; ret -= other; return ret; } 97 | const Fix16 operator-() const { return -value; } 98 | 99 | #ifndef FIXMATH_NO_OVERFLOW 100 | const Fix16 ssub(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, -other.value); return ret; } 101 | const Fix16 ssub(const fix16_t other) const { Fix16 ret = fix16_sadd(value, -other); return ret; } 102 | const Fix16 ssub(const double other) const { Fix16 ret = fix16_sadd(value, -fix16_from_dbl(other)); return ret; } 103 | const Fix16 ssub(const float other) const { Fix16 ret = fix16_sadd(value, -fix16_from_float(other)); return ret; } 104 | const Fix16 ssub(const int16_t other) const { Fix16 ret = fix16_sadd(value, -fix16_from_int(other)); return ret; } 105 | #endif 106 | 107 | const Fix16 operator*(const Fix16 &other) const { Fix16 ret = *this; ret *= other; return ret; } 108 | const Fix16 operator*(const fix16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 109 | const Fix16 operator*(const double other) const { Fix16 ret = *this; ret *= other; return ret; } 110 | const Fix16 operator*(const float other) const { Fix16 ret = *this; ret *= other; return ret; } 111 | const Fix16 operator*(const int16_t other) const { Fix16 ret = *this; ret *= other; return ret; } 112 | 113 | #ifndef FIXMATH_NO_OVERFLOW 114 | const Fix16 smul(const Fix16 &other) const { Fix16 ret = fix16_smul(value, other.value); return ret; } 115 | const Fix16 smul(const fix16_t other) const { Fix16 ret = fix16_smul(value, other); return ret; } 116 | const Fix16 smul(const double other) const { Fix16 ret = fix16_smul(value, fix16_from_dbl(other)); return ret; } 117 | const Fix16 smul(const float other) const { Fix16 ret = fix16_smul(value, fix16_from_float(other)); return ret; } 118 | const Fix16 smul(const int16_t other) const { Fix16 ret = fix16_smul(value, fix16_from_int(other)); return ret; } 119 | #endif 120 | 121 | const Fix16 operator/(const Fix16 &other) const { Fix16 ret = *this; ret /= other; return ret; } 122 | const Fix16 operator/(const fix16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 123 | const Fix16 operator/(const double other) const { Fix16 ret = *this; ret /= other; return ret; } 124 | const Fix16 operator/(const float other) const { Fix16 ret = *this; ret /= other; return ret; } 125 | const Fix16 operator/(const int16_t other) const { Fix16 ret = *this; ret /= other; return ret; } 126 | 127 | #ifndef FIXMATH_NO_OVERFLOW 128 | const Fix16 sdiv(const Fix16 &other) const { Fix16 ret = fix16_sdiv(value, other.value); return ret; } 129 | const Fix16 sdiv(const fix16_t other) const { Fix16 ret = fix16_sdiv(value, other); return ret; } 130 | const Fix16 sdiv(const double other) const { Fix16 ret = fix16_sdiv(value, fix16_from_dbl(other)); return ret; } 131 | const Fix16 sdiv(const float other) const { Fix16 ret = fix16_sdiv(value, fix16_from_float(other)); return ret; } 132 | const Fix16 sdiv(const int16_t other) const { Fix16 ret = fix16_sdiv(value, fix16_from_int(other)); return ret; } 133 | #endif 134 | 135 | int operator==(const Fix16 &other) const { return (value == other.value); } 136 | int operator==(const fix16_t other) const { return (value == other); } 137 | int operator==(const double other) const { return (value == fix16_from_dbl(other)); } 138 | int operator==(const float other) const { return (value == fix16_from_float(other)); } 139 | int operator==(const int16_t other) const { return (value == fix16_from_int(other)); } 140 | 141 | int operator!=(const Fix16 &other) const { return (value != other.value); } 142 | int operator!=(const fix16_t other) const { return (value != other); } 143 | int operator!=(const double other) const { return (value != fix16_from_dbl(other)); } 144 | int operator!=(const float other) const { return (value != fix16_from_float(other)); } 145 | int operator!=(const int16_t other) const { return (value != fix16_from_int(other)); } 146 | 147 | int operator<=(const Fix16 &other) const { return (value <= other.value); } 148 | int operator<=(const fix16_t other) const { return (value <= other); } 149 | int operator<=(const double other) const { return (value <= fix16_from_dbl(other)); } 150 | int operator<=(const float other) const { return (value <= fix16_from_float(other)); } 151 | int operator<=(const int16_t other) const { return (value <= fix16_from_int(other)); } 152 | 153 | int operator>=(const Fix16 &other) const { return (value >= other.value); } 154 | int operator>=(const fix16_t other) const { return (value >= other); } 155 | int operator>=(const double other) const { return (value >= fix16_from_dbl(other)); } 156 | int operator>=(const float other) const { return (value >= fix16_from_float(other)); } 157 | int operator>=(const int16_t other) const { return (value >= fix16_from_int(other)); } 158 | 159 | int operator< (const Fix16 &other) const { return (value < other.value); } 160 | int operator< (const fix16_t other) const { return (value < other); } 161 | int operator< (const double other) const { return (value < fix16_from_dbl(other)); } 162 | int operator< (const float other) const { return (value < fix16_from_float(other)); } 163 | int operator< (const int16_t other) const { return (value < fix16_from_int(other)); } 164 | 165 | int operator> (const Fix16 &other) const { return (value > other.value); } 166 | int operator> (const fix16_t other) const { return (value > other); } 167 | int operator> (const double other) const { return (value > fix16_from_dbl(other)); } 168 | int operator> (const float other) const { return (value > fix16_from_float(other)); } 169 | int operator> (const int16_t other) const { return (value > fix16_from_int(other)); } 170 | 171 | Fix16 sin() const { return Fix16(fix16_sin(value)); } 172 | Fix16 cos() const { return Fix16(fix16_cos(value)); } 173 | Fix16 tan() const { return Fix16(fix16_tan(value)); } 174 | Fix16 asin() const { return Fix16(fix16_asin(value)); } 175 | Fix16 acos() const { return Fix16(fix16_acos(value)); } 176 | Fix16 atan() const { return Fix16(fix16_atan(value)); } 177 | Fix16 atan2(const Fix16 &inY) const { return Fix16(fix16_atan2(value, inY.value)); } 178 | Fix16 sqrt() const { return Fix16(fix16_sqrt(value)); } 179 | }; 180 | 181 | #endif 182 | -------------------------------------------------------------------------------- /libfixmath/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; 14 | uint32_t _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; 28 | uint32_t _b = b; 29 | uint32_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) & 0x80000000) && ((_a ^ diff) & 0x80000000)) 34 | return fix16_overflow; 35 | 36 | return diff; 37 | } 38 | 39 | /* Saturating arithmetic */ 40 | fix16_t fix16_sadd(fix16_t a, fix16_t b) 41 | { 42 | fix16_t result = fix16_add(a, b); 43 | 44 | if (result == fix16_overflow) 45 | return (a >= 0) ? fix16_maximum : fix16_minimum; 46 | 47 | return result; 48 | } 49 | 50 | fix16_t fix16_ssub(fix16_t a, fix16_t b) 51 | { 52 | fix16_t result = fix16_sub(a, b); 53 | 54 | if (result == fix16_overflow) 55 | return (a >= 0) ? fix16_maximum : fix16_minimum; 56 | 57 | return result; 58 | } 59 | #endif 60 | 61 | 62 | 63 | /* 64-bit implementation for fix16_mul. Fastest version for e.g. ARM Cortex M3. 64 | * Performs a 32*32 -> 64bit multiplication. The middle 32 bits are the result, 65 | * bottom 16 bits are used for rounding, and upper 16 bits are used for overflow 66 | * detection. 67 | */ 68 | 69 | #if !defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 70 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 71 | { 72 | int64_t product = (int64_t)inArg0 * inArg1; 73 | 74 | #ifndef FIXMATH_NO_OVERFLOW 75 | // The upper 17 bits should all be the same (the sign). 76 | uint32_t upper = (product >> 47); 77 | #endif 78 | 79 | if (product < 0) 80 | { 81 | #ifndef FIXMATH_NO_OVERFLOW 82 | if (~upper) 83 | return fix16_overflow; 84 | #endif 85 | 86 | #ifndef FIXMATH_NO_ROUNDING 87 | // This adjustment is required in order to round -1/2 correctly 88 | product--; 89 | #endif 90 | } 91 | else 92 | { 93 | #ifndef FIXMATH_NO_OVERFLOW 94 | if (upper) 95 | return fix16_overflow; 96 | #endif 97 | } 98 | 99 | #ifdef FIXMATH_NO_ROUNDING 100 | return product >> 16; 101 | #else 102 | fix16_t result = product >> 16; 103 | result += (product & 0x8000) >> 15; 104 | 105 | return result; 106 | #endif 107 | } 108 | #endif 109 | 110 | /* 32-bit implementation of fix16_mul. Potentially fast on 16-bit processors, 111 | * and this is a relatively good compromise for compilers that do not support 112 | * uint64_t. Uses 16*16->32bit multiplications. 113 | */ 114 | #if defined(FIXMATH_NO_64BIT) && !defined(FIXMATH_OPTIMIZE_8BIT) 115 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 116 | { 117 | // Each argument is divided to 16-bit parts. 118 | // AB 119 | // * CD 120 | // ----------- 121 | // BD 16 * 16 -> 32 bit products 122 | // CB 123 | // AD 124 | // AC 125 | // |----| 64 bit product 126 | int32_t A = (inArg0 >> 16), C = (inArg1 >> 16); 127 | uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF); 128 | 129 | int32_t AC = A*C; 130 | int32_t AD_CB = A*D + C*B; 131 | uint32_t BD = B*D; 132 | 133 | int32_t product_hi = AC + (AD_CB >> 16); 134 | 135 | // Handle carry from lower 32 bits to upper part of result. 136 | uint32_t ad_cb_temp = AD_CB << 16; 137 | uint32_t product_lo = BD + ad_cb_temp; 138 | if (product_lo < BD) 139 | product_hi++; 140 | 141 | #ifndef FIXMATH_NO_OVERFLOW 142 | // The upper 17 bits should all be the same (the sign). 143 | if (product_hi >> 31 != product_hi >> 15) 144 | return fix16_overflow; 145 | #endif 146 | 147 | #ifdef FIXMATH_NO_ROUNDING 148 | return (product_hi << 16) | (product_lo >> 16); 149 | #else 150 | // Subtracting 0x8000 (= 0.5) and then using signed right shift 151 | // achieves proper rounding to result-1, except in the corner 152 | // case of negative numbers and lowest word = 0x8000. 153 | // To handle that, we also have to subtract 1 for negative numbers. 154 | uint32_t product_lo_tmp = product_lo; 155 | product_lo -= 0x8000; 156 | product_lo -= (uint32_t)product_hi >> 31; 157 | if (product_lo > product_lo_tmp) 158 | product_hi--; 159 | 160 | // Discard the lowest 16 bits. Note that this is not exactly the same 161 | // as dividing by 0x10000. For example if product = -1, result will 162 | // also be -1 and not 0. This is compensated by adding +1 to the result 163 | // and compensating this in turn in the rounding above. 164 | fix16_t result = (product_hi << 16) | (product_lo >> 16); 165 | result += 1; 166 | return result; 167 | #endif 168 | } 169 | #endif 170 | 171 | /* 8-bit implementation of fix16_mul. Fastest on e.g. Atmel AVR. 172 | * Uses 8*8->16bit multiplications, and also skips any bytes that 173 | * are zero. 174 | */ 175 | #if defined(FIXMATH_OPTIMIZE_8BIT) 176 | fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) 177 | { 178 | uint32_t _a = fix_abs(inArg0); 179 | uint32_t _b = fix_abs(inArg1); 180 | 181 | uint8_t va[4] = {_a, (_a >> 8), (_a >> 16), (_a >> 24)}; 182 | uint8_t vb[4] = {_b, (_b >> 8), (_b >> 16), (_b >> 24)}; 183 | 184 | uint32_t low = 0; 185 | uint32_t mid = 0; 186 | 187 | // Result column i depends on va[0..i] and vb[i..0] 188 | 189 | #ifndef FIXMATH_NO_OVERFLOW 190 | // i = 6 191 | if (va[3] && vb[3]) return fix16_overflow; 192 | #endif 193 | 194 | // i = 5 195 | if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3]; 196 | if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2]; 197 | mid <<= 8; 198 | 199 | // i = 4 200 | if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3]; 201 | if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2]; 202 | if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1]; 203 | 204 | #ifndef FIXMATH_NO_OVERFLOW 205 | if (mid & 0xFF000000) return fix16_overflow; 206 | #endif 207 | mid <<= 8; 208 | 209 | // i = 3 210 | if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3]; 211 | if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2]; 212 | if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1]; 213 | if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0]; 214 | 215 | #ifndef FIXMATH_NO_OVERFLOW 216 | if (mid & 0xFF000000) return fix16_overflow; 217 | #endif 218 | mid <<= 8; 219 | 220 | // i = 2 221 | if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2]; 222 | if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1]; 223 | if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0]; 224 | 225 | // i = 1 226 | if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1]; 227 | if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0]; 228 | low <<= 8; 229 | 230 | // i = 0 231 | if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0]; 232 | 233 | #ifndef FIXMATH_NO_ROUNDING 234 | low += 0x8000; 235 | #endif 236 | mid += (low >> 16); 237 | 238 | #ifndef FIXMATH_NO_OVERFLOW 239 | if (mid & 0x80000000) 240 | return fix16_overflow; 241 | #endif 242 | 243 | fix16_t result = mid; 244 | 245 | /* Figure out the sign of result */ 246 | if ((inArg0 >= 0) != (inArg1 >= 0)) 247 | { 248 | result = -result; 249 | } 250 | 251 | return result; 252 | } 253 | #endif 254 | 255 | #ifndef FIXMATH_NO_OVERFLOW 256 | /* Wrapper around fix16_mul to add saturating arithmetic. */ 257 | fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) 258 | { 259 | fix16_t result = fix16_mul(inArg0, inArg1); 260 | 261 | if (result == fix16_overflow) 262 | { 263 | if ((inArg0 >= 0) == (inArg1 >= 0)) 264 | return fix16_maximum; 265 | else 266 | return fix16_minimum; 267 | } 268 | 269 | return result; 270 | } 271 | #endif 272 | 273 | /* 32-bit implementation of fix16_div. Fastest version for e.g. ARM Cortex M3. 274 | * Performs 32-bit divisions repeatedly to reduce the remainder. For this to 275 | * be efficient, the processor has to have 32-bit hardware division. 276 | */ 277 | #if !defined(FIXMATH_NO_HARD_DIVISION) 278 | #ifdef __GNUC__ 279 | // Count leading zeros, using processor-specific instruction if available. 280 | #define clz(x) (__builtin_clzl(x) - (8 * sizeof(long) - 32)) 281 | #else 282 | static uint8_t clz(uint32_t x) 283 | { 284 | uint8_t result = 0; 285 | if (x == 0) return 32; 286 | while (!(x & 0xF0000000)) { result += 4; x <<= 4; } 287 | while (!(x & 0x80000000)) { result += 1; x <<= 1; } 288 | return result; 289 | } 290 | #endif 291 | 292 | fix16_t fix16_div(fix16_t a, fix16_t b) 293 | { 294 | // This uses a hardware 32/32 bit division multiple times, until we have 295 | // computed all the bits in (a<<17)/b. Usually this takes 1-3 iterations. 296 | 297 | if (b == 0) 298 | return fix16_minimum; 299 | 300 | uint32_t remainder = fix_abs(a); 301 | uint32_t divider = fix_abs(b); 302 | uint64_t quotient = 0; 303 | int bit_pos = 17; 304 | 305 | // Kick-start the division a bit. 306 | // This improves speed in the worst-case scenarios where N and D are large 307 | // It gets a lower estimate for the result by N/(D >> 17 + 1). 308 | if (divider & 0xFFF00000) 309 | { 310 | uint32_t shifted_div = ((divider >> 17) + 1); 311 | quotient = remainder / shifted_div; 312 | uint64_t tmp = ((uint64_t)quotient * (uint64_t)divider) >> 17; 313 | remainder -= (uint32_t)(tmp); 314 | } 315 | 316 | // If the divider is divisible by 2^n, take advantage of it. 317 | while (!(divider & 0xF) && bit_pos >= 4) 318 | { 319 | divider >>= 4; 320 | bit_pos -= 4; 321 | } 322 | 323 | while (remainder && bit_pos >= 0) 324 | { 325 | // Shift remainder as much as we can without overflowing 326 | int shift = clz(remainder); 327 | if (shift > bit_pos) shift = bit_pos; 328 | remainder <<= shift; 329 | bit_pos -= shift; 330 | 331 | uint32_t div = remainder / divider; 332 | remainder = remainder % divider; 333 | quotient += (uint64_t)div << bit_pos; 334 | 335 | #ifndef FIXMATH_NO_OVERFLOW 336 | if (div & ~(0xFFFFFFFF >> bit_pos)) 337 | return fix16_overflow; 338 | #endif 339 | 340 | remainder <<= 1; 341 | bit_pos--; 342 | } 343 | 344 | #ifndef FIXMATH_NO_ROUNDING 345 | // Quotient is always positive so rounding is easy 346 | quotient++; 347 | #endif 348 | 349 | fix16_t result = quotient >> 1; 350 | 351 | // Figure out the sign of the result 352 | if ((a ^ b) & 0x80000000) 353 | { 354 | #ifndef FIXMATH_NO_OVERFLOW 355 | if (result == fix16_minimum) 356 | return fix16_overflow; 357 | #endif 358 | 359 | result = -result; 360 | } 361 | 362 | return result; 363 | } 364 | #endif /* !defined(FIXMATH_NO_HARD_DIVISION) */ 365 | 366 | /* Alternative 32-bit implementation of fix16_div. Fastest on e.g. Atmel AVR. 367 | * This does the division manually, and is therefore good for processors that 368 | * do not have hardware division. 369 | */ 370 | #if defined(FIXMATH_NO_HARD_DIVISION) 371 | fix16_t fix16_div(fix16_t a, fix16_t b) 372 | { 373 | // This uses the basic binary restoring division algorithm. 374 | // It appears to be faster to do the whole division manually than 375 | // trying to compose a 64-bit divide out of 32-bit divisions on 376 | // platforms without hardware divide. 377 | 378 | if (b == 0) 379 | return fix16_minimum; 380 | 381 | uint32_t remainder = fix_abs(a); 382 | uint32_t divider = fix_abs(b); 383 | 384 | uint32_t quotient = 0; 385 | uint32_t bit = 0x10000; 386 | 387 | /* The algorithm requires D >= R */ 388 | while (divider < remainder) 389 | { 390 | divider <<= 1; 391 | bit <<= 1; 392 | } 393 | 394 | #ifndef FIXMATH_NO_OVERFLOW 395 | if (!bit) 396 | return fix16_overflow; 397 | #endif 398 | 399 | if (divider & 0x80000000) 400 | { 401 | // Perform one step manually to avoid overflows later. 402 | // We know that divider's bottom bit is 0 here. 403 | if (remainder >= divider) 404 | { 405 | quotient |= bit; 406 | remainder -= divider; 407 | } 408 | divider >>= 1; 409 | bit >>= 1; 410 | } 411 | 412 | /* Main division loop */ 413 | while (bit && remainder) 414 | { 415 | if (remainder >= divider) 416 | { 417 | quotient |= bit; 418 | remainder -= divider; 419 | } 420 | 421 | remainder <<= 1; 422 | bit >>= 1; 423 | } 424 | 425 | #ifndef FIXMATH_NO_ROUNDING 426 | if (remainder >= divider) 427 | { 428 | quotient++; 429 | } 430 | #endif 431 | 432 | fix16_t result = quotient; 433 | 434 | /* Figure out the sign of result */ 435 | if ((a ^ b) & 0x80000000) 436 | { 437 | #ifndef FIXMATH_NO_OVERFLOW 438 | if (result == fix16_minimum) 439 | return fix16_overflow; 440 | #endif 441 | 442 | result = -result; 443 | } 444 | 445 | return result; 446 | } 447 | #endif /* defined(FIXMATH_NO_HARD_DIVISION) */ 448 | 449 | #ifndef FIXMATH_NO_OVERFLOW 450 | /* Wrapper around fix16_div to add saturating arithmetic. */ 451 | fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) 452 | { 453 | fix16_t result = fix16_div(inArg0, inArg1); 454 | 455 | if (result == fix16_overflow) 456 | { 457 | if ((inArg0 >= 0) == (inArg1 >= 0)) 458 | return fix16_maximum; 459 | else 460 | return fix16_minimum; 461 | } 462 | 463 | return result; 464 | } 465 | #endif 466 | 467 | fix16_t fix16_mod(fix16_t x, fix16_t y) 468 | { 469 | #ifdef FIXMATH_NO_HARD_DIVISION 470 | /* The reason we do this, rather than use a modulo operator 471 | * is that if you don't have a hardware divider, this will result 472 | * in faster operations when the angles are close to the bounds. 473 | */ 474 | while(x >= y) x -= y; 475 | while(x <= -y) x += y; 476 | #else 477 | /* Note that in C90, the sign of result of the modulo operation is 478 | * undefined. in C99, it's the same as the dividend (aka numerator). 479 | */ 480 | x %= y; 481 | #endif 482 | 483 | return x; 484 | } 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, (((int32_t)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 | if(inFract == 0) 505 | return inArg0; 506 | int64_t inFract64 = int64_const(0, inFract); 507 | int64_t subbed = int64_sub(int64_const(1,0), inFract64); 508 | int64_t tempOut = int64_mul_i64_i32(subbed, inArg0); 509 | tempOut = int64_add(tempOut, int64_mul_i64_i32(inFract64, inArg1)); 510 | return int64_hi(tempOut); 511 | } 512 | --------------------------------------------------------------------------------