Math Lib: add divide_floor_i
[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
37 #ifdef __SSE2__
38 #  include <emmintrin.h>
39 #endif
40
41 #include "BLI_math_base.h"
42
43 /* copied from BLI_utildefines.h */
44 #ifdef __GNUC__
45 #  define UNLIKELY(x)     __builtin_expect(!!(x), 0)
46 #else
47 #  define UNLIKELY(x)     (x)
48 #endif
49
50 /* powf is really slow for raising to integer powers. */
51 MINLINE float pow2f(float x)
52 {
53         return x * x;
54 }
55 MINLINE float pow3f(float x)
56 {
57         return pow2f(x) * x;
58 }
59 MINLINE float pow4f(float x)
60 {
61         return pow2f(pow2f(x));
62 }
63 MINLINE float pow7f(float x)
64 {
65         return pow2f(pow3f(x)) * x;
66 }
67
68 MINLINE float sqrt3f(float f)
69 {
70         if      (UNLIKELY(f == 0.0f)) return 0.0f;
71         else if (UNLIKELY(f <  0.0f)) return -(float)(exp(log(-f) / 3.0));
72         else                          return  (float)(exp(log( f) / 3.0));
73 }
74
75 MINLINE double sqrt3d(double d)
76 {
77         if      (UNLIKELY(d == 0.0)) return 0.0;
78         else if (UNLIKELY(d <  0.0)) return -exp(log(-d) / 3.0);
79         else                         return  exp(log( d) / 3.0);
80 }
81
82 MINLINE float sqrtf_signed(float f)
83 {
84         return (f >= 0.0f) ? sqrtf(f) : -sqrtf(-f);
85 }
86
87 MINLINE float saacos(float fac)
88 {
89         if      (UNLIKELY(fac <= -1.0f)) return (float)M_PI;
90         else if (UNLIKELY(fac >=  1.0f)) return 0.0f;
91         else                             return acosf(fac);
92 }
93
94 MINLINE float saasin(float fac)
95 {
96         if      (UNLIKELY(fac <= -1.0f)) return (float)-M_PI / 2.0f;
97         else if (UNLIKELY(fac >=  1.0f)) return (float) M_PI / 2.0f;
98         else                             return asinf(fac);
99 }
100
101 MINLINE float sasqrt(float fac)
102 {
103         if (UNLIKELY(fac <= 0.0f)) return 0.0f;
104         else                       return sqrtf(fac);
105 }
106
107 MINLINE float saacosf(float fac)
108 {
109         if      (UNLIKELY(fac <= -1.0f)) return (float)M_PI;
110         else if (UNLIKELY(fac >=  1.0f)) return 0.0f;
111         else                             return acosf(fac);
112 }
113
114 MINLINE float saasinf(float fac)
115 {
116         if      (UNLIKELY(fac <= -1.0f)) return (float)-M_PI / 2.0f;
117         else if (UNLIKELY(fac >=  1.0f)) return (float) M_PI / 2.0f;
118         else                             return asinf(fac);
119 }
120
121 MINLINE float sasqrtf(float fac)
122 {
123         if (UNLIKELY(fac <= 0.0f)) return 0.0f;
124         else                       return sqrtf(fac);
125 }
126
127 MINLINE float interpf(float target, float origin, float fac)
128 {
129         return (fac * target) + (1.0f - fac) * origin;
130 }
131
132 /* used for zoom values*/
133 MINLINE float power_of_2(float val)
134 {
135         return (float)pow(2.0, ceil(log((double)val) / M_LN2));
136 }
137
138 MINLINE int is_power_of_2_i(int n)
139 {
140         return (n & (n - 1)) == 0;
141 }
142
143 MINLINE int power_of_2_max_i(int n)
144 {
145         if (is_power_of_2_i(n))
146                 return n;
147
148         do {
149                 n = n & (n - 1);
150         } while (!is_power_of_2_i(n));
151
152         return n * 2;
153 }
154
155 MINLINE int power_of_2_min_i(int n)
156 {
157         while (!is_power_of_2_i(n))
158                 n = n & (n - 1);
159
160         return n;
161 }
162
163 MINLINE unsigned int power_of_2_max_u(unsigned int x)
164 {
165         x -= 1;
166         x |= (x >>  1);
167         x |= (x >>  2);
168         x |= (x >>  4);
169         x |= (x >>  8);
170         x |= (x >> 16);
171         return x + 1;
172 }
173
174 MINLINE unsigned power_of_2_min_u(unsigned x)
175 {
176         x |= (x >>  1);
177         x |= (x >>  2);
178         x |= (x >>  4);
179         x |= (x >>  8);
180         x |= (x >> 16);
181         return x - (x >> 1);
182 }
183
184 MINLINE int iroundf(float a)
185 {
186         return (int)floorf(a + 0.5f);
187 }
188
189 /* integer division that rounds 0.5 up, particularly useful for color blending
190  * with integers, to avoid gradual darkening when rounding down */
191 MINLINE int divide_round_i(int a, int b)
192 {
193         return (2 * a + b) / (2 * b);
194 }
195
196 /**
197  * Integer division that floors negative result.
198  * \note This works like Python's int division.
199  */
200 MINLINE int divide_floor_i(int a, int b)
201 {
202         int d = a / b;
203         int r = a % b;  /* Optimizes into a single division. */
204         return r ? d - ((a < 0) ^ (b < 0)) : d;
205 }
206
207 /**
208  * modulo that handles negative numbers, works the same as Python's.
209  */
210 MINLINE int mod_i(int i, int n)
211 {
212         return (i % n + n) % n;
213 }
214
215 MINLINE float min_ff(float a, float b)
216 {
217         return (a < b) ? a : b;
218 }
219 MINLINE float max_ff(float a, float b)
220 {
221         return (a > b) ? a : b;
222 }
223
224 MINLINE int min_ii(int a, int b)
225 {
226         return (a < b) ? a : b;
227 }
228 MINLINE int max_ii(int a, int b)
229 {
230         return (b < a) ? a : b;
231 }
232
233 MINLINE float min_fff(float a, float b, float c)
234 {
235         return min_ff(min_ff(a, b), c);
236 }
237 MINLINE float max_fff(float a, float b, float c)
238 {
239         return max_ff(max_ff(a, b), c);
240 }
241
242 MINLINE int min_iii(int a, int b, int c)
243 {
244         return min_ii(min_ii(a, b), c);
245 }
246 MINLINE int max_iii(int a, int b, int c)
247 {
248         return max_ii(max_ii(a, b), c);
249 }
250
251 MINLINE float min_ffff(float a, float b, float c, float d)
252 {
253         return min_ff(min_fff(a, b, c), d);
254 }
255 MINLINE float max_ffff(float a, float b, float c, float d)
256 {
257         return max_ff(max_fff(a, b, c), d);
258 }
259
260 MINLINE int min_iiii(int a, int b, int c, int d)
261 {
262         return min_ii(min_iii(a, b, c), d);
263 }
264 MINLINE int max_iiii(int a, int b, int c, int d)
265 {
266         return max_ii(max_iii(a, b, c), d);
267 }
268
269 /**
270  * Almost-equal for IEEE floats, using absolute difference method.
271  *
272  * \param max_diff the maximum absolute difference.
273  */
274 MINLINE int compare_ff(float a, float b, const float max_diff)
275 {
276         return fabsf(a - b) <= max_diff;
277 }
278
279 /**
280  * Almost-equal for IEEE floats, using their integer representation (mixing ULP and absolute difference methods).
281  *
282  * \param max_diff is the maximum absolute difference (allows to take care of the near-zero area,
283  *                 where relative difference methods cannot really work).
284  * \param max_ulps is the 'maximum number of floats + 1' allowed between \a a and \a b to consider them equal.
285  *
286  * \see https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
287  */
288 MINLINE int compare_ff_relative(float a, float b, const float max_diff, const int max_ulps)
289 {
290         union {float f; int i;} ua, ub;
291
292 #if 0  /* No BLI_assert in INLINE :/ */
293         BLI_assert(sizeof(float) == sizeof(int));
294         BLI_assert(max_ulps < (1 << 22));
295 #endif
296
297         if (fabsf(a - b) <= max_diff) {
298                 return 1;
299         }
300
301         ua.f = a;
302         ub.f = b;
303
304         /* Important to compare sign from integers, since (-0.0f < 0) is false
305          * (though this shall not be an issue in common cases)... */
306         return ((ua.i < 0) != (ub.i < 0)) ? 0 : (abs(ua.i - ub.i) <= max_ulps) ? 1 : 0;
307 }
308
309 MINLINE float signf(float f)
310 {
311         return (f < 0.f) ? -1.f : 1.f;
312 }
313
314 MINLINE int signum_i_ex(float a, float eps)
315 {
316         if (a >  eps) return  1;
317         if (a < -eps) return -1;
318         else          return  0;
319 }
320
321 MINLINE int signum_i(float a)
322 {
323         if (a > 0.0f) return  1;
324         if (a < 0.0f) return -1;
325         else          return  0;
326 }
327
328 /** Returns number of (base ten) *significant* digits of integer part of given float
329  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
330 MINLINE int integer_digits_f(const float f)
331 {
332         return (f == 0.0f) ? 0 : (int)floor(log10(fabs(f))) + 1;
333 }
334
335 /** Returns number of (base ten) *significant* digits of integer part of given double
336  * (negative in case of decimal-only floats, 0.01 returns -1 e.g.). */
337 MINLINE int integer_digits_d(const double d)
338 {
339         return (d == 0.0) ? 0 : (int)floor(log10(fabs(d))) + 1;
340 }
341
342
343 /* Internal helpers for SSE2 implementation.
344  *
345  * NOTE: Are to be called ONLY from inside `#ifdef __SSE2__` !!!
346  */
347
348 #ifdef __SSE2__
349
350 /* Calculate initial guess for arg^exp based on float representation
351  * This method gives a constant bias, which can be easily compensated by
352  * multiplicating with bias_coeff.
353  * Gives better results for exponents near 1 (e. g. 4/5).
354  * exp = exponent, encoded as uint32_t
355  * e2coeff = 2^(127/exponent - 127) * bias_coeff^(1/exponent), encoded as
356  * uint32_t
357  *
358  * We hope that exp and e2coeff gets properly inlined
359  */
360 MALWAYS_INLINE __m128 _bli_math_fastpow(const int exp,
361                                         const int e2coeff,
362                                         const __m128 arg)
363 {
364         __m128 ret;
365         ret = _mm_mul_ps(arg, _mm_castsi128_ps(_mm_set1_epi32(e2coeff)));
366         ret = _mm_cvtepi32_ps(_mm_castps_si128(ret));
367         ret = _mm_mul_ps(ret, _mm_castsi128_ps(_mm_set1_epi32(exp)));
368         ret = _mm_castsi128_ps(_mm_cvtps_epi32(ret));
369         return ret;
370 }
371
372 /* Improve x ^ 1.0f/5.0f solution with Newton-Raphson method */
373 MALWAYS_INLINE __m128 _bli_math_improve_5throot_solution(
374     const __m128 old_result,
375     const __m128 x)
376 {
377         __m128 approx2 = _mm_mul_ps(old_result, old_result);
378         __m128 approx4 = _mm_mul_ps(approx2, approx2);
379         __m128 t = _mm_div_ps(x, approx4);
380         __m128 summ = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(4.0f), old_result), t); /* fma */
381         return _mm_mul_ps(summ, _mm_set1_ps(1.0f / 5.0f));
382 }
383
384 /* Calculate powf(x, 2.4). Working domain: 1e-10 < x < 1e+10 */
385 MALWAYS_INLINE __m128 _bli_math_fastpow24(const __m128 arg)
386 {
387         /* max, avg and |avg| errors were calculated in gcc without FMA instructions
388          * The final precision should be better than powf in glibc */
389
390         /* Calculate x^4/5, coefficient 0.994 was constructed manually to minimize
391          * avg error.
392          */
393         /* 0x3F4CCCCD = 4/5 */
394         /* 0x4F55A7FB = 2^(127/(4/5) - 127) * 0.994^(1/(4/5)) */
395         /* error max = 0.17     avg = 0.0018    |avg| = 0.05 */
396         __m128 x = _bli_math_fastpow(0x3F4CCCCD, 0x4F55A7FB, arg);
397         __m128 arg2 = _mm_mul_ps(arg, arg);
398         __m128 arg4 = _mm_mul_ps(arg2, arg2);
399         /* error max = 0.018        avg = 0.0031    |avg| = 0.0031  */
400         x = _bli_math_improve_5throot_solution(x, arg4);
401         /* error max = 0.00021    avg = 1.6e-05    |avg| = 1.6e-05 */
402         x = _bli_math_improve_5throot_solution(x, arg4);
403         /* error max = 6.1e-07    avg = 5.2e-08    |avg| = 1.1e-07 */
404         x = _bli_math_improve_5throot_solution(x, arg4);
405         return _mm_mul_ps(x, _mm_mul_ps(x, x));
406 }
407
408 /* Calculate powf(x, 1.0f / 2.4) */
409 MALWAYS_INLINE __m128 _bli_math_fastpow512(const __m128 arg)
410 {
411         /* 5/12 is too small, so compute the 4th root of 20/12 instead.
412          * 20/12 = 5/3 = 1 + 2/3 = 2 - 1/3. 2/3 is a suitable argument for fastpow.
413          * weighting coefficient: a^-1/2 = 2 a; a = 2^-2/3
414          */
415         __m128 xf = _bli_math_fastpow(0x3f2aaaab, 0x5eb504f3, arg);
416         __m128 xover = _mm_mul_ps(arg, xf);
417         __m128 xfm1 = _mm_rsqrt_ps(xf);
418         __m128 x2 = _mm_mul_ps(arg, arg);
419         __m128 xunder = _mm_mul_ps(x2, xfm1);
420         /* sqrt2 * over + 2 * sqrt2 * under */
421         __m128 xavg = _mm_mul_ps(_mm_set1_ps(1.0f / (3.0f * 0.629960524947437f) * 0.999852f),
422                                  _mm_add_ps(xover, xunder));
423         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
424         xavg = _mm_mul_ps(xavg, _mm_rsqrt_ps(xavg));
425         return xavg;
426 }
427
428 MALWAYS_INLINE __m128 _bli_math_blend_sse(const __m128 mask,
429                                           const __m128 a,
430                                           const __m128 b)
431 {
432         return _mm_or_ps(_mm_and_ps(mask, a), _mm_andnot_ps(mask, b));
433 }
434
435 #endif  /* __SSE2__ */
436
437 #endif /* __MATH_BASE_INLINE_C__ */