-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathq.c
1839 lines (1656 loc) · 55.8 KB
/
q.c
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
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/* Project: Q-Number (Q16.16, signed) library
* Author: Richard James Howe
* License: The Unlicense
* Email: [email protected]
* Repo: <https://github.com/q>
*
*
* A Q32.32 version would be useful.
*
* The following should be changed/done for this library:
*
* - Moving towards a header-only model.
* - Removal of dependencies such as 'isalpha', 'tolower'
* as they are locale dependent.
* - Make components optional (filters, expression parser, ...)
* - Make hyperbolic arc sin/cos/tan functions.
* - Fix bugs / inaccuracies in CORDIC code.
* - Improve accuracy of all the functions and quantify error and
* their limits.
*
* BUG: Enter: 2.71791, get 2.0625, 2.7179 works fine. (Need to
* limit decimal places).
*/
#include "q.h"
#include <assert.h>
#include <ctype.h>
#include <inttypes.h>
#include <limits.h>
#include <stdarg.h> /* for expression evaluator error handling */
#include <stdio.h> /* vsnprintf, for expression evaluator */
#include <string.h>
#define UNUSED(X) ((void)(X))
#define BOOLIFY(X) (!!(X))
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
#define MULTIPLIER (INT16_MAX)
#define DMIN (INT32_MIN)
#define DMAX (INT32_MAX)
#define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
#define MAX(X, Y) ((X) < (Y) ? (Y) : (X))
#ifndef CONFIG_Q_HIDE_FUNCS /* 1 = hide hidden (testing) functions, 0 = enable them */
#define CONFIG_Q_HIDE_FUNCS (0)
#endif
typedef int16_t hd_t; /* half Q width, signed */
typedef uint64_t lu_t; /* double Q width, unsigned */
const qinfo_t qinfo = {
.whole = QBITS,
.fractional = QBITS,
.zero = (u_t)0uL << QBITS,
.bit = 1uL,
.one = (u_t)1uL << QBITS,
.min = (u_t)(QHIGH << QBITS),
.max = (u_t)((QHIGH << QBITS) - 1uL),
.pi = QPI, /* 3.243F6 A8885 A308D 31319 8A2E0... */
.e = QMK(0x2, 0xB7E1, 16), /* 2.B7E1 5162 8A... */
.sqrt2 = QMK(0x1, 0x6A09, 16), /* 1.6A09 E667 F3... */
.sqrt3 = QMK(0x1, 0xBB67, 16), /* 1.BB67 AE85 84... */
.ln2 = QMK(0x0, 0xB172, 16), /* 0.B172 17F7 D1... */
.ln10 = QMK(0x2, 0x4D76, 16), /* 2.4D76 3776 AA... */
.version = QVERSION,
};
qconf_t qconf = { /* Global Configuration Options */
.bound = qbound_saturate,
.dp = 4,
.base = 10,
};
/********* Basic Library Routines ********************************************/
static inline void implies(const int x, const int y) {
assert(!x || y);
}
static inline void mutual(const int x, const int y) { /* mutual implication */
assert(BOOLIFY(x) == BOOLIFY(y));
}
static inline void exclusive(const int x, const int y) {
assert(BOOLIFY(x) != BOOLIFY(y));
}
static inline void static_assertions(void) {
BUILD_BUG_ON(CHAR_BIT != 8);
BUILD_BUG_ON((sizeof(q_t)*CHAR_BIT) != (QBITS * 2));
BUILD_BUG_ON( sizeof(q_t) != sizeof(u_t));
BUILD_BUG_ON( sizeof(u_t) != sizeof(d_t));
BUILD_BUG_ON(sizeof(lu_t) != sizeof(ld_t));
BUILD_BUG_ON(sizeof(d_t) != (sizeof(hd_t) * 2));
BUILD_BUG_ON(sizeof(lu_t) != (sizeof(u_t) * 2));
}
q_t qbound_saturate(const ld_t s) { /**< default saturation handler */
assert(s > DMAX || s < DMIN);
if (s > DMAX) return DMAX;
return DMIN;
}
q_t qbound_wrap(const ld_t s) { /**< wrap numbers on overflow */
assert(s > DMAX || s < DMIN);
if (s > DMAX) return DMIN + (s % DMAX);
return DMAX - ((-s) % DMAX);
}
static inline q_t qsat(const ld_t s) {
static_assertions();
if (s > DMAX || s < DMIN) return qconf.bound(s);
return s;
}
d_t arshift(const d_t v, const unsigned p) {
u_t vn = v;
if (v >= 0l)
return vn >> p;
const u_t leading = ((u_t)(-1l)) << ((sizeof(v) * CHAR_BIT) - p - 1);
return leading | (vn >> p);
}
static inline d_t divn(const d_t v, const unsigned p) {
/* return v / (1l << p); */
const u_t shifted = ((u_t)v) >> p;
if (qispositive(v))
return shifted;
const u_t leading = ((u_t)(-1l)) << ((sizeof(v)*CHAR_BIT) - p - 1);
return leading | shifted;
}
/* These really all should be moved the header for efficiency reasons */
static inline u_t qhigh(const q_t q) { return ((u_t)q) >> QBITS; }
static inline u_t qlow(const q_t q) { return ((u_t)q) & QMASK; }
static inline q_t qcons(const u_t hi, const u_t lo) { return (hi << QBITS) | (lo & QMASK); }
int qtoi(const q_t toi) { return ((lu_t)((ld_t)toi)) >> QBITS; }
q_t qint(const int toq) { return ((u_t)((d_t)toq)) << QBITS; }
signed char qtoc(const q_t q) { return qtoi(q); }
q_t qchar(signed char c) { return qint(c); }
short qtoh(const q_t q) { return qtoi(q); }
q_t qshort(short s) { return qint(s); }
long qtol(const q_t q) { return qtoi(q); }
q_t qlong(long l) { return qint(l); }
long long qtoll(const q_t q) { return qtoi(q); }
q_t qvlong(long long ll) { return qint(ll); }
q_t qisnegative(const q_t a) { return QINT(BOOLIFY(qhigh(a) & QHIGH)); }
q_t qispositive(const q_t a) { return QINT(!(qhigh(a) & QHIGH)); }
q_t qisinteger(const q_t a) { return QINT(!qlow(a)); }
q_t qisodd(const q_t a) { return QINT(qisinteger(a) && (qhigh(a) & 1)); }
q_t qiseven(const q_t a) { return QINT(qisinteger(a) && !(qhigh(a) & 1)); }
q_t qless(const q_t a, const q_t b) { return QINT(a < b); }
q_t qeqless(const q_t a, const q_t b) { return QINT(a <= b); }
q_t qmore(const q_t a, const q_t b) { return QINT(a > b); }
q_t qeqmore(const q_t a, const q_t b) { return QINT(a >= b); }
q_t qequal(const q_t a, const q_t b) { return QINT(a == b); }
q_t qunequal(const q_t a, const q_t b) { return QINT(a != b); }
q_t qnegate(const q_t a) { return (~(u_t)a) + 1ULL; }
q_t qmin(const q_t a, const q_t b) { return qless(a, b) ? a : b; }
q_t qmax(const q_t a, const q_t b) { return qmore(a, b) ? a : b; }
q_t qabs(const q_t a) { return qisnegative(a) ? qnegate(a) : a; }
q_t qadd(const q_t a, const q_t b) { return qsat((ld_t)a + (ld_t)b); }
q_t qsub(const q_t a, const q_t b) { return qsat((ld_t)a - (ld_t)b); }
q_t qcopysign(const q_t a, const q_t b) { return qisnegative(b) ? qnegate(qabs(a)) : qabs(a); }
q_t qand(const q_t a, const q_t b) { return a & b; }
q_t qxor(const q_t a, const q_t b) { return a ^ b; }
q_t qor(const q_t a, const q_t b) { return a | b; }
q_t qinvert(const q_t a) { return ~a; }
q_t qnot(const q_t a) { return QINT(!a); }
q_t qlogical(const q_t a) { return QINT(BOOLIFY(a)); }
q_t qlrs(const q_t a, const q_t b) { /* assert low bits == 0? */ return (u_t)a >> (u_t)qtoi(b); }
q_t qlls(const q_t a, const q_t b) { return (u_t)a << b; }
q_t qars(const q_t a, const q_t b) { return arshift(a, qtoi(b)); }
q_t qals(const q_t a, const q_t b) { return qsat((lu_t)a << b); }
q_t qsign(const q_t a) { return qisnegative(a) ? -QINT(1) : QINT(1); }
q_t qsignum(const q_t a) { return a ? qsign(a) : QINT(0); }
q_t qapproxequal(const q_t a, const q_t b, const q_t epsilon) {
assert(qeqmore(epsilon, qint(0)));
return QINT(qless(qabs(qsub(a, b)), epsilon));
}
q_t qapproxunequal(const q_t a, const q_t b, const q_t epsilon) {
return QINT(!qapproxequal(a, b, epsilon));
}
q_t qwithin(q_t v, q_t b1, q_t b2) {
const q_t hi = qmax(b1, b2);
const q_t lo = qmin(b1, b2);
if (qequal(v, b1) || qequal(v, b2))
return 1;
return qless(v, hi) && qmore(v, lo) ? QINT(1) : QINT(0);
}
q_t qwithin_interval(q_t v, q_t expected, q_t allowance) {
const q_t b1 = qadd(expected, allowance);
const q_t b2 = qsub(expected, allowance);
return qwithin(v, b1, b2);
}
q_t qfloor(const q_t q) {
return q & ~QMASK;
}
q_t qceil(q_t q) {
const q_t adj = qisinteger(q) ? QINT(0) : QINT(1);
q = qadd(q, adj);
return ((u_t)q) & (QMASK << QBITS);
}
q_t qtrunc(q_t q) {
const q_t adj = qisnegative(q) && qlow(q) ? QINT(1) : QINT(0);
q = qadd(q, adj);
return ((u_t)q) & (QMASK << QBITS);
}
q_t qround(q_t q) {
const int negative = qisnegative(q);
q = qabs(q);
const q_t adj = (qlow(q) & QHIGH) ? QINT(1) : QINT(0);
q = qadd(q, adj);
q = ((u_t)q) & (QMASK << QBITS);
return negative ? qnegate(q) : q;
}
int qpack(const q_t *q, char *buffer, const size_t length) {
assert(buffer);
if (length < sizeof(*q))
return -1;
q_t qn = *q;
uint8_t *b = (uint8_t*)buffer;
for (size_t i = 0; i < sizeof(qn); i++) {
b[i] = qn;
qn = (u_t)qn >> CHAR_BIT;
}
return sizeof(qn);
}
int qunpack(q_t *q, const char *buffer, const size_t length) {
assert(q);
assert(buffer);
if (length < sizeof(*q))
return -1;
uint8_t *b = (uint8_t*)buffer;
u_t nq = 0;
for (size_t i = 0; i < sizeof(*q); i++) {
nq <<= CHAR_BIT;
nq |= b[sizeof(*q)-i-1];
}
*q = nq;
return sizeof(*q);
}
static inline ld_t multiply(const q_t a, const q_t b) {
const ld_t dd = ((ld_t)a * (ld_t)b) + (lu_t)QHIGH;
/* N.B. portable version of "dd >> QBITS", for double width signed values */
return dd < 0 ? (-1ull << (2 * QBITS)) | ((lu_t)dd >> QBITS) : ((lu_t)dd) >> QBITS;
}
q_t qmul(const q_t a, const q_t b) {
return qsat(multiply(a, b));
}
q_t qfma(const q_t a, const q_t b, const q_t c) {
return qsat(multiply(a, b) + (ld_t)c);
}
q_t qdiv(const q_t a, const q_t b) {
assert(b);
const ld_t dd = ((ld_t)a) << QBITS;
ld_t bd2 = divn(b, 1);
if (!((dd >= 0 && b > 0) || (dd < 0 && b < 0)))
bd2 = -bd2;
/* Overflow not checked! */
/*return (dd/b) + (bd2/b);*/
return (dd + bd2) / b;
}
q_t qrem(const q_t a, const q_t b) {
return qsub(a, qmul(qtrunc(qdiv(a, b)), b));
}
q_t qmod(q_t a, q_t b) {
return qsub(a, qmul(qfloor(qdiv(a, b)), b));
}
static char itoch(const unsigned ch) {
assert(ch < 36);
if (ch <= 9)
return ch + '0';
return ch + 'A' - 10;
}
static inline void swap(char *a, char *b) {
assert(a);
assert(b);
const int c = *a;
*a = *b;
*b = c;
}
static void reverse(char *s, const size_t length) {
assert(s);
for (size_t i = 0; i < length/2; i++)
swap(&s[i], &s[length - i - 1]);
}
static int uprint(u_t p, char *s, const size_t length, const d_t base) {
assert(s);
assert(base >= 2 && base <= 36);
if (length < 2)
return -1;
size_t i = 0;
do {
unsigned ch = p % base;
p /= base;
s[i++] = itoch(ch);
} while (p && i < length);
if (p && i >= length)
return -1;
reverse(s, i);
return i;
}
/* <https://codereview.stackexchange.com/questions/109212> */
int qsprintbdp(q_t p, char *s, size_t length, const u_t base, const d_t idp) {
assert(s);
const int negative = BOOLIFY(qisnegative(p));
if (negative)
p = qnegate(p);
const d_t hi = qhigh(p);
char frac[QBITS + 2] = { '.', };
memset(s, 0, length);
assert(base >= 2 && base <= 36);
u_t lo = qlow(p);
size_t i = 1;
for (i = 1; lo; i++) {
if (idp >= 0 && (int)i > idp)
break;
lo *= base;
assert(i < (QBITS + 2));
frac[i] = itoch(lo >> QBITS);
lo &= QMASK;
}
if (negative)
s[0] = '-';
const int hisz = uprint(hi, s + negative, length - (1 + negative), base);
if (hisz < 0 || (hisz + i + negative + 1) > length)
return -1;
memcpy(s + hisz + negative, frac, i);
return i + hisz;
}
int qsprintb(q_t p, char *s, size_t length, const u_t base) {
return qsprintbdp(p, s, length, base, qconf.dp);
}
int qsprint(const q_t p, char *s, const size_t length) {
return qsprintb(p, s, length, qconf.base);
}
static inline int extract(unsigned char c, const int radix) {
c = tolower(c);
if (c >= '0' && c <= '9')
c -= '0';
else if (c >= 'a' && c <= 'z')
c -= ('a' - 10);
else
return -1;
if (c < radix)
return c;
return -1;
}
static inline q_t qmk(d_t integer, u_t fractional) {
const int negative = integer < 0;
integer = negative ? -integer : integer;
const q_t r = qcons((d_t)integer, fractional);
return negative ? qnegate(r) : r;
}
static inline u_t integer_logarithm(u_t num, const u_t base) {
assert(num > 0 && base >= 2 && base <= 36);
u_t r = -1;
do r++; while (num /= base);
return r;
}
int qnconvbdp(q_t *q, const char *s, size_t length, const d_t base, const u_t idp) {
assert(q);
assert(s);
assert(base >= 2 && base <= 36);
*q = QINT(0);
if (length < 1)
return -1;
d_t hi = 0, lo = 0, places = 1, negative = 0, overflow = 0;
size_t sidx = 0;
if (s[sidx] == '-') {
if (length < 2)
return -1;
negative = 1;
sidx++;
}
for (; sidx < length && s[sidx]; sidx++) {
const d_t e = extract(s[sidx], base);
if (e < 0)
break;
if (hi > MULTIPLIER) { /* continue on with conversion, do not accumulate */
overflow = 1;
} else {
hi = (hi * base) + e;
}
}
if (sidx >= length || !s[sidx])
goto done;
if (s[sidx] != '.')
return -2;
sidx++;
const u_t ilog = integer_logarithm(0x10000, base);
const u_t max = MIN(idp, ilog); /* Calculate maximum decimal places given base */
for (u_t dp = 0; sidx < length && s[sidx]; sidx++, dp++) {
const int ch = extract(s[sidx], base);
if (ch < 0)
return -3;
if (dp < max) { /* continue on with conversion , do not accumulate */
/* We could get more accuracy by looking at one digit
* passed the maximum digits allowed and rounding if
* that digit exists in the input. */
lo = (lo * base) + ch;
if (places >= (DMAX / base))
return -4;
places *= base;
}
assert((dp + 1) > dp);
}
if (!places)
return -5;
lo = ((d_t)((u_t)lo << QBITS) / places);
done:
if (overflow) {
*q = negative ? qinfo.min : qinfo.max;
return -6;
} else {
const q_t nq = qmk(hi, lo);
*q = negative ? qnegate(nq) : nq;
}
return 0;
}
int qnconvb(q_t *q, const char *s, size_t length, const d_t base) {
return qnconvbdp(q, s, length, base, qconf.dp);
}
int qnconv(q_t *q, const char *s, size_t length) {
return qnconvb(q, s, length, qconf.base);
}
int qconv(q_t *q, const char * const s) {
assert(s);
return qnconv(q, s, strlen(s));
}
int qconvb(q_t *q, const char * const s, const d_t base) {
assert(s);
return qnconvb(q, s, strlen(s), base);
}
typedef enum {
CORDIC_MODE_VECTOR_E/* = 'VECT'*/,
CORDIC_MODE_ROTATE_E/* = 'ROT'*/,
} cordic_mode_e;
typedef enum {
CORDIC_COORD_HYPERBOLIC_E = -1,
CORDIC_COORD_LINEAR_E = 0,
CORDIC_COORD_CIRCULAR_E = 1,
} cordic_coordinates_e;
static const d_t cordic_circular_inverse_scaling = 0x9B74; /* 1/scaling-factor */
static const d_t cordic_hyperbolic_inverse_scaling = 0x13520; /* 1/scaling-factor */
static inline int mulsign(d_t a, d_t b) { /* sign(a*b) */
const int aneg = a < 0;
const int bneg = b < 0;
return aneg ^ bneg ? -QINT(1) : QINT(1);
}
/* Universal CORDIC <https://en.wikibooks.org/wiki/Digital_Circuits/CORDIC>
*
* x(i+1) = x(i) - u.d(i).y(i).pow(2, -i)
* y(i+1) = y(i) + d(i).x(i).pow(2, -i)
* z(i+1) = z(i) - d(i).a(i)
*
* d(i) = sgn(z(i)) (rotation)
* d(i) = -sgn(x(i).y(i)) (vectoring)
*
* hyperbolic linear circular
* u = -1 0 1
* a = atanh(pow(2, -i)) pow(2, -i) atan(pow(2, -i))
*
* linear shift sequence: i = 0, 1, 2, 3, ...
* circular shift sequence: i = 1, 2, 3, 4, ...
* hyperbolic shift sequence: i = 1, 2, 3, 4, 4, 5, ... */
static int cordic(const cordic_coordinates_e coord, const cordic_mode_e mode, int iterations, d_t *x0, d_t *y0, d_t *z0) {
assert(x0);
assert(y0);
assert(z0);
if (mode != CORDIC_MODE_VECTOR_E && mode != CORDIC_MODE_ROTATE_E)
return -1;
BUILD_BUG_ON(sizeof(d_t) != sizeof(uint32_t));
BUILD_BUG_ON(sizeof(u_t) != sizeof(uint32_t));
static const u_t arctans[] = { /* atan(2^0), atan(2^-1), atan(2^-2), ... */
0xC90FuL, 0x76B1uL, 0x3EB6uL, 0x1FD5uL,
0x0FFAuL, 0x07FFuL, 0x03FFuL, 0x01FFuL,
0x00FFuL, 0x007FuL, 0x003FuL, 0x001FuL,
0x000FuL, 0x0007uL, 0x0003uL, 0x0001uL,
0x0000uL, // 0x0000uL,
};
static const size_t arctans_length = sizeof arctans / sizeof arctans[0];
static const u_t arctanhs[] = { /* atanh(2^-1), atanh(2^-2), ... */
0x8c9fuL, 0x4162uL, 0x202buL, 0x1005uL,
0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
0x0000uL, // 0x0000uL,
};
static const size_t arctanhs_length = sizeof arctanhs / sizeof arctanhs[0];
static const u_t halfs[] = { /* 2^0, 2^-1, 2^-2, ..*/
0x10000uL,
0x8000uL, 0x4000uL, 0x2000uL, 0x1000uL,
0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
//0x0000uL, // 0x0000uL,
};
static const size_t halfs_length = sizeof halfs / sizeof halfs[0];
const u_t *lookup = NULL;
size_t i = 0, j = 0, k = 0, length = 0;
const size_t *shiftx = NULL, *shifty = NULL;
int hyperbolic = 0;
switch (coord) {
case CORDIC_COORD_CIRCULAR_E:
lookup = arctans;
length = arctans_length;
i = 0;
shifty = &i;
shiftx = &i;
break;
case CORDIC_COORD_HYPERBOLIC_E:
lookup = arctanhs;
length = arctanhs_length;
hyperbolic = 1;
i = 1;
shifty = &i;
shiftx = &i;
break;
case CORDIC_COORD_LINEAR_E:
lookup = halfs;
length = halfs_length;
shifty = &j;
shiftx = NULL;
i = 1;
break;
default: /* not implemented */
return -2;
}
iterations = iterations > (int)length ? (int)length : iterations;
iterations = iterations < 0 ? (int)length : iterations;
d_t x = *x0, y = *y0, z = *z0;
/* rotation mode: z determines direction,
* vector mode: y determines direction */
for (; j < (unsigned)iterations; i++, j++) {
again:
{
const d_t m = mode == CORDIC_MODE_ROTATE_E ? z : -y /*-mulsign(x, y)*/;
const d_t d = -!!(m < 0);
const d_t xs = ((((shiftx ? divn(y, *shiftx) : 0)) ^ d) - d);
const d_t ys = (divn(x, *shifty) ^ d) - d;
const d_t xn = x - (hyperbolic ? -xs : xs);
const d_t yn = y + ys;
const d_t zn = z - ((lookup[j] ^ d) - d);
x = xn; /* cosine, in circular, rotation mode */
y = yn; /* sine, in circular, rotation mode */
z = zn;
}
if (hyperbolic) { /* Experimental/Needs bug fixing */
switch (1) { // TODO: Correct hyperbolic redo of iteration
case 0: break;
case 1: if (k++ >= 3) { k = 0; goto again; } break;
case 2: {
assert(j <= 120);
size_t cmp = j + 1;
if (cmp == 4 || cmp == 13 /*|| cmp == 40 || cmp == 121 || cmp == floor(pow(3,i-1)/2) */) {
if (k) {
k = 0;
} else {
k = 1;
goto again;
}
}
break;
}
}
}
}
*x0 = x;
*y0 = y;
*z0 = z;
return iterations;
}
/* See: - <https://dspguru.com/dsp/faqs/cordic/>
* - <https://en.wikipedia.org/wiki/CORDIC> */
static int qcordic(q_t theta, const int iterations, q_t *sine, q_t *cosine) {
assert(sine);
assert(cosine);
static const q_t pi = QPI, npi = -QPI;
static const q_t hpi = QPI/2, hnpi = -(QPI/2);
static const q_t qpi = QPI/4, qnpi = -(QPI/4);
static const q_t dpi = QPI*2, dnpi = -(QPI*2);
/* Convert to range -pi to pi, we could use qmod,
* however that uses multiplication and division, and
* if we can use those operators freely then there are
* other, better algorithms we can use instead of CORDIC
* for sine/cosine calculation. */
while (qless(theta, npi)) theta = qadd(theta, dpi);
while (qmore(theta, pi)) theta = qadd(theta, dnpi);
int negate = 0, shift = 0;
/* convert to range -pi/2 to pi/2 */
if (qless(theta, hnpi)) {
theta = qadd(theta, pi);
negate = 1;
} else if (qmore(theta, hpi)) {
theta = qadd(theta, npi);
negate = 1;
}
/* convert to range -pi/4 to pi/4 */
if (qless(theta, qnpi)) {
theta = qadd(theta, hpi);
shift = -1;
} else if (qmore(theta, qpi)) {
theta = qadd(theta, hnpi);
shift = 1;
}
d_t x = cordic_circular_inverse_scaling, y = 0, z = theta /* no theta scaling needed */;
/* CORDIC in Q2.16 format */
if (cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, iterations, &x, &y, &z) < 0)
return -1;
/* undo shifting and quadrant changes */
if (shift > 0) {
const d_t yt = y;
y = x;
x = -yt;
} else if (shift < 0) {
const d_t yt = y;
y = -x;
x = yt;
}
if (negate) {
x = -x;
y = -y;
}
/* set output; no scaling needed */
*cosine = x;
*sine = y;
return 0;
}
q_t qatan(const q_t t) {
q_t x = qint(1), y = t, z = QINT(0);
cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
return z;
}
q_t qatan2(const q_t a, const q_t b) {
q_t x = b, y = a, z = QINT(0);
if (qequal(b, QINT(0))) {
assert(qunequal(a, QINT(0)));
if (qmore(a, QINT(0)))
return QPI/2;
return -(QPI/2);
} else if (qless(b, QINT(0))) {
if (qeqmore(a, QINT(0)))
return qadd(qatan(qdiv(a, b)), QPI);
return qsub(qatan(qdiv(a, b)), QPI);
}
cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
return z;
}
void qsincos(q_t theta, q_t *sine, q_t *cosine) {
assert(sine);
assert(cosine);
const int r = qcordic(theta, -1, sine, cosine);
assert(r >= 0);
}
q_t qsin(const q_t theta) {
q_t sine = QINT(0), cosine = QINT(0);
qsincos(theta, &sine, &cosine);
return sine;
}
q_t qcos(const q_t theta) {
q_t sine = QINT(0), cosine = QINT(0);
qsincos(theta, &sine, &cosine);
return cosine;
}
q_t qtan(const q_t theta) {
q_t sine = QINT(0), cosine = QINT(0);
qsincos(theta, &sine, &cosine);
return qdiv(sine, cosine); /* can use qcordic_div, with range limits it imposes */
}
q_t qcot(const q_t theta) {
q_t sine = QINT(0), cosine = QINT(0);
qsincos(theta, &sine, &cosine);
return qdiv(cosine, sine); /* can use qcordic_div, with range limits it imposes */
}
q_t qcordic_mul(const q_t a, const q_t b) { /* works for small values; result < 4 */
q_t x = a, y = QINT(0), z = b;
const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
assert(r >= 0);
return y;
}
q_t qcordic_div(const q_t a, const q_t b) {
q_t x = b, y = a, z = QINT(0);
const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
assert(r >= 0);
return z;
}
void qsincosh(const q_t a, q_t *sinh, q_t *cosh) {
assert(sinh);
assert(cosh);
q_t x = cordic_hyperbolic_inverse_scaling, y = QINT(0), z = a; /* (e^2x - 1) / (e^2x + 1) */
const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
assert(r >= 0);
*sinh = y;
*cosh = x;
}
q_t qtanh(const q_t a) {
q_t sinh = QINT(0), cosh = QINT(0);
qsincosh(a, &sinh, &cosh);
return qdiv(sinh, cosh);
}
q_t qcosh(const q_t a) {
q_t sinh = QINT(0), cosh = QINT(0);
qsincosh(a, &sinh, &cosh);
return cosh;
}
q_t qsinh(const q_t a) {
q_t sinh = QINT(0), cosh = QINT(0);
qsincosh(a, &sinh, &cosh);
return sinh;
}
q_t qcordic_exp(const q_t e) {
q_t s = QINT(0), h = QINT(0);
qsincosh(e, &s, &h);
return qadd(s, h);
}
q_t qcordic_ln(const q_t d) {
q_t x = qadd(d, QINT(1)), y = qsub(d, QINT(1)), z = QINT(0);
const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
assert(r >= 0);
return qadd(z, z);
}
q_t qcordic_sqrt(const q_t n) { /* testing only; works for 0 < x < 2 */
const q_t quarter = 1uLL << (QBITS - 2); /* 0.25 */
q_t x = qadd(n, quarter),
y = qsub(n, quarter),
z = 0;
const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
assert(r >= 0);
return qmul(x, cordic_hyperbolic_inverse_scaling);
}
q_t qhypot(const q_t a, const q_t b) {
q_t x = qabs(a), y = qabs(b), z = QINT(0); /* abs() should not be needed? */
const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
assert(r >= 0);
return qmul(x, cordic_circular_inverse_scaling);
}
q_t qatanh(q_t x) {
assert(qabs(qless(x, QINT(1))));
return qmul(qlog(qdiv(qadd(QINT(1), x), qsub(QINT(1), x))), QMK(0, 0x8000, 16));
}
q_t qasinh(q_t x) {
return qlog(qadd(x, qsqrt(qadd(qmul(x, x), QINT(1)))));
}
q_t qacosh(q_t x) {
assert(qeqmore(x, QINT(1)));
return qlog(qadd(x, qsqrt(qsub(qmul(x, x), QINT(1)))));
}
void qpol2rec(const q_t magnitude, const q_t theta, q_t *i, q_t *j) {
assert(i);
assert(j);
q_t sin = QINT(0), cos = QINT(0);
qsincos(theta, &sin, &cos);
*i = qmul(sin, magnitude);
*j = qmul(cos, magnitude);
}
void qrec2pol(const q_t i, const q_t j, q_t *magnitude, q_t *theta) {
assert(magnitude);
assert(theta);
const int is = qisnegative(i), js = qisnegative(j);
q_t x = qabs(i), y = qabs(j), z = QINT(0);
const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
assert(r >= 0);
*magnitude = qmul(x, cordic_circular_inverse_scaling);
if (is && js)
z = qadd(z, QPI);
else if (js)
z = qadd(z, QPI/2l);
else if (is)
z = qadd(z, (3l*QPI)/2l);
*theta = z;
}
q_t qcordic_hyperbolic_gain(const int n) {
q_t x = QINT(1), y = QINT(0), z = QINT(0);
const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
assert(r >= 0);
return x;
}
q_t qcordic_circular_gain(const int n) {
q_t x = QINT(1), y = QINT(0), z = QINT(0);
const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
assert(r >= 0);
return x;
}
static inline int isodd(const unsigned n) {
return n & 1;
}
d_t dpower(d_t b, unsigned e) { /* https://stackoverflow.com/questions/101439 */
d_t result = 1;
for (;;) {
if (isodd(e))
result *= b;
e >>= 1;
if (!e)
break;
b *= b;
}
return result;
}
d_t dlog(d_t x, const unsigned base) { /* rounds up, look at remainder to round down */
d_t b = 0;
assert(x && base > 1);
while ((x /= (d_t)base)) /* can use >> for base that are powers of two */
b++;
return b;
}
q_t qlog(q_t x) {
q_t logs = 0;
assert(qmore(x, 0));
static const q_t lmax = QMK(9, 0x8000, 16); /* 9.5, lower limit needs checking */
for (; qmore(x, lmax); x = divn(x, 1))
logs = qadd(logs, qinfo.ln2);
return qadd(logs, qcordic_ln(x));
}
q_t qsqr(const q_t x) {
return qmul(x, x);
}
q_t qexp(const q_t e) { /* exp(e) = exp(e/2)*exp(e/2) */
if (qless(e, QINT(1))) /* 1.1268 is approximately the limit for qcordic_exp */
return qcordic_exp(e);
return qsqr(qexp(divn(e, 1)));
}
q_t qpow(q_t n, q_t exp) {
implies(qisnegative(n), qisinteger(exp));
implies(qequal(n, QINT(0)), qunequal(exp, QINT(0)));
if (qequal(QINT(0), n))
return QINT(1);
if (qisnegative(n)) {
const q_t abspow = qpow(qabs(n), exp);
return qisodd(exp) ? qnegate(abspow) : abspow;
}
if (qisnegative(exp))
return qdiv(QINT(1), qpow(n, qabs(exp)));
return qexp(multiply(qlog(n), exp));
}
q_t qsqrt(const q_t x) { /* Newton-Rhaphson method */
assert(qeqmore(x, 0));
const q_t difference = qmore(x, QINT(100)) ? 0x0100 : 0x0010;
if (qequal(QINT(0), x))
return QINT(0);
q_t guess = qmore(x, qinfo.sqrt2) ? divn(x, 1) : QINT(1);
while (qmore(qabs(qsub(qmul(guess, guess), x)), difference))
guess = divn(qadd(qdiv(x, guess), guess), 1);
return qabs(guess); /* correct for overflow int very large numbers */
}
q_t qasin(const q_t t) {
assert(qless(qabs(t), QINT(1)));
/* can also use: return qatan(qdiv(t, qsqrt(qsub(QINT(1), qmul(t, t))))); */
return qatan2(t, qsqrt(qsub(QINT(1), qmul(t, t))));
}
q_t qacos(const q_t t) {
assert(qeqless(qabs(t), QINT(1)));
/* can also use: return qatan(qdiv(qsqrt(qsub(QINT(1), qmul(t, t))), t)); */
return qatan2(qsqrt(qsub(QINT(1), qmul(t, t))), t);
}
q_t qdeg2rad(const q_t deg) {
return qdiv(qmul(QPI, deg), QINT(180));
}
q_t qrad2deg(const q_t rad) {
return qdiv(qmul(QINT(180), rad), QPI);
}
void qfilter_init(qfilter_t *f, const q_t time, const q_t rc, const q_t seed) {
assert(f);
memset(f, 0, sizeof(*f));
f->time = time;
f->rc = rc;
f->filtered = seed; /* alpha * seed for LPF */
f->raw = seed;
}
q_t qfilter_low_pass(qfilter_t *f, const q_t time, const q_t data) {
assert(f);
/* If the calling rate is constant (for example the function is
* guaranteed to be always called at a rate of 5 milliseconds) we
* can avoid the costly alpha calculation! */
const q_t dt = (u_t)time - (u_t)f->time;
const q_t alpha = qdiv(dt, qadd(f->rc, dt));
f->filtered = qfma(alpha, qsub(data, f->filtered), f->filtered);
f->time = time;
f->raw = data;
return f->filtered;
}
q_t qfilter_high_pass(qfilter_t *f, const q_t time, const q_t data) {
assert(f);
const q_t dt = (u_t)time - (u_t)f->time;
const q_t alpha = qdiv(f->rc, qadd(f->rc, dt));
f->filtered = qmul(alpha, qadd(f->filtered, qsub(data, f->raw)));
f->time = time;
f->raw = data;
return f->filtered;
}
q_t qfilter_value(const qfilter_t *f) {
assert(f);