YAHAL
Yet Another Hardware Abstraction Library
Loading...
Searching...
No Matches
float_math.c
1/*
2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7//#include "pico/types.h"
8//#include "pico/float.h"
9//#include "pico/platform.h"
10
11#define WRAPPER_FUNC(x) __wrap_ ## x
12#define WRAPPER_FUNC_NAME(x) __wrap_##x
13
14#include <math.h>
15#include <stdint.h>
16#include <stdbool.h>
17
18float fix2float(int32_t m, int e);
19
20// opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
21_Pragma("GCC diagnostic push")
22_Pragma("GCC diagnostic ignored \"-Wconversion\"")
23_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")
24
25typedef uint32_t ui32;
26typedef int32_t i32;
27
28#define FPINF ( HUGE_VALF)
29#define FMINF (-HUGE_VALF)
30#define NANF ((float)NAN)
31#define PZERO (+0.0)
32#define MZERO (-0.0)
33
34#define PI 3.14159265358979323846
35#define LOG2 0.69314718055994530941
36// Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
37#define LOG10 2.30258509299404568401
38#define LOG2E 1.44269504088896340737
39#define LOG10E 0.43429448190325182765
40#define ONETHIRD 0.33333333333333333333
41
42#define PIf 3.14159265358979323846f
43#define LOG2f 0.69314718055994530941f
44#define LOG2Ef 1.44269504088896340737f
45#define LOG10Ef 0.43429448190325182765f
46#define ONETHIRDf 0.33333333333333333333f
47
48#define FUNPACK(x,e,m) e=((x)>>23)&0xff,m=((x)&0x007fffff)|0x00800000
49#define FUNPACKS(x,s,e,m) s=((x)>>31),FUNPACK((x),(e),(m))
50
51typedef union {
52 float f;
53 ui32 ix;
54} float_ui32;
55
56static inline float ui322float(ui32 ix) {
57 float_ui32 tmp;
58 tmp.ix = ix;
59 return tmp.f;
60}
61
62static inline ui32 float2ui32(float f) {
63 float_ui32 tmp;
64 tmp.f = f;
65 return tmp.ix;
66}
67
68static inline bool fisnan(float x) {
69 ui32 ix=float2ui32(x);
70 return ix * 2 > 0xff000000u;
71}
72
73#if PICO_FLOAT_PROPAGATE_NANS
74#define check_nan_f1(x) if (fisnan((x))) return (x)
75#define check_nan_f2(x,y) if (fisnan((x))) return (x); else if (fisnan((y))) return (y);
76#else
77#define check_nan_f1(x) ((void)0)
78#define check_nan_f2(x,y) ((void)0)
79#endif
80
81static inline int fgetsignexp(float x) {
82 ui32 ix=float2ui32(x);
83 return (ix>>23)&0x1ff;
84}
85
86static inline int fgetexp(float x) {
87 ui32 ix=float2ui32(x);
88 return (ix>>23)&0xff;
89}
90
91static inline float fldexp(float x,int de) {
92 ui32 ix=float2ui32(x),iy;
93 int e;
94 e=fgetexp(x);
95 if(e==0||e==0xff) return x;
96 e+=de;
97 if(e<=0) iy=ix&0x80000000; // signed zero for underflow
98 else if(e>=0xff) iy=(ix&0x80000000)|0x7f800000ULL; // signed infinity on overflow
99 else iy=ix+((ui32)de<<23);
100 return ui322float(iy);
101}
102
103float WRAPPER_FUNC(ldexpf)(float x, int de) {
104 check_nan_f1(x);
105 return fldexp(x, de);
106}
107
108static inline float fcopysign(float x,float y) {
109 ui32 ix=float2ui32(x),iy=float2ui32(y);
110 ix=((ix&0x7fffffff)|(iy&0x80000000));
111 return ui322float(ix);
112}
113
114float WRAPPER_FUNC(copysignf)(float x, float y) {
115 check_nan_f2(x,y);
116 return fcopysign(x, y);
117}
118
119static inline int fiszero(float x) { return fgetexp (x)==0; }
120static inline int fispzero(float x) { return fgetsignexp(x)==0; }
121static inline int fismzero(float x) { return fgetsignexp(x)==0x100; }
122static inline int fisinf(float x) { return fgetexp (x)==0xff; }
123static inline int fispinf(float x) { return fgetsignexp(x)==0xff; }
124static inline int fisminf(float x) { return fgetsignexp(x)==0x1ff; }
125
126static inline int fisint(float x) {
127 ui32 ix=float2ui32(x),m;
128 int e=fgetexp(x);
129 if(e==0) return 1; // 0 is an integer
130 e-=0x7f; // remove exponent bias
131 if(e<0) return 0; // |x|<1
132 e=23-e; // bit position in mantissa with significance 1
133 if(e<=0) return 1; // |x| large, so must be an integer
134 m=(1<<e)-1; // mask for bits of significance <1
135 if(ix&m) return 0; // not an integer
136 return 1;
137}
138
139static inline int fisoddint(float x) {
140 ui32 ix=float2ui32(x),m;
141 int e=fgetexp(x);
142 e-=0x7f; // remove exponent bias
143 if(e<0) return 0; // |x|<1; 0 is not odd
144 e=23-e; // bit position in mantissa with significance 1
145 if(e<0) return 0; // |x| large, so must be even
146 m=(1<<e)-1; // mask for bits of significance <1 (if any)
147 if(ix&m) return 0; // not an integer
148 if(e==23) return 1; // value is exactly 1
149 return (ix>>e)&1;
150}
151
152static inline int fisstrictneg(float x) {
153 ui32 ix=float2ui32(x);
154 if(fiszero(x)) return 0;
155 return ix>>31;
156}
157
158static inline int fisneg(float x) {
159 ui32 ix=float2ui32(x);
160 return ix>>31;
161}
162
163static inline float fneg(float x) {
164 ui32 ix=float2ui32(x);
165 ix^=0x80000000;
166 return ui322float(ix);
167}
168
169static inline int fispo2(float x) {
170 ui32 ix=float2ui32(x);
171 if(fiszero(x)) return 0;
172 if(fisinf(x)) return 0;
173 ix&=0x007fffff;
174 return ix==0;
175}
176
177static inline float fnan_or(float x) {
178#if PICO_FLOAT_PROPAGATE_NANS
179 return NANF;
180#else
181 return x;
182#endif
183}
184
185float WRAPPER_FUNC(truncf)(float x) {
186 check_nan_f1(x);
187 ui32 ix=float2ui32(x),m;
188 int e=fgetexp(x);
189 e-=0x7f; // remove exponent bias
190 if(e<0) { // |x|<1
191 ix&=0x80000000;
192 return ui322float(ix);
193 }
194 e=23-e; // bit position in mantissa with significance 1
195 if(e<=0) return x; // |x| large, so must be an integer
196 m=(1<<e)-1; // mask for bits of significance <1
197 ix&=~m;
198 return ui322float(ix);
199}
200
201float WRAPPER_FUNC(roundf)(float x) {
202 check_nan_f1(x);
203 ui32 ix=float2ui32(x),m;
204 int e=fgetexp(x);
205 e-=0x7f; // remove exponent bias
206 if(e<-1) { // |x|<0.5
207 ix&=0x80000000;
208 return ui322float(ix);
209 }
210 if(e==-1) { // 0.5<=|x|<1
211 ix&=0x80000000;
212 ix|=0x3f800000; // ±1
213 return ui322float(ix);
214 }
215 e=23-e; // bit position in mantissa with significance 1, <=23
216 if(e<=0) return x; // |x| large, so must be an integer
217 m=1<<(e-1); // mask for bit of significance 0.5
218 ix+=m;
219 m=m+m-1; // mask for bits of significance <1
220 ix&=~m;
221 return ui322float(ix);
222}
223
224float WRAPPER_FUNC(floorf)(float x) {
225 check_nan_f1(x);
226 ui32 ix=float2ui32(x),m;
227 int e=fgetexp(x);
228 if(e==0) { // x==0
229 ix&=0x80000000;
230 return ui322float(ix);
231 }
232 e-=0x7f; // remove exponent bias
233 if(e<0) { // |x|<1, not zero
234 if(fisneg(x)) return -1;
235 return PZERO;
236 }
237 e=23-e; // bit position in mantissa with significance 1
238 if(e<=0) return x; // |x| large, so must be an integer
239 m=(1<<e)-1; // mask for bit of significance <1
240 if(fisneg(x)) ix+=m; // add 1-ε to magnitude if negative
241 ix&=~m; // truncate
242 return ui322float(ix);
243}
244
245float WRAPPER_FUNC(ceilf)(float x) {
246 check_nan_f1(x);
247 ui32 ix=float2ui32(x),m;
248 int e=fgetexp(x);
249 if(e==0) { // x==0
250 ix&=0x80000000;
251 return ui322float(ix);
252 }
253 e-=0x7f; // remove exponent bias
254 if(e<0) { // |x|<1, not zero
255 if(fisneg(x)) return MZERO;
256 return 1;
257 }
258 e=23-e; // bit position in mantissa with significance 1
259 if(e<=0) return x; // |x| large, so must be an integer
260 m=(1<<e)-1; // mask for bit of significance <1
261 if(!fisneg(x)) ix+=m; // add 1-ε to magnitude if positive
262 ix&=~m; // truncate
263 return ui322float(ix);
264}
265
266float WRAPPER_FUNC(asinf)(float x) {
267 check_nan_f1(x);
268 float u;
269 u=(1.0f-x)*(1.0f+x);
270 if(fisstrictneg(u)) return fnan_or(FPINF);
271 return atan2f(x,sqrtf(u));
272}
273
274float WRAPPER_FUNC(acosf)(float x) {
275 check_nan_f1(x);
276 float u;
277 u=(1.0f-x)*(1.0f+x);
278 if(fisstrictneg(u)) return fnan_or(FPINF);
279 return atan2f(sqrtf(u),x);
280}
281
282float WRAPPER_FUNC(atanf)(float x) {
283 check_nan_f1(x);
284 if(fispinf(x)) return (float)( PIf/2);
285 if(fisminf(x)) return (float)(-PIf/2);
286 return atan2f(x,1.0f);
287}
288
289float WRAPPER_FUNC(sinhf)(float x) {
290 check_nan_f1(x);
291 return fldexp((expf(x)-expf(fneg(x))),-1);
292}
293
294float WRAPPER_FUNC(coshf)(float x) {
295 check_nan_f1(x);
296 return fldexp((expf(x)+expf(fneg(x))),-1);
297}
298
299float WRAPPER_FUNC(tanhf)(float x) {
300 check_nan_f1(x);
301 float u;
302 int e;
303 e=fgetexp(x);
304 if(e>=4+0x7f) { // |x|>=16?
305 if(!fisneg(x)) return 1; // 1 << exp 2x; avoid generating infinities later
306 else return -1; // 1 >> exp 2x
307 }
308 u=expf(fldexp(x,1));
309 return (u-1.0f)/(u+1.0f);
310}
311
312float WRAPPER_FUNC(asinhf)(float x) {
313 check_nan_f1(x);
314 int e;
315 e=fgetexp(x);
316 if(e>=16+0x7f) { // |x|>=2^16?
317 if(!fisneg(x)) return logf( x )+LOG2f; // 1/x^2 << 1
318 else return fneg(logf(fneg(x))+LOG2f); // 1/x^2 << 1
319 }
320 if(x>0) return (float)log(sqrt((double)x*(double)x+1.0)+(double)x);
321 else return fneg((float)log(sqrt((double)x*(double)x+1.0)-(double)x));
322}
323
324float WRAPPER_FUNC(acoshf)(float x) {
325 check_nan_f1(x);
326 int e;
327 if(fisneg(x)) x=fneg(x);
328 e=fgetexp(x);
329 if(e>=16+0x7f) return logf(x)+LOG2f; // |x|>=2^16?
330 return (float)log(sqrt(((double)x+1.0)*((double)x-1.0))+(double)x);
331}
332
333float WRAPPER_FUNC(atanhf)(float x) {
334 check_nan_f1(x);
335 return fldexp(logf((1.0f+x)/(1.0f-x)),-1);
336}
337
338float WRAPPER_FUNC(exp2f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG2); }
339float WRAPPER_FUNC(log2f)(float x) { check_nan_f1(x); return logf(x)*LOG2Ef; }
340float WRAPPER_FUNC(exp10f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG10); }
341float WRAPPER_FUNC(log10f)(float x) { check_nan_f1(x); return logf(x)*LOG10Ef; }
342
343float WRAPPER_FUNC(expm1f)(float x) { check_nan_f1(x); return (float)(exp((double)x)-1); }
344float WRAPPER_FUNC(log1pf)(float x) { check_nan_f1(x); return (float)(log(1+(double)x)); }
345float WRAPPER_FUNC(fmaf)(float x,float y,float z) {
346 check_nan_f2(x,y);
347 check_nan_f1(z);
348 return (float)((double)x*(double)y+(double)z);
349} // has double rounding so not exact
350
351// general power, x>0
352static inline float fpow_1(float x,float y) {
353 return (float)exp(log((double)x)*(double)y); // using double-precision intermediates for better accuracy
354}
355
356static float fpow_int2(float x,int y) {
357 float u;
358 if(y==1) return x;
359 u=fpow_int2(x,y/2);
360 u*=u;
361 if(y&1) u*=x;
362 return u;
363}
364
365// for the case where x not zero or infinity, y small and not zero
366static inline float fpowint_1(float x,int y) {
367 if(y<0) x=1.0f/x,y=-y;
368 return fpow_int2(x,y);
369}
370
371// for the case where x not zero or infinity
372static float fpowint_0(float x,int y) {
373 int e;
374 if(fisneg(x)) {
375 if(fisoddint(y)) return fneg(fpowint_0(fneg(x),y));
376 else return fpowint_0(fneg(x),y);
377 }
378 if(fispo2(x)) {
379 e=fgetexp(x)-0x7f;
380 if(y>=256) y= 255; // avoid overflow
381 if(y<-256) y=-256;
382 y*=e;
383 return fldexp(1,y);
384 }
385 if(y==0) return 1;
386 if(y>=-32&&y<=32) return fpowint_1(x,y);
387 return fpow_1(x,y);
388}
389
390float WRAPPER_FUNC(powintf)(float x,int y) {
391 _Pragma("GCC diagnostic push")
392 _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
393 if(x==1.0f||y==0) return 1;
394 if(x==0.0f) {
395 if(y>0) {
396 if(y&1) return x;
397 else return 0;
398 }
399 if((y&1)) return fcopysign(FPINF,x);
400 return FPINF;
401 }
402 _Pragma("GCC diagnostic pop")
403 check_nan_f1(x);
404 if(fispinf(x)) {
405 if(y<0) return 0;
406 else return FPINF;
407 }
408 if(fisminf(x)) {
409 if(y>0) {
410 if((y&1)) return FMINF;
411 else return FPINF;
412 }
413 if((y&1)) return MZERO;
414 else return PZERO;
415 }
416 return fpowint_0(x,y);
417}
418
419// for the case where y is guaranteed a finite integer, x not zero or infinity
420static float fpow_0(float x,float y) {
421 int e,p;
422 if(fisneg(x)) {
423 if(fisoddint(y)) return fneg(fpow_0(fneg(x),y));
424 else return fpow_0(fneg(x),y);
425 }
426 p=(int)y;
427 if(fispo2(x)) {
428 e=fgetexp(x)-0x7f;
429 if(p>=256) p= 255; // avoid overflow
430 if(p<-256) p=-256;
431 p*=e;
432 return fldexp(1,p);
433 }
434 if(p==0) return 1;
435 if(p>=-32&&p<=32) return fpowint_1(x,p);
436 return fpow_1(x,y);
437}
438
439float WRAPPER_FUNC(powf)(float x,float y) {
440 _Pragma("GCC diagnostic push")
441 _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
442 if(x==1.0f||fiszero(y)) return 1;
443 check_nan_f2(x,y);
444 if(x==-1.0f&&fisinf(y)) return 1;
445 _Pragma("GCC diagnostic pop")
446 if(fiszero(x)) {
447 if(!fisneg(y)) {
448 if(fisoddint(y)) return x;
449 else return 0;
450 }
451 if(fisoddint(y)) return fcopysign(FPINF,x);
452 return FPINF;
453 }
454 if(fispinf(x)) {
455 if(fisneg(y)) return 0;
456 else return FPINF;
457 }
458 if(fisminf(x)) {
459 if(!fisneg(y)) {
460 if(fisoddint(y)) return FMINF;
461 else return FPINF;
462 }
463 if(fisoddint(y)) return MZERO;
464 else return PZERO;
465 }
466 if(fispinf(y)) {
467 if(fgetexp(x)<0x7f) return PZERO;
468 else return FPINF;
469 }
470 if(fisminf(y)) {
471 if(fgetexp(x)<0x7f) return FPINF;
472 else return PZERO;
473 }
474 if(fisint(y)) return fpow_0(x,y);
475 if(fisneg(x)) return FPINF;
476 return fpow_1(x,y);
477}
478
479float WRAPPER_FUNC(hypotf)(float x,float y) {
480 check_nan_f2(x,y);
481 int ex,ey;
482 ex=fgetexp(x); ey=fgetexp(y);
483 if(ex>=0x7f+50||ey>=0x7f+50) { // overflow, or nearly so
484 x=fldexp(x,-70),y=fldexp(y,-70);
485 return fldexp(sqrtf(x*x+y*y), 70);
486 }
487 else if(ex<=0x7f-50&&ey<=0x7f-50) { // underflow, or nearly so
488 x=fldexp(x, 70),y=fldexp(y, 70);
489 return fldexp(sqrtf(x*x+y*y),-70);
490 }
491 return sqrtf(x*x+y*y);
492}
493
494float WRAPPER_FUNC(cbrtf)(float x) {
495 check_nan_f1(x);
496 int e;
497 if(fisneg(x)) return fneg(cbrtf(fneg(x)));
498 if(fiszero(x)) return fcopysign(PZERO,x);
499 e=fgetexp(x)-0x7f;
500 e=(e*0x5555+0x8000)>>16; // ~e/3, rounded
501 x=fldexp(x,-e*3);
502 x=expf(logf(x)*ONETHIRDf);
503 return fldexp(x,e);
504}
505
506// reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
507// 2^23<=|mx|,my<2^24, e>=0; 0<=result<my
508static i32 frem_0(i32 mx,i32 my,int e,int*pquo) {
509 int quo=0,q,r=0,s;
510 if(e>0) {
511 r=0xffffffffU/(ui32)(my>>7); // reciprocal estimate Q16
512 }
513 while(e>0) {
514 s=e; if(s>12) s=12; // gain up to 12 bits on each iteration
515 q=(mx>>9)*r; // Q30
516 q=((q>>(29-s))+1)>>1; // Q(s), rounded
517 mx=(mx<<s)-my*q;
518 quo=(quo<<s)+q;
519 e-=s;
520 }
521 if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
522 if(mx>=my) mx-=my,quo++;
523 if(mx<0) mx+=my,quo--;
524 if(mx<0) mx+=my,quo--;
525 if(pquo) *pquo=quo;
526 return mx;
527}
528
529float WRAPPER_FUNC(fmodf)(float x,float y) {
530 check_nan_f2(x,y);
531 ui32 ix=float2ui32(x),iy=float2ui32(y);
532 int sx,ex,ey;
533 i32 mx,my;
534 FUNPACKS(ix,sx,ex,mx);
535 FUNPACK(iy,ey,my);
536 if(ex==0xff) {
537 return fnan_or(FPINF);
538 }
539 if(ey==0) return FPINF;
540 if(ex==0) {
541 if(!fisneg(x)) return PZERO;
542 return MZERO;
543 }
544 if(ex<ey) return x; // |x|<|y|, including case x=±0
545 mx=frem_0(mx,my,ex-ey,0);
546 if(sx) mx=-mx;
547 return fix2float(mx,0x7f-ey+23);
548}
549
550float WRAPPER_FUNC(remquof)(float x,float y,int*quo) {
551 check_nan_f2(x,y);
552 ui32 ix=float2ui32(x),iy=float2ui32(y);
553 int sx,sy,ex,ey,q;
554 i32 mx,my;
555 FUNPACKS(ix,sx,ex,mx);
556 FUNPACKS(iy,sy,ey,my);
557 if(quo) *quo=0;
558 if(ex==0xff) return FPINF;
559 if(ey==0) return FPINF;
560 if(ex==0) return PZERO;
561 if(ey==0xff) return x;
562 if(ex<ey-1) return x; // |x|<|y|/2
563 if(ex==ey-1) {
564 if(mx<=my) return x; // |x|<=|y|/2, even quotient
565 // here |y|/2<|x|<|y|
566 if(!sx) { // x>|y|/2
567 mx-=my+my;
568 ey--;
569 q=1;
570 } else { // x<-|y|/2
571 mx=my+my-mx;
572 ey--;
573 q=-1;
574 }
575 }
576 else {
577 if(sx) mx=-mx;
578 mx=frem_0(mx,my,ex-ey,&q);
579 if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
580 mx-=my;
581 q++;
582 }
583 }
584 if(sy) q=-q;
585 if(quo) *quo=q;
586 return fix2float(mx,0x7f-ey+23);
587}
588
589float WRAPPER_FUNC(dremf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
590
591float WRAPPER_FUNC(remainderf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
592
593_Pragma("GCC diagnostic pop") // conversion
Generated by doxygen 1.12.0