Outline ![]()
Files ![]()
loading...
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
/*
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <math.h>
#include "pico/double.h"
// opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
GCC_Pragma("GCC diagnostic push")
GCC_Pragma("GCC diagnostic ignored \"-Wconversion\"")
GCC_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")
typedef uint64_t ui64;
typedef uint32_t ui32;
typedef int64_t i64;
#define PINF ( HUGE_VAL)
#define MINF (-HUGE_VAL)
#define PZERO (+0.0)
#define MZERO (-0.0)
#define PI 3.14159265358979323846
#define LOG2 0.69314718055994530941
// Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
#define LOG10 2.30258509299404568401
#define LOG2E 1.44269504088896340737
#define LOG10E 0.43429448190325182765
#define ONETHIRD 0.33333333333333333333
#define PIf 3.14159265358979323846f
#define LOG2f 0.69314718055994530941f
#define LOG2Ef 1.44269504088896340737f
#define LOG10Ef 0.43429448190325182765f
#define ONETHIRDf 0.33333333333333333333f
#define DUNPACK(x,e,m) e=((x)>>52)&0x7ff,m=((x)&0x000fffffffffffffULL)|0x0010000000000000ULL
#define DUNPACKS(x,s,e,m) s=((x)>>63),DUNPACK((x),(e),(m))
typedef union {
double d;
ui64 ix;
} double_ui64;
static inline double ui642double(ui64 ix) {
double_ui64 tmp;
tmp.ix = ix;
return tmp.d;
}
static inline ui64 double2ui64(double d) {
double_ui64 tmp;
tmp.d = d;
return tmp.ix;
}
#if PICO_DOUBLE_PROPAGATE_NANS
static inline bool disnan(double x) {
ui64 ix= double2ui64(x);
// checks the top bit of the low 32 bit of the NAN, but it I think that is ok
return ((uint32_t)(ix >> 31)) > 0xffe00000u;
}
#define check_nan_d1(x) if (disnan((x))) return (x)
#define check_nan_d2(x,y) if (disnan((x))) return (x); else if (disnan((y))) return (y);
#else
#define check_nan_d1(x) ((void)0)
#define check_nan_d2(x,y) ((void)0)
#endif
static inline int dgetsignexp(double x) {
ui64 ix=double2ui64(x);
return (ix>>52)&0xfff;
}
static inline int dgetexp(double x) {
ui64 ix=double2ui64(x);
return (ix>>52)&0x7ff;
}
static inline double dldexp(double x,int de) {
ui64 ix=double2ui64(x),iy;
int e;
e=dgetexp(x);
if(e==0||e==0x7ff) return x;
e+=de;
if(e<=0) iy=ix&0x8000000000000000ULL; // signed zero for underflow
else if(e>=0x7ff) iy=(ix&0x8000000000000000ULL)|0x7ff0000000000000ULL; // signed infinity on overflow
else iy=ix+((ui64)de<<52);
return ui642double(iy);
}
double WRAPPER_FUNC(ldexp)(double x, int de) {
check_nan_d1(x);
return dldexp(x, de);
}
static inline double dcopysign(double x,double y) {
ui64 ix=double2ui64(x),iy=double2ui64(y);
ix=((ix&0x7fffffffffffffffULL)|(iy&0x8000000000000000ULL));
return ui642double(ix);
}
double WRAPPER_FUNC(copysign)(double x, double y) {
check_nan_d2(x,y);
return dcopysign(x, y);
}
static inline int diszero(double x) { return dgetexp (x)==0; }
//static inline int dispzero(double x) { return dgetsignexp(x)==0; }
//static inline int dismzero(double x) { return dgetsignexp(x)==0x800; }
static inline int disinf(double x) { return dgetexp (x)==0x7ff; }
static inline int dispinf(double x) { return dgetsignexp(x)==0x7ff; }
static inline int disminf(double x) { return dgetsignexp(x)==0xfff; }
static inline int disint(double x) {
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
if(e==0) return 1; // 0 is an integer
e-=0x3ff; // remove exponent bias
if(e<0) return 0; // |x|<1
e=52-e; // bit position in mantissa with significance 1
if(e<=0) return 1; // |x| large, so must be an integer
m=(1ULL<<e)-1; // mask for bits of significance <1
if(ix&m) return 0; // not an integer
return 1;
}
static inline int disoddint(double x) {
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
e-=0x3ff; // remove exponent bias
if(e<0) return 0; // |x|<1; 0 is not odd
e=52-e; // bit position in mantissa with significance 1
if(e<0) return 0; // |x| large, so must be even
m=(1ULL<<e)-1; // mask for bits of significance <1 (if any)
if(ix&m) return 0; // not an integer
if(e==52) return 1; // value is exactly 1
return (ix>>e)&1;
}
static inline int disstrictneg(double x) {
ui64 ix=double2ui64(x);
if(diszero(x)) return 0;
return ix>>63;
}
static inline int disneg(double x) {
ui64 ix=double2ui64(x);
return ix>>63;
}
static inline double dneg(double x) {
ui64 ix=double2ui64(x);
ix^=0x8000000000000000ULL;
return ui642double(ix);
}
static inline int dispo2(double x) {
ui64 ix=double2ui64(x);
if(diszero(x)) return 0;
if(disinf(x)) return 0;
ix&=0x000fffffffffffffULL;
return ix==0;
}
static inline double dnan_or(double x) {
#if PICO_DOUBLE_PROPAGATE_NANS
return NAN;
#else
return x;
#endif
}
double WRAPPER_FUNC(trunc)(double x) {
check_nan_d1(x);
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
e-=0x3ff; // remove exponent bias
if(e<0) { // |x|<1
ix&=0x8000000000000000ULL;
return ui642double(ix);
}
e=52-e; // bit position in mantissa with significance 1
if(e<=0) return x; // |x| large, so must be an integer
m=(1ULL<<e)-1; // mask for bits of significance <1
ix&=~m;
return ui642double(ix);
}
double WRAPPER_FUNC(round)(double x) {
check_nan_d1(x);
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
e-=0x3ff; // remove exponent bias
if(e<-1) { // |x|<0.5
ix&=0x8000000000000000ULL;
return ui642double(ix);
}
if(e==-1) { // 0.5<=|x|<1
ix&=0x8000000000000000ULL;
ix|=0x3ff0000000000000ULL; // ±1
return ui642double(ix);
}
e=52-e; // bit position in mantissa with significance 1, <=52
if(e<=0) return x; // |x| large, so must be an integer
m=1ULL<<(e-1); // mask for bit of significance 0.5
ix+=m;
m=m+m-1; // mask for bits of significance <1
ix&=~m;
return ui642double(ix);
}
double WRAPPER_FUNC(floor)(double x) {
check_nan_d1(x);
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
if(e==0) { // x==0
ix&=0x8000000000000000ULL;
return ui642double(ix);
}
e-=0x3ff; // remove exponent bias
if(e<0) { // |x|<1, not zero
if(disneg(x)) return -1;
return PZERO;
}
e=52-e; // bit position in mantissa with significance 1
if(e<=0) return x; // |x| large, so must be an integer
m=(1ULL<<e)-1; // mask for bit of significance <1
if(disneg(x)) ix+=m; // add 1-ε to magnitude if negative
ix&=~m; // truncate
return ui642double(ix);
}
double WRAPPER_FUNC(ceil)(double x) {
check_nan_d1(x);
ui64 ix=double2ui64(x),m;
int e=dgetexp(x);
if(e==0) { // x==0
ix&=0x8000000000000000ULL;
return ui642double(ix);
}
e-=0x3ff; // remove exponent bias
if(e<0) { // |x|<1, not zero
if(disneg(x)) return MZERO;
return 1;
}
e=52-e; // bit position in mantissa with significance 1
if(e<=0) return x; // |x| large, so must be an integer
m=(1ULL<<e)-1; // mask for bit of significance <1
if(!disneg(x)) ix+=m; // add 1-ε to magnitude if positive
ix&=~m; // truncate
return ui642double(ix);
}
double WRAPPER_FUNC(asin)(double x) {
check_nan_d1(x);
double u;
u=(1-x)*(1+x);
if(disstrictneg(u)) return dnan_or(PINF);
return atan2(x,sqrt(u));
}
double WRAPPER_FUNC(acos)(double x) {
check_nan_d1(x);
double u;
u=(1-x)*(1+x);
if(disstrictneg(u)) return dnan_or(PINF);
return atan2(sqrt(u),x);
}
double WRAPPER_FUNC(atan)(double x) {
check_nan_d1(x);
if(dispinf(x)) return PI/2;
if(disminf(x)) return -PI/2;
return atan2(x,1);
}
double WRAPPER_FUNC(sinh)(double x) {
check_nan_d1(x);
return dldexp((exp(x)-exp(dneg(x))),-1);
}
double WRAPPER_FUNC(cosh)(double x) {
check_nan_d1(x);
return dldexp((exp(x)+exp(dneg(x))),-1);
}
double WRAPPER_FUNC(tanh)(double x) {
check_nan_d1(x);
double u;
int e;
e=dgetexp(x);
if(e>=5+0x3ff) { // |x|>=32?
if(!disneg(x)) return 1; // 1 << exp 2x; avoid generating infinities later
else return -1; // 1 >> exp 2x
}
u=exp(dldexp(x,1));
return (u-1)/(u+1);
}
double WRAPPER_FUNC(asinh)(double x) {
check_nan_d1(x);
int e;
e=dgetexp(x);
if(e>=32+0x3ff) { // |x|>=2^32?
if(!disneg(x)) return log( x )+LOG2; // 1/x^2 << 1
else return dneg(log(dneg(x))+LOG2); // 1/x^2 << 1
}
if(x>0) return log(sqrt(x*x+1)+x);
else return dneg(log(sqrt(x*x+1)-x));
}
double WRAPPER_FUNC(acosh)(double x) {
check_nan_d1(x);
int e;
if(disneg(x)) x=dneg(x);
e=dgetexp(x);
if(e>=32+0x3ff) return log(x)+LOG2; // |x|>=2^32?
return log(sqrt((x-1)*(x+1))+x);
}
double WRAPPER_FUNC(atanh)(double x) {
check_nan_d1(x);
return dldexp(log((1+x)/(1-x)),-1);
}
double WRAPPER_FUNC(exp2)(double x) {
check_nan_d1(x);
int e;
// extra check for disminf as this catches -Nan, and x<=-4096 doesn't.
if (disminf(x) || x<=-4096) return 0; // easily underflows
else if (x>=4096) return PINF; // easily overflows
e=(int)round(x);
x-=e;
return dldexp(exp(x*LOG2),e);
}
double WRAPPER_FUNC(log2)(double x) { check_nan_d1(x); return log(x)*LOG2E; }
double WRAPPER_FUNC(exp10)(double x) { check_nan_d1(x); return pow(10,x); }
double WRAPPER_FUNC(log10)(double x) { check_nan_d1(x); return log(x)*LOG10E; }
// todo these are marked as lofi
double WRAPPER_FUNC(expm1)(double x) { check_nan_d1(x); return exp(x)-1; }
double WRAPPER_FUNC(log1p)(double x) { check_nan_d1(x); return log(1+x); }
#if !HAS_DOUBLE_COPROCESSOR
double WRAPPER_FUNC(fma)(double x,double y,double z) { check_nan_d1(x); return x*y+z; }
#endif
// general power, x>0, finite
static double dpow_1(double x,double y) {
int a,b,c;
double t,rt,u,v,v0,v1,w,ry;
a=dgetexp(x)-0x3ff;
u=log2(dldexp(x,-a)); // now log_2 x = a+u
if(u>0.5) u-=1,a++; // |u|<=~0.5
if(a==0) return exp2(u*y);
// here |log_2 x| >~0.5
if(y>= 4096) { // then easily over/underflows
if(a<0) return 0;
return PINF;
}
if(y<=-4096) { // then easily over/underflows
if(a<0) return PINF;
return 0;
}
ry=round(y);
v=y-ry;
v0=dldexp(round(ldexp(v,26)),-26);
v1=v-v0;
b=(int)ry; // guaranteed to fit in an int; y=b+v0+v1
// now the result is exp2( (a+u) * (b+v0+v1) )
c=a*b; // integer
t=a*v0;
rt=round(t);
c+=(int)rt;
w=t-rt;
t=a*v1;
w+=t;
t=u*b;
rt=round(t);
c+=(int)rt;
w+=t-rt;
w+=u*v;
return dldexp(exp2(w),c);
}
static double dpow_int2(double x,int y) {
double u;
if(y==1) return x;
u=dpow_int2(x,y/2);
u*=u;
if(y&1) u*=x;
return u;
}
// for the case where x not zero or infinity, y small and not zero
static inline double dpowint_1(double x,int y) {
if(y<0) x=1/x,y=-y;
return dpow_int2(x,y);
}
// for the case where x not zero or infinity
static double dpowint_0(double x,int y) {
int e;
if(disneg(x)) {
if(disoddint(y)) return dneg(dpowint_0(dneg(x),y));
else return dpowint_0(dneg(x),y);
}
if(dispo2(x)) {
e=dgetexp(x)-0x3ff;
if(y>=2048) y= 2047; // avoid overflow
if(y<-2048) y=-2048;
y*=e;
return dldexp(1,y);
}
if(y==0) return 1;
if(y>=-32&&y<=32) return dpowint_1(x,y);
return dpow_1(x,y);
}
double WRAPPER_FUNC(powint)(double x,int y) {
GCC_Like_Pragma("GCC diagnostic push")
GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
if(x==1.0||y==0) return 1;
GCC_Like_Pragma("GCC diagnostic pop")
check_nan_d1(x);
if(diszero(x)) {
if(y>0) {
if(y&1) return x;
else return 0;
}
if((y&1)) return dcopysign(PINF,x);
return PINF;
}
if(dispinf(x)) {
if(y<0) return 0;
else return PINF;
}
if(disminf(x)) {
if(y>0) {
if((y&1)) return MINF;
else return PINF;
}
if((y&1)) return MZERO;
else return PZERO;
}
return dpowint_0(x,y);
}
// for the case where y is guaranteed a finite integer, x not zero or infinity
static double dpow_0(double x,double y) {
int e,p;
if(disneg(x)) {
if(disoddint(y)) return dneg(dpow_0(dneg(x),y));
else return dpow_0(dneg(x),y);
}
p=(int)y;
if(dispo2(x)) {
e=dgetexp(x)-0x3ff;
if(p>=2048) p= 2047; // avoid overflow
if(p<-2048) p=-2048;
p*=e;
return dldexp(1,p);
}
if(p==0) return 1;
if(p>=-32&&p<=32) return dpowint_1(x,p);
return dpow_1(x,y);
}
double WRAPPER_FUNC(pow)(double x,double y) {
GCC_Like_Pragma("GCC diagnostic push")
GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
if(x==1.0||diszero(y)) return 1;
check_nan_d2(x, y);
if(x==-1.0&&disinf(y)) return 1;
GCC_Like_Pragma("GCC diagnostic pop")
if(diszero(x)) {
if(!disneg(y)) {
if(disoddint(y)) return x;
else return 0;
}
if(disoddint(y)) return dcopysign(PINF,x);
return PINF;
}
if(dispinf(x)) {
if(disneg(y)) return 0;
else return PINF;
}
if(disminf(x)) {
if(!disneg(y)) {
if(disoddint(y)) return MINF;
else return PINF;
}
if(disoddint(y)) return MZERO;
else return PZERO;
}
if(dispinf(y)) {
if(dgetexp(x)<0x3ff) return PZERO;
else return PINF;
}
if(disminf(y)) {
if(dgetexp(x)<0x3ff) return PINF;
else return PZERO;
}
if(disint(y)) return dpow_0(x,y);
if(disneg(x)) return PINF;
return dpow_1(x,y);
}
double WRAPPER_FUNC(hypot)(double x,double y) {
check_nan_d2(x, y);
int ex,ey;
ex=dgetexp(x); ey=dgetexp(y);
if(ex>=0x3ff+400||ey>=0x3ff+400) { // overflow, or nearly so
x=dldexp(x,-600),y=dldexp(y,-600);
return dldexp(sqrt(x*x+y*y), 600);
}
else if(ex<=0x3ff-400&&ey<=0x3ff-400) { // underflow, or nearly so
x=dldexp(x, 600),y=dldexp(y, 600);
return dldexp(sqrt(x*x+y*y),-600);
}
return sqrt(x*x+y*y);
}
double WRAPPER_FUNC(cbrt)(double x) {
check_nan_d1(x);
int e;
if(disneg(x)) return dneg(cbrt(dneg(x)));
if(diszero(x)) return dcopysign(PZERO,x);
e=dgetexp(x)-0x3ff;
e=(e*0x5555+0x8000)>>16; // ~e/3, rounded
x=dldexp(x,-e*3);
x=exp(log(x)*ONETHIRD);
return dldexp(x,e);
}
// reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
// 2^52<=|mx|,my<2^53, e>=0; 0<=result<my
static i64 drem_0(i64 mx,i64 my,int e,int*pquo) {
int quo=0,q,r=0,s;
if(e>0) {
r=0xffffffffU/(ui32)(my>>36); // reciprocal estimate Q16
}
while(e>0) {
s=e; if(s>12) s=12; // gain up to 12 bits on each iteration
q=(mx>>38)*r; // Q30
q=((q>>(29-s))+1)>>1; // Q(s), rounded
mx=(mx<<s)-my*q;
quo=(quo<<s)+q;
e-=s;
}
if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
if(mx>=my) mx-=my,quo++;
if(mx<0) mx+=my,quo--;
if(mx<0) mx+=my,quo--;
if(pquo) *pquo=quo;
return mx;
}
double WRAPPER_FUNC(fmod)(double x,double y) {
check_nan_d2(x, y);
ui64 ix=double2ui64(x),iy=double2ui64(y);
int sx,ex,ey;
i64 mx,my;
DUNPACKS(ix,sx,ex,mx);
DUNPACK(iy,ey,my);
if(ex==0x7ff) return dnan_or(PINF);
if(ey==0) return PINF;
if(ex==0) {
if(!disneg(x)) return PZERO;
return MZERO;
}
if(ex<ey) return x; // |x|<|y|, including case x=±0
mx=drem_0(mx,my,ex-ey,0);
if(sx) mx=-mx;
return fix642double(mx,0x3ff-ey+52);
}
double WRAPPER_FUNC(remquo)(double x,double y,int*quo) {
check_nan_d2(x, y);
ui64 ix=double2ui64(x),iy=double2ui64(y);
int sx,sy,ex,ey,q;
i64 mx,my;
DUNPACKS(ix,sx,ex,mx);
DUNPACKS(iy,sy,ey,my);
if(quo) *quo=0;
if(ex==0x7ff) return PINF;
if(ey==0) return PINF;
if(ex==0) return PZERO;
if(ey==0x7ff) return x;
if(ex<ey-1) return x; // |x|<|y|/2
if(ex==ey-1) {
if(mx<=my) return x; // |x|<=|y|/2, even quotient
// here |y|/2<|x|<|y|
if(!sx) { // x>|y|/2
mx-=my+my;
ey--;
q=1;
} else { // x<-|y|/2
mx=my+my-mx;
ey--;
q=-1;
}
}
else {
if(sx) mx=-mx;
mx=drem_0(mx,my,ex-ey,&q);
if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
mx-=my;
q++;
}
}
if(sy) q=-q;
if(quo) *quo=q;
return fix642double(mx,0x3ff-ey+52);
}
double WRAPPER_FUNC(drem)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
double WRAPPER_FUNC(remainder)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
GCC_Pragma("GCC diagnostic pop") // conversion
Details ![]()
Show: from Types: Columns: ![]( "Show Project Names")
![]( "Show Locations")
![]( "Show Referring Symbols")
![]( "Show Scope")
This file uses the notable symbols shown below. Click anywhere in the file to view more details.