Math Lib: Add non-clamped round_* functions
[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 /**
319  * Almost-equal for IEEE floats, using absolute difference method.
320  *
321  * \param max_diff the maximum absolute difference.
322  */
323 MINLINE int compare_ff(float a, float b, const float max_diff)
324 {
325         return fabsf(a - b) <= max_diff;
326 }
327
328 /**
329  * Almost-equal for IEEE floats, using their integer representation (mixing ULP and absolute difference methods).
330  *
331  * \param max_diff is the maximum absolute difference (allows to take care of the near-zero area,
332  *                 where relative difference methods cannot really work).
333  * \param max_ulps is the 'maximum number of floats + 1' allowed between \a a and \a b to consider them equal.
334  *
335  * \see https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
336  */
337 MINLINE int compare_ff_relative(float a, float b, const float max_diff, const int max_ulps)
338 {
339         union {float f; int i;} ua, ub;
340
341 #if 0  /* No BLI_assert in INLINE :/ */
342         BLI_assert(sizeof(float) == sizeof(int));
343         BLI_assert(max_ulps < (1 << 22));
344 #endif
345
346         if (fabsf(a - b) <= max_diff) {
347                 return 1;
348         }
349
350         ua.f = a;
351         ub.f = b;
352
353         /* Important to compare sign from integers, since (-0.0f < 0) is false
354          * (though this shall not be an issue in common cases)... */
355         return ((ua.i < 0) != (ub.i < 0)) ? 0 : (abs(ua.i - ub.i) <= max_ulps) ? 1 : 0;
356 }
357
358 MINLINE float signf(float f)
359 {
360         return (f < 0.f) ? -1.f : 1.f;
361 }
362
363 MINLINE int signum_i_ex(float a, float eps)
364 {
365         if (a >  eps) return  1;
366         if (a < -eps) return -1;
367         else          return  0;
368 }
369
370 MINLINE int signum_i(float a)
371 {
372         if (a > 0.0f) return  1;
373         if (a < 0.0f) return -1;
374         else          return  0;
375 }
376
377 /** Returns number of (base ten) *significant* digits of integer part of given float
378  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
379 MINLINE int integer_digits_f(const float f)
380 {
381         return (f == 0.0f) ? 0 : (int)floor(log10(fabs(f))) + 1;
382 }
383
384 /** Returns number of (base ten) *significant* digits of integer part of given double
385  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
386 MINLINE int integer_digits_d(const double d)
387 {
388         return (d == 0.0) ? 0 : (int)floor(log10(fabs(d))) + 1;
389 }
390
391
392 /* Internal helpers for SSE2 implementation.
393  *
394  * NOTE: Are to be called ONLY from inside `#ifdef __SSE2__` !!!
395  */
396
397 #ifdef __SSE2__
398
399 /* Calculate initial guess for arg^exp based on float representation
400  * This method gives a constant bias, which can be easily compensated by
401  * multiplicating with bias_coeff.
402  * Gives better results for exponents near 1 (e. g. 4/5).
403  * exp = exponent, encoded as uint32_t
404  * e2coeff = 2^(127/exponent - 127) * bias_coeff^(1/exponent), encoded as
405  * uint32_t
406  *
407  * We hope that exp and e2coeff gets properly inlined
408  */
409 MALWAYS_INLINE __m128 _bli_math_fastpow(const int exp,
410                                         const int e2coeff,
411                                         const __m128 arg)
412 {
413         __m128 ret;
414         ret = _mm_mul_ps(arg, _mm_castsi128_ps(_mm_set1_epi32(e2coeff)));
415         ret = _mm_cvtepi32_ps(_mm_castps_si128(ret));
416         ret = _mm_mul_ps(ret, _mm_castsi128_ps(_mm_set1_epi32(exp)));
417         ret = _mm_castsi128_ps(_mm_cvtps_epi32(ret));
418         return ret;
419 }
420
421 /* Improve x ^ 1.0f/5.0f solution with Newton-Raphson method */
422 MALWAYS_INLINE __m128 _bli_math_improve_5throot_solution(
423     const __m128 old_result,
424     const __m128 x)
425 {
426         __m128 approx2 = _mm_mul_ps(old_result, old_result);
427         __m128 approx4 = _mm_mul_ps(approx2, approx2);
428         __m128 t = _mm_div_ps(x, approx4);
429         __m128 summ = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(4.0f), old_result), t); /* fma */
430         return _mm_mul_ps(summ, _mm_set1_ps(1.0f / 5.0f));
431 }
432
433 /* Calculate powf(x, 2.4). Working domain: 1e-10 < x < 1e+10 */
434 MALWAYS_INLINE __m128 _bli_math_fastpow24(const __m128 arg)
435 {
436         /* max, avg and |avg| errors were calculated in gcc without FMA instructions
437          * The final precision should be better than powf in glibc */
438
439         /* Calculate x^4/5, coefficient 0.994 was constructed manually to minimize
440          * avg error.
441          */
442         /* 0x3F4CCCCD = 4/5 */
443         /* 0x4F55A7FB = 2^(127/(4/5) - 127) * 0.994^(1/(4/5)) */
444         /* error max = 0.17     avg = 0.0018    |avg| = 0.05 */
445         __m128 x = _bli_math_fastpow(0x3F4CCCCD, 0x4F55A7FB, arg);
446         __m128 arg2 = _mm_mul_ps(arg, arg);
447         __m128 arg4 = _mm_mul_ps(arg2, arg2);
448         /* error max = 0.018        avg = 0.0031    |avg| = 0.0031  */
449         x = _bli_math_improve_5throot_solution(x, arg4);
450         /* error max = 0.00021    avg = 1.6e-05    |avg| = 1.6e-05 */
451         x = _bli_math_improve_5throot_solution(x, arg4);
452         /* error max = 6.1e-07    avg = 5.2e-08    |avg| = 1.1e-07 */
453         x = _bli_math_improve_5throot_solution(x, arg4);
454         return _mm_mul_ps(x, _mm_mul_ps(x, x));
455 }
456
457 /* Calculate powf(x, 1.0f / 2.4) */
458 MALWAYS_INLINE __m128 _bli_math_fastpow512(const __m128 arg)
459 {
460         /* 5/12 is too small, so compute the 4th root of 20/12 instead.
461          * 20/12 = 5/3 = 1 + 2/3 = 2 - 1/3. 2/3 is a suitable argument for fastpow.
462          * weighting coefficient: a^-1/2 = 2 a; a = 2^-2/3
463          */
464         __m128 xf = _bli_math_fastpow(0x3f2aaaab, 0x5eb504f3, arg);
465         __m128 xover = _mm_mul_ps(arg, xf);
466         __m128 xfm1 = _mm_rsqrt_ps(xf);
467         __m128 x2 = _mm_mul_ps(arg, arg);
468         __m128 xunder = _mm_mul_ps(x2, xfm1);
469         /* sqrt2 * over + 2 * sqrt2 * under */
470         __m128 xavg = _mm_mul_ps(_mm_set1_ps(1.0f / (3.0f * 0.629960524947437f) * 0.999852f),
471                                  _mm_add_ps(xover, xunder));
472         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
473         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
474         return xavg;
475 }
476
477 MALWAYS_INLINE __m128 _bli_math_blend_sse(const __m128 mask,
478                                           const __m128 a,
479                                           const __m128 b)
480 {
481         return _mm_or_ps(_mm_and_ps(mask, a), _mm_andnot_ps(mask, b));
482 }
483
484 #endif  /* __SSE2__ */
485
486 #endif /* __MATH_BASE_INLINE_C__ */