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
|
/*
*
* Copyright (C) 2020 Google, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*/
#include <stdint.h>
/*
* This file implements operations for a simple 32.32 fixed-point math type.
* This is intended for speed-critical stuff (e.g. graphics) so there are
* intentionally no overflow checks or assertions, and operations are written
* to prefer speed over precision (e.g. multiplying by 1 may lose precision).
* For best results, only use for applications where 16.16 would fit.
*/
typedef struct { /* wrap in struct to prevent direct access */
int64_t v;
} fpmath_t;
#define FPMATH_SHIFT 32 /* define where to place the decimal point */
/* Turn an integer into an fpmath_t. */
static inline fpmath_t fp(int32_t a)
{
return (fpmath_t){ .v = (int64_t)a << FPMATH_SHIFT };
}
/* Create an fpmath_t from a fraction. (numerator / denominator) */
static inline fpmath_t fpfrac(int32_t numerator, int32_t denominator)
{
return (fpmath_t){ .v = ((int64_t)numerator << FPMATH_SHIFT) / denominator };
}
/* Turn an fpmath_t back into an integer, rounding towards -INF. */
static inline int32_t fpfloor(fpmath_t a)
{
return a.v >> FPMATH_SHIFT;
}
/* Turn an fpmath_t back into an integer, rounding towards nearest. */
static inline int32_t fpround(fpmath_t a)
{
return (a.v + ((int64_t)1 << (FPMATH_SHIFT - 1))) >> FPMATH_SHIFT;
}
/* Turn an fpmath_t back into an integer, rounding towards +INF. */
static inline int32_t fpceil(fpmath_t a)
{
return (a.v + ((int64_t)1 << FPMATH_SHIFT) - 1) >> FPMATH_SHIFT;
}
/* Add two fpmath_t. (a + b) */
static inline fpmath_t fpadd(fpmath_t a, fpmath_t b)
{
return (fpmath_t){ .v = a.v + b.v };
}
/* Add an fpmath_t and an integer. (a + b) */
static inline fpmath_t fpaddi(fpmath_t a, int32_t b)
{
return (fpmath_t){ .v = a.v + ((int64_t)b << FPMATH_SHIFT) };
}
/* Subtract one fpmath_t from another. (a + b) */
static inline fpmath_t fpsub(fpmath_t a, fpmath_t b)
{
return (fpmath_t){ .v = a.v - b.v };
}
/* Subtract an integer from an fpmath_t. (a - b) */
static inline fpmath_t fpsubi(fpmath_t a, int32_t b)
{
return (fpmath_t){ .v = a.v - ((int64_t)b << FPMATH_SHIFT) };
}
/* Subtract an fpmath_t from an integer. (a - b) */
static inline fpmath_t fpisub(int32_t a, fpmath_t b)
{
return (fpmath_t){ .v = ((int64_t)a << FPMATH_SHIFT) - b.v };
}
/* Multiply two fpmath_t. (a * b)
Looses 16 bits fractional precision on each. */
static inline fpmath_t fpmul(fpmath_t a, fpmath_t b)
{
return (fpmath_t){ .v = (a.v >> (FPMATH_SHIFT/2)) * (b.v >> (FPMATH_SHIFT/2)) };
}
/* Multiply an fpmath_t and an integer. (a * b) */
static inline fpmath_t fpmuli(fpmath_t a, int32_t b)
{
return (fpmath_t){ .v = a.v * b };
}
/* Divide an fpmath_t by another. (a / b)
Truncates integral part of a to 16 bits! Careful with this one! */
static inline fpmath_t fpdiv(fpmath_t a, fpmath_t b)
{
return (fpmath_t){ .v = (a.v << (FPMATH_SHIFT/2)) / (b.v >> (FPMATH_SHIFT/2)) };
}
/* Divide an fpmath_t by an integer. (a / b) */
static inline fpmath_t fpdivi(fpmath_t a, int32_t b)
{
return (fpmath_t){ .v = a.v / b };
}
/* Calculate absolute value of an fpmath_t. (ABS(a)) */
static inline fpmath_t fpabs(fpmath_t a)
{
return (fpmath_t){ .v = (a.v < 0 ? -a.v : a.v) };
}
/* Return true iff two fpmath_t are exactly equal. (a == b)
Like with floats, you probably don't want to use this most of the time. */
static inline int fpequals(fpmath_t a, fpmath_t b)
{
return a.v == b.v;
}
/* Return true iff one fpmath_t is less than another. (a < b) */
static inline int fpless(fpmath_t a, fpmath_t b)
{
return a.v < b.v;
}
/* Return true iff one fpmath_t is more than another. (a > b) */
static inline int fpmore(fpmath_t a, fpmath_t b)
{
return a.v > b.v;
}
/* Return the smaller of two fpmath_t. (MIN(a, b)) */
static inline fpmath_t fpmin(fpmath_t a, fpmath_t b)
{
if (a.v < b.v)
return a;
else
return b;
}
/* Return the larger of two fpmath_t. (MAX(a, b)) */
static inline fpmath_t fpmax(fpmath_t a, fpmath_t b)
{
if (a.v > b.v)
return a;
else
return b;
}
/* Return the constant PI as an fpmath_t. */
static inline fpmath_t fppi(void)
{
/* Rounded (uint64_t)(M_PI * (1UL << 60)) to nine hex digits. */
return (fpmath_t){ .v = 0x3243f6a89 };
}
/*
* Returns the "one-based" sine of an fpmath_t, meaning the input is interpreted as if the range
* 0.0-1.0 corresponded to 0.0-PI/2 for radians. This is mostly here as the base primitives for
* the other trig stuff, but it may be useful to use directly if your input value already needs
* to be multiplied by some factor of PI and you want to save the instructions (and precision)
* for multiplying it in just so that the trig functions can divide it right out again.
*/
fpmath_t fpsin1(fpmath_t x);
/* Returns the "one-based" cosine of an fpmath_t (analogous definition to fpsin1()). */
static inline fpmath_t fpcos1(fpmath_t x)
{
return fpsin1(fpaddi(x, 1));
}
/* Returns the sine of an fpmath_t interpreted as radians. */
static inline fpmath_t fpsinr(fpmath_t radians)
{
return fpsin1(fpdiv(radians, fpdivi(fppi(), 2)));
}
/* Returns the sine of an fpmath_t interpreted as degrees. */
static inline fpmath_t fpsind(fpmath_t degrees)
{
return fpsin1(fpdivi(degrees, 90));
}
/* Returns the cosine of an fpmath_t interpreted as radians. */
static inline fpmath_t fpcosr(fpmath_t radians)
{
return fpcos1(fpdiv(radians, fpdivi(fppi(), 2)));
}
/* Returns the cosine of an fpmath_t interpreted as degrees. */
static inline fpmath_t fpcosd(fpmath_t degrees)
{
return fpcos1(fpdivi(degrees, 90));
}
/* Returns the tangent of an fpmath_t interpreted as radians.
No guard rails, don't call this at the poles or you'll divide by 0! */
static inline fpmath_t fptanr(fpmath_t radians)
{
fpmath_t one_based = fpdiv(radians, fpdivi(fppi(), 2));
return fpdiv(fpsin1(one_based), fpcos1(one_based));
}
/* Returns the tangent of an fpmath_t interpreted as degrees.
No guard rails, don't call this at the poles or you'll divide by 0! */
static inline fpmath_t fptand(fpmath_t degrees)
{
fpmath_t one_based = fpdivi(degrees, 90);
return fpdiv(fpsin1(one_based), fpcos1(one_based));
}
|
