Cleanup: style
[blender.git] / source / blender / blenlib / intern / math_base_inline.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: some of this file.
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  * */
25
26 /** \file blender/blenlib/intern/math_base_inline.c
27  *  \ingroup bli
28  */
29
30 #ifndef __MATH_BASE_INLINE_C__
31 #define __MATH_BASE_INLINE_C__
32
33 #include <float.h>
34 #include <stdio.h>
35 #include <stdlib.h>
36 #include <limits.h>
37
38 #ifdef __SSE2__
39 #  include <emmintrin.h>
40 #endif
41
42 #include "BLI_math_base.h"
43
44 /* copied from BLI_utildefines.h */
45 #ifdef __GNUC__
46 #  define UNLIKELY(x)     __builtin_expect(!!(x), 0)
47 #else
48 #  define UNLIKELY(x)     (x)
49 #endif
50
51 /* powf is really slow for raising to integer powers. */
52 MINLINE float pow2f(float x)
53 {
54         return x * x;
55 }
56 MINLINE float pow3f(float x)
57 {
58         return pow2f(x) * x;
59 }
60 MINLINE float pow4f(float x)
61 {
62         return pow2f(pow2f(x));
63 }
64 MINLINE float pow7f(float x)
65 {
66         return pow2f(pow3f(x)) * x;
67 }
68
69 MINLINE float sqrt3f(float f)
70 {
71         if      (UNLIKELY(f == 0.0f)) return 0.0f;
72         else if (UNLIKELY(f <  0.0f)) return -(float)(exp(log(-f) / 3.0));
73         else                          return  (float)(exp(log( f) / 3.0));
74 }
75
76 MINLINE double sqrt3d(double d)
77 {
78         if      (UNLIKELY(d == 0.0)) return 0.0;
79         else if (UNLIKELY(d <  0.0)) return -exp(log(-d) / 3.0);
80         else                         return  exp(log( d) / 3.0);
81 }
82
83 MINLINE float sqrtf_signed(float f)
84 {
85         return (f >= 0.0f) ? sqrtf(f) : -sqrtf(-f);
86 }
87
88 MINLINE float saacos(float fac)
89 {
90         if      (UNLIKELY(fac <= -1.0f)) return (float)M_PI;
91         else if (UNLIKELY(fac >=  1.0f)) return 0.0f;
92         else                             return acosf(fac);
93 }
94
95 MINLINE float saasin(float fac)
96 {
97         if      (UNLIKELY(fac <= -1.0f)) return (float)-M_PI / 2.0f;
98         else if (UNLIKELY(fac >=  1.0f)) return (float) M_PI / 2.0f;
99         else                             return asinf(fac);
100 }
101
102 MINLINE float sasqrt(float fac)
103 {
104         if (UNLIKELY(fac <= 0.0f)) return 0.0f;
105         else                       return sqrtf(fac);
106 }
107
108 MINLINE float saacosf(float fac)
109 {
110         if      (UNLIKELY(fac <= -1.0f)) return (float)M_PI;
111         else if (UNLIKELY(fac >=  1.0f)) return 0.0f;
112         else                             return acosf(fac);
113 }
114
115 MINLINE float saasinf(float fac)
116 {
117         if      (UNLIKELY(fac <= -1.0f)) return (float)-M_PI / 2.0f;
118         else if (UNLIKELY(fac >=  1.0f)) return (float) M_PI / 2.0f;
119         else                             return asinf(fac);
120 }
121
122 MINLINE float sasqrtf(float fac)
123 {
124         if (UNLIKELY(fac <= 0.0f)) return 0.0f;
125         else                       return sqrtf(fac);
126 }
127
128 MINLINE float interpf(float target, float origin, float fac)
129 {
130         return (fac * target) + (1.0f - fac) * origin;
131 }
132
133 /* used for zoom values*/
134 MINLINE float power_of_2(float val)
135 {
136         return (float)pow(2.0, ceil(log((double)val) / M_LN2));
137 }
138
139 MINLINE int is_power_of_2_i(int n)
140 {
141         return (n & (n - 1)) == 0;
142 }
143
144 MINLINE int power_of_2_max_i(int n)
145 {
146         if (is_power_of_2_i(n))
147                 return n;
148
149         do {
150                 n = n & (n - 1);
151         } while (!is_power_of_2_i(n));
152
153         return n * 2;
154 }
155
156 MINLINE int power_of_2_min_i(int n)
157 {
158         while (!is_power_of_2_i(n))
159                 n = n & (n - 1);
160
161         return n;
162 }
163
164 MINLINE unsigned int power_of_2_max_u(unsigned int x)
165 {
166         x -= 1;
167         x |= (x >>  1);
168         x |= (x >>  2);
169         x |= (x >>  4);
170         x |= (x >>  8);
171         x |= (x >> 16);
172         return x + 1;
173 }
174
175 MINLINE unsigned power_of_2_min_u(unsigned x)
176 {
177         x |= (x >>  1);
178         x |= (x >>  2);
179         x |= (x >>  4);
180         x |= (x >>  8);
181         x |= (x >> 16);
182         return x - (x >> 1);
183 }
184
185 /* rounding and clamping */
186
187 #define _round_clamp_fl_impl(arg, ty, min, max) { \
188         float r = floorf(arg + 0.5f); \
189         if      (UNLIKELY(r <= (float)min)) return (ty)min; \
190         else if (UNLIKELY(r >= (float)max)) return (ty)max; \
191         else return (ty)r; \
192 }
193
194 #define _round_clamp_db_impl(arg, ty, min, max) { \
195         double r = floor(arg + 0.5); \
196         if      (UNLIKELY(r <= (double)min)) return (ty)min; \
197         else if (UNLIKELY(r >= (double)max)) return (ty)max; \
198         else return (ty)r; \
199 }
200
201 #define _round_fl_impl(arg, ty) { return (ty)floorf(arg + 0.5f); }
202 #define _round_db_impl(arg, ty) { return (ty)floor(arg + 0.5); }
203
204 MINLINE signed char    round_fl_to_char(float a) { _round_fl_impl(a, signed char) }
205 MINLINE unsigned char  round_fl_to_uchar(float a) { _round_fl_impl(a, unsigned char) }
206 MINLINE short          round_fl_to_short(float a) { _round_fl_impl(a, short) }
207 MINLINE unsigned short round_fl_to_ushort(float a) { _round_fl_impl(a, unsigned short) }
208 MINLINE int            round_fl_to_int(float a) { _round_fl_impl(a, int) }
209 MINLINE unsigned int   round_fl_to_uint(float a) { _round_fl_impl(a, unsigned int) }
210
211 MINLINE signed char    round_db_to_char(double a) { _round_db_impl(a, signed char) }
212 MINLINE unsigned char  round_db_to_uchar(double a) { _round_db_impl(a, unsigned char) }
213 MINLINE short          round_db_to_short(double a) { _round_db_impl(a, short) }
214 MINLINE unsigned short round_db_to_ushort(double a) { _round_db_impl(a, unsigned short) }
215 MINLINE int            round_db_to_int(double a) { _round_db_impl(a, int) }
216 MINLINE unsigned int   round_db_to_uint(double a) { _round_db_impl(a, unsigned int) }
217
218 #undef _round_fl_impl
219 #undef _round_db_impl
220
221 MINLINE signed char    round_fl_to_char_clamp(float a) { _round_clamp_fl_impl(a, signed char, SCHAR_MIN, SCHAR_MAX) }
222 MINLINE unsigned char  round_fl_to_uchar_clamp(float a) { _round_clamp_fl_impl(a, unsigned char, 0, UCHAR_MAX) }
223 MINLINE short          round_fl_to_short_clamp(float a) { _round_clamp_fl_impl(a, short, SHRT_MIN, SHRT_MAX) }
224 MINLINE unsigned short round_fl_to_ushort_clamp(float a) { _round_clamp_fl_impl(a, unsigned short, 0, USHRT_MAX) }
225 MINLINE int            round_fl_to_int_clamp(float a) { _round_clamp_fl_impl(a, int, INT_MIN, INT_MAX) }
226 MINLINE unsigned int   round_fl_to_uint_clamp(float a) { _round_clamp_fl_impl(a, unsigned int, 0, UINT_MAX) }
227
228 MINLINE signed char    round_db_to_char_clamp(double a) { _round_clamp_db_impl(a, signed char, SCHAR_MIN, SCHAR_MAX) }
229 MINLINE unsigned char  round_db_to_uchar_clamp(double a) { _round_clamp_db_impl(a, unsigned char, 0, UCHAR_MAX) }
230 MINLINE short          round_db_to_short_clamp(double a) { _round_clamp_db_impl(a, short, SHRT_MIN, SHRT_MAX) }
231 MINLINE unsigned short round_db_to_ushort_clamp(double a) { _round_clamp_db_impl(a, unsigned short, 0, USHRT_MAX) }
232 MINLINE int            round_db_to_int_clamp(double a) { _round_clamp_db_impl(a, int, INT_MIN, INT_MAX) }
233 MINLINE unsigned int   round_db_to_uint_clamp(double a) { _round_clamp_db_impl(a, unsigned int, 0, UINT_MAX) }
234
235 #undef _round_clamp_fl_impl
236 #undef _round_clamp_db_impl
237
238 /* integer division that rounds 0.5 up, particularly useful for color blending
239  * with integers, to avoid gradual darkening when rounding down */
240 MINLINE int divide_round_i(int a, int b)
241 {
242         return (2 * a + b) / (2 * b);
243 }
244
245 /**
246  * Integer division that floors negative result.
247  * \note This works like Python's int division.
248  */
249 MINLINE int divide_floor_i(int a, int b)
250 {
251         int d = a / b;
252         int r = a % b;  /* Optimizes into a single division. */
253         return r ? d - ((a < 0) ^ (b < 0)) : d;
254 }
255
256 /**
257  * modulo that handles negative numbers, works the same as Python's.
258  */
259 MINLINE int mod_i(int i, int n)
260 {
261         return (i % n + n) % n;
262 }
263
264 MINLINE float min_ff(float a, float b)
265 {
266         return (a < b) ? a : b;
267 }
268 MINLINE float max_ff(float a, float b)
269 {
270         return (a > b) ? a : b;
271 }
272
273 MINLINE int min_ii(int a, int b)
274 {
275         return (a < b) ? a : b;
276 }
277 MINLINE int max_ii(int a, int b)
278 {
279         return (b < a) ? a : b;
280 }
281
282 MINLINE float min_fff(float a, float b, float c)
283 {
284         return min_ff(min_ff(a, b), c);
285 }
286 MINLINE float max_fff(float a, float b, float c)
287 {
288         return max_ff(max_ff(a, b), c);
289 }
290
291 MINLINE int min_iii(int a, int b, int c)
292 {
293         return min_ii(min_ii(a, b), c);
294 }
295 MINLINE int max_iii(int a, int b, int c)
296 {
297         return max_ii(max_ii(a, b), c);
298 }
299
300 MINLINE float min_ffff(float a, float b, float c, float d)
301 {
302         return min_ff(min_fff(a, b, c), d);
303 }
304 MINLINE float max_ffff(float a, float b, float c, float d)
305 {
306         return max_ff(max_fff(a, b, c), d);
307 }
308
309 MINLINE int min_iiii(int a, int b, int c, int d)
310 {
311         return min_ii(min_iii(a, b, c), d);
312 }
313 MINLINE int max_iiii(int a, int b, int c, int d)
314 {
315         return max_ii(max_iii(a, b, c), d);
316 }
317
318 MINLINE size_t min_zz(size_t a, size_t b)
319 {
320         return (a < b) ? a : b;
321 }
322 MINLINE size_t max_zz(size_t a, size_t b)
323 {
324         return (b < a) ? a : b;
325 }
326
327 MINLINE int clamp_i(int value, int min, int max)
328 {
329         return min_ii(max_ii(value, min), max);
330 }
331
332 MINLINE float clamp_f(float value, float min, float max)
333 {
334         if (value > max) {
335                 return max;
336         }
337         else if (value < min) {
338                 return min;
339         }
340         return value;
341 }
342
343 MINLINE size_t clamp_z(size_t value, size_t min, size_t max)
344 {
345         return min_zz(max_zz(value, min), max);
346 }
347
348 /**
349  * Almost-equal for IEEE floats, using absolute difference method.
350  *
351  * \param max_diff the maximum absolute difference.
352  */
353 MINLINE int compare_ff(float a, float b, const float max_diff)
354 {
355         return fabsf(a - b) <= max_diff;
356 }
357
358 /**
359  * Almost-equal for IEEE floats, using their integer representation (mixing ULP and absolute difference methods).
360  *
361  * \param max_diff is the maximum absolute difference (allows to take care of the near-zero area,
362  *                 where relative difference methods cannot really work).
363  * \param max_ulps is the 'maximum number of floats + 1' allowed between \a a and \a b to consider them equal.
364  *
365  * \see https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
366  */
367 MINLINE int compare_ff_relative(float a, float b, const float max_diff, const int max_ulps)
368 {
369         union {float f; int i;} ua, ub;
370
371 #if 0  /* No BLI_assert in INLINE :/ */
372         BLI_assert(sizeof(float) == sizeof(int));
373         BLI_assert(max_ulps < (1 << 22));
374 #endif
375
376         if (fabsf(a - b) <= max_diff) {
377                 return 1;
378         }
379
380         ua.f = a;
381         ub.f = b;
382
383         /* Important to compare sign from integers, since (-0.0f < 0) is false
384          * (though this shall not be an issue in common cases)... */
385         return ((ua.i < 0) != (ub.i < 0)) ? 0 : (abs(ua.i - ub.i) <= max_ulps) ? 1 : 0;
386 }
387
388 MINLINE float signf(float f)
389 {
390         return (f < 0.f) ? -1.f : 1.f;
391 }
392
393 MINLINE int signum_i_ex(float a, float eps)
394 {
395         if (a >  eps) return  1;
396         if (a < -eps) return -1;
397         else          return  0;
398 }
399
400 MINLINE int signum_i(float a)
401 {
402         if (a > 0.0f) return  1;
403         if (a < 0.0f) return -1;
404         else          return  0;
405 }
406
407 /** Returns number of (base ten) *significant* digits of integer part of given float
408  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
409 MINLINE int integer_digits_f(const float f)
410 {
411         return (f == 0.0f) ? 0 : (int)floor(log10(fabs(f))) + 1;
412 }
413
414 /** Returns number of (base ten) *significant* digits of integer part of given double
415  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
416 MINLINE int integer_digits_d(const double d)
417 {
418         return (d == 0.0) ? 0 : (int)floor(log10(fabs(d))) + 1;
419 }
420
421 MINLINE int integer_digits_i(const int i)
422 {
423         return (int)log10((double)i) + 1;
424 }
425
426 /* Internal helpers for SSE2 implementation.
427  *
428  * NOTE: Are to be called ONLY from inside `#ifdef __SSE2__` !!!
429  */
430
431 #ifdef __SSE2__
432
433 /* Calculate initial guess for arg^exp based on float representation
434  * This method gives a constant bias, which can be easily compensated by
435  * multiplicating with bias_coeff.
436  * Gives better results for exponents near 1 (e. g. 4/5).
437  * exp = exponent, encoded as uint32_t
438  * e2coeff = 2^(127/exponent - 127) * bias_coeff^(1/exponent), encoded as
439  * uint32_t
440  *
441  * We hope that exp and e2coeff gets properly inlined
442  */
443 MALWAYS_INLINE __m128 _bli_math_fastpow(const int exp,
444                                         const int e2coeff,
445                                         const __m128 arg)
446 {
447         __m128 ret;
448         ret = _mm_mul_ps(arg, _mm_castsi128_ps(_mm_set1_epi32(e2coeff)));
449         ret = _mm_cvtepi32_ps(_mm_castps_si128(ret));
450         ret = _mm_mul_ps(ret, _mm_castsi128_ps(_mm_set1_epi32(exp)));
451         ret = _mm_castsi128_ps(_mm_cvtps_epi32(ret));
452         return ret;
453 }
454
455 /* Improve x ^ 1.0f/5.0f solution with Newton-Raphson method */
456 MALWAYS_INLINE __m128 _bli_math_improve_5throot_solution(
457     const __m128 old_result,
458     const __m128 x)
459 {
460         __m128 approx2 = _mm_mul_ps(old_result, old_result);
461         __m128 approx4 = _mm_mul_ps(approx2, approx2);
462         __m128 t = _mm_div_ps(x, approx4);
463         __m128 summ = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(4.0f), old_result), t); /* fma */
464         return _mm_mul_ps(summ, _mm_set1_ps(1.0f / 5.0f));
465 }
466
467 /* Calculate powf(x, 2.4). Working domain: 1e-10 < x < 1e+10 */
468 MALWAYS_INLINE __m128 _bli_math_fastpow24(const __m128 arg)
469 {
470         /* max, avg and |avg| errors were calculated in gcc without FMA instructions
471          * The final precision should be better than powf in glibc */
472
473         /* Calculate x^4/5, coefficient 0.994 was constructed manually to minimize
474          * avg error.
475          */
476         /* 0x3F4CCCCD = 4/5 */
477         /* 0x4F55A7FB = 2^(127/(4/5) - 127) * 0.994^(1/(4/5)) */
478         /* error max = 0.17     avg = 0.0018    |avg| = 0.05 */
479         __m128 x = _bli_math_fastpow(0x3F4CCCCD, 0x4F55A7FB, arg);
480         __m128 arg2 = _mm_mul_ps(arg, arg);
481         __m128 arg4 = _mm_mul_ps(arg2, arg2);
482         /* error max = 0.018        avg = 0.0031    |avg| = 0.0031  */
483         x = _bli_math_improve_5throot_solution(x, arg4);
484         /* error max = 0.00021    avg = 1.6e-05    |avg| = 1.6e-05 */
485         x = _bli_math_improve_5throot_solution(x, arg4);
486         /* error max = 6.1e-07    avg = 5.2e-08    |avg| = 1.1e-07 */
487         x = _bli_math_improve_5throot_solution(x, arg4);
488         return _mm_mul_ps(x, _mm_mul_ps(x, x));
489 }
490
491 /* Calculate powf(x, 1.0f / 2.4) */
492 MALWAYS_INLINE __m128 _bli_math_fastpow512(const __m128 arg)
493 {
494         /* 5/12 is too small, so compute the 4th root of 20/12 instead.
495          * 20/12 = 5/3 = 1 + 2/3 = 2 - 1/3. 2/3 is a suitable argument for fastpow.
496          * weighting coefficient: a^-1/2 = 2 a; a = 2^-2/3
497          */
498         __m128 xf = _bli_math_fastpow(0x3f2aaaab, 0x5eb504f3, arg);
499         __m128 xover = _mm_mul_ps(arg, xf);
500         __m128 xfm1 = _mm_rsqrt_ps(xf);
501         __m128 x2 = _mm_mul_ps(arg, arg);
502         __m128 xunder = _mm_mul_ps(x2, xfm1);
503         /* sqrt2 * over + 2 * sqrt2 * under */
504         __m128 xavg = _mm_mul_ps(_mm_set1_ps(1.0f / (3.0f * 0.629960524947437f) * 0.999852f),
505                                  _mm_add_ps(xover, xunder));
506         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
507         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
508         return xavg;
509 }
510
511 MALWAYS_INLINE __m128 _bli_math_blend_sse(const __m128 mask,
512                                           const __m128 a,
513                                           const __m128 b)
514 {
515         return _mm_or_ps(_mm_and_ps(mask, a), _mm_andnot_ps(mask, b));
516 }
517
518 #endif  /* __SSE2__ */
519
520 #endif /* __MATH_BASE_INLINE_C__ */