|
14
|
1 /* Project: Q-Number (Q16.16, signed) library
|
|
|
2 * Author: Richard James Howe
|
|
|
3 * License: The Unlicense
|
|
|
4 * Email: howe.r.j.89@gmail.com
|
|
|
5 * Repo: <https://github.com/q>
|
|
|
6 *
|
|
|
7 *
|
|
|
8 * A Q32.32 version would be useful.
|
|
|
9 *
|
|
|
10 * The following should be changed/done for this library:
|
|
|
11 *
|
|
|
12 * - Moving towards a header-only model.
|
|
|
13 * - Removal of dependencies such as 'isalpha', 'tolower'
|
|
|
14 * as they are locale dependent.
|
|
|
15 * - Make components optional (filters, expression parser, ...)
|
|
|
16 * - Make hyperbolic arc sin/cos/tan functions.
|
|
|
17 * - Fix bugs / inaccuracies in CORDIC code.
|
|
|
18 * - Improve accuracy of all the functions and quantify error and
|
|
|
19 * their limits.
|
|
|
20 *
|
|
|
21 * BUG: Enter: 2.71791, get 2.0625, 2.7179 works fine. (Need to
|
|
|
22 * limit decimal places).
|
|
|
23 */
|
|
|
24
|
|
|
25 #include "q.h"
|
|
|
26 #include <assert.h>
|
|
|
27 #include <ctype.h>
|
|
|
28 #include <inttypes.h>
|
|
|
29 #include <limits.h>
|
|
|
30 #include <stdarg.h> /* for expression evaluator error handling */
|
|
|
31 #include <stdio.h> /* vsnprintf, for expression evaluator */
|
|
|
32 #include <string.h>
|
|
|
33
|
|
|
34 #define UNUSED(X) ((void)(X))
|
|
|
35 #define BOOLIFY(X) (!!(X))
|
|
|
36 #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
|
|
37 #define MULTIPLIER (INT16_MAX)
|
|
|
38 #define DMIN (INT32_MIN)
|
|
|
39 #define DMAX (INT32_MAX)
|
|
|
40 #define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
|
|
|
41 #define MAX(X, Y) ((X) < (Y) ? (Y) : (X))
|
|
|
42
|
|
|
43 #ifndef CONFIG_Q_HIDE_FUNCS /* 1 = hide hidden (testing) functions, 0 = enable them */
|
|
|
44 #define CONFIG_Q_HIDE_FUNCS (0)
|
|
|
45 #endif
|
|
|
46
|
|
|
47 typedef int16_t hd_t; /* half Q width, signed */
|
|
|
48 typedef uint64_t lu_t; /* double Q width, unsigned */
|
|
|
49
|
|
|
50 const qinfo_t qinfo = {
|
|
|
51 .whole = QBITS,
|
|
|
52 .fractional = QBITS,
|
|
|
53 .zero = (u_t)0uL << QBITS,
|
|
|
54 .bit = 1uL,
|
|
|
55 .one = (u_t)1uL << QBITS,
|
|
|
56 .min = (u_t)(QHIGH << QBITS),
|
|
|
57 .max = (u_t)((QHIGH << QBITS) - 1uL),
|
|
|
58
|
|
|
59 .pi = QPI, /* 3.243F6 A8885 A308D 31319 8A2E0... */
|
|
|
60 .e = QMK(0x2, 0xB7E1, 16), /* 2.B7E1 5162 8A... */
|
|
|
61 .sqrt2 = QMK(0x1, 0x6A09, 16), /* 1.6A09 E667 F3... */
|
|
|
62 .sqrt3 = QMK(0x1, 0xBB67, 16), /* 1.BB67 AE85 84... */
|
|
|
63 .ln2 = QMK(0x0, 0xB172, 16), /* 0.B172 17F7 D1... */
|
|
|
64 .ln10 = QMK(0x2, 0x4D76, 16), /* 2.4D76 3776 AA... */
|
|
|
65
|
|
|
66 .version = QVERSION,
|
|
|
67 };
|
|
|
68
|
|
|
69 qconf_t qconf = { /* Global Configuration Options */
|
|
|
70 .bound = qbound_saturate,
|
|
|
71 .dp = 4,
|
|
|
72 .base = 10,
|
|
|
73 };
|
|
|
74
|
|
|
75 /********* Basic Library Routines ********************************************/
|
|
|
76
|
|
|
77
|
|
|
78 static inline void implies(const int x, const int y) {
|
|
|
79 assert(!x || y);
|
|
|
80 }
|
|
|
81
|
|
|
82 static inline void mutual(const int x, const int y) { /* mutual implication */
|
|
|
83 assert(BOOLIFY(x) == BOOLIFY(y));
|
|
|
84 }
|
|
|
85
|
|
|
86 static inline void exclusive(const int x, const int y) {
|
|
|
87 assert(BOOLIFY(x) != BOOLIFY(y));
|
|
|
88 }
|
|
|
89
|
|
|
90 static inline void static_assertions(void) {
|
|
|
91 BUILD_BUG_ON(CHAR_BIT != 8);
|
|
|
92 // BUILD_BUG_ON((sizeof(q_t)*CHAR_BIT) != (QBITS * 2));
|
|
|
93 BUILD_BUG_ON( sizeof(q_t) != sizeof(u_t));
|
|
|
94 BUILD_BUG_ON( sizeof(u_t) != sizeof(d_t));
|
|
|
95 BUILD_BUG_ON(sizeof(lu_t) != sizeof(ld_t));
|
|
|
96 BUILD_BUG_ON(sizeof(d_t) != (sizeof(hd_t) * 2));
|
|
|
97 BUILD_BUG_ON(sizeof(lu_t) != (sizeof(u_t) * 2));
|
|
|
98 }
|
|
|
99
|
|
|
100 q_t qbound_saturate(const ld_t s) { /**< default saturation handler */
|
|
|
101 assert(s > DMAX || s < DMIN);
|
|
|
102 if (s > DMAX) return DMAX;
|
|
|
103 return DMIN;
|
|
|
104 }
|
|
|
105
|
|
|
106 q_t qbound_wrap(const ld_t s) { /**< wrap numbers on overflow */
|
|
|
107 assert(s > DMAX || s < DMIN);
|
|
|
108 if (s > DMAX) return DMIN + (s % DMAX);
|
|
|
109 return DMAX - ((-s) % DMAX);
|
|
|
110 }
|
|
|
111
|
|
|
112 static inline q_t qsat(const ld_t s) {
|
|
|
113 static_assertions();
|
|
|
114 if (s > DMAX || s < DMIN) return qconf.bound(s);
|
|
|
115 return s;
|
|
|
116 }
|
|
|
117
|
|
|
118 d_t arshift(const d_t v, const unsigned p) {
|
|
|
119 u_t vn = v;
|
|
|
120 if (v >= 0l)
|
|
|
121 return vn >> p;
|
|
|
122 const u_t leading = ((u_t)(-1l)) << ((sizeof(v) * CHAR_BIT) - p - 1);
|
|
|
123 return leading | (vn >> p);
|
|
|
124 }
|
|
|
125
|
|
|
126 static inline d_t divn(const d_t v, const unsigned p) {
|
|
|
127 /* return v / (1l << p); */
|
|
|
128 const u_t shifted = ((u_t)v) >> p;
|
|
|
129 if (qispositive(v))
|
|
|
130 return shifted;
|
|
|
131 const u_t leading = ((u_t)(-1l)) << ((sizeof(v)*CHAR_BIT) - p - 1);
|
|
|
132 return leading | shifted;
|
|
|
133 }
|
|
|
134
|
|
|
135 /* These really all should be moved the header for efficiency reasons */
|
|
|
136 static inline u_t qhigh(const q_t q) { return ((u_t)q) >> QBITS; }
|
|
|
137 static inline u_t qlow(const q_t q) { return ((u_t)q) & QMASK; }
|
|
|
138 static inline q_t qcons(const u_t hi, const u_t lo) { return (hi << QBITS) | (lo & QMASK); }
|
|
|
139
|
|
|
140 int qtoi(const q_t toi) { return ((lu_t)((ld_t)toi)) >> QBITS; }
|
|
|
141 q_t qint(const int toq) { return ((u_t)((d_t)toq)) << QBITS; }
|
|
|
142 signed char qtoc(const q_t q) { return qtoi(q); }
|
|
|
143 q_t qchar(signed char c) { return qint(c); }
|
|
|
144 short qtoh(const q_t q) { return qtoi(q); }
|
|
|
145 q_t qshort(short s) { return qint(s); }
|
|
|
146 long qtol(const q_t q) { return qtoi(q); }
|
|
|
147 q_t qlong(long l) { return qint(l); }
|
|
|
148 long long qtoll(const q_t q) { return qtoi(q); }
|
|
|
149 q_t qvlong(long long ll) { return qint(ll); }
|
|
|
150
|
|
|
151 q_t qisnegative(const q_t a) { return QINT(BOOLIFY(qhigh(a) & QHIGH)); }
|
|
|
152 q_t qispositive(const q_t a) { return QINT(!(qhigh(a) & QHIGH)); }
|
|
|
153 q_t qisinteger(const q_t a) { return QINT(!qlow(a)); }
|
|
|
154 q_t qisodd(const q_t a) { return QINT(qisinteger(a) && (qhigh(a) & 1)); }
|
|
|
155 q_t qiseven(const q_t a) { return QINT(qisinteger(a) && !(qhigh(a) & 1)); }
|
|
|
156 q_t qless(const q_t a, const q_t b) { return QINT(a < b); }
|
|
|
157 q_t qeqless(const q_t a, const q_t b) { return QINT(a <= b); }
|
|
|
158 q_t qmore(const q_t a, const q_t b) { return QINT(a > b); }
|
|
|
159 q_t qeqmore(const q_t a, const q_t b) { return QINT(a >= b); }
|
|
|
160 q_t qequal(const q_t a, const q_t b) { return QINT(a == b); }
|
|
|
161 q_t qunequal(const q_t a, const q_t b) { return QINT(a != b); }
|
|
|
162
|
|
|
163 q_t qnegate(const q_t a) { return (~(u_t)a) + 1ULL; }
|
|
|
164 q_t qmin(const q_t a, const q_t b) { return qless(a, b) ? a : b; }
|
|
|
165 q_t qmax(const q_t a, const q_t b) { return qmore(a, b) ? a : b; }
|
|
|
166 q_t qabs(const q_t a) { return qisnegative(a) ? qnegate(a) : a; }
|
|
|
167 q_t qadd(const q_t a, const q_t b) { return qsat((ld_t)a + (ld_t)b); }
|
|
|
168 q_t qsub(const q_t a, const q_t b) { return qsat((ld_t)a - (ld_t)b); }
|
|
|
169 q_t qcopysign(const q_t a, const q_t b) { return qisnegative(b) ? qnegate(qabs(a)) : qabs(a); }
|
|
|
170 q_t qand(const q_t a, const q_t b) { return a & b; }
|
|
|
171 q_t qxor(const q_t a, const q_t b) { return a ^ b; }
|
|
|
172 q_t qor(const q_t a, const q_t b) { return a | b; }
|
|
|
173 q_t qinvert(const q_t a) { return ~a; }
|
|
|
174 q_t qnot(const q_t a) { return QINT(!a); }
|
|
|
175 q_t qlogical(const q_t a) { return QINT(BOOLIFY(a)); }
|
|
|
176
|
|
|
177 q_t qlrs(const q_t a, const q_t b) { /* assert low bits == 0? */ return (u_t)a >> (u_t)qtoi(b); }
|
|
|
178 q_t qlls(const q_t a, const q_t b) { return (u_t)a << b; }
|
|
|
179 q_t qars(const q_t a, const q_t b) { return arshift(a, qtoi(b)); }
|
|
|
180 q_t qals(const q_t a, const q_t b) { return qsat((lu_t)a << b); }
|
|
|
181 q_t qsign(const q_t a) { return qisnegative(a) ? -QINT(1) : QINT(1); }
|
|
|
182 q_t qsignum(const q_t a) { return a ? qsign(a) : QINT(0); }
|
|
|
183
|
|
|
184 q_t qapproxequal(const q_t a, const q_t b, const q_t epsilon) {
|
|
|
185 assert(qeqmore(epsilon, qint(0)));
|
|
|
186 return QINT(qless(qabs(qsub(a, b)), epsilon));
|
|
|
187 }
|
|
|
188
|
|
|
189 q_t qapproxunequal(const q_t a, const q_t b, const q_t epsilon) {
|
|
|
190 return QINT(!qapproxequal(a, b, epsilon));
|
|
|
191 }
|
|
|
192
|
|
|
193 q_t qwithin(q_t v, q_t b1, q_t b2) {
|
|
|
194 const q_t hi = qmax(b1, b2);
|
|
|
195 const q_t lo = qmin(b1, b2);
|
|
|
196 if (qequal(v, b1) || qequal(v, b2))
|
|
|
197 return 1;
|
|
|
198 return qless(v, hi) && qmore(v, lo) ? QINT(1) : QINT(0);
|
|
|
199 }
|
|
|
200
|
|
|
201 q_t qwithin_interval(q_t v, q_t expected, q_t allowance) {
|
|
|
202 const q_t b1 = qadd(expected, allowance);
|
|
|
203 const q_t b2 = qsub(expected, allowance);
|
|
|
204 return qwithin(v, b1, b2);
|
|
|
205 }
|
|
|
206
|
|
|
207 q_t qfloor(const q_t q) {
|
|
|
208 return q & ~QMASK;
|
|
|
209 }
|
|
|
210
|
|
|
211 q_t qceil(q_t q) {
|
|
|
212 const q_t adj = qisinteger(q) ? QINT(0) : QINT(1);
|
|
|
213 q = qadd(q, adj);
|
|
|
214 return ((u_t)q) & (QMASK << QBITS);
|
|
|
215 }
|
|
|
216
|
|
|
217 q_t qtrunc(q_t q) {
|
|
|
218 const q_t adj = qisnegative(q) && qlow(q) ? QINT(1) : QINT(0);
|
|
|
219 q = qadd(q, adj);
|
|
|
220 return ((u_t)q) & (QMASK << QBITS);
|
|
|
221 }
|
|
|
222
|
|
|
223 q_t qround(q_t q) {
|
|
|
224 const int negative = qisnegative(q);
|
|
|
225 q = qabs(q);
|
|
|
226 const q_t adj = (qlow(q) & QHIGH) ? QINT(1) : QINT(0);
|
|
|
227 q = qadd(q, adj);
|
|
|
228 q = ((u_t)q) & (QMASK << QBITS);
|
|
|
229 return negative ? qnegate(q) : q;
|
|
|
230 }
|
|
|
231
|
|
|
232 int qpack(const q_t *q, char *buffer, const size_t length) {
|
|
|
233 assert(buffer);
|
|
|
234 if (length < sizeof(*q))
|
|
|
235 return -1;
|
|
|
236 q_t qn = *q;
|
|
|
237 uint8_t *b = (uint8_t*)buffer;
|
|
|
238 for (size_t i = 0; i < sizeof(qn); i++) {
|
|
|
239 b[i] = qn;
|
|
|
240 qn = (u_t)qn >> CHAR_BIT;
|
|
|
241 }
|
|
|
242 return sizeof(qn);
|
|
|
243 }
|
|
|
244
|
|
|
245 int qunpack(q_t *q, const char *buffer, const size_t length) {
|
|
|
246 assert(q);
|
|
|
247 assert(buffer);
|
|
|
248 if (length < sizeof(*q))
|
|
|
249 return -1;
|
|
|
250 uint8_t *b = (uint8_t*)buffer;
|
|
|
251 u_t nq = 0;
|
|
|
252 for (size_t i = 0; i < sizeof(*q); i++) {
|
|
|
253 nq <<= CHAR_BIT;
|
|
|
254 nq |= b[sizeof(*q)-i-1];
|
|
|
255 }
|
|
|
256 *q = nq;
|
|
|
257 return sizeof(*q);
|
|
|
258 }
|
|
|
259
|
|
|
260 static inline ld_t multiply(const q_t a, const q_t b) {
|
|
|
261 const ld_t dd = ((ld_t)a * (ld_t)b) + (lu_t)QHIGH;
|
|
|
262 /* N.B. portable version of "dd >> QBITS", for double width signed values */
|
|
|
263 return dd < 0 ? (-1ull << (2 * QBITS)) | ((lu_t)dd >> QBITS) : ((lu_t)dd) >> QBITS;
|
|
|
264 }
|
|
|
265
|
|
|
266 q_t qmul(const q_t a, const q_t b) {
|
|
|
267 return qsat(multiply(a, b));
|
|
|
268 }
|
|
|
269
|
|
|
270 q_t qfma(const q_t a, const q_t b, const q_t c) {
|
|
|
271 return qsat(multiply(a, b) + (ld_t)c);
|
|
|
272 }
|
|
|
273
|
|
|
274 q_t qdiv(const q_t a, const q_t b) {
|
|
|
275 assert(b);
|
|
|
276 const ld_t dd = ((ld_t)a) << QBITS;
|
|
|
277 ld_t bd2 = divn(b, 1);
|
|
|
278 if (!((dd >= 0 && b > 0) || (dd < 0 && b < 0)))
|
|
|
279 bd2 = -bd2;
|
|
|
280 /* Overflow not checked! */
|
|
|
281 /*return (dd/b) + (bd2/b);*/
|
|
|
282 return (dd + bd2) / b;
|
|
|
283 }
|
|
|
284
|
|
|
285 q_t qrem(const q_t a, const q_t b) {
|
|
|
286 return qsub(a, qmul(qtrunc(qdiv(a, b)), b));
|
|
|
287 }
|
|
|
288
|
|
|
289 q_t qmod(q_t a, q_t b) {
|
|
|
290 return qsub(a, qmul(qfloor(qdiv(a, b)), b));
|
|
|
291 }
|
|
|
292
|
|
|
293 static char itoch(const unsigned ch) {
|
|
|
294 assert(ch < 36);
|
|
|
295 if (ch <= 9)
|
|
|
296 return ch + '0';
|
|
|
297 return ch + 'A' - 10;
|
|
|
298 }
|
|
|
299
|
|
|
300 static inline void swap(char *a, char *b) {
|
|
|
301 assert(a);
|
|
|
302 assert(b);
|
|
|
303 const int c = *a;
|
|
|
304 *a = *b;
|
|
|
305 *b = c;
|
|
|
306 }
|
|
|
307
|
|
|
308 static void reverse(char *s, const size_t length) {
|
|
|
309 assert(s);
|
|
|
310 for (size_t i = 0; i < length/2; i++)
|
|
|
311 swap(&s[i], &s[length - i - 1]);
|
|
|
312 }
|
|
|
313
|
|
|
314 static int uprint(u_t p, char *s, const size_t length, const d_t base) {
|
|
|
315 assert(s);
|
|
|
316 assert(base >= 2 && base <= 36);
|
|
|
317 if (length < 2)
|
|
|
318 return -1;
|
|
|
319 size_t i = 0;
|
|
|
320 do {
|
|
|
321 unsigned ch = p % base;
|
|
|
322 p /= base;
|
|
|
323 s[i++] = itoch(ch);
|
|
|
324 } while (p && i < length);
|
|
|
325 if (p && i >= length)
|
|
|
326 return -1;
|
|
|
327 reverse(s, i);
|
|
|
328 return i;
|
|
|
329 }
|
|
|
330
|
|
|
331 /* <https://codereview.stackexchange.com/questions/109212> */
|
|
|
332 int qsprintbdp(q_t p, char *s, size_t length, const u_t base, const d_t idp) {
|
|
|
333 assert(s);
|
|
|
334 const int negative = BOOLIFY(qisnegative(p));
|
|
|
335 if (negative)
|
|
|
336 p = qnegate(p);
|
|
|
337 const d_t hi = qhigh(p);
|
|
|
338 char frac[QBITS + 2] = { '.', };
|
|
|
339 memset(s, 0, length);
|
|
|
340 assert(base >= 2 && base <= 36);
|
|
|
341 u_t lo = qlow(p);
|
|
|
342 size_t i = 1;
|
|
|
343 for (i = 1; lo; i++) {
|
|
|
344 if (idp >= 0 && (int)i > idp)
|
|
|
345 break;
|
|
|
346 lo *= base;
|
|
|
347 assert(i < (QBITS + 2));
|
|
|
348 frac[i] = itoch(lo >> QBITS);
|
|
|
349 lo &= QMASK;
|
|
|
350 }
|
|
|
351 if (negative)
|
|
|
352 s[0] = '-';
|
|
|
353 const int hisz = uprint(hi, s + negative, length - (1 + negative), base);
|
|
|
354 if (hisz < 0 || (hisz + i + negative + 1) > length)
|
|
|
355 return -1;
|
|
|
356 memcpy(s + hisz + negative, frac, i);
|
|
|
357 return i + hisz;
|
|
|
358 }
|
|
|
359
|
|
|
360 int qsprintb(q_t p, char *s, size_t length, const u_t base) {
|
|
|
361 return qsprintbdp(p, s, length, base, qconf.dp);
|
|
|
362 }
|
|
|
363
|
|
|
364 int qsprint(const q_t p, char *s, const size_t length) {
|
|
|
365 return qsprintb(p, s, length, qconf.base);
|
|
|
366 }
|
|
|
367
|
|
|
368 static inline int extract(unsigned char c, const int radix) {
|
|
|
369 c = tolower(c);
|
|
|
370 if (c >= '0' && c <= '9')
|
|
|
371 c -= '0';
|
|
|
372 else if (c >= 'a' && c <= 'z')
|
|
|
373 c -= ('a' - 10);
|
|
|
374 else
|
|
|
375 return -1;
|
|
|
376 if (c < radix)
|
|
|
377 return c;
|
|
|
378 return -1;
|
|
|
379 }
|
|
|
380
|
|
|
381 static inline q_t qmk(d_t integer, u_t fractional) {
|
|
|
382 const int negative = integer < 0;
|
|
|
383 integer = negative ? -integer : integer;
|
|
|
384 const q_t r = qcons((d_t)integer, fractional);
|
|
|
385 return negative ? qnegate(r) : r;
|
|
|
386 }
|
|
|
387
|
|
|
388 static inline u_t integer_logarithm(u_t num, const u_t base) {
|
|
|
389 assert(num > 0 && base >= 2 && base <= 36);
|
|
|
390 u_t r = -1;
|
|
|
391 do r++; while (num /= base);
|
|
|
392 return r;
|
|
|
393 }
|
|
|
394
|
|
|
395 int qnconvbdp(q_t *q, const char *s, size_t length, const d_t base, const u_t idp) {
|
|
|
396 assert(q);
|
|
|
397 assert(s);
|
|
|
398 assert(base >= 2 && base <= 36);
|
|
|
399 *q = QINT(0);
|
|
|
400 if (length < 1)
|
|
|
401 return -1;
|
|
|
402 d_t hi = 0, lo = 0, places = 1, negative = 0, overflow = 0;
|
|
|
403 size_t sidx = 0;
|
|
|
404
|
|
|
405 if (s[sidx] == '-') {
|
|
|
406 if (length < 2)
|
|
|
407 return -1;
|
|
|
408 negative = 1;
|
|
|
409 sidx++;
|
|
|
410 }
|
|
|
411
|
|
|
412 for (; sidx < length && s[sidx]; sidx++) {
|
|
|
413 const d_t e = extract(s[sidx], base);
|
|
|
414 if (e < 0)
|
|
|
415 break;
|
|
|
416 if (hi > MULTIPLIER) { /* continue on with conversion, do not accumulate */
|
|
|
417 overflow = 1;
|
|
|
418 } else {
|
|
|
419 hi = (hi * base) + e;
|
|
|
420 }
|
|
|
421 }
|
|
|
422 if (sidx >= length || !s[sidx])
|
|
|
423 goto done;
|
|
|
424 if (s[sidx] != '.')
|
|
|
425 return -2;
|
|
|
426 sidx++;
|
|
|
427
|
|
|
428 const u_t ilog = integer_logarithm(0x10000, base);
|
|
|
429 const u_t max = MIN(idp, ilog); /* Calculate maximum decimal places given base */
|
|
|
430
|
|
|
431 for (u_t dp = 0; sidx < length && s[sidx]; sidx++, dp++) {
|
|
|
432 const int ch = extract(s[sidx], base);
|
|
|
433 if (ch < 0)
|
|
|
434 return -3;
|
|
|
435 if (dp < max) { /* continue on with conversion , do not accumulate */
|
|
|
436 /* We could get more accuracy by looking at one digit
|
|
|
437 * passed the maximum digits allowed and rounding if
|
|
|
438 * that digit exists in the input. */
|
|
|
439 lo = (lo * base) + ch;
|
|
|
440 if (places >= (DMAX / base))
|
|
|
441 return -4;
|
|
|
442 places *= base;
|
|
|
443 }
|
|
|
444 assert((dp + 1) > dp);
|
|
|
445 }
|
|
|
446 if (!places)
|
|
|
447 return -5;
|
|
|
448 lo = ((d_t)((u_t)lo << QBITS) / places);
|
|
|
449 done:
|
|
|
450 if (overflow) {
|
|
|
451 *q = negative ? qinfo.min : qinfo.max;
|
|
|
452 return -6;
|
|
|
453 } else {
|
|
|
454 const q_t nq = qmk(hi, lo);
|
|
|
455 *q = negative ? qnegate(nq) : nq;
|
|
|
456
|
|
|
457 }
|
|
|
458 return 0;
|
|
|
459 }
|
|
|
460
|
|
|
461 int qnconvb(q_t *q, const char *s, size_t length, const d_t base) {
|
|
|
462 return qnconvbdp(q, s, length, base, qconf.dp);
|
|
|
463 }
|
|
|
464
|
|
|
465 int qnconv(q_t *q, const char *s, size_t length) {
|
|
|
466 return qnconvb(q, s, length, qconf.base);
|
|
|
467 }
|
|
|
468
|
|
|
469 int qconv(q_t *q, const char * const s) {
|
|
|
470 assert(s);
|
|
|
471 return qnconv(q, s, strlen(s));
|
|
|
472 }
|
|
|
473
|
|
|
474 int qconvb(q_t *q, const char * const s, const d_t base) {
|
|
|
475 assert(s);
|
|
|
476 return qnconvb(q, s, strlen(s), base);
|
|
|
477 }
|
|
|
478
|
|
|
479 typedef enum {
|
|
|
480 CORDIC_MODE_VECTOR_E/* = 'VECT'*/,
|
|
|
481 CORDIC_MODE_ROTATE_E/* = 'ROT'*/,
|
|
|
482 } cordic_mode_e;
|
|
|
483
|
|
|
484 typedef enum {
|
|
|
485 CORDIC_COORD_HYPERBOLIC_E = -1,
|
|
|
486 CORDIC_COORD_LINEAR_E = 0,
|
|
|
487 CORDIC_COORD_CIRCULAR_E = 1,
|
|
|
488 } cordic_coordinates_e;
|
|
|
489
|
|
|
490 static const d_t cordic_circular_inverse_scaling = 0x9B74; /* 1/scaling-factor */
|
|
|
491 static const d_t cordic_hyperbolic_inverse_scaling = 0x13520; /* 1/scaling-factor */
|
|
|
492
|
|
|
493 static inline int mulsign(d_t a, d_t b) { /* sign(a*b) */
|
|
|
494 const int aneg = a < 0;
|
|
|
495 const int bneg = b < 0;
|
|
|
496 return aneg ^ bneg ? -QINT(1) : QINT(1);
|
|
|
497 }
|
|
|
498
|
|
|
499 /* Universal CORDIC <https://en.wikibooks.org/wiki/Digital_Circuits/CORDIC>
|
|
|
500 *
|
|
|
501 * x(i+1) = x(i) - u.d(i).y(i).pow(2, -i)
|
|
|
502 * y(i+1) = y(i) + d(i).x(i).pow(2, -i)
|
|
|
503 * z(i+1) = z(i) - d(i).a(i)
|
|
|
504 *
|
|
|
505 * d(i) = sgn(z(i)) (rotation)
|
|
|
506 * d(i) = -sgn(x(i).y(i)) (vectoring)
|
|
|
507 *
|
|
|
508 * hyperbolic linear circular
|
|
|
509 * u = -1 0 1
|
|
|
510 * a = atanh(pow(2, -i)) pow(2, -i) atan(pow(2, -i))
|
|
|
511 *
|
|
|
512 * linear shift sequence: i = 0, 1, 2, 3, ...
|
|
|
513 * circular shift sequence: i = 1, 2, 3, 4, ...
|
|
|
514 * hyperbolic shift sequence: i = 1, 2, 3, 4, 4, 5, ... */
|
|
|
515 static int cordic(const cordic_coordinates_e coord, const cordic_mode_e mode, int iterations, d_t *x0, d_t *y0, d_t *z0) {
|
|
|
516 assert(x0);
|
|
|
517 assert(y0);
|
|
|
518 assert(z0);
|
|
|
519 if (mode != CORDIC_MODE_VECTOR_E && mode != CORDIC_MODE_ROTATE_E)
|
|
|
520 return -1;
|
|
|
521
|
|
|
522 BUILD_BUG_ON(sizeof(d_t) != sizeof(uint32_t));
|
|
|
523 BUILD_BUG_ON(sizeof(u_t) != sizeof(uint32_t));
|
|
|
524
|
|
|
525 static const u_t arctans[] = { /* atan(2^0), atan(2^-1), atan(2^-2), ... */
|
|
|
526 0xC90FuL, 0x76B1uL, 0x3EB6uL, 0x1FD5uL,
|
|
|
527 0x0FFAuL, 0x07FFuL, 0x03FFuL, 0x01FFuL,
|
|
|
528 0x00FFuL, 0x007FuL, 0x003FuL, 0x001FuL,
|
|
|
529 0x000FuL, 0x0007uL, 0x0003uL, 0x0001uL,
|
|
|
530 0x0000uL, // 0x0000uL,
|
|
|
531 };
|
|
|
532 static const size_t arctans_length = sizeof arctans / sizeof arctans[0];
|
|
|
533
|
|
|
534 static const u_t arctanhs[] = { /* atanh(2^-1), atanh(2^-2), ... */
|
|
|
535 0x8c9fuL, 0x4162uL, 0x202buL, 0x1005uL,
|
|
|
536 0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
|
|
|
537 0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
|
|
|
538 0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
|
|
|
539 0x0000uL, // 0x0000uL,
|
|
|
540 };
|
|
|
541 static const size_t arctanhs_length = sizeof arctanhs / sizeof arctanhs[0];
|
|
|
542
|
|
|
543 static const u_t halfs[] = { /* 2^0, 2^-1, 2^-2, ..*/
|
|
|
544 0x10000uL,
|
|
|
545 0x8000uL, 0x4000uL, 0x2000uL, 0x1000uL,
|
|
|
546 0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
|
|
|
547 0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
|
|
|
548 0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
|
|
|
549 //0x0000uL, // 0x0000uL,
|
|
|
550 };
|
|
|
551 static const size_t halfs_length = sizeof halfs / sizeof halfs[0];
|
|
|
552
|
|
|
553 const u_t *lookup = NULL;
|
|
|
554 size_t i = 0, j = 0, k = 0, length = 0;
|
|
|
555 const size_t *shiftx = NULL, *shifty = NULL;
|
|
|
556 int hyperbolic = 0;
|
|
|
557
|
|
|
558 switch (coord) {
|
|
|
559 case CORDIC_COORD_CIRCULAR_E:
|
|
|
560 lookup = arctans;
|
|
|
561 length = arctans_length;
|
|
|
562 i = 0;
|
|
|
563 shifty = &i;
|
|
|
564 shiftx = &i;
|
|
|
565 break;
|
|
|
566 case CORDIC_COORD_HYPERBOLIC_E:
|
|
|
567 lookup = arctanhs;
|
|
|
568 length = arctanhs_length;
|
|
|
569 hyperbolic = 1;
|
|
|
570 i = 1;
|
|
|
571 shifty = &i;
|
|
|
572 shiftx = &i;
|
|
|
573 break;
|
|
|
574 case CORDIC_COORD_LINEAR_E:
|
|
|
575 lookup = halfs;
|
|
|
576 length = halfs_length;
|
|
|
577 shifty = &j;
|
|
|
578 shiftx = NULL;
|
|
|
579 i = 1;
|
|
|
580 break;
|
|
|
581 default: /* not implemented */
|
|
|
582 return -2;
|
|
|
583 }
|
|
|
584
|
|
|
585 iterations = iterations > (int)length ? (int)length : iterations;
|
|
|
586 iterations = iterations < 0 ? (int)length : iterations;
|
|
|
587
|
|
|
588 d_t x = *x0, y = *y0, z = *z0;
|
|
|
589
|
|
|
590 /* rotation mode: z determines direction,
|
|
|
591 * vector mode: y determines direction */
|
|
|
592 for (; j < (unsigned)iterations; i++, j++) {
|
|
|
593 again:
|
|
|
594 {
|
|
|
595 const d_t m = mode == CORDIC_MODE_ROTATE_E ? z : -y /*-mulsign(x, y)*/;
|
|
|
596 const d_t d = -!!(m < 0);
|
|
|
597 const d_t xs = ((((shiftx ? divn(y, *shiftx) : 0)) ^ d) - d);
|
|
|
598 const d_t ys = (divn(x, *shifty) ^ d) - d;
|
|
|
599 const d_t xn = x - (hyperbolic ? -xs : xs);
|
|
|
600 const d_t yn = y + ys;
|
|
|
601 const d_t zn = z - ((lookup[j] ^ d) - d);
|
|
|
602 x = xn; /* cosine, in circular, rotation mode */
|
|
|
603 y = yn; /* sine, in circular, rotation mode */
|
|
|
604 z = zn;
|
|
|
605 }
|
|
|
606 if (hyperbolic) { /* Experimental/Needs bug fixing */
|
|
|
607 switch (1) { // TODO: Correct hyperbolic redo of iteration
|
|
|
608 case 0: break;
|
|
|
609 case 1: if (k++ >= 3) { k = 0; goto again; } break;
|
|
|
610 case 2: {
|
|
|
611 assert(j <= 120);
|
|
|
612 size_t cmp = j + 1;
|
|
|
613 if (cmp == 4 || cmp == 13 /*|| cmp == 40 || cmp == 121 || cmp == floor(pow(3,i-1)/2) */) {
|
|
|
614 if (k) {
|
|
|
615 k = 0;
|
|
|
616 } else {
|
|
|
617 k = 1;
|
|
|
618 goto again;
|
|
|
619 }
|
|
|
620 }
|
|
|
621 break;
|
|
|
622 }
|
|
|
623 }
|
|
|
624 }
|
|
|
625 }
|
|
|
626 *x0 = x;
|
|
|
627 *y0 = y;
|
|
|
628 *z0 = z;
|
|
|
629
|
|
|
630 return iterations;
|
|
|
631 }
|
|
|
632
|
|
|
633 /* See: - <https://dspguru.com/dsp/faqs/cordic/>
|
|
|
634 * - <https://en.wikipedia.org/wiki/CORDIC> */
|
|
|
635 static int qcordic(q_t theta, const int iterations, q_t *sine, q_t *cosine) {
|
|
|
636 assert(sine);
|
|
|
637 assert(cosine);
|
|
|
638
|
|
|
639 static const q_t pi = QPI, npi = -QPI;
|
|
|
640 static const q_t hpi = QPI/2, hnpi = -(QPI/2);
|
|
|
641 static const q_t qpi = QPI/4, qnpi = -(QPI/4);
|
|
|
642 static const q_t dpi = QPI*2, dnpi = -(QPI*2);
|
|
|
643
|
|
|
644 /* Convert to range -pi to pi, we could use qmod,
|
|
|
645 * however that uses multiplication and division, and
|
|
|
646 * if we can use those operators freely then there are
|
|
|
647 * other, better algorithms we can use instead of CORDIC
|
|
|
648 * for sine/cosine calculation. */
|
|
|
649 while (qless(theta, npi)) theta = qadd(theta, dpi);
|
|
|
650 while (qmore(theta, pi)) theta = qadd(theta, dnpi);
|
|
|
651
|
|
|
652 int negate = 0, shift = 0;
|
|
|
653
|
|
|
654 /* convert to range -pi/2 to pi/2 */
|
|
|
655 if (qless(theta, hnpi)) {
|
|
|
656 theta = qadd(theta, pi);
|
|
|
657 negate = 1;
|
|
|
658 } else if (qmore(theta, hpi)) {
|
|
|
659 theta = qadd(theta, npi);
|
|
|
660 negate = 1;
|
|
|
661 }
|
|
|
662
|
|
|
663 /* convert to range -pi/4 to pi/4 */
|
|
|
664 if (qless(theta, qnpi)) {
|
|
|
665 theta = qadd(theta, hpi);
|
|
|
666 shift = -1;
|
|
|
667 } else if (qmore(theta, qpi)) {
|
|
|
668 theta = qadd(theta, hnpi);
|
|
|
669 shift = 1;
|
|
|
670 }
|
|
|
671
|
|
|
672 d_t x = cordic_circular_inverse_scaling, y = 0, z = theta /* no theta scaling needed */;
|
|
|
673
|
|
|
674 /* CORDIC in Q2.16 format */
|
|
|
675 if (cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, iterations, &x, &y, &z) < 0)
|
|
|
676 return -1;
|
|
|
677
|
|
|
678 /* undo shifting and quadrant changes */
|
|
|
679 if (shift > 0) {
|
|
|
680 const d_t yt = y;
|
|
|
681 y = x;
|
|
|
682 x = -yt;
|
|
|
683 } else if (shift < 0) {
|
|
|
684 const d_t yt = y;
|
|
|
685 y = -x;
|
|
|
686 x = yt;
|
|
|
687 }
|
|
|
688
|
|
|
689 if (negate) {
|
|
|
690 x = -x;
|
|
|
691 y = -y;
|
|
|
692 }
|
|
|
693 /* set output; no scaling needed */
|
|
|
694 *cosine = x;
|
|
|
695 *sine = y;
|
|
|
696 return 0;
|
|
|
697 }
|
|
|
698
|
|
|
699 q_t qatan(const q_t t) {
|
|
|
700 q_t x = qint(1), y = t, z = QINT(0);
|
|
|
701 cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
702 return z;
|
|
|
703 }
|
|
|
704
|
|
|
705 q_t qatan2(const q_t a, const q_t b) {
|
|
|
706 q_t x = b, y = a, z = QINT(0);
|
|
|
707 if (qequal(b, QINT(0))) {
|
|
|
708 assert(qunequal(a, QINT(0)));
|
|
|
709 if (qmore(a, QINT(0)))
|
|
|
710 return QPI/2;
|
|
|
711 return -(QPI/2);
|
|
|
712 } else if (qless(b, QINT(0))) {
|
|
|
713 if (qeqmore(a, QINT(0)))
|
|
|
714 return qadd(qatan(qdiv(a, b)), QPI);
|
|
|
715 return qsub(qatan(qdiv(a, b)), QPI);
|
|
|
716 }
|
|
|
717 cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
718 return z;
|
|
|
719 }
|
|
|
720
|
|
|
721 void qsincos(q_t theta, q_t *sine, q_t *cosine) {
|
|
|
722 assert(sine);
|
|
|
723 assert(cosine);
|
|
|
724 const int r = qcordic(theta, -1, sine, cosine);
|
|
|
725 assert(r >= 0);
|
|
|
726 }
|
|
|
727
|
|
|
728 q_t qsin(const q_t theta) {
|
|
|
729 q_t sine = QINT(0), cosine = QINT(0);
|
|
|
730 qsincos(theta, &sine, &cosine);
|
|
|
731 return sine;
|
|
|
732 }
|
|
|
733
|
|
|
734 q_t qcos(const q_t theta) {
|
|
|
735 q_t sine = QINT(0), cosine = QINT(0);
|
|
|
736 qsincos(theta, &sine, &cosine);
|
|
|
737 return cosine;
|
|
|
738 }
|
|
|
739
|
|
|
740 q_t qtan(const q_t theta) {
|
|
|
741 q_t sine = QINT(0), cosine = QINT(0);
|
|
|
742 qsincos(theta, &sine, &cosine);
|
|
|
743 return qdiv(sine, cosine); /* can use qcordic_div, with range limits it imposes */
|
|
|
744 }
|
|
|
745
|
|
|
746 q_t qcot(const q_t theta) {
|
|
|
747 q_t sine = QINT(0), cosine = QINT(0);
|
|
|
748 qsincos(theta, &sine, &cosine);
|
|
|
749 return qdiv(cosine, sine); /* can use qcordic_div, with range limits it imposes */
|
|
|
750 }
|
|
|
751
|
|
|
752 q_t qcordic_mul(const q_t a, const q_t b) { /* works for small values; result < 4 */
|
|
|
753 q_t x = a, y = QINT(0), z = b;
|
|
|
754 const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
|
|
|
755 assert(r >= 0);
|
|
|
756 return y;
|
|
|
757 }
|
|
|
758
|
|
|
759 q_t qcordic_div(const q_t a, const q_t b) {
|
|
|
760 q_t x = b, y = a, z = QINT(0);
|
|
|
761 const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
762 assert(r >= 0);
|
|
|
763 return z;
|
|
|
764 }
|
|
|
765
|
|
|
766 void qsincosh(const q_t a, q_t *sinh, q_t *cosh) {
|
|
|
767 assert(sinh);
|
|
|
768 assert(cosh);
|
|
|
769 q_t x = cordic_hyperbolic_inverse_scaling, y = QINT(0), z = a; /* (e^2x - 1) / (e^2x + 1) */
|
|
|
770 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
|
|
|
771 assert(r >= 0);
|
|
|
772 *sinh = y;
|
|
|
773 *cosh = x;
|
|
|
774 }
|
|
|
775
|
|
|
776 q_t qtanh(const q_t a) {
|
|
|
777 q_t sinh = QINT(0), cosh = QINT(0);
|
|
|
778 qsincosh(a, &sinh, &cosh);
|
|
|
779 return qdiv(sinh, cosh);
|
|
|
780 }
|
|
|
781
|
|
|
782 q_t qcosh(const q_t a) {
|
|
|
783 q_t sinh = QINT(0), cosh = QINT(0);
|
|
|
784 qsincosh(a, &sinh, &cosh);
|
|
|
785 return cosh;
|
|
|
786 }
|
|
|
787
|
|
|
788 q_t qsinh(const q_t a) {
|
|
|
789 q_t sinh = QINT(0), cosh = QINT(0);
|
|
|
790 qsincosh(a, &sinh, &cosh);
|
|
|
791 return sinh;
|
|
|
792 }
|
|
|
793
|
|
|
794 q_t qcordic_exp(const q_t e) {
|
|
|
795 q_t s = QINT(0), h = QINT(0);
|
|
|
796 qsincosh(e, &s, &h);
|
|
|
797 return qadd(s, h);
|
|
|
798 }
|
|
|
799
|
|
|
800 q_t qcordic_ln(const q_t d) {
|
|
|
801 q_t x = qadd(d, QINT(1)), y = qsub(d, QINT(1)), z = QINT(0);
|
|
|
802 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
803 assert(r >= 0);
|
|
|
804 return qadd(z, z);
|
|
|
805 }
|
|
|
806
|
|
|
807 q_t qcordic_sqrt(const q_t n) { /* testing only; works for 0 < x < 2 */
|
|
|
808 const q_t quarter = 1uLL << (QBITS - 2); /* 0.25 */
|
|
|
809 q_t x = qadd(n, quarter),
|
|
|
810 y = qsub(n, quarter),
|
|
|
811 z = 0;
|
|
|
812 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
813 assert(r >= 0);
|
|
|
814 return qmul(x, cordic_hyperbolic_inverse_scaling);
|
|
|
815 }
|
|
|
816
|
|
|
817 q_t qhypot(const q_t a, const q_t b) {
|
|
|
818 q_t x = qabs(a), y = qabs(b), z = QINT(0); /* abs() should not be needed? */
|
|
|
819 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
820 assert(r >= 0);
|
|
|
821 return qmul(x, cordic_circular_inverse_scaling);
|
|
|
822 }
|
|
|
823
|
|
|
824 q_t qatanh(q_t x) {
|
|
|
825 assert(qabs(qless(x, QINT(1))));
|
|
|
826 return qmul(qlog(qdiv(qadd(QINT(1), x), qsub(QINT(1), x))), QMK(0, 0x8000, 16));
|
|
|
827 }
|
|
|
828
|
|
|
829 q_t qasinh(q_t x) {
|
|
|
830 return qlog(qadd(x, qsqrt(qadd(qmul(x, x), QINT(1)))));
|
|
|
831 }
|
|
|
832
|
|
|
833 q_t qacosh(q_t x) {
|
|
|
834 assert(qeqmore(x, QINT(1)));
|
|
|
835 return qlog(qadd(x, qsqrt(qsub(qmul(x, x), QINT(1)))));
|
|
|
836 }
|
|
|
837
|
|
|
838 void qpol2rec(const q_t magnitude, const q_t theta, q_t *i, q_t *j) {
|
|
|
839 assert(i);
|
|
|
840 assert(j);
|
|
|
841 q_t sin = QINT(0), cos = QINT(0);
|
|
|
842 qsincos(theta, &sin, &cos);
|
|
|
843 *i = qmul(sin, magnitude);
|
|
|
844 *j = qmul(cos, magnitude);
|
|
|
845 }
|
|
|
846
|
|
|
847 void qrec2pol(const q_t i, const q_t j, q_t *magnitude, q_t *theta) {
|
|
|
848 assert(magnitude);
|
|
|
849 assert(theta);
|
|
|
850 const int is = qisnegative(i), js = qisnegative(j);
|
|
|
851 q_t x = qabs(i), y = qabs(j), z = QINT(0);
|
|
|
852 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
|
|
|
853 assert(r >= 0);
|
|
|
854 *magnitude = qmul(x, cordic_circular_inverse_scaling);
|
|
|
855 if (is && js)
|
|
|
856 z = qadd(z, QPI);
|
|
|
857 else if (js)
|
|
|
858 z = qadd(z, QPI/2l);
|
|
|
859 else if (is)
|
|
|
860 z = qadd(z, (3l*QPI)/2l);
|
|
|
861 *theta = z;
|
|
|
862 }
|
|
|
863
|
|
|
864 q_t qcordic_hyperbolic_gain(const int n) {
|
|
|
865 q_t x = QINT(1), y = QINT(0), z = QINT(0);
|
|
|
866 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
|
|
|
867 assert(r >= 0);
|
|
|
868 return x;
|
|
|
869 }
|
|
|
870
|
|
|
871 q_t qcordic_circular_gain(const int n) {
|
|
|
872 q_t x = QINT(1), y = QINT(0), z = QINT(0);
|
|
|
873 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
|
|
|
874 assert(r >= 0);
|
|
|
875 return x;
|
|
|
876 }
|
|
|
877
|
|
|
878 static inline int isodd(const unsigned n) {
|
|
|
879 return n & 1;
|
|
|
880 }
|
|
|
881
|
|
|
882 d_t dpower(d_t b, unsigned e) { /* https://stackoverflow.com/questions/101439 */
|
|
|
883 d_t result = 1;
|
|
|
884 for (;;) {
|
|
|
885 if (isodd(e))
|
|
|
886 result *= b;
|
|
|
887 e >>= 1;
|
|
|
888 if (!e)
|
|
|
889 break;
|
|
|
890 b *= b;
|
|
|
891 }
|
|
|
892 return result;
|
|
|
893 }
|
|
|
894
|
|
|
895 d_t dlog(d_t x, const unsigned base) { /* rounds up, look at remainder to round down */
|
|
|
896 d_t b = 0;
|
|
|
897 assert(x && base > 1);
|
|
|
898 while ((x /= (d_t)base)) /* can use >> for base that are powers of two */
|
|
|
899 b++;
|
|
|
900 return b;
|
|
|
901 }
|
|
|
902
|
|
|
903 q_t qlog(q_t x) {
|
|
|
904 q_t logs = 0;
|
|
|
905 assert(qmore(x, 0));
|
|
|
906 static const q_t lmax = QMK(9, 0x8000, 16); /* 9.5, lower limit needs checking */
|
|
|
907 for (; qmore(x, lmax); x = divn(x, 1))
|
|
|
908 logs = qadd(logs, qinfo.ln2);
|
|
|
909 return qadd(logs, qcordic_ln(x));
|
|
|
910 }
|
|
|
911
|
|
|
912 q_t qsqr(const q_t x) {
|
|
|
913 return qmul(x, x);
|
|
|
914 }
|
|
|
915
|
|
|
916 q_t qexp(const q_t e) { /* exp(e) = exp(e/2)*exp(e/2) */
|
|
|
917 if (qless(e, QINT(1))) /* 1.1268 is approximately the limit for qcordic_exp */
|
|
|
918 return qcordic_exp(e);
|
|
|
919 return qsqr(qexp(divn(e, 1)));
|
|
|
920 }
|
|
|
921
|
|
|
922 q_t qpow(q_t n, q_t exp) {
|
|
|
923 implies(qisnegative(n), qisinteger(exp));
|
|
|
924 implies(qequal(n, QINT(0)), qunequal(exp, QINT(0)));
|
|
|
925 if (qequal(QINT(0), n))
|
|
|
926 return QINT(1);
|
|
|
927 if (qisnegative(n)) {
|
|
|
928 const q_t abspow = qpow(qabs(n), exp);
|
|
|
929 return qisodd(exp) ? qnegate(abspow) : abspow;
|
|
|
930 }
|
|
|
931 if (qisnegative(exp))
|
|
|
932 return qdiv(QINT(1), qpow(n, qabs(exp)));
|
|
|
933 return qexp(multiply(qlog(n), exp));
|
|
|
934 }
|
|
|
935
|
|
|
936 q_t qsqrt(const q_t x) { /* Newton-Rhaphson method */
|
|
|
937 assert(qeqmore(x, 0));
|
|
|
938 const q_t difference = qmore(x, QINT(100)) ? 0x0100 : 0x0010;
|
|
|
939 if (qequal(QINT(0), x))
|
|
|
940 return QINT(0);
|
|
|
941 q_t guess = qmore(x, qinfo.sqrt2) ? divn(x, 1) : QINT(1);
|
|
|
942 while (qmore(qabs(qsub(qmul(guess, guess), x)), difference))
|
|
|
943 guess = divn(qadd(qdiv(x, guess), guess), 1);
|
|
|
944 return qabs(guess); /* correct for overflow int very large numbers */
|
|
|
945 }
|
|
|
946
|
|
|
947 q_t qasin(const q_t t) {
|
|
|
948 assert(qless(qabs(t), QINT(1)));
|
|
|
949 /* can also use: return qatan(qdiv(t, qsqrt(qsub(QINT(1), qmul(t, t))))); */
|
|
|
950 return qatan2(t, qsqrt(qsub(QINT(1), qmul(t, t))));
|
|
|
951 }
|
|
|
952
|
|
|
953 q_t qacos(const q_t t) {
|
|
|
954 assert(qeqless(qabs(t), QINT(1)));
|
|
|
955 /* can also use: return qatan(qdiv(qsqrt(qsub(QINT(1), qmul(t, t))), t)); */
|
|
|
956 return qatan2(qsqrt(qsub(QINT(1), qmul(t, t))), t);
|
|
|
957 }
|
|
|
958
|
|
|
959 q_t qdeg2rad(const q_t deg) {
|
|
|
960 return qdiv(qmul(QPI, deg), QINT(180));
|
|
|
961 }
|
|
|
962
|
|
|
963 q_t qrad2deg(const q_t rad) {
|
|
|
964 return qdiv(qmul(QINT(180), rad), QPI);
|
|
|
965 }
|
|
|
966
|
|
|
967 void qfilter_init(qfilter_t *f, const q_t time, const q_t rc, const q_t seed) {
|
|
|
968 assert(f);
|
|
|
969 memset(f, 0, sizeof(*f));
|
|
|
970 f->time = time;
|
|
|
971 f->rc = rc;
|
|
|
972 f->filtered = seed; /* alpha * seed for LPF */
|
|
|
973 f->raw = seed;
|
|
|
974 }
|
|
|
975
|
|
|
976 q_t qfilter_low_pass(qfilter_t *f, const q_t time, const q_t data) {
|
|
|
977 assert(f);
|
|
|
978 /* If the calling rate is constant (for example the function is
|
|
|
979 * guaranteed to be always called at a rate of 5 milliseconds) we
|
|
|
980 * can avoid the costly alpha calculation! */
|
|
|
981 const q_t dt = (u_t)time - (u_t)f->time;
|
|
|
982 const q_t alpha = qdiv(dt, qadd(f->rc, dt));
|
|
|
983 f->filtered = qfma(alpha, qsub(data, f->filtered), f->filtered);
|
|
|
984 f->time = time;
|
|
|
985 f->raw = data;
|
|
|
986 return f->filtered;
|
|
|
987 }
|
|
|
988
|
|
|
989 q_t qfilter_high_pass(qfilter_t *f, const q_t time, const q_t data) {
|
|
|
990 assert(f);
|
|
|
991 const q_t dt = (u_t)time - (u_t)f->time;
|
|
|
992 const q_t alpha = qdiv(f->rc, qadd(f->rc, dt));
|
|
|
993 f->filtered = qmul(alpha, qadd(f->filtered, qsub(data, f->raw)));
|
|
|
994 f->time = time;
|
|
|
995 f->raw = data;
|
|
|
996 return f->filtered;
|
|
|
997 }
|
|
|
998
|
|
|
999 q_t qfilter_value(const qfilter_t *f) {
|
|
|
1000 assert(f);
|
|
|
1001 return f->filtered;
|
|
|
1002 }
|
|
|
1003
|
|
|
1004 /* Must be called at a constant rate; perhaps a PID which takes call time
|
|
|
1005 * into account could be made, but that would complicate things. Differentiator
|
|
|
1006 * term needs filtering also. It would be nice to create a version that took
|
|
|
1007 * into account the time delta, see
|
|
|
1008 * <https://www.quora.com/Do-I-need-to-sample-at-a-constant-rate-for-PID-control-or-is-it-sufficient-to-know-the-time-at-which-my-sample-was-taken-even-if-the-increment-varies>
|
|
|
1009 * */
|
|
|
1010 q_t qpid_update(qpid_t *pid, const q_t error, const q_t position) {
|
|
|
1011 assert(pid);
|
|
|
1012 const q_t p = qmul(pid->p_gain, error);
|
|
|
1013 pid->i_state = qadd(pid->i_state, error);
|
|
|
1014 pid->i_state = qmax(pid->i_state, pid->i_min);
|
|
|
1015 pid->i_state = qmin(pid->i_state, pid->i_max);
|
|
|
1016 const q_t i = qmul(pid->i_state, pid->i_gain);
|
|
|
1017 const q_t d = qmul(pid->d_gain, qsub(position, pid->d_state));
|
|
|
1018 pid->d_state = position;
|
|
|
1019 return qsub(qadd(p, i), d);
|
|
|
1020 }
|
|
|
1021
|
|
|
1022 /* Simpsons method for numerical integration, from "Math Toolkit for
|
|
|
1023 * Real-Time Programming" by Jack Crenshaw */
|
|
|
1024 q_t qsimpson(q_t (*f)(q_t), const q_t x1, const q_t x2, const unsigned n) {
|
|
|
1025 assert(f);
|
|
|
1026 assert((n & 1) == 0);
|
|
|
1027 const q_t h = qdiv(qsub(x2, x1), QINT(n));
|
|
|
1028 q_t sum = 0, x = x1;
|
|
|
1029 for (unsigned i = 0; i < (n / 2u); i++){
|
|
|
1030 sum = qadd(sum, qadd(f(x), qmul(QINT(2), f(qadd(x,h)))));
|
|
|
1031 x = qadd(x, qmul(QINT(2), h));
|
|
|
1032 }
|
|
|
1033 sum = qsub(qmul(QINT(2), sum), qadd(f(x1), f(x2)));
|
|
|
1034 return qdiv(qmul(h, sum), QINT(3));
|
|
|
1035 }
|
|
|
1036
|
|
|
1037 /* The matrix meta-data field is not used at the moment, but could be
|
|
|
1038 * used for things like versioning, determining whether the matrix is
|
|
|
1039 * all zeros, or is the identify matrix, whether it contains valid data,
|
|
|
1040 * and more. Some common matrix operations are missing, such as factorization
|
|
|
1041 *
|
|
|
1042 * A function for image kernels might be useful. */
|
|
|
1043
|
|
|
1044 enum { METADATA, LENGTH, ROW, COLUMN, DATA, };
|
|
|
1045
|
|
|
1046 int qmatrix_is_valid(const q_t *m) {
|
|
|
1047 const size_t size = m[LENGTH], row = m[ROW], column = m[COLUMN];
|
|
|
1048 const size_t elements = row * column;
|
|
|
1049 if (elements < row || elements < column) /* overflow */
|
|
|
1050 return 0;
|
|
|
1051 if (elements > size)
|
|
|
1052 return 0;
|
|
|
1053 return 1;
|
|
|
1054 }
|
|
|
1055
|
|
|
1056 int qmatrix_resize(q_t *m, const size_t row, const size_t column) {
|
|
|
1057 const size_t rc = row * column;
|
|
|
1058 const size_t sz = m[LENGTH];
|
|
|
1059 if ((row && column) && (rc < row || rc < column)) /* overflow */
|
|
|
1060 return -1;
|
|
|
1061 if (rc > sz)
|
|
|
1062 return -1;
|
|
|
1063 m[ROW] = row;
|
|
|
1064 m[COLUMN] = column;
|
|
|
1065 return 0;
|
|
|
1066 }
|
|
|
1067
|
|
|
1068 int qmatrix_apply_unary(q_t *r, const q_t *a, q_t (*func)(q_t)) {
|
|
|
1069 assert(r);
|
|
|
1070 assert(qmatrix_is_valid(r));
|
|
|
1071 assert(a);
|
|
|
1072 assert(qmatrix_is_valid(a));
|
|
|
1073 assert(func);
|
|
|
1074 const q_t *ma = &a[DATA];
|
|
|
1075 q_t *mr = &r[DATA];
|
|
|
1076 const size_t arows = a[ROW], acolumns = a[COLUMN];
|
|
|
1077 if (qmatrix_resize(r, arows, acolumns) < 0)
|
|
|
1078 return -1;
|
|
|
1079 for (size_t i = 0; i < arows; i++)
|
|
|
1080 for (size_t j = 0; j < acolumns; j++)
|
|
|
1081 mr[i*acolumns + j] = func(ma[i*acolumns + j]);
|
|
|
1082 return 0;
|
|
|
1083 }
|
|
|
1084
|
|
|
1085 int qmatrix_apply_scalar(q_t *r, const q_t *a, q_t (*func)(q_t, q_t), const q_t c) {
|
|
|
1086 assert(r);
|
|
|
1087 assert(qmatrix_is_valid(r));
|
|
|
1088 assert(a);
|
|
|
1089 assert(qmatrix_is_valid(a));
|
|
|
1090 assert(func);
|
|
|
1091 const q_t *ma = &a[DATA];
|
|
|
1092 q_t *mr = &r[DATA];
|
|
|
1093 const size_t arows = a[ROW], acolumns = a[COLUMN];
|
|
|
1094 if (qmatrix_resize(r, arows, acolumns) < 0)
|
|
|
1095 return -1;
|
|
|
1096 for (size_t i = 0; i < arows; i++)
|
|
|
1097 for (size_t j = 0; j < acolumns; j++)
|
|
|
1098 mr[i*acolumns + j] = func(ma[i*acolumns + j], c);
|
|
|
1099 return 0;
|
|
|
1100 }
|
|
|
1101
|
|
|
1102 int qmatrix_apply_binary(q_t *r, const q_t *a, const q_t *b, q_t (*func)(q_t, q_t)) {
|
|
|
1103 assert(a);
|
|
|
1104 assert(qmatrix_is_valid(a));
|
|
|
1105 assert(b);
|
|
|
1106 assert(qmatrix_is_valid(b));
|
|
|
1107 assert(r);
|
|
|
1108 assert(qmatrix_is_valid(r));
|
|
|
1109 assert(func);
|
|
|
1110 const q_t *ma = &a[DATA], *mb = &b[DATA];
|
|
|
1111 q_t *mr = &r[DATA];
|
|
|
1112 const size_t arows = a[ROW], acolumns = a[COLUMN];
|
|
|
1113 const size_t brows = b[ROW], bcolumns = b[COLUMN];
|
|
|
1114 const size_t rrows = r[ROW], rcolumns = r[COLUMN];
|
|
|
1115 if (arows != brows || acolumns != bcolumns)
|
|
|
1116 return -1;
|
|
|
1117 if (arows != rrows || acolumns != rcolumns)
|
|
|
1118 return -1;
|
|
|
1119 for (size_t i = 0; i < arows; i++)
|
|
|
1120 for (size_t j = 0; j < acolumns; j++) {
|
|
|
1121 const size_t idx = (i*acolumns) + j;
|
|
|
1122 mr[idx] = func(ma[idx], mb[idx]);
|
|
|
1123 }
|
|
|
1124 return 0;
|
|
|
1125 }
|
|
|
1126
|
|
|
1127 static q_t qfz(q_t a) { UNUSED(a); return QINT(0); }
|
|
|
1128 static q_t qf1(q_t a) { UNUSED(a); return QINT(1); }
|
|
|
1129
|
|
|
1130 int qmatrix_zero(q_t *r) { return qmatrix_apply_unary(r, r, qfz); }
|
|
|
1131 int qmatrix_one(q_t *r) { return qmatrix_apply_unary(r, r, qf1); }
|
|
|
1132 int qmatrix_logical(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qlogical); }
|
|
|
1133 int qmatrix_not(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qnot); }
|
|
|
1134 int qmatrix_signum(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qsignum); }
|
|
|
1135 int qmatrix_invert(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qinvert); }
|
|
|
1136 int qmatrix_add(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qadd); }
|
|
|
1137 int qmatrix_sub(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qsub); }
|
|
|
1138 int qmatrix_and(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qand); }
|
|
|
1139 int qmatrix_or (q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qor); }
|
|
|
1140 int qmatrix_xor(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qxor); }
|
|
|
1141
|
|
|
1142 int qmatrix_scalar_add(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qadd, scalar); }
|
|
|
1143 int qmatrix_scalar_sub(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qsub, scalar); }
|
|
|
1144 int qmatrix_scalar_mul(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qmul, scalar); }
|
|
|
1145 int qmatrix_scalar_div(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qdiv, scalar); }
|
|
|
1146 int qmatrix_scalar_mod(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qmod, scalar); }
|
|
|
1147 int qmatrix_scalar_rem(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qrem, scalar); }
|
|
|
1148 int qmatrix_scalar_and(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qand, scalar); }
|
|
|
1149 int qmatrix_scalar_or (q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qor, scalar); }
|
|
|
1150 int qmatrix_scalar_xor(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qxor, scalar); }
|
|
|
1151
|
|
|
1152 int qmatrix_is_square(const q_t *m) {
|
|
|
1153 assert(m);
|
|
|
1154 assert(qmatrix_is_valid(m));
|
|
|
1155 return m[COLUMN] == m[ROW];
|
|
|
1156 }
|
|
|
1157
|
|
|
1158 int qmatrix_identity(q_t *r) {
|
|
|
1159 assert(r);
|
|
|
1160 assert(qmatrix_is_valid(r));
|
|
|
1161 if (!qmatrix_is_square(r))
|
|
|
1162 return -1;
|
|
|
1163 q_t *mr = &r[DATA];
|
|
|
1164 const size_t length = r[ROW];
|
|
|
1165 for (size_t i = 0; i < length; i++)
|
|
|
1166 for (size_t j = 0; j < length; j++)
|
|
|
1167 mr[i*length + j] = i == j ? QINT(1) : QINT(0);
|
|
|
1168 return 0;
|
|
|
1169 }
|
|
|
1170
|
|
|
1171 int qmatrix_copy(q_t *r, const q_t *a) {
|
|
|
1172 assert(r);
|
|
|
1173 assert(qmatrix_is_valid(r));
|
|
|
1174 assert(a);
|
|
|
1175 assert(qmatrix_is_valid(a));
|
|
|
1176 const size_t arows = a[ROW], acolumns = a[COLUMN];
|
|
|
1177 const size_t copy = arows * acolumns * sizeof (q_t);
|
|
|
1178 if ((arows && acolumns) && (copy < arows || copy < acolumns))
|
|
|
1179 return -1;
|
|
|
1180 if (qmatrix_resize(r, arows, acolumns) < 0)
|
|
|
1181 return -1;
|
|
|
1182 memcpy(&r[DATA], &a[DATA], copy);
|
|
|
1183 return 0;
|
|
|
1184 }
|
|
|
1185
|
|
|
1186 q_t qmatrix_trace(const q_t *m) {
|
|
|
1187 assert(m);
|
|
|
1188 assert(qmatrix_is_square(m));
|
|
|
1189 const size_t length = m[ROW];
|
|
|
1190 const q_t *mm = &m[DATA];
|
|
|
1191 q_t tr = QINT(0);
|
|
|
1192 for (size_t i = 0; i < length; i++)
|
|
|
1193 for (size_t j = 0; j < length; j++)
|
|
|
1194 if (i == j)
|
|
|
1195 tr = qadd(tr, mm[i*length + j]);
|
|
|
1196 return tr;
|
|
|
1197 }
|
|
|
1198
|
|
|
1199 q_t qmatrix_equal(const q_t *a, const q_t *b) {
|
|
|
1200 assert(a);
|
|
|
1201 assert(qmatrix_is_valid(a));
|
|
|
1202 assert(b);
|
|
|
1203 assert(qmatrix_is_valid(b));
|
|
|
1204 const size_t arow = a[ROW], acolumn = a[COLUMN];
|
|
|
1205 const size_t brow = b[ROW], bcolumn = b[COLUMN];
|
|
|
1206 const q_t *ma = &a[DATA];
|
|
|
1207 const q_t *mb = &a[DATA];
|
|
|
1208 if (a == b)
|
|
|
1209 return QINT(1);
|
|
|
1210 if (arow != brow && acolumn != bcolumn)
|
|
|
1211 return QINT(0);
|
|
|
1212 return !memcmp(ma, mb, sizeof(q_t) * arow * brow);
|
|
|
1213 }
|
|
|
1214
|
|
|
1215 static q_t determine(const q_t *m, const size_t length) {
|
|
|
1216 assert(m);
|
|
|
1217 if (length == 1)
|
|
|
1218 return m[0];
|
|
|
1219 if (length == 2)
|
|
|
1220 return qsub(qmul(m[0], m[3]), qmul(m[1], m[2]));
|
|
|
1221 size_t co1 = 0, co2 = 0;
|
|
|
1222 q_t det = QINT(0), sgn = QINT(1);
|
|
|
1223 q_t co[length*length]; /* This should really be passed in */
|
|
|
1224 for (size_t i = 0; i < length; i++) {
|
|
|
1225 for (size_t j = 0; j < length; j++)
|
|
|
1226 for (size_t k = 0; k < length; k++)
|
|
|
1227 if (j && k != i) {
|
|
|
1228 co[co1*length + co2] = m[j*length + k];
|
|
|
1229 if (++co2 > (length - 2)) {
|
|
|
1230 co1++;
|
|
|
1231 co2 = 0;
|
|
|
1232 }
|
|
|
1233 }
|
|
|
1234 det = qadd(det, qcopysign(qmul(m[(0*length) + i], determine(co, length - 1)), sgn));
|
|
|
1235 sgn = qnegate(sgn);
|
|
|
1236 }
|
|
|
1237 return det;
|
|
|
1238 }
|
|
|
1239
|
|
|
1240 q_t qmatrix_determinant(const q_t *m) {
|
|
|
1241 assert(m);
|
|
|
1242 assert(qmatrix_is_square(m));
|
|
|
1243 assert(m[ROW] < 16);
|
|
|
1244 const size_t length = m[ROW];
|
|
|
1245 const q_t *mm = &m[DATA];
|
|
|
1246 return determine(mm, length);
|
|
|
1247 }
|
|
|
1248
|
|
|
1249 int qmatrix_transpose(q_t *r, const q_t *m) {
|
|
|
1250 assert(r);
|
|
|
1251 assert(qmatrix_is_valid(r));
|
|
|
1252 assert(m);
|
|
|
1253 assert(qmatrix_is_valid(m));
|
|
|
1254 q_t *mr = &r[DATA];
|
|
|
1255 const q_t *mm = &m[DATA];
|
|
|
1256 const size_t mrows = m[ROW], mcolumns = m[COLUMN];
|
|
|
1257 const size_t msize = mrows * mcolumns;
|
|
|
1258 const size_t rsize = r[LENGTH];
|
|
|
1259 if (msize > rsize)
|
|
|
1260 return -1;
|
|
|
1261 for (size_t i = 0; i < mrows; i++)
|
|
|
1262 for (size_t j = 0; j < mcolumns; j++)
|
|
|
1263 mr[i*mcolumns + j] = mm[j*mcolumns + i];
|
|
|
1264 r[ROW] = mcolumns;
|
|
|
1265 r[COLUMN] = mrows;
|
|
|
1266 return 0;
|
|
|
1267 }
|
|
|
1268
|
|
|
1269 int qmatrix_mul(q_t *r, const q_t *a, const q_t *b) {
|
|
|
1270 assert(a);
|
|
|
1271 assert(qmatrix_is_valid(a));
|
|
|
1272 assert(b);
|
|
|
1273 assert(qmatrix_is_valid(b));
|
|
|
1274 assert(r);
|
|
|
1275 assert(qmatrix_is_valid(r));
|
|
|
1276 q_t *mr = &r[DATA];
|
|
|
1277 const q_t *ma = &a[DATA], *mb = &b[DATA];
|
|
|
1278 const size_t arows = a[ROW], acolumns = a[COLUMN];
|
|
|
1279 const size_t brows = b[ROW], bcolumns = b[COLUMN];
|
|
|
1280 if (acolumns != brows)
|
|
|
1281 return -1;
|
|
|
1282 if (qmatrix_resize(r, arows, bcolumns) < 0)
|
|
|
1283 return -1;
|
|
|
1284 for (size_t i = 0; i < arows; i++)
|
|
|
1285 for (size_t j = 0; j < bcolumns; j++) {
|
|
|
1286 q_t s = QINT(0);
|
|
|
1287 for (size_t k = 0; k < brows; k++)
|
|
|
1288 s = qadd(s, qmul(ma[i*acolumns + k], mb[k*bcolumns + j]));
|
|
|
1289 mr[i*arows + j] = s;
|
|
|
1290 }
|
|
|
1291 return 0;
|
|
|
1292 }
|
|
|
1293
|
|
|
1294 static int addchar(char **str, size_t *length, const int ch) {
|
|
|
1295 assert(str && *str);
|
|
|
1296 assert(length);
|
|
|
1297 if (!length)
|
|
|
1298 return -1;
|
|
|
1299 char *s = *str;
|
|
|
1300 *s++ = ch;
|
|
|
1301 *str = s;
|
|
|
1302 *length -= 1;
|
|
|
1303 return 0;
|
|
|
1304 }
|
|
|
1305
|
|
|
1306 static int addstr(char **str, size_t *length, char *addme) {
|
|
|
1307 assert(str && *str);
|
|
|
1308 assert(length);
|
|
|
1309 assert(addme);
|
|
|
1310 const size_t sz = strlen(addme);
|
|
|
1311 for (size_t i = 0; i < sz; i++)
|
|
|
1312 if (addchar(str, length, addme[i]) < 0)
|
|
|
1313 return -1;
|
|
|
1314 return 0;
|
|
|
1315 }
|
|
|
1316
|
|
|
1317 int qmatrix_sprintb(const q_t *m, char *str, size_t length, unsigned base) {
|
|
|
1318 assert(str);
|
|
|
1319 assert(m);
|
|
|
1320 const q_t *mm = &m[DATA];
|
|
|
1321 const size_t rows = m[ROW], columns = m[COLUMN];
|
|
|
1322 if (base < 2 || base > 36)
|
|
|
1323 return -1;
|
|
|
1324 if (!qmatrix_is_valid(m))
|
|
|
1325 return addstr(&str, &length, "[ INVALID ]");
|
|
|
1326 if (addstr(&str, &length, "[ ") < 0)
|
|
|
1327 return -1;
|
|
|
1328 for (size_t i = 0; i < rows; i++) {
|
|
|
1329 for (size_t j = 0; j < columns; j++) {
|
|
|
1330 const int r = qsprintb(mm[i*columns + j], str, length, base);
|
|
|
1331 if (r < 0)
|
|
|
1332 return -1;
|
|
|
1333 if ((length - r) > length)
|
|
|
1334 return -1;
|
|
|
1335 length -= r;
|
|
|
1336 str += r;
|
|
|
1337 if (rows)
|
|
|
1338 if (addchar(&str, &length, columns && j < (columns - 1) ? ',' : i < rows - 1 ? ';' : ' ') < 0)
|
|
|
1339 return -1;
|
|
|
1340 if ((columns && j < (columns - 1)) || (i < (rows - 1)))
|
|
|
1341 if (addchar(&str, &length, ' ') < 0)
|
|
|
1342 return -1;
|
|
|
1343 }
|
|
|
1344 }
|
|
|
1345 if (addchar(&str, &length, ']') < 0)
|
|
|
1346 return -1;
|
|
|
1347 return 0;
|
|
|
1348 }
|
|
|
1349
|
|
|
1350 size_t qmatrix_string_length(const q_t *m) {
|
|
|
1351 assert(m);
|
|
|
1352 if (!qmatrix_is_valid(m))
|
|
|
1353 return 128; /* space for invalid matrix message */
|
|
|
1354 const size_t msize = m[LENGTH];
|
|
|
1355 const size_t r = (msize *
|
|
|
1356 (32 /*max length if base 2 used)*/
|
|
|
1357 + 2 /* '-' and '.' */
|
|
|
1358 + 2 /* space and comma/semi colon separator */
|
|
|
1359 )) + 16 /* space for extra formatting */;
|
|
|
1360 return r;
|
|
|
1361 }
|
|
|
1362
|
|
|
1363 /* See <https://github.com/jamesbowman/sincos>
|
|
|
1364 * and "Math Toolkit for Real-Time Programming" by Jack Crenshaw
|
|
|
1365 *
|
|
|
1366 * The naming of these functions ('furman_') is incorrect, they do their
|
|
|
1367 * computation on numbers represented in Furmans but they do not use a 'Furman
|
|
|
1368 * algorithm'. As I do not have a better name, the name shall stick. */
|
|
|
1369 static int16_t _sine(const int16_t y) {
|
|
|
1370 const int16_t s1 = 0x6487, s3 = -0x2953, s5 = 0x04f8;
|
|
|
1371 const int16_t z = arshift((int32_t)y * y, 12);
|
|
|
1372 int16_t prod = arshift((int32_t)z * s5, 16);
|
|
|
1373 int16_t sum = s3 + prod;
|
|
|
1374 prod = arshift((int32_t)z * sum, 16);
|
|
|
1375 sum = s1 + prod;
|
|
|
1376 return arshift((int32_t)y * sum, 13);
|
|
|
1377 }
|
|
|
1378
|
|
|
1379 static int16_t _cosine(int16_t y) {
|
|
|
1380 const int16_t c0 = 0x7fff, c2 = -0x4ee9, c4 = 0x0fbd;
|
|
|
1381 const int16_t z = arshift((int32_t)y * y, 12);
|
|
|
1382 int16_t prod = arshift((int32_t)z * c4, 16);
|
|
|
1383 const int16_t sum = c2 + prod;
|
|
|
1384 prod = arshift((int32_t)z * sum, 15);
|
|
|
1385 return c0 + prod;
|
|
|
1386 }
|
|
|
1387
|
|
|
1388 int16_t furman_sin(int16_t x) {
|
|
|
1389 const int16_t n = 3 & arshift(x + 0x2000, 14);
|
|
|
1390 x -= n << 14;
|
|
|
1391 const int16_t r = (n & 1) ? _cosine(x) : _sine(x);
|
|
|
1392 return (n & 2) ? -r : r;
|
|
|
1393 }
|
|
|
1394
|
|
|
1395 int16_t furman_cos(int16_t x) {
|
|
|
1396 return furman_sin(x + 0x4000);
|
|
|
1397 }
|
|
|
1398
|
|
|
1399 /* expression evaluator */
|
|
|
1400
|
|
|
1401 enum { ASSOCIATE_NONE, ASSOCIATE_LEFT, ASSOCIATE_RIGHT, };
|
|
|
1402 enum { LEX_NUMBER, LEX_OPERATOR, LEX_END, };
|
|
|
1403
|
|
|
1404 int qexpr_init(qexpr_t *e) {
|
|
|
1405 assert(e);
|
|
|
1406 e->lpar = qop("(");
|
|
|
1407 e->rpar = qop(")");
|
|
|
1408 e->negate = qop("negate");
|
|
|
1409 e->minus = qop("-");
|
|
|
1410 e->initialized = 1;
|
|
|
1411 assert(e->lpar && e->rpar && e->negate && e->minus);
|
|
|
1412 return 0;
|
|
|
1413 }
|
|
|
1414
|
|
|
1415 static int error(qexpr_t *e, const char *fmt, ...) {
|
|
|
1416 assert(e);
|
|
|
1417 assert(fmt);
|
|
|
1418 if (e->error)
|
|
|
1419 return 0;
|
|
|
1420 va_list ap;
|
|
|
1421 va_start(ap, fmt);
|
|
|
1422 (void)vsnprintf(e->error_string, sizeof (e->error_string), fmt, ap);
|
|
|
1423 va_end(ap);
|
|
|
1424 e->error = -1;
|
|
|
1425 return -QINT(1);
|
|
|
1426 }
|
|
|
1427
|
|
|
1428 static q_t numberify(const char *s) {
|
|
|
1429 assert(s);
|
|
|
1430 q_t q = 0;
|
|
|
1431 (void) qconv(&q, s);
|
|
|
1432 return q;
|
|
|
1433 }
|
|
|
1434
|
|
|
1435 static q_t qbase(q_t b) {
|
|
|
1436 int nb = qtoi(b);
|
|
|
1437 if (nb < 2 || nb > 36)
|
|
|
1438 return -QINT(1);
|
|
|
1439 qconf.base = nb;
|
|
|
1440 return b;
|
|
|
1441 }
|
|
|
1442
|
|
|
1443 static q_t qplaces(q_t places) {
|
|
|
1444 /* TODO: Bounds checks given base */
|
|
|
1445 qconf.dp = qtoi(places);
|
|
|
1446 return places;
|
|
|
1447 }
|
|
|
1448
|
|
|
1449 static q_t check_div0(qexpr_t *e, q_t a, q_t b) {
|
|
|
1450 assert(e);
|
|
|
1451 UNUSED(a);
|
|
|
1452 if (!b)
|
|
|
1453 return error(e, "division by zero");
|
|
|
1454 return QINT(0);
|
|
|
1455 }
|
|
|
1456
|
|
|
1457 static q_t check_nlz(qexpr_t *e, q_t a) { // Not Less Zero
|
|
|
1458 assert(e);
|
|
|
1459 if (qless(a, QINT(0)))
|
|
|
1460 return error(e, "negative argument");
|
|
|
1461 return QINT(0);
|
|
|
1462 }
|
|
|
1463
|
|
|
1464 static q_t check_nlez(qexpr_t *e, q_t a) { // Not Less Equal Zero
|
|
|
1465 assert(e);
|
|
|
1466 if (qeqless(a, QINT(0)))
|
|
|
1467 return error(e, "negative or zero argument");
|
|
|
1468 return QINT(0);
|
|
|
1469 }
|
|
|
1470
|
|
|
1471 static q_t check_nlo(qexpr_t *e, q_t a) { // Not less than one
|
|
|
1472 assert(e);
|
|
|
1473 if (qless(a, QINT(1)))
|
|
|
1474 return error(e, "out of range [1, INF]");
|
|
|
1475 return QINT(0);
|
|
|
1476 }
|
|
|
1477
|
|
|
1478 static q_t check_alo(qexpr_t *e, q_t a) {
|
|
|
1479 assert(e);
|
|
|
1480 if (qmore(qabs(a), QINT(1)))
|
|
|
1481 return error(e, "out of range [-1, 1]");
|
|
|
1482 return QINT(0);
|
|
|
1483 }
|
|
|
1484
|
|
|
1485 const qoperations_t *qop(const char *op) {
|
|
|
1486 assert(op);
|
|
|
1487 static const qoperations_t ops[] = {
|
|
|
1488 /* Binary Search Table: Use 'LC_ALL="C" sort -k 2 < table' to sort this */
|
|
|
1489 /* name function check function precedence arity left/right-assoc hidden */
|
|
|
1490 { "!", .eval.unary = qnot, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1491 { "!=", .eval.binary = qunequal, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1492 { "%", .eval.binary = qrem,/*!*/ .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1493 { "&", .eval.binary = qand, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1494 { "(", .eval.unary = NULL, .check.unary = NULL, 0, 0, ASSOCIATE_NONE, 0, },
|
|
|
1495 { ")", .eval.unary = NULL, .check.unary = NULL, 0, 0, ASSOCIATE_NONE, 0, },
|
|
|
1496 { "*", .eval.binary = qmul, .check.binary = NULL, 3, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1497 { "+", .eval.binary = qadd, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1498 { "-", .eval.binary = qsub, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1499 { "/", .eval.binary = qdiv, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1500 { "<", .eval.binary = qless, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1501 { "<<", .eval.binary = qlls, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 0, },
|
|
|
1502 { "<=", .eval.binary = qeqless, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1503 { "==", .eval.binary = qequal, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1504 { ">", .eval.binary = qmore, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1505 { ">=", .eval.binary = qeqmore, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1506 { ">>", .eval.binary = qlrs, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 0, },
|
|
|
1507 { "^", .eval.binary = qxor, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1508 { "_div", .eval.binary = qcordic_div, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1509 { "_exp", .eval.unary = qcordic_exp, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 1, },
|
|
|
1510 { "_ln", .eval.unary = qcordic_ln, .check.unary = check_nlez, 5, 1, ASSOCIATE_RIGHT, 1, },
|
|
|
1511 { "_mul", .eval.binary = qcordic_mul, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1512 { "_sqrt", .eval.unary = qcordic_sqrt, .check.unary = check_nlz, 5, 1, ASSOCIATE_RIGHT, 1, },
|
|
|
1513 { "abs", .eval.unary = qabs, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1514 { "acos", .eval.unary = qacos, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1515 { "acosh", .eval.unary = qacosh, .check.unary = check_nlo, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1516 { "arshift", .eval.binary = qars, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1517 { "asin", .eval.unary = qasin, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1518 { "asinh", .eval.unary = qasinh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1519 { "atan", .eval.unary = qatan, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1520 { "atan2", .eval.binary = qatan2, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1521 { "atanh", .eval.unary = qatanh, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1522 { "base", .eval.unary = qbase, .check.unary = NULL, 2, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1523 { "ceil", .eval.unary = qceil, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1524 { "copysign", .eval.binary = qcopysign, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1525 { "cos", .eval.unary = qcos, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1526 { "cosh", .eval.unary = qcosh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1527 { "cot", .eval.unary = qcot, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1528 { "deg2rad", .eval.unary = qdeg2rad, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1529 { "even?", .eval.unary = qiseven, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1530 { "exp", .eval.unary = qexp, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1531 { "floor", .eval.unary = qfloor, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1532 { "hypot", .eval.binary = qhypot, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 0, },
|
|
|
1533 { "int?", .eval.unary = qisinteger, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1534 { "log", .eval.unary = qlog, .check.unary = check_nlez, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1535 { "lshift", .eval.binary = qlls, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1536 { "max", .eval.binary = qmax, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1537 { "min", .eval.binary = qmin, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1538 { "mod", .eval.binary = qmod, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1539 { "neg?", .eval.unary = qisnegative, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1540 { "negate", .eval.unary = qnegate, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1541 { "odd?", .eval.unary = qisodd, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1542 { "places", .eval.unary = qplaces, .check.unary = NULL, 2, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1543 { "pos?", .eval.unary = qispositive, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1544 { "pow", .eval.binary = qpow, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 0, },
|
|
|
1545 { "rad2deg", .eval.unary = qrad2deg, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1546 { "rem", .eval.binary = qrem, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1547 { "round", .eval.unary = qround, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1548 { "rshift", .eval.binary = qlrs, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
|
|
|
1549 { "sign", .eval.unary = qsign, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1550 { "signum", .eval.unary = qsignum, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1551 { "sin", .eval.unary = qsin, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1552 { "sinh", .eval.unary = qsinh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1553 { "sqrt", .eval.unary = qsqrt, .check.unary = check_nlz, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1554 { "tan", .eval.unary = qtan, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1555 { "tanh", .eval.unary = qtanh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1556 { "trunc", .eval.unary = qtrunc, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1557 { "|", .eval.binary = qor, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
|
|
|
1558 { "~", .eval.unary = qinvert, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
|
|
|
1559 };
|
|
|
1560 const size_t length = (sizeof ops / sizeof ops[0]);
|
|
|
1561 size_t l = 0, r = length - 1;
|
|
|
1562 while (l <= r) { // Iterative Binary Search
|
|
|
1563 size_t m = l + ((r - l)/2u);
|
|
|
1564 assert (m < length);
|
|
|
1565 const int comp = strcmp(ops[m].name, op);
|
|
|
1566 if (comp == 0)
|
|
|
1567 return &ops[m];
|
|
|
1568 if (comp < 0)
|
|
|
1569 l = m + 1;
|
|
|
1570 else
|
|
|
1571 r = m - 1;
|
|
|
1572 }
|
|
|
1573 return NULL;
|
|
|
1574 }
|
|
|
1575
|
|
|
1576 static int number_push(qexpr_t *e, q_t num) {
|
|
|
1577 assert(e);
|
|
|
1578 if (e->error)
|
|
|
1579 return -1;
|
|
|
1580 if (e->numbers_count > (e->numbers_max - 1)) {
|
|
|
1581 error(e, "number stack overflow");
|
|
|
1582 return -1;
|
|
|
1583 }
|
|
|
1584 e->numbers[e->numbers_count++] = num;
|
|
|
1585 return 0;
|
|
|
1586 }
|
|
|
1587
|
|
|
1588 static q_t number_pop(qexpr_t *e) {
|
|
|
1589 assert(e);
|
|
|
1590 if (e->error)
|
|
|
1591 return -1;
|
|
|
1592 if (!(e->numbers_count)) {
|
|
|
1593 error(e, "number stack empty");
|
|
|
1594 return -1; /* error handled elsewhere */
|
|
|
1595 }
|
|
|
1596 return e->numbers[--(e->numbers_count)];
|
|
|
1597 }
|
|
|
1598
|
|
|
1599 static int op_push(qexpr_t *e, const qoperations_t *op) {
|
|
|
1600 assert(e);
|
|
|
1601 assert(op);
|
|
|
1602 if (e->error)
|
|
|
1603 return -1;
|
|
|
1604 if (e->ops_count > (e->ops_max - 1)) {
|
|
|
1605 error(e, "operator stack overflow");
|
|
|
1606 return -1;
|
|
|
1607 }
|
|
|
1608 e->ops[e->ops_count++] = op;
|
|
|
1609 return 0;
|
|
|
1610 }
|
|
|
1611
|
|
|
1612 int qexpr_error(qexpr_t *e) {
|
|
|
1613 assert(e);
|
|
|
1614 assert(e->initialized);
|
|
|
1615 return e->error;
|
|
|
1616 }
|
|
|
1617
|
|
|
1618 q_t qexpr_result(qexpr_t *e) {
|
|
|
1619 assert(e);
|
|
|
1620 assert(e->initialized);
|
|
|
1621 assert(e->error == 0);
|
|
|
1622 assert(e->numbers_count == 1);
|
|
|
1623 return e->numbers[0];
|
|
|
1624 }
|
|
|
1625
|
|
|
1626 static const qoperations_t *op_pop(qexpr_t *e) {
|
|
|
1627 assert(e);
|
|
|
1628 if (e->error)
|
|
|
1629 return NULL;
|
|
|
1630 if (!(e->ops_count)) {
|
|
|
1631 error(e, "operator stack empty");
|
|
|
1632 return NULL;
|
|
|
1633 }
|
|
|
1634 return e->ops[--(e->ops_count)];
|
|
|
1635 }
|
|
|
1636
|
|
|
1637 static int op_eval(qexpr_t *e) {
|
|
|
1638 assert(e);
|
|
|
1639 const qoperations_t *pop = op_pop(e);
|
|
|
1640 if (!pop)
|
|
|
1641 return -1;
|
|
|
1642 const q_t a = number_pop(e);
|
|
|
1643 const int exists = pop->arity == 1 ? BOOLIFY(pop->eval.unary) : BOOLIFY(pop->eval.binary);
|
|
|
1644 if (!exists) {
|
|
|
1645 error(e, "syntax error");
|
|
|
1646 return -1;
|
|
|
1647 }
|
|
|
1648 if (pop->arity == 1) {
|
|
|
1649 if (pop->check.unary && pop->check.unary(e, a) < 0) {
|
|
|
1650 error(e, "unary check failed");
|
|
|
1651 return -1;
|
|
|
1652 }
|
|
|
1653 return number_push(e, pop->eval.unary(a));
|
|
|
1654 }
|
|
|
1655 const q_t b = number_pop(e);
|
|
|
1656 if (pop->check.binary && pop->check.binary(e, b, a)) {
|
|
|
1657 error(e, "binary check failed");
|
|
|
1658 return -1;
|
|
|
1659 }
|
|
|
1660
|
|
|
1661 return number_push(e, pop->eval.binary(b, a));
|
|
|
1662 }
|
|
|
1663
|
|
|
1664 static int shunt(qexpr_t *e, const qoperations_t *op) {
|
|
|
1665 assert(e);
|
|
|
1666 assert(op);
|
|
|
1667 if (op == e->lpar) {
|
|
|
1668 return op_push(e, op);
|
|
|
1669 } else if (op == e->rpar) {
|
|
|
1670 while (e->ops_count && e->ops[e->ops_count - 1] != e->lpar)
|
|
|
1671 if (op_eval(e) < 0 || e->error)
|
|
|
1672 break;
|
|
|
1673 const qoperations_t *pop = op_pop(e);
|
|
|
1674 if (!pop || (pop != e->lpar)) {
|
|
|
1675 e->error = 0; /* clear error so following error is printed */
|
|
|
1676 error(e, "expected \"(\"");
|
|
|
1677 return -1;
|
|
|
1678 }
|
|
|
1679 return 0;
|
|
|
1680 } else if (op->assocativity == ASSOCIATE_RIGHT) {
|
|
|
1681 while (e->ops_count && op->precedence < e->ops[e->ops_count - 1]->precedence)
|
|
|
1682 if (op_eval(e) < 0 || e->error)
|
|
|
1683 break;
|
|
|
1684 } else {
|
|
|
1685 while (e->ops_count && op->precedence <= e->ops[e->ops_count - 1]->precedence)
|
|
|
1686 if (op_eval(e) < 0 || e->error)
|
|
|
1687 break;
|
|
|
1688 }
|
|
|
1689 return op_push(e, op);
|
|
|
1690 }
|
|
|
1691
|
|
|
1692 static int variable_name_is_valid(const char *n) {
|
|
|
1693 assert(n);
|
|
|
1694 if (!isalpha(*n) && !(*n == '_'))
|
|
|
1695 return 0;
|
|
|
1696 for (n++; *n; n++)
|
|
|
1697 if (!isalnum(*n) && !(*n == '_'))
|
|
|
1698 return 0;
|
|
|
1699 return 1;
|
|
|
1700 }
|
|
|
1701
|
|
|
1702 static qvariable_t *variable_lookup(qexpr_t *e, const char *name) {
|
|
|
1703 assert(e);
|
|
|
1704 assert(name);
|
|
|
1705 for (size_t i = 0; i < e->vars_max; i++) {
|
|
|
1706 qvariable_t *v = e->vars[i];
|
|
|
1707 assert(v->name);
|
|
|
1708 assert(variable_name_is_valid(v->name));
|
|
|
1709 if (!strcmp(v->name, name))
|
|
|
1710 return v;
|
|
|
1711 }
|
|
|
1712 return NULL;
|
|
|
1713 }
|
|
|
1714
|
|
|
1715 static int lex(qexpr_t *e, const char **expr) {
|
|
|
1716 assert(e);
|
|
|
1717 assert(expr && *expr);
|
|
|
1718 int r = 0;
|
|
|
1719 const char *s = *expr;
|
|
|
1720 qvariable_t *v = NULL;
|
|
|
1721 e->id_count = 0;
|
|
|
1722 e->number = 0;
|
|
|
1723 e->op = NULL;
|
|
|
1724 memset(e->id, 0, sizeof (e->id));
|
|
|
1725 for (; *s && isspace(*s); s++)
|
|
|
1726 ;
|
|
|
1727 if (!(*s))
|
|
|
1728 return LEX_END;
|
|
|
1729 if (isalpha(*s) || *s == '_') {
|
|
|
1730 for (; e->id_count < sizeof(e->id) && *s && (isalnum(*s) || *s == '_');)
|
|
|
1731 e->id[e->id_count++] = *s++;
|
|
|
1732 if ((v = variable_lookup(e, e->id))) {
|
|
|
1733 e->number = v->value;
|
|
|
1734 r = LEX_NUMBER;
|
|
|
1735 } else if ((e->op = qop(e->id))) {
|
|
|
1736 r = LEX_OPERATOR;
|
|
|
1737 } else {
|
|
|
1738 r = -1;
|
|
|
1739 }
|
|
|
1740 } else {
|
|
|
1741 if (ispunct(*s)) {
|
|
|
1742 const qoperations_t *op1 = NULL, *op2 = NULL;
|
|
|
1743 int set = 0;
|
|
|
1744 e->id[e->id_count++] = *s++;
|
|
|
1745 op1 = qop(e->id);
|
|
|
1746 if (*s && ispunct(*s)) {
|
|
|
1747 set = 1;
|
|
|
1748 e->id[e->id_count++] = *s++;
|
|
|
1749 op2 = qop(e->id);
|
|
|
1750 }
|
|
|
1751 r = (op1 || op2) ? LEX_OPERATOR : -1;
|
|
|
1752 e->op = op2 ? op2 : op1;
|
|
|
1753 if (e->op == op1 && set) {
|
|
|
1754 s--;
|
|
|
1755 e->id_count--;
|
|
|
1756 e->id[1] = 0;
|
|
|
1757 }
|
|
|
1758 } else if (isdigit(*s)) {
|
|
|
1759 r = LEX_NUMBER;
|
|
|
1760 int dot = 0;
|
|
|
1761 for (; e->id_count < sizeof(e->id) && *s; s++) {
|
|
|
1762 const int ch = *s;
|
|
|
1763 if (!(isdigit(ch) || (ch == '.' && !dot)))
|
|
|
1764 break;
|
|
|
1765 e->id[e->id_count++] = ch;
|
|
|
1766 if (ch == '.')
|
|
|
1767 dot = 1;
|
|
|
1768 }
|
|
|
1769 e->number = numberify(e->id);
|
|
|
1770 } else {
|
|
|
1771 r = -1;
|
|
|
1772 }
|
|
|
1773 }
|
|
|
1774 /*printf("id(%d) %d => %s\n", (int)(s - *expr), r, e->id);*/
|
|
|
1775 *expr = s;
|
|
|
1776 return r;
|
|
|
1777 }
|
|
|
1778
|
|
|
1779 int qexpr(qexpr_t *e, const char *expr) {
|
|
|
1780 assert(e);
|
|
|
1781 assert(expr);
|
|
|
1782 int firstop = 1;
|
|
|
1783 const qoperations_t *previous = NULL;
|
|
|
1784 if (e->initialized) {
|
|
|
1785 memset(e->error_string, 0, sizeof (e->error_string));
|
|
|
1786 e->error = 0;
|
|
|
1787 e->ops_count = 0;
|
|
|
1788 e->numbers_count = 0;
|
|
|
1789 e->initialized = 1;
|
|
|
1790 }
|
|
|
1791 for (int l = 0; l != LEX_END && !(e->error);) {
|
|
|
1792 switch ((l = lex(e, &expr))) {
|
|
|
1793 case LEX_NUMBER:
|
|
|
1794 number_push(e, e->number);
|
|
|
1795 previous = NULL;
|
|
|
1796 firstop = 0;
|
|
|
1797 break;
|
|
|
1798 case LEX_OPERATOR: {
|
|
|
1799 const qoperations_t *op = e->op;
|
|
|
1800 if (CONFIG_Q_HIDE_FUNCS && op->hidden) {
|
|
|
1801 error(e, "unknown operator \"%s\"", op->name);
|
|
|
1802 goto end;
|
|
|
1803 }
|
|
|
1804 if (firstop || (previous && previous != e->rpar)) {
|
|
|
1805 if (e->op == e->minus) {
|
|
|
1806 op = e->negate;
|
|
|
1807 } else if (e->op->arity == 1) {
|
|
|
1808 /* do nothing */
|
|
|
1809 } else if (e->op != e->lpar) {
|
|
|
1810 assert(e->op);
|
|
|
1811 error(e, "invalid use of \"%s\"", e->op->name);
|
|
|
1812 goto end;
|
|
|
1813 }
|
|
|
1814 }
|
|
|
1815 shunt(e, op);
|
|
|
1816 previous = op;
|
|
|
1817 firstop = 0;
|
|
|
1818 break;
|
|
|
1819 }
|
|
|
1820 case LEX_END: break;
|
|
|
1821 default:
|
|
|
1822 error(e, "invalid symbol: %s", e->id);
|
|
|
1823 l = LEX_END;
|
|
|
1824 }
|
|
|
1825 }
|
|
|
1826 while (e->ops_count)
|
|
|
1827 if (op_eval(e) < 0 || e->error)
|
|
|
1828 break;
|
|
|
1829 if (e->numbers_count != 1) {
|
|
|
1830 error(e, "invalid expression: %d", e->numbers_count);
|
|
|
1831 return -1;
|
|
|
1832 }
|
|
|
1833 implies(e->error == 0, e->numbers_count == 1);
|
|
|
1834 end:
|
|
|
1835 return e->error == 0 ? 0 : -1;
|
|
|
1836 }
|
|
|
1837
|
|
|
1838
|
|
|
1839
|
