1 /*
2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <math.h>
8 #include "pico/double.h"
9
10 // opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
11 GCC_Pragma("GCC diagnostic push")
12 GCC_Pragma("GCC diagnostic ignored \"-Wconversion\"")
13 GCC_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")
14
15 typedef uint64_t ui64;
16 typedef uint32_t ui32;
17 typedef int64_t i64;
18
19 #define PINF ( HUGE_VAL)
20 #define MINF (-HUGE_VAL)
21 #define PZERO (+0.0)
22 #define MZERO (-0.0)
23
24
25 #define PI 3.14159265358979323846
26 #define LOG2 0.69314718055994530941
27 // Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
28 #define LOG10 2.30258509299404568401
29 #define LOG2E 1.44269504088896340737
30 #define LOG10E 0.43429448190325182765
31 #define ONETHIRD 0.33333333333333333333
32
33 #define PIf 3.14159265358979323846f
34 #define LOG2f 0.69314718055994530941f
35 #define LOG2Ef 1.44269504088896340737f
36 #define LOG10Ef 0.43429448190325182765f
37 #define ONETHIRDf 0.33333333333333333333f
38
39 #define DUNPACK(x,e,m) e=((x)>>52)&0x7ff,m=((x)&0x000fffffffffffffULL)|0x0010000000000000ULL
40 #define DUNPACKS(x,s,e,m) s=((x)>>63),DUNPACK((x),(e),(m))
41
42 typedef union {
43 double d;
44 ui64 ix;
45 } double_ui64;
46
ui642double(ui64 ix)47 static inline double ui642double(ui64 ix) {
48 double_ui64 tmp;
49 tmp.ix = ix;
50 return tmp.d;
51 }
52
double2ui64(double d)53 static inline ui64 double2ui64(double d) {
54 double_ui64 tmp;
55 tmp.d = d;
56 return tmp.ix;
57 }
58
59 #if PICO_DOUBLE_PROPAGATE_NANS
disnan(double x)60 static inline bool disnan(double x) {
61 ui64 ix= double2ui64(x);
62 // checks the top bit of the low 32 bit of the NAN, but it I think that is ok
63 return ((uint32_t)(ix >> 31)) > 0xffe00000u;
64 }
65
66 #define check_nan_d1(x) if (disnan((x))) return (x)
67 #define check_nan_d2(x,y) if (disnan((x))) return (x); else if (disnan((y))) return (y);
68 #else
69 #define check_nan_d1(x) ((void)0)
70 #define check_nan_d2(x,y) ((void)0)
71 #endif
72
dgetsignexp(double x)73 static inline int dgetsignexp(double x) {
74 ui64 ix=double2ui64(x);
75 return (ix>>52)&0xfff;
76 }
77
dgetexp(double x)78 static inline int dgetexp(double x) {
79 ui64 ix=double2ui64(x);
80 return (ix>>52)&0x7ff;
81 }
82
dldexp(double x,int de)83 static inline double dldexp(double x,int de) {
84 ui64 ix=double2ui64(x),iy;
85 int e;
86 e=dgetexp(x);
87 if(e==0||e==0x7ff) return x;
88 e+=de;
89 if(e<=0) iy=ix&0x8000000000000000ULL; // signed zero for underflow
90 else if(e>=0x7ff) iy=(ix&0x8000000000000000ULL)|0x7ff0000000000000ULL; // signed infinity on overflow
91 else iy=ix+((ui64)de<<52);
92 return ui642double(iy);
93 }
94
WRAPPER_FUNC(ldexp)95 double WRAPPER_FUNC(ldexp)(double x, int de) {
96 check_nan_d1(x);
97 return dldexp(x, de);
98 }
99
100
dcopysign(double x,double y)101 static inline double dcopysign(double x,double y) {
102 ui64 ix=double2ui64(x),iy=double2ui64(y);
103 ix=((ix&0x7fffffffffffffffULL)|(iy&0x8000000000000000ULL));
104 return ui642double(ix);
105 }
106
WRAPPER_FUNC(copysign)107 double WRAPPER_FUNC(copysign)(double x, double y) {
108 check_nan_d2(x,y);
109 return dcopysign(x, y);
110 }
diszero(double x)111 static inline int diszero(double x) { return dgetexp (x)==0; }
112 //static inline int dispzero(double x) { return dgetsignexp(x)==0; }
113 //static inline int dismzero(double x) { return dgetsignexp(x)==0x800; }
disinf(double x)114 static inline int disinf(double x) { return dgetexp (x)==0x7ff; }
dispinf(double x)115 static inline int dispinf(double x) { return dgetsignexp(x)==0x7ff; }
disminf(double x)116 static inline int disminf(double x) { return dgetsignexp(x)==0xfff; }
117
disint(double x)118 static inline int disint(double x) {
119 ui64 ix=double2ui64(x),m;
120 int e=dgetexp(x);
121 if(e==0) return 1; // 0 is an integer
122 e-=0x3ff; // remove exponent bias
123 if(e<0) return 0; // |x|<1
124 e=52-e; // bit position in mantissa with significance 1
125 if(e<=0) return 1; // |x| large, so must be an integer
126 m=(1ULL<<e)-1; // mask for bits of significance <1
127 if(ix&m) return 0; // not an integer
128 return 1;
129 }
130
disoddint(double x)131 static inline int disoddint(double x) {
132 ui64 ix=double2ui64(x),m;
133 int e=dgetexp(x);
134 e-=0x3ff; // remove exponent bias
135 if(e<0) return 0; // |x|<1; 0 is not odd
136 e=52-e; // bit position in mantissa with significance 1
137 if(e<0) return 0; // |x| large, so must be even
138 m=(1ULL<<e)-1; // mask for bits of significance <1 (if any)
139 if(ix&m) return 0; // not an integer
140 if(e==52) return 1; // value is exactly 1
141 return (ix>>e)&1;
142 }
143
disstrictneg(double x)144 static inline int disstrictneg(double x) {
145 ui64 ix=double2ui64(x);
146 if(diszero(x)) return 0;
147 return ix>>63;
148 }
149
disneg(double x)150 static inline int disneg(double x) {
151 ui64 ix=double2ui64(x);
152 return ix>>63;
153 }
154
dneg(double x)155 static inline double dneg(double x) {
156 ui64 ix=double2ui64(x);
157 ix^=0x8000000000000000ULL;
158 return ui642double(ix);
159 }
160
dispo2(double x)161 static inline int dispo2(double x) {
162 ui64 ix=double2ui64(x);
163 if(diszero(x)) return 0;
164 if(disinf(x)) return 0;
165 ix&=0x000fffffffffffffULL;
166 return ix==0;
167 }
168
dnan_or(double x)169 static inline double dnan_or(double x) {
170 #if PICO_DOUBLE_PROPAGATE_NANS
171 return NAN;
172 #else
173 return x;
174 #endif
175 }
176
WRAPPER_FUNC(trunc)177 double WRAPPER_FUNC(trunc)(double x) {
178 check_nan_d1(x);
179 ui64 ix=double2ui64(x),m;
180 int e=dgetexp(x);
181 e-=0x3ff; // remove exponent bias
182 if(e<0) { // |x|<1
183 ix&=0x8000000000000000ULL;
184 return ui642double(ix);
185 }
186 e=52-e; // bit position in mantissa with significance 1
187 if(e<=0) return x; // |x| large, so must be an integer
188 m=(1ULL<<e)-1; // mask for bits of significance <1
189 ix&=~m;
190 return ui642double(ix);
191 }
192
WRAPPER_FUNC(round)193 double WRAPPER_FUNC(round)(double x) {
194 check_nan_d1(x);
195 ui64 ix=double2ui64(x),m;
196 int e=dgetexp(x);
197 e-=0x3ff; // remove exponent bias
198 if(e<-1) { // |x|<0.5
199 ix&=0x8000000000000000ULL;
200 return ui642double(ix);
201 }
202 if(e==-1) { // 0.5<=|x|<1
203 ix&=0x8000000000000000ULL;
204 ix|=0x3ff0000000000000ULL; // ±1
205 return ui642double(ix);
206 }
207 e=52-e; // bit position in mantissa with significance 1, <=52
208 if(e<=0) return x; // |x| large, so must be an integer
209 m=1ULL<<(e-1); // mask for bit of significance 0.5
210 ix+=m;
211 m=m+m-1; // mask for bits of significance <1
212 ix&=~m;
213 return ui642double(ix);
214 }
215
WRAPPER_FUNC(floor)216 double WRAPPER_FUNC(floor)(double x) {
217 check_nan_d1(x);
218 ui64 ix=double2ui64(x),m;
219 int e=dgetexp(x);
220 if(e==0) { // x==0
221 ix&=0x8000000000000000ULL;
222 return ui642double(ix);
223 }
224 e-=0x3ff; // remove exponent bias
225 if(e<0) { // |x|<1, not zero
226 if(disneg(x)) return -1;
227 return PZERO;
228 }
229 e=52-e; // bit position in mantissa with significance 1
230 if(e<=0) return x; // |x| large, so must be an integer
231 m=(1ULL<<e)-1; // mask for bit of significance <1
232 if(disneg(x)) ix+=m; // add 1-ε to magnitude if negative
233 ix&=~m; // truncate
234 return ui642double(ix);
235 }
236
WRAPPER_FUNC(ceil)237 double WRAPPER_FUNC(ceil)(double x) {
238 check_nan_d1(x);
239 ui64 ix=double2ui64(x),m;
240 int e=dgetexp(x);
241 if(e==0) { // x==0
242 ix&=0x8000000000000000ULL;
243 return ui642double(ix);
244 }
245 e-=0x3ff; // remove exponent bias
246 if(e<0) { // |x|<1, not zero
247 if(disneg(x)) return MZERO;
248 return 1;
249 }
250 e=52-e; // bit position in mantissa with significance 1
251 if(e<=0) return x; // |x| large, so must be an integer
252 m=(1ULL<<e)-1; // mask for bit of significance <1
253 if(!disneg(x)) ix+=m; // add 1-ε to magnitude if positive
254 ix&=~m; // truncate
255 return ui642double(ix);
256 }
257
WRAPPER_FUNC(asin)258 double WRAPPER_FUNC(asin)(double x) {
259 check_nan_d1(x);
260 double u;
261 u=(1-x)*(1+x);
262 if(disstrictneg(u)) return dnan_or(PINF);
263 return atan2(x,sqrt(u));
264 }
265
WRAPPER_FUNC(acos)266 double WRAPPER_FUNC(acos)(double x) {
267 check_nan_d1(x);
268 double u;
269 u=(1-x)*(1+x);
270 if(disstrictneg(u)) return dnan_or(PINF);
271 return atan2(sqrt(u),x);
272 }
273
WRAPPER_FUNC(atan)274 double WRAPPER_FUNC(atan)(double x) {
275 check_nan_d1(x);
276 if(dispinf(x)) return PI/2;
277 if(disminf(x)) return -PI/2;
278 return atan2(x,1);
279 }
280
WRAPPER_FUNC(sinh)281 double WRAPPER_FUNC(sinh)(double x) {
282 check_nan_d1(x);
283 return dldexp((exp(x)-exp(dneg(x))),-1);
284 }
285
WRAPPER_FUNC(cosh)286 double WRAPPER_FUNC(cosh)(double x) {
287 check_nan_d1(x);
288 return dldexp((exp(x)+exp(dneg(x))),-1);
289 }
290
WRAPPER_FUNC(tanh)291 double WRAPPER_FUNC(tanh)(double x) {
292 check_nan_d1(x);
293 double u;
294 int e;
295 e=dgetexp(x);
296 if(e>=5+0x3ff) { // |x|>=32?
297 if(!disneg(x)) return 1; // 1 << exp 2x; avoid generating infinities later
298 else return -1; // 1 >> exp 2x
299 }
300 u=exp(dldexp(x,1));
301 return (u-1)/(u+1);
302 }
303
WRAPPER_FUNC(asinh)304 double WRAPPER_FUNC(asinh)(double x) {
305 check_nan_d1(x);
306 int e;
307 e=dgetexp(x);
308 if(e>=32+0x3ff) { // |x|>=2^32?
309 if(!disneg(x)) return log( x )+LOG2; // 1/x^2 << 1
310 else return dneg(log(dneg(x))+LOG2); // 1/x^2 << 1
311 }
312 if(x>0) return log(sqrt(x*x+1)+x);
313 else return dneg(log(sqrt(x*x+1)-x));
314 }
315
WRAPPER_FUNC(acosh)316 double WRAPPER_FUNC(acosh)(double x) {
317 check_nan_d1(x);
318 int e;
319 if(disneg(x)) x=dneg(x);
320 e=dgetexp(x);
321 if(e>=32+0x3ff) return log(x)+LOG2; // |x|>=2^32?
322 return log(sqrt((x-1)*(x+1))+x);
323 }
324
WRAPPER_FUNC(atanh)325 double WRAPPER_FUNC(atanh)(double x) {
326 check_nan_d1(x);
327 return dldexp(log((1+x)/(1-x)),-1);
328 }
329
WRAPPER_FUNC(exp2)330 double WRAPPER_FUNC(exp2)(double x) {
331 check_nan_d1(x);
332 int e;
333 // extra check for disminf as this catches -Nan, and x<=-4096 doesn't.
334 if (disminf(x) || x<=-4096) return 0; // easily underflows
335 else if (x>=4096) return PINF; // easily overflows
336 e=(int)round(x);
337 x-=e;
338 return dldexp(exp(x*LOG2),e);
339 }
WRAPPER_FUNC(log2)340 double WRAPPER_FUNC(log2)(double x) { check_nan_d1(x); return log(x)*LOG2E; }
WRAPPER_FUNC(exp10)341 double WRAPPER_FUNC(exp10)(double x) { check_nan_d1(x); return pow(10,x); }
WRAPPER_FUNC(log10)342 double WRAPPER_FUNC(log10)(double x) { check_nan_d1(x); return log(x)*LOG10E; }
343
344 // todo these are marked as lofi
345 double WRAPPER_FUNC(expm1(double x) { check_nan_d1(x); return exp)(x)-1; }
346 double WRAPPER_FUNC(log1p(double x) { check_nan_d1(x); return log)(1+x); }
347 double WRAPPER_FUNC(fma)(double x,double y,double z) { check_nan_d1(x); return x*y+z; }
348
349 // general power, x>0, finite
350 static double dpow_1(double x,double y) {
351 int a,b,c;
352 double t,rt,u,v,v0,v1,w,ry;
353 a=dgetexp(x)-0x3ff;
354 u=log2(dldexp(x,-a)); // now log_2 x = a+u
355 if(u>0.5) u-=1,a++; // |u|<=~0.5
356 if(a==0) return exp2(u*y);
357 // here |log_2 x| >~0.5
358 if(y>= 4096) { // then easily over/underflows
359 if(a<0) return 0;
360 return PINF;
361 }
362 if(y<=-4096) { // then easily over/underflows
363 if(a<0) return PINF;
364 return 0;
365 }
366 ry=round(y);
367 v=y-ry;
368 v0=dldexp(round(ldexp(v,26)),-26);
369 v1=v-v0;
370 b=(int)ry; // guaranteed to fit in an int; y=b+v0+v1
371 // now the result is exp2( (a+u) * (b+v0+v1) )
372 c=a*b; // integer
373 t=a*v0;
374 rt=round(t);
375 c+=(int)rt;
376 w=t-rt;
377 t=a*v1;
378 w+=t;
379 t=u*b;
380 rt=round(t);
381 c+=(int)rt;
382 w+=t-rt;
383 w+=u*v;
384 return dldexp(exp2(w),c);
385 }
386
387 static double dpow_int2(double x,int y) {
388 double u;
389 if(y==1) return x;
390 u=dpow_int2(x,y/2);
391 u*=u;
392 if(y&1) u*=x;
393 return u;
394 }
395
396 // for the case where x not zero or infinity, y small and not zero
397 static inline double dpowint_1(double x,int y) {
398 if(y<0) x=1/x,y=-y;
399 return dpow_int2(x,y);
400 }
401
402 // for the case where x not zero or infinity
403 static double dpowint_0(double x,int y) {
404 int e;
405 if(disneg(x)) {
406 if(disoddint(y)) return dneg(dpowint_0(dneg(x),y));
407 else return dpowint_0(dneg(x),y);
408 }
409 if(dispo2(x)) {
410 e=dgetexp(x)-0x3ff;
411 if(y>=2048) y= 2047; // avoid overflow
412 if(y<-2048) y=-2048;
413 y*=e;
414 return dldexp(1,y);
415 }
416 if(y==0) return 1;
417 if(y>=-32&&y<=32) return dpowint_1(x,y);
418 return dpow_1(x,y);
419 }
420
421 double WRAPPER_FUNC(powint)(double x,int y) {
422 GCC_Like_Pragma("GCC diagnostic push")
423 GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
424 if(x==1.0||y==0) return 1;
425 GCC_Like_Pragma("GCC diagnostic pop")
426 check_nan_d1(x);
427 if(diszero(x)) {
428 if(y>0) {
429 if(y&1) return x;
430 else return 0;
431 }
432 if((y&1)) return dcopysign(PINF,x);
433 return PINF;
434 }
435 if(dispinf(x)) {
436 if(y<0) return 0;
437 else return PINF;
438 }
439 if(disminf(x)) {
440 if(y>0) {
441 if((y&1)) return MINF;
442 else return PINF;
443 }
444 if((y&1)) return MZERO;
445 else return PZERO;
446 }
447 return dpowint_0(x,y);
448 }
449
450 // for the case where y is guaranteed a finite integer, x not zero or infinity
451 static double dpow_0(double x,double y) {
452 int e,p;
453 if(disneg(x)) {
454 if(disoddint(y)) return dneg(dpow_0(dneg(x),y));
455 else return dpow_0(dneg(x),y);
456 }
457 p=(int)y;
458 if(dispo2(x)) {
459 e=dgetexp(x)-0x3ff;
460 if(p>=2048) p= 2047; // avoid overflow
461 if(p<-2048) p=-2048;
462 p*=e;
463 return dldexp(1,p);
464 }
465 if(p==0) return 1;
466 if(p>=-32&&p<=32) return dpowint_1(x,p);
467 return dpow_1(x,y);
468 }
469
470 double WRAPPER_FUNC(pow)(double x,double y) {
471 GCC_Like_Pragma("GCC diagnostic push")
472 GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
473
474 if(x==1.0||diszero(y)) return 1;
475 check_nan_d2(x, y);
476 if(x==-1.0&&disinf(y)) return 1;
477 GCC_Like_Pragma("GCC diagnostic pop")
478
479 if(diszero(x)) {
480 if(!disneg(y)) {
481 if(disoddint(y)) return x;
482 else return 0;
483 }
484 if(disoddint(y)) return dcopysign(PINF,x);
485 return PINF;
486 }
487 if(dispinf(x)) {
488 if(disneg(y)) return 0;
489 else return PINF;
490 }
491 if(disminf(x)) {
492 if(!disneg(y)) {
493 if(disoddint(y)) return MINF;
494 else return PINF;
495 }
496 if(disoddint(y)) return MZERO;
497 else return PZERO;
498 }
499 if(dispinf(y)) {
500 if(dgetexp(x)<0x3ff) return PZERO;
501 else return PINF;
502 }
503 if(disminf(y)) {
504 if(dgetexp(x)<0x3ff) return PINF;
505 else return PZERO;
506 }
507 if(disint(y)) return dpow_0(x,y);
508 if(disneg(x)) return PINF;
509 return dpow_1(x,y);
510 }
511
512 double WRAPPER_FUNC(hypot)(double x,double y) {
513 check_nan_d2(x, y);
514 int ex,ey;
515 ex=dgetexp(x); ey=dgetexp(y);
516 if(ex>=0x3ff+400||ey>=0x3ff+400) { // overflow, or nearly so
517 x=dldexp(x,-600),y=dldexp(y,-600);
518 return dldexp(sqrt(x*x+y*y), 600);
519 }
520 else if(ex<=0x3ff-400&&ey<=0x3ff-400) { // underflow, or nearly so
521 x=dldexp(x, 600),y=dldexp(y, 600);
522 return dldexp(sqrt(x*x+y*y),-600);
523 }
524 return sqrt(x*x+y*y);
525 }
526
527 double WRAPPER_FUNC(cbrt)(double x) {
528 check_nan_d1(x);
529 int e;
530 if(disneg(x)) return dneg(cbrt(dneg(x)));
531 if(diszero(x)) return dcopysign(PZERO,x);
532 e=dgetexp(x)-0x3ff;
533 e=(e*0x5555+0x8000)>>16; // ~e/3, rounded
534 x=dldexp(x,-e*3);
535 x=exp(log(x)*ONETHIRD);
536 return dldexp(x,e);
537 }
538
539 // reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
540 // 2^52<=|mx|,my<2^53, e>=0; 0<=result<my
541 static i64 drem_0(i64 mx,i64 my,int e,int*pquo) {
542 int quo=0,q,r=0,s;
543 if(e>0) {
544 r=0xffffffffU/(ui32)(my>>36); // reciprocal estimate Q16
545 }
546 while(e>0) {
547 s=e; if(s>12) s=12; // gain up to 12 bits on each iteration
548 q=(mx>>38)*r; // Q30
549 q=((q>>(29-s))+1)>>1; // Q(s), rounded
550 mx=(mx<<s)-my*q;
551 quo=(quo<<s)+q;
552 e-=s;
553 }
554 if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
555 if(mx>=my) mx-=my,quo++;
556 if(mx<0) mx+=my,quo--;
557 if(mx<0) mx+=my,quo--;
558 if(pquo) *pquo=quo;
559 return mx;
560 }
561
562 double WRAPPER_FUNC(fmod)(double x,double y) {
563 check_nan_d2(x, y);
564 ui64 ix=double2ui64(x),iy=double2ui64(y);
565 int sx,ex,ey;
566 i64 mx,my;
567 DUNPACKS(ix,sx,ex,mx);
568 DUNPACK(iy,ey,my);
569 if(ex==0x7ff) return dnan_or(PINF);
570 if(ey==0) return PINF;
571 if(ex==0) {
572 if(!disneg(x)) return PZERO;
573 return MZERO;
574 }
575 if(ex<ey) return x; // |x|<|y|, including case x=±0
576 mx=drem_0(mx,my,ex-ey,0);
577 if(sx) mx=-mx;
578 return fix642double(mx,0x3ff-ey+52);
579 }
580
581 double WRAPPER_FUNC(remquo)(double x,double y,int*quo) {
582 check_nan_d2(x, y);
583 ui64 ix=double2ui64(x),iy=double2ui64(y);
584 int sx,sy,ex,ey,q;
585 i64 mx,my;
586 DUNPACKS(ix,sx,ex,mx);
587 DUNPACKS(iy,sy,ey,my);
588 if(quo) *quo=0;
589 if(ex==0x7ff) return PINF;
590 if(ey==0) return PINF;
591 if(ex==0) return PZERO;
592 if(ey==0x7ff) return x;
593 if(ex<ey-1) return x; // |x|<|y|/2
594 if(ex==ey-1) {
595 if(mx<=my) return x; // |x|<=|y|/2, even quotient
596 // here |y|/2<|x|<|y|
597 if(!sx) { // x>|y|/2
598 mx-=my+my;
599 ey--;
600 q=1;
601 } else { // x<-|y|/2
602 mx=my+my-mx;
603 ey--;
604 q=-1;
605 }
606 }
607 else {
608 if(sx) mx=-mx;
609 mx=drem_0(mx,my,ex-ey,&q);
610 if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
611 mx-=my;
612 q++;
613 }
614 }
615 if(sy) q=-q;
616 if(quo) *quo=q;
617 return fix642double(mx,0x3ff-ey+52);
618 }
619
620 double WRAPPER_FUNC(drem)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
621
622 double WRAPPER_FUNC(remainder)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
623
624 GCC_Pragma("GCC diagnostic pop") // conversion