-
Notifications
You must be signed in to change notification settings - Fork 8
/
fix16.h
237 lines (185 loc) · 7.7 KB
/
fix16.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
#ifndef __libfixmath_fix16_h__
#define __libfixmath_fix16_h__
#ifdef __cplusplus
extern "C"
{
#endif
/* These options may let the optimizer to remove some calls to the functions.
* Refer to http://gcc.gnu.org/onlinedocs/gcc/Function-Attributes.html
*/
#ifndef FIXMATH_FUNC_ATTRS
# ifdef __GNUC__
# if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 6)
# define FIXMATH_FUNC_ATTRS __attribute__((leaf, nothrow, const))
# else
# define FIXMATH_FUNC_ATTRS __attribute__((nothrow, const))
# endif
# else
# define FIXMATH_FUNC_ATTRS
# endif
#endif
#include "libfixmath_conf.h"
#include <stdint.h>
typedef int32_t fix16_t;
static const fix16_t FOUR_DIV_PI = 0x145F3; /*!< Fix16 value of 4/PI */
static const fix16_t _FOUR_DIV_PI2 = 0xFFFF9840; /*!< Fix16 value of -4/PI² */
static const fix16_t X4_CORRECTION_COMPONENT = 0x399A; /*!< Fix16 value of 0.225 */
static const fix16_t PI_DIV_4 = 0x0000C90F; /*!< Fix16 value of PI/4 */
static const fix16_t THREE_PI_DIV_4 = 0x00025B2F; /*!< Fix16 value of 3PI/4 */
static const fix16_t fix16_maximum = 0x7FFFFFFF; /*!< the maximum value of fix16_t */
static const fix16_t fix16_minimum = 0x80000000; /*!< the minimum value of fix16_t */
static const fix16_t fix16_overflow = 0x80000000; /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not specified */
static const fix16_t fix16_pi = 205887; /*!< fix16_t value of pi */
static const fix16_t fix16_e = 178145; /*!< fix16_t value of e */
static const fix16_t fix16_one = 0x00010000; /*!< fix16_t value of 1 */
/* Conversion functions between fix16_t and float/integer.
* These are inlined to allow compiler to optimize away constant numbers
*/
static inline fix16_t fix16_from_int(int a) { return a * fix16_one; }
static inline float fix16_to_float(fix16_t a) { return (float)a / fix16_one; }
static inline double fix16_to_dbl(fix16_t a) { return (double)a / fix16_one; }
static inline int fix16_to_int(fix16_t a)
{
#ifdef FIXMATH_NO_ROUNDING
return (a >> 16);
#else
if (a >= 0)
return (a + (fix16_one >> 1)) / fix16_one;
return (a - (fix16_one >> 1)) / fix16_one;
#endif
}
static inline fix16_t fix16_from_float(float a)
{
float temp = a * fix16_one;
#ifndef FIXMATH_NO_ROUNDING
temp += (temp >= 0) ? 0.5f : -0.5f;
#endif
return (fix16_t)temp;
}
static inline fix16_t fix16_from_dbl(double a)
{
double temp = a * fix16_one;
#ifndef FIXMATH_NO_ROUNDING
temp += (temp >= 0) ? 0.5f : -0.5f;
#endif
return (fix16_t)temp;
}
/* Macro for defining fix16_t constant values.
The functions above can't be used from e.g. global variable initializers,
and their names are quite long also. This macro is useful for constants
springled alongside code, e.g. F16(1.234).
Note that the argument is evaluated multiple times, and also otherwise
you should only use this for constant values. For runtime-conversions,
use the functions above.
*/
#define F16(x) ((fix16_t)(((x) >= 0) ? ((x) * 65536.0 + 0.5) : ((x) * 65536.0 - 0.5)))
static inline fix16_t fix16_abs(fix16_t x)
{ return (x < 0 ? -x : x); }
static inline fix16_t fix16_floor(fix16_t x)
{ return (x & 0xFFFF0000UL); }
static inline fix16_t fix16_ceil(fix16_t x)
{ return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? fix16_one : 0); }
static inline fix16_t fix16_min(fix16_t x, fix16_t y)
{ return (x < y ? x : y); }
static inline fix16_t fix16_max(fix16_t x, fix16_t y)
{ return (x > y ? x : y); }
static inline fix16_t fix16_clamp(fix16_t x, fix16_t lo, fix16_t hi)
{ return fix16_min(fix16_max(x, lo), hi); }
/* Subtraction and addition with (optional) overflow detection. */
#ifdef FIXMATH_NO_OVERFLOW
static inline fix16_t fix16_add(fix16_t inArg0, fix16_t inArg1) { return (inArg0 + inArg1); }
static inline fix16_t fix16_sub(fix16_t inArg0, fix16_t inArg1) { return (inArg0 - inArg1); }
#else
extern fix16_t fix16_add(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS;
extern fix16_t fix16_sub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS;
/* Saturating arithmetic */
extern fix16_t fix16_sadd(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS;
extern fix16_t fix16_ssub(fix16_t a, fix16_t b) FIXMATH_FUNC_ATTRS;
#endif
/*! Multiplies the two given fix16_t's and returns the result.
*/
extern fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
/*! Divides the first given fix16_t by the second and returns the result.
*/
extern fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
#ifndef FIXMATH_NO_OVERFLOW
/*! Performs a saturated multiplication (overflow-protected) of the two given fix16_t's and returns the result.
*/
extern fix16_t fix16_smul(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
/*! Performs a saturated division (overflow-protected) of the first fix16_t by the second and returns the result.
*/
extern fix16_t fix16_sdiv(fix16_t inArg0, fix16_t inArg1) FIXMATH_FUNC_ATTRS;
#endif
/*! Divides the first given fix16_t by the second and returns the result.
*/
extern fix16_t fix16_mod(fix16_t x, fix16_t y) FIXMATH_FUNC_ATTRS;
/*! Returns the linear interpolation: (inArg0 * (1 - inFract)) + (inArg1 * inFract)
*/
extern fix16_t fix16_lerp8(fix16_t inArg0, fix16_t inArg1, uint8_t inFract) FIXMATH_FUNC_ATTRS;
extern fix16_t fix16_lerp16(fix16_t inArg0, fix16_t inArg1, uint16_t inFract) FIXMATH_FUNC_ATTRS;
#ifndef FIXMATH_NO_64BIT
extern fix16_t fix16_lerp32(fix16_t inArg0, fix16_t inArg1, uint32_t inFract) FIXMATH_FUNC_ATTRS;
#endif
/*! Returns the sine of the given fix16_t.
*/
extern fix16_t fix16_sin_parabola(fix16_t inAngle) FIXMATH_FUNC_ATTRS;
/*! Returns the sine of the given fix16_t.
*/
extern fix16_t fix16_sin(fix16_t inAngle) FIXMATH_FUNC_ATTRS;
/*! Returns the cosine of the given fix16_t.
*/
extern fix16_t fix16_cos(fix16_t inAngle) FIXMATH_FUNC_ATTRS;
/*! Returns the tangent of the given fix16_t.
*/
extern fix16_t fix16_tan(fix16_t inAngle) FIXMATH_FUNC_ATTRS;
/*! Returns the arcsine of the given fix16_t.
*/
extern fix16_t fix16_asin(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the arccosine of the given fix16_t.
*/
extern fix16_t fix16_acos(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the arctangent of the given fix16_t.
*/
extern fix16_t fix16_atan(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the arctangent of inY/inX.
*/
extern fix16_t fix16_atan2(fix16_t inY, fix16_t inX) FIXMATH_FUNC_ATTRS;
static const fix16_t fix16_rad_to_deg_mult = 3754936;
static inline fix16_t fix16_rad_to_deg(fix16_t radians)
{ return fix16_mul(radians, fix16_rad_to_deg_mult); }
static const fix16_t fix16_deg_to_rad_mult = 1144;
static inline fix16_t fix16_deg_to_rad(fix16_t degrees)
{ return fix16_mul(degrees, fix16_deg_to_rad_mult); }
/*! Returns the square root of the given fix16_t.
*/
extern fix16_t fix16_sqrt(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the square of the given fix16_t.
*/
static inline fix16_t fix16_sq(fix16_t x)
{ return fix16_mul(x, x); }
/*! Returns the exponent (e^) of the given fix16_t.
*/
extern fix16_t fix16_exp(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the natural logarithm of the given fix16_t.
*/
extern fix16_t fix16_log(fix16_t inValue) FIXMATH_FUNC_ATTRS;
/*! Returns the base 2 logarithm of the given fix16_t.
*/
extern fix16_t fix16_log2(fix16_t x) FIXMATH_FUNC_ATTRS;
/*! Returns the saturated base 2 logarithm of the given fix16_t.
*/
extern fix16_t fix16_slog2(fix16_t x) FIXMATH_FUNC_ATTRS;
/*! Convert fix16_t value to a string.
* Required buffer length for largest values is 13 bytes.
*/
extern void fix16_to_str(fix16_t value, char *buf, int decimals);
/*! Convert string to a fix16_t value
* Ignores spaces at beginning and end. Returns fix16_overflow if
* value is too large or there were garbage characters.
*/
extern fix16_t fix16_from_str(const char *buf);
#ifdef __cplusplus
}
#include "fix16.hpp"
#endif
#endif