1/**************************************************************************/
2/* math_funcs.h */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#ifndef MATH_FUNCS_H
32#define MATH_FUNCS_H
33
34#include "core/error/error_macros.h"
35#include "core/math/math_defs.h"
36#include "core/math/random_pcg.h"
37#include "core/typedefs.h"
38
39#include "thirdparty/misc/pcg.h"
40
41#include <float.h>
42#include <math.h>
43
44class Math {
45 static RandomPCG default_rand;
46
47public:
48 Math() {} // useless to instance
49
50 // Not using 'RANDOM_MAX' to avoid conflict with system headers on some OSes (at least NetBSD).
51 static const uint64_t RANDOM_32BIT_MAX = 0xFFFFFFFF;
52
53 static _ALWAYS_INLINE_ double sin(double p_x) { return ::sin(p_x); }
54 static _ALWAYS_INLINE_ float sin(float p_x) { return ::sinf(p_x); }
55
56 static _ALWAYS_INLINE_ double cos(double p_x) { return ::cos(p_x); }
57 static _ALWAYS_INLINE_ float cos(float p_x) { return ::cosf(p_x); }
58
59 static _ALWAYS_INLINE_ double tan(double p_x) { return ::tan(p_x); }
60 static _ALWAYS_INLINE_ float tan(float p_x) { return ::tanf(p_x); }
61
62 static _ALWAYS_INLINE_ double sinh(double p_x) { return ::sinh(p_x); }
63 static _ALWAYS_INLINE_ float sinh(float p_x) { return ::sinhf(p_x); }
64
65 static _ALWAYS_INLINE_ float sinc(float p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; }
66 static _ALWAYS_INLINE_ double sinc(double p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; }
67
68 static _ALWAYS_INLINE_ float sincn(float p_x) { return sinc((float)Math_PI * p_x); }
69 static _ALWAYS_INLINE_ double sincn(double p_x) { return sinc(Math_PI * p_x); }
70
71 static _ALWAYS_INLINE_ double cosh(double p_x) { return ::cosh(p_x); }
72 static _ALWAYS_INLINE_ float cosh(float p_x) { return ::coshf(p_x); }
73
74 static _ALWAYS_INLINE_ double tanh(double p_x) { return ::tanh(p_x); }
75 static _ALWAYS_INLINE_ float tanh(float p_x) { return ::tanhf(p_x); }
76
77 // Always does clamping so always safe to use.
78 static _ALWAYS_INLINE_ double asin(double p_x) { return p_x < -1 ? (-Math_PI / 2) : (p_x > 1 ? (Math_PI / 2) : ::asin(p_x)); }
79 static _ALWAYS_INLINE_ float asin(float p_x) { return p_x < -1 ? (-Math_PI / 2) : (p_x > 1 ? (Math_PI / 2) : ::asinf(p_x)); }
80
81 // Always does clamping so always safe to use.
82 static _ALWAYS_INLINE_ double acos(double p_x) { return p_x < -1 ? Math_PI : (p_x > 1 ? 0 : ::acos(p_x)); }
83 static _ALWAYS_INLINE_ float acos(float p_x) { return p_x < -1 ? Math_PI : (p_x > 1 ? 0 : ::acosf(p_x)); }
84
85 static _ALWAYS_INLINE_ double atan(double p_x) { return ::atan(p_x); }
86 static _ALWAYS_INLINE_ float atan(float p_x) { return ::atanf(p_x); }
87
88 static _ALWAYS_INLINE_ double atan2(double p_y, double p_x) { return ::atan2(p_y, p_x); }
89 static _ALWAYS_INLINE_ float atan2(float p_y, float p_x) { return ::atan2f(p_y, p_x); }
90
91 static _ALWAYS_INLINE_ double asinh(double p_x) { return ::asinh(p_x); }
92 static _ALWAYS_INLINE_ float asinh(float p_x) { return ::asinhf(p_x); }
93
94 // Always does clamping so always safe to use.
95 static _ALWAYS_INLINE_ double acosh(double p_x) { return p_x < 1 ? 0 : ::acosh(p_x); }
96 static _ALWAYS_INLINE_ float acosh(float p_x) { return p_x < 1 ? 0 : ::acoshf(p_x); }
97
98 // Always does clamping so always safe to use.
99 static _ALWAYS_INLINE_ double atanh(double p_x) { return p_x <= -1 ? -INFINITY : (p_x >= 1 ? INFINITY : ::atanh(p_x)); }
100 static _ALWAYS_INLINE_ float atanh(float p_x) { return p_x <= -1 ? -INFINITY : (p_x >= 1 ? INFINITY : ::atanhf(p_x)); }
101
102 static _ALWAYS_INLINE_ double sqrt(double p_x) { return ::sqrt(p_x); }
103 static _ALWAYS_INLINE_ float sqrt(float p_x) { return ::sqrtf(p_x); }
104
105 static _ALWAYS_INLINE_ double fmod(double p_x, double p_y) { return ::fmod(p_x, p_y); }
106 static _ALWAYS_INLINE_ float fmod(float p_x, float p_y) { return ::fmodf(p_x, p_y); }
107
108 static _ALWAYS_INLINE_ double floor(double p_x) { return ::floor(p_x); }
109 static _ALWAYS_INLINE_ float floor(float p_x) { return ::floorf(p_x); }
110
111 static _ALWAYS_INLINE_ double ceil(double p_x) { return ::ceil(p_x); }
112 static _ALWAYS_INLINE_ float ceil(float p_x) { return ::ceilf(p_x); }
113
114 static _ALWAYS_INLINE_ double pow(double p_x, double p_y) { return ::pow(p_x, p_y); }
115 static _ALWAYS_INLINE_ float pow(float p_x, float p_y) { return ::powf(p_x, p_y); }
116
117 static _ALWAYS_INLINE_ double log(double p_x) { return ::log(p_x); }
118 static _ALWAYS_INLINE_ float log(float p_x) { return ::logf(p_x); }
119
120 static _ALWAYS_INLINE_ double log1p(double p_x) { return ::log1p(p_x); }
121 static _ALWAYS_INLINE_ float log1p(float p_x) { return ::log1pf(p_x); }
122
123 static _ALWAYS_INLINE_ double log2(double p_x) { return ::log2(p_x); }
124 static _ALWAYS_INLINE_ float log2(float p_x) { return ::log2f(p_x); }
125
126 static _ALWAYS_INLINE_ double exp(double p_x) { return ::exp(p_x); }
127 static _ALWAYS_INLINE_ float exp(float p_x) { return ::expf(p_x); }
128
129 static _ALWAYS_INLINE_ bool is_nan(double p_val) {
130#ifdef _MSC_VER
131 return _isnan(p_val);
132#elif defined(__GNUC__) && __GNUC__ < 6
133 union {
134 uint64_t u;
135 double f;
136 } ieee754;
137 ieee754.f = p_val;
138 // (unsigned)(0x7ff0000000000001 >> 32) : 0x7ff00000
139 return ((((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0)) > 0x7ff00000);
140#else
141 return isnan(p_val);
142#endif
143 }
144
145 static _ALWAYS_INLINE_ bool is_nan(float p_val) {
146#ifdef _MSC_VER
147 return _isnan(p_val);
148#elif defined(__GNUC__) && __GNUC__ < 6
149 union {
150 uint32_t u;
151 float f;
152 } ieee754;
153 ieee754.f = p_val;
154 // -----------------------------------
155 // (single-precision floating-point)
156 // NaN : s111 1111 1xxx xxxx xxxx xxxx xxxx xxxx
157 // : (> 0x7f800000)
158 // where,
159 // s : sign
160 // x : non-zero number
161 // -----------------------------------
162 return ((ieee754.u & 0x7fffffff) > 0x7f800000);
163#else
164 return isnan(p_val);
165#endif
166 }
167
168 static _ALWAYS_INLINE_ bool is_inf(double p_val) {
169#ifdef _MSC_VER
170 return !_finite(p_val);
171// use an inline implementation of isinf as a workaround for problematic libstdc++ versions from gcc 5.x era
172#elif defined(__GNUC__) && __GNUC__ < 6
173 union {
174 uint64_t u;
175 double f;
176 } ieee754;
177 ieee754.f = p_val;
178 return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 &&
179 ((unsigned)ieee754.u == 0);
180#else
181 return isinf(p_val);
182#endif
183 }
184
185 static _ALWAYS_INLINE_ bool is_inf(float p_val) {
186#ifdef _MSC_VER
187 return !_finite(p_val);
188// use an inline implementation of isinf as a workaround for problematic libstdc++ versions from gcc 5.x era
189#elif defined(__GNUC__) && __GNUC__ < 6
190 union {
191 uint32_t u;
192 float f;
193 } ieee754;
194 ieee754.f = p_val;
195 return (ieee754.u & 0x7fffffff) == 0x7f800000;
196#else
197 return isinf(p_val);
198#endif
199 }
200
201 static _ALWAYS_INLINE_ bool is_finite(double p_val) { return isfinite(p_val); }
202 static _ALWAYS_INLINE_ bool is_finite(float p_val) { return isfinite(p_val); }
203
204 static _ALWAYS_INLINE_ double abs(double g) { return absd(g); }
205 static _ALWAYS_INLINE_ float abs(float g) { return absf(g); }
206 static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; }
207
208 static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) {
209 double value = Math::fmod(p_x, p_y);
210 if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
211 value += p_y;
212 }
213 value += 0.0;
214 return value;
215 }
216 static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) {
217 float value = Math::fmod(p_x, p_y);
218 if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
219 value += p_y;
220 }
221 value += 0.0f;
222 return value;
223 }
224 static _ALWAYS_INLINE_ float fposmodp(float p_x, float p_y) {
225 float value = Math::fmod(p_x, p_y);
226 if (value < 0) {
227 value += p_y;
228 }
229 value += 0.0f;
230 return value;
231 }
232 static _ALWAYS_INLINE_ double fposmodp(double p_x, double p_y) {
233 double value = Math::fmod(p_x, p_y);
234 if (value < 0) {
235 value += p_y;
236 }
237 value += 0.0;
238 return value;
239 }
240
241 static _ALWAYS_INLINE_ int64_t posmod(int64_t p_x, int64_t p_y) {
242 ERR_FAIL_COND_V_MSG(p_y == 0, 0, "Division by zero in posmod is undefined. Returning 0 as fallback.");
243 int64_t value = p_x % p_y;
244 if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
245 value += p_y;
246 }
247 return value;
248 }
249
250 static _ALWAYS_INLINE_ double deg_to_rad(double p_y) { return p_y * (Math_PI / 180.0); }
251 static _ALWAYS_INLINE_ float deg_to_rad(float p_y) { return p_y * (float)(Math_PI / 180.0); }
252
253 static _ALWAYS_INLINE_ double rad_to_deg(double p_y) { return p_y * (180.0 / Math_PI); }
254 static _ALWAYS_INLINE_ float rad_to_deg(float p_y) { return p_y * (float)(180.0 / Math_PI); }
255
256 static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; }
257 static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; }
258
259 static _ALWAYS_INLINE_ double cubic_interpolate(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
260 return 0.5 *
261 ((p_from * 2.0) +
262 (-p_pre + p_to) * p_weight +
263 (2.0 * p_pre - 5.0 * p_from + 4.0 * p_to - p_post) * (p_weight * p_weight) +
264 (-p_pre + 3.0 * p_from - 3.0 * p_to + p_post) * (p_weight * p_weight * p_weight));
265 }
266 static _ALWAYS_INLINE_ float cubic_interpolate(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
267 return 0.5f *
268 ((p_from * 2.0f) +
269 (-p_pre + p_to) * p_weight +
270 (2.0f * p_pre - 5.0f * p_from + 4.0f * p_to - p_post) * (p_weight * p_weight) +
271 (-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight));
272 }
273
274 static _ALWAYS_INLINE_ double cubic_interpolate_angle(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
275 double from_rot = fmod(p_from, Math_TAU);
276
277 double pre_diff = fmod(p_pre - from_rot, Math_TAU);
278 double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff;
279
280 double to_diff = fmod(p_to - from_rot, Math_TAU);
281 double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff;
282
283 double post_diff = fmod(p_post - to_rot, Math_TAU);
284 double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff;
285
286 return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
287 }
288
289 static _ALWAYS_INLINE_ float cubic_interpolate_angle(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
290 float from_rot = fmod(p_from, (float)Math_TAU);
291
292 float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU);
293 float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff;
294
295 float to_diff = fmod(p_to - from_rot, (float)Math_TAU);
296 float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff;
297
298 float post_diff = fmod(p_post - to_rot, (float)Math_TAU);
299 float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff;
300
301 return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
302 }
303
304 static _ALWAYS_INLINE_ double cubic_interpolate_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
305 double p_to_t, double p_pre_t, double p_post_t) {
306 /* Barry-Goldman method */
307 double t = Math::lerp(0.0, p_to_t, p_weight);
308 double a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0 : (t - p_pre_t) / -p_pre_t);
309 double a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5 : t / p_to_t);
310 double a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0 : (t - p_to_t) / (p_post_t - p_to_t));
311 double b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0 : (t - p_pre_t) / (p_to_t - p_pre_t));
312 double b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t);
313 return Math::lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t);
314 }
315
316 static _ALWAYS_INLINE_ float cubic_interpolate_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
317 float p_to_t, float p_pre_t, float p_post_t) {
318 /* Barry-Goldman method */
319 float t = Math::lerp(0.0f, p_to_t, p_weight);
320 float a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0f : (t - p_pre_t) / -p_pre_t);
321 float a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5f : t / p_to_t);
322 float a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0f : (t - p_to_t) / (p_post_t - p_to_t));
323 float b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0f : (t - p_pre_t) / (p_to_t - p_pre_t));
324 float b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0f : t / p_post_t);
325 return Math::lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_t);
326 }
327
328 static _ALWAYS_INLINE_ double cubic_interpolate_angle_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
329 double p_to_t, double p_pre_t, double p_post_t) {
330 double from_rot = fmod(p_from, Math_TAU);
331
332 double pre_diff = fmod(p_pre - from_rot, Math_TAU);
333 double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff;
334
335 double to_diff = fmod(p_to - from_rot, Math_TAU);
336 double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff;
337
338 double post_diff = fmod(p_post - to_rot, Math_TAU);
339 double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff;
340
341 return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
342 }
343
344 static _ALWAYS_INLINE_ float cubic_interpolate_angle_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
345 float p_to_t, float p_pre_t, float p_post_t) {
346 float from_rot = fmod(p_from, (float)Math_TAU);
347
348 float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU);
349 float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff;
350
351 float to_diff = fmod(p_to - from_rot, (float)Math_TAU);
352 float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff;
353
354 float post_diff = fmod(p_post - to_rot, (float)Math_TAU);
355 float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff;
356
357 return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
358 }
359
360 static _ALWAYS_INLINE_ double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
361 /* Formula from Wikipedia article on Bezier curves. */
362 double omt = (1.0 - p_t);
363 double omt2 = omt * omt;
364 double omt3 = omt2 * omt;
365 double t2 = p_t * p_t;
366 double t3 = t2 * p_t;
367
368 return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
369 }
370
371 static _ALWAYS_INLINE_ float bezier_interpolate(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) {
372 /* Formula from Wikipedia article on Bezier curves. */
373 float omt = (1.0f - p_t);
374 float omt2 = omt * omt;
375 float omt3 = omt2 * omt;
376 float t2 = p_t * p_t;
377 float t3 = t2 * p_t;
378
379 return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0f + p_control_2 * omt * t2 * 3.0f + p_end * t3;
380 }
381
382 static _ALWAYS_INLINE_ double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
383 /* Formula from Wikipedia article on Bezier curves. */
384 double omt = (1.0 - p_t);
385 double omt2 = omt * omt;
386 double t2 = p_t * p_t;
387
388 double d = (p_control_1 - p_start) * 3.0 * omt2 + (p_control_2 - p_control_1) * 6.0 * omt * p_t + (p_end - p_control_2) * 3.0 * t2;
389 return d;
390 }
391
392 static _ALWAYS_INLINE_ float bezier_derivative(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) {
393 /* Formula from Wikipedia article on Bezier curves. */
394 float omt = (1.0f - p_t);
395 float omt2 = omt * omt;
396 float t2 = p_t * p_t;
397
398 float d = (p_control_1 - p_start) * 3.0f * omt2 + (p_control_2 - p_control_1) * 6.0f * omt * p_t + (p_end - p_control_2) * 3.0f * t2;
399 return d;
400 }
401
402 static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
403 double difference = fmod(p_to - p_from, Math_TAU);
404 double distance = fmod(2.0 * difference, Math_TAU) - difference;
405 return p_from + distance * p_weight;
406 }
407 static _ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) {
408 float difference = fmod(p_to - p_from, (float)Math_TAU);
409 float distance = fmod(2.0f * difference, (float)Math_TAU) - difference;
410 return p_from + distance * p_weight;
411 }
412
413 static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) {
414 return (p_value - p_from) / (p_to - p_from);
415 }
416 static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) {
417 return (p_value - p_from) / (p_to - p_from);
418 }
419
420 static _ALWAYS_INLINE_ double remap(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) {
421 return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value));
422 }
423 static _ALWAYS_INLINE_ float remap(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) {
424 return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value));
425 }
426
427 static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_s) {
428 if (is_equal_approx(p_from, p_to)) {
429 return p_from;
430 }
431 double s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0, 1.0);
432 return s * s * (3.0 - 2.0 * s);
433 }
434 static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_s) {
435 if (is_equal_approx(p_from, p_to)) {
436 return p_from;
437 }
438 float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f);
439 return s * s * (3.0f - 2.0f * s);
440 }
441 static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) {
442 return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
443 }
444 static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) {
445 return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
446 }
447
448 static _ALWAYS_INLINE_ double linear_to_db(double p_linear) {
449 return Math::log(p_linear) * 8.6858896380650365530225783783321;
450 }
451 static _ALWAYS_INLINE_ float linear_to_db(float p_linear) {
452 return Math::log(p_linear) * (float)8.6858896380650365530225783783321;
453 }
454
455 static _ALWAYS_INLINE_ double db_to_linear(double p_db) {
456 return Math::exp(p_db * 0.11512925464970228420089957273422);
457 }
458 static _ALWAYS_INLINE_ float db_to_linear(float p_db) {
459 return Math::exp(p_db * (float)0.11512925464970228420089957273422);
460 }
461
462 static _ALWAYS_INLINE_ double round(double p_val) { return ::round(p_val); }
463 static _ALWAYS_INLINE_ float round(float p_val) { return ::roundf(p_val); }
464
465 static _ALWAYS_INLINE_ int64_t wrapi(int64_t value, int64_t min, int64_t max) {
466 int64_t range = max - min;
467 return range == 0 ? min : min + ((((value - min) % range) + range) % range);
468 }
469 static _ALWAYS_INLINE_ double wrapf(double value, double min, double max) {
470 double range = max - min;
471 if (is_zero_approx(range)) {
472 return min;
473 }
474 double result = value - (range * Math::floor((value - min) / range));
475 if (is_equal_approx(result, max)) {
476 return min;
477 }
478 return result;
479 }
480 static _ALWAYS_INLINE_ float wrapf(float value, float min, float max) {
481 float range = max - min;
482 if (is_zero_approx(range)) {
483 return min;
484 }
485 float result = value - (range * Math::floor((value - min) / range));
486 if (is_equal_approx(result, max)) {
487 return min;
488 }
489 return result;
490 }
491
492 static _ALWAYS_INLINE_ float fract(float value) {
493 return value - floor(value);
494 }
495 static _ALWAYS_INLINE_ double fract(double value) {
496 return value - floor(value);
497 }
498 static _ALWAYS_INLINE_ float pingpong(float value, float length) {
499 return (length != 0.0f) ? abs(fract((value - length) / (length * 2.0f)) * length * 2.0f - length) : 0.0f;
500 }
501 static _ALWAYS_INLINE_ double pingpong(double value, double length) {
502 return (length != 0.0) ? abs(fract((value - length) / (length * 2.0)) * length * 2.0 - length) : 0.0;
503 }
504
505 // double only, as these functions are mainly used by the editor and not performance-critical,
506 static double ease(double p_x, double p_c);
507 static int step_decimals(double p_step);
508 static int range_step_decimals(double p_step); // For editor use only.
509 static double snapped(double p_value, double p_step);
510
511 static uint32_t larger_prime(uint32_t p_val);
512
513 static void seed(uint64_t x);
514 static void randomize();
515 static uint32_t rand_from_seed(uint64_t *seed);
516 static uint32_t rand();
517 static _ALWAYS_INLINE_ double randd() { return (double)rand() / (double)Math::RANDOM_32BIT_MAX; }
518 static _ALWAYS_INLINE_ float randf() { return (float)rand() / (float)Math::RANDOM_32BIT_MAX; }
519 static double randfn(double mean, double deviation);
520
521 static double random(double from, double to);
522 static float random(float from, float to);
523 static int random(int from, int to);
524
525 static _ALWAYS_INLINE_ bool is_equal_approx(float a, float b) {
526 // Check for exact equality first, required to handle "infinity" values.
527 if (a == b) {
528 return true;
529 }
530 // Then check for approximate equality.
531 float tolerance = (float)CMP_EPSILON * abs(a);
532 if (tolerance < (float)CMP_EPSILON) {
533 tolerance = (float)CMP_EPSILON;
534 }
535 return abs(a - b) < tolerance;
536 }
537
538 static _ALWAYS_INLINE_ bool is_equal_approx(float a, float b, float tolerance) {
539 // Check for exact equality first, required to handle "infinity" values.
540 if (a == b) {
541 return true;
542 }
543 // Then check for approximate equality.
544 return abs(a - b) < tolerance;
545 }
546
547 static _ALWAYS_INLINE_ bool is_zero_approx(float s) {
548 return abs(s) < (float)CMP_EPSILON;
549 }
550
551 static _ALWAYS_INLINE_ bool is_equal_approx(double a, double b) {
552 // Check for exact equality first, required to handle "infinity" values.
553 if (a == b) {
554 return true;
555 }
556 // Then check for approximate equality.
557 double tolerance = CMP_EPSILON * abs(a);
558 if (tolerance < CMP_EPSILON) {
559 tolerance = CMP_EPSILON;
560 }
561 return abs(a - b) < tolerance;
562 }
563
564 static _ALWAYS_INLINE_ bool is_equal_approx(double a, double b, double tolerance) {
565 // Check for exact equality first, required to handle "infinity" values.
566 if (a == b) {
567 return true;
568 }
569 // Then check for approximate equality.
570 return abs(a - b) < tolerance;
571 }
572
573 static _ALWAYS_INLINE_ bool is_zero_approx(double s) {
574 return abs(s) < CMP_EPSILON;
575 }
576
577 static _ALWAYS_INLINE_ float absf(float g) {
578 union {
579 float f;
580 uint32_t i;
581 } u;
582
583 u.f = g;
584 u.i &= 2147483647u;
585 return u.f;
586 }
587
588 static _ALWAYS_INLINE_ double absd(double g) {
589 union {
590 double d;
591 uint64_t i;
592 } u;
593 u.d = g;
594 u.i &= (uint64_t)9223372036854775807ll;
595 return u.d;
596 }
597
598 // This function should be as fast as possible and rounding mode should not matter.
599 static _ALWAYS_INLINE_ int fast_ftoi(float a) {
600 // Assuming every supported compiler has `lrint()`.
601 return lrintf(a);
602 }
603
604 static _ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t h) {
605 uint16_t h_exp, h_sig;
606 uint32_t f_sgn, f_exp, f_sig;
607
608 h_exp = (h & 0x7c00u);
609 f_sgn = ((uint32_t)h & 0x8000u) << 16;
610 switch (h_exp) {
611 case 0x0000u: /* 0 or subnormal */
612 h_sig = (h & 0x03ffu);
613 /* Signed zero */
614 if (h_sig == 0) {
615 return f_sgn;
616 }
617 /* Subnormal */
618 h_sig <<= 1;
619 while ((h_sig & 0x0400u) == 0) {
620 h_sig <<= 1;
621 h_exp++;
622 }
623 f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
624 f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
625 return f_sgn + f_exp + f_sig;
626 case 0x7c00u: /* inf or NaN */
627 /* All-ones exponent and a copy of the significand */
628 return f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
629 default: /* normalized */
630 /* Just need to adjust the exponent and shift */
631 return f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
632 }
633 }
634
635 static _ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *h) {
636 union {
637 uint32_t u32;
638 float f32;
639 } u;
640
641 u.u32 = halfbits_to_floatbits(*h);
642 return u.f32;
643 }
644
645 static _ALWAYS_INLINE_ float half_to_float(const uint16_t h) {
646 return halfptr_to_float(&h);
647 }
648
649 static _ALWAYS_INLINE_ uint16_t make_half_float(float f) {
650 union {
651 float fv;
652 uint32_t ui;
653 } ci;
654 ci.fv = f;
655
656 uint32_t x = ci.ui;
657 uint32_t sign = (unsigned short)(x >> 31);
658 uint32_t mantissa;
659 uint32_t exponent;
660 uint16_t hf;
661
662 // get mantissa
663 mantissa = x & ((1 << 23) - 1);
664 // get exponent bits
665 exponent = x & (0xFF << 23);
666 if (exponent >= 0x47800000) {
667 // check if the original single precision float number is a NaN
668 if (mantissa && (exponent == (0xFF << 23))) {
669 // we have a single precision NaN
670 mantissa = (1 << 23) - 1;
671 } else {
672 // 16-bit half-float representation stores number as Inf
673 mantissa = 0;
674 }
675 hf = (((uint16_t)sign) << 15) | (uint16_t)((0x1F << 10)) |
676 (uint16_t)(mantissa >> 13);
677 }
678 // check if exponent is <= -15
679 else if (exponent <= 0x38000000) {
680 /*
681 // store a denorm half-float value or zero
682 exponent = (0x38000000 - exponent) >> 23;
683 mantissa >>= (14 + exponent);
684
685 hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa);
686 */
687 hf = 0; //denormals do not work for 3D, convert to zero
688 } else {
689 hf = (((uint16_t)sign) << 15) |
690 (uint16_t)((exponent - 0x38000000) >> 13) |
691 (uint16_t)(mantissa >> 13);
692 }
693
694 return hf;
695 }
696
697 static _ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) {
698 return p_step != 0 ? Math::snapped(p_target - p_offset, p_step) + p_offset : p_target;
699 }
700
701 static _ALWAYS_INLINE_ float snap_scalar_separation(float p_offset, float p_step, float p_target, float p_separation) {
702 if (p_step != 0) {
703 float a = Math::snapped(p_target - p_offset, p_step + p_separation) + p_offset;
704 float b = a;
705 if (p_target >= 0) {
706 b -= p_separation;
707 } else {
708 b += p_step;
709 }
710 return (Math::abs(p_target - a) < Math::abs(p_target - b)) ? a : b;
711 }
712 return p_target;
713 }
714};
715
716#endif // MATH_FUNCS_H
717