Skip to content

Instantly share code, notes, and snippets.

@lexszero

lexszero/cos_table.h

Last active September 29, 2020 07:37
Show Gist options
  • Save lexszero/d5e6a7939bdc4b7cce78 to your computer and use it in GitHub Desktop.
Save lexszero/d5e6a7939bdc4b7cce78 to your computer and use it in GitHub Desktop.
Download ZIP
Simple implementation of fixed point arithmetics with trigonometry functions
Raw
cos_table.h
/*
* cos_table.h
*
* Table for more accurate computation of cosine function near zero argument
* See Common/fix.c:fix_sincos
*
* Created on: 20.06.2011
* Author: Alex Ignatov
*/
#ifndef __cos_table_H
#define __cos_table_H
// table used if argument in [COS_MIN, COS_MAX)
#define COS_MIN -18301
#define COS_MAX 18301
#define COS_SHIFT 6
#define COS_COUNT 572
static const fix_t cos_table[COS_COUNT] = {
1048416,
1048417,
1048419,
1048420,
1048421,
1048422,
1048423,
1048424,
1048425,
1048426,
1048427,
1048428,
1048429,
1048430,
1048432,
1048433,
1048434,
1048435,
1048436,
1048437,
1048438,
1048439,
1048440,
1048441,
1048442,
1048443,
1048444,
1048445,
1048446,
1048447,
1048448,
1048449,
1048450,
1048451,
1048452,
1048453,
1048454,
1048455,
1048456,
1048457,
1048458,
1048459,
1048460,
1048461,
1048462,
1048463,
1048464,
1048464,
1048465,
1048466,
1048467,
1048468,
1048469,
1048470,
1048471,
1048472,
1048473,
1048474,
1048475,
1048475,
1048476,
1048477,
1048478,
1048479,
1048480,
1048481,
1048482,
1048482,
1048483,
1048484,
1048485,
1048486,
1048487,
1048487,
1048488,
1048489,
1048490,
1048491,
1048492,
1048492,
1048493,
1048494,
1048495,
1048496,
1048496,
1048497,
1048498,
1048499,
1048499,
1048500,
1048501,
1048502,
1048503,
1048503,
1048504,
1048505,
1048506,
1048506,
1048507,
1048508,
1048508,
1048509,
1048510,
1048511,
1048511,
1048512,
1048513,
1048513,
1048514,
1048515,
1048516,
1048516,
1048517,
1048518,
1048518,
1048519,
1048520,
1048520,
1048521,
1048522,
1048522,
1048523,
1048523,
1048524,
1048525,
1048525,
1048526,
1048527,
1048527,
1048528,
1048528,
1048529,
1048530,
1048530,
1048531,
1048531,
1048532,
1048533,
1048533,
1048534,
1048534,
1048535,
1048536,
1048536,
1048537,
1048537,
1048538,
1048538,
1048539,
1048539,
1048540,
1048540,
1048541,
1048541,
1048542,
1048543,
1048543,
1048544,
1048544,
1048545,
1048545,
1048546,
1048546,
1048546,
1048547,
1048547,
1048548,
1048548,
1048549,
1048549,
1048550,
1048550,
1048551,
1048551,
1048552,
1048552,
1048552,
1048553,
1048553,
1048554,
1048554,
1048554,
1048555,
1048555,
1048556,
1048556,
1048556,
1048557,
1048557,
1048558,
1048558,
1048558,
1048559,
1048559,
1048559,
1048560,
1048560,
1048561,
1048561,
1048561,
1048562,
1048562,
1048562,
1048563,
1048563,
1048563,
1048564,
1048564,
1048564,
1048564,
1048565,
1048565,
1048565,
1048566,
1048566,
1048566,
1048566,
1048567,
1048567,
1048567,
1048568,
1048568,
1048568,
1048568,
1048569,
1048569,
1048569,
1048569,
1048569,
1048570,
1048570,
1048570,
1048570,
1048571,
1048571,
1048571,
1048571,
1048571,
1048572,
1048572,
1048572,
1048572,
1048572,
1048572,
1048573,
1048573,
1048573,
1048573,
1048573,
1048573,
1048573,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048576,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048575,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048574,
1048573,
1048573,
1048573,
1048573,
1048573,
1048573,
1048573,
1048572,
1048572,
1048572,
1048572,
1048572,
1048571,
1048571,
1048571,
1048571,
1048571,
1048571,
1048570,
1048570,
1048570,
1048570,
1048569,
1048569,
1048569,
1048569,
1048568,
1048568,
1048568,
1048568,
1048567,
1048567,
1048567,
1048567,
1048566,
1048566,
1048566,
1048566,
1048565,
1048565,
1048565,
1048564,
1048564,
1048564,
1048563,
1048563,
1048563,
1048563,
1048562,
1048562,
1048562,
1048561,
1048561,
1048561,
1048560,
1048560,
1048559,
1048559,
1048559,
1048558,
1048558,
1048558,
1048557,
1048557,
1048556,
1048556,
1048556,
1048555,
1048555,
1048554,
1048554,
1048554,
1048553,
1048553,
1048552,
1048552,
1048551,
1048551,
1048551,
1048550,
1048550,
1048549,
1048549,
1048548,
1048548,
1048547,
1048547,
1048546,
1048546,
1048545,
1048545,
1048544,
1048544,
1048543,
1048543,
1048542,
1048542,
1048541,
1048541,
1048540,
1048540,
1048539,
1048539,
1048538,
1048538,
1048537,
1048537,
1048536,
1048535,
1048535,
1048534,
1048534,
1048533,
1048533,
1048532,
1048531,
1048531,
1048530,
1048530,
1048529,
1048528,
1048528,
1048527,
1048527,
1048526,
1048525,
1048525,
1048524,
1048523,
1048523,
1048522,
1048522,
1048521,
1048520,
1048520,
1048519,
1048518,
1048518,
1048517,
1048516,
1048515,
1048515,
1048514,
1048513,
1048513,
1048512,
1048511,
1048511,
1048510,
1048509,
1048508,
1048508,
1048507,
1048506,
1048505,
1048505,
1048504,
1048503,
1048502,
1048502,
1048501,
1048500,
1048499,
1048499,
1048498,
1048497,
1048496,
1048495,
1048495,
1048494,
1048493,
1048492,
1048491,
1048491,
1048490,
1048489,
1048488,
1048487,
1048487,
1048486,
1048485,
1048484,
1048483,
1048482,
1048481,
1048481,
1048480,
1048479,
1048478,
1048477,
1048476,
1048475,
1048474,
1048474,
1048473,
1048472,
1048471,
1048470,
1048469,
1048468,
1048467,
1048466,
1048465,
1048464,
1048463,
1048463,
1048462,
1048461,
1048460,
1048459,
1048458,
1048457,
1048456,
1048455,
1048454,
1048453,
1048452,
1048451,
1048450,
1048449,
1048448,
1048447,
1048446,
1048445,
1048444,
1048443,
1048442,
1048441,
1048440,
1048439,
1048438,
1048437,
1048436,
1048435,
1048434,
1048433,
1048431,
1048430,
1048429,
1048428,
1048427,
1048426,
1048425,
1048424,
1048423,
1048422,
1048421,
1048420,
1048418,
1048417,
};
#endif
Raw
fix.c
/*
* fix.c
*
* Fixed-point arithmetics.
*
* Created on: 12.10.2010
* Author: Alex Ignatov
*/
#include "fix.h"
#include "cos_table.h"
#define CORDIC_COUNT 16
const fix_t cordic[] = { 823549, 486169, 256878, 130395, 65450, 32757, 16382, 8191, 4095, 2047, 1023, 511, 255, 127, 63, 31 };
#define K1 (int32_t)(0.6072529350088812561694 * fix_base)
#define HALF_PI PI/2
/* atan2 using CORDIC method */
fix_t fix_atan2(fix_t y, fix_t x) {
int32_t theta, i, t;
theta = 0;
if (x<0) {
if (y >= 0) {
theta = HALF_PI;
t = x;
x = y;
y = -t;
}
else {
theta = -HALF_PI;
t = x;
x = -y;
y = t;
}
}
else
theta = 0;
int32_t last_x;
for (i = 0; i < CORDIC_COUNT; i++) {
last_x = x;
if (y < 0) { // sign=1
x -= y >> i;
y += last_x >> i;
theta -= cordic[i];
}
else {
x += y >> i;
y -= last_x >> i;
theta += cordic[i];
}
}
return theta;
}
void fix_sincos(fix_t theta, fix_t *r_sin, fix_t *r_cos) {
if ((theta >= COS_MIN) && (theta < COS_MAX)) {
// angle small enough to use linear approximation for sin and table lookup for cos
*r_sin = theta;
*r_cos = cos_table[(theta - COS_MIN) >> COS_SHIFT];
return;
}
int32_t i, d, tx, ty, tz;
int32_t x = K1, y = 0, z;
uint8_t flip = 0; // flip sign
// rotate argument to I quadrant, remembering sign changes
if (theta < -HALF_PI || theta > HALF_PI) {
if (theta < 0)
z = theta + PI;
else
z = theta - PI;
flip = 1;
}
else {
z = theta;
flip = 0;
}
// actual CORDIC algorithm, see wiki
for (i = 0; i < CORDIC_COUNT; i++) {
d = z>>31;
tx = x - (((y>>i) ^ d) - d);
ty = y + (((x>>i) ^ d) - d);
tz = z - ((cordic[i] ^ d) - d);
x = tx; y = ty; z = tz;
}
if (flip) {
x = -x;
y = -y;
}
*r_cos = x;
*r_sin = y;
}
/* fix_sin, fix_cos, fix_tan - wrappers for fix_sincos */
fix_t fix_sin(fix_t theta) {
fix_t r_sin, r_cos;
fix_sincos(theta, &r_sin, &r_cos);
return r_sin;
}
fix_t fix_cos(fix_t theta) {
fix_t r_sin, r_cos;
fix_sincos(theta, &r_sin, &r_cos);
return r_cos;
}
fix_t fix_tan(fix_t theta) {
fix_t r_sin, r_cos;
fix_sincos(theta, &r_sin, &r_cos);
return fix_div(r_sin, r_cos);
}
/* Square root using cordic-like algorithm
* http://www.convict.lu/Jeunes/Math/square_root_CORDIC.htm
*/
fix_t fix_sqrt(fix_t x) {
fix_t base, y;
int i;
if (x == 0)
return 0;
base = 33554432;
y = 0;
for (i = 0; i < 26; i++) {
y += base;
if (fix_mul(y, y) > x)
y -= base;
base >>= 1;
}
return y;
}
fix_t fix_asin(fix_t x) {
// asin(x) = atan(x/sqrt(1-x^2))
return fix_atan2(x, fix_sqrt(ONE - fix_pow2(x)));
}
fix_t fix_acos(fix_t x) {
// acos(x) = atan(sqrt(1-x^2)/x)
return fix_atan2(fix_sqrt(ONE - fix_pow2(x)), x);
}
Raw
fix.h
/*
* fix.h
*
* Fixed-point arithmetics.
*
* Using signed 32-bit integers: 12 bits for sign and integer part, 20 bits for fractional part
*
* Created on: 12.10.2010
* Author: Alex Ignatov
*/
#ifndef __fix_H
#define __fix_H
#include <stdint.h>
typedef long long int int64_t;
#define fix_point 20 // bit length of fractional part. don't change, something will broke.
#define fix_base (1 << fix_point)
#define ONE (1 << fix_point)
#define PI 3294199 // just fixpoint PI constant
typedef int32_t fix_t;
#define _fix(x) ((int64_t)x << fix_point)
/* Simple conversion and arithmetic functions put in header file for further inlining */
static inline fix_t float2fix(float a) {
return (fix_t)(a * fix_base);
}
static inline fix_t int2fix(int a) {
return a << fix_point;
}
static inline float fix2float(fix_t a) {
return a / (float)fix_base;
}
static inline fix_t fix_add(fix_t a, fix_t b) {
return a + b;
}
static inline fix_t fix_sub(fix_t a, fix_t b) {
return a - b;
}
/* Multiplication and division functions prefixes meaning:
*
* fix_fix_* takes two fixed-point args, returns fixed-point
* fix_int_* takes fixed-point and integer args, returns fixed-point
* int_int_* takes two integer args, returns fixed-point
*/
static inline fix_t fix_fix_mul(fix_t a, fix_t b) {
return (fix_t)(((int64_t)a * (int64_t)b) >> fix_point);
}
static inline fix_t fix_int_mul(fix_t a, int b) {
return (fix_t)(a * b);
}
static inline fix_t fix_fix_div(fix_t a, fix_t b) {
return (a < (0xFFFFFFFFU >> fix_point))
? (fix_t)((a << fix_point) / b)
: (fix_t)(((int64_t)a << fix_point) / b);
}
static inline fix_t int_int_div(int a, int b) {
return (fix_t)(((int64_t)a << fix_point) / b);
}
static inline fix_t fix_int_div(fix_t a, int b) {
return (fix_t)(a / b);
}
static inline fix_t fix_max(fix_t a, fix_t b) {
return (a > b) ? a : b;
}
static inline fix_t fix_min(fix_t a, fix_t b) {
return (a < b) ? a : b;
}
/* Shortcut macros for most frequently using functions */
#define fix_mul(a, b) fix_fix_mul(a, b)
#define fix_div(a, b) fix_fix_div(a, b)
#define fix_pow2(x) fix_mul(x, x)
#define fix_pow3(x) fix_mul(fix_pow2(x), x)
#define fix_pow4(x) fix_mul(fix_pow3(x), x)
#define fix_sign(a) (a & 0x80000000)
#define fix_abs(a) (fix_sign(a) ? -(a) : (a))
/* Trigonometry (via cordic) and other complex functions */
extern fix_t fix_atan2(fix_t y, fix_t x);
extern void fix_sincos(fix_t theta, fix_t *r_sin, fix_t *r_cos);
extern fix_t fix_sin(fix_t theta);
extern fix_t fix_cos(fix_t theta);
extern fix_t fix_tan(fix_t theta);
extern fix_t fix_sqrt(fix_t x);
extern fix_t fix_asin(fix_t x);
extern fix_t fix_acos(fix_t x);
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment