1 /*************************************************************************/ 2 /* math_funcs.h */ 3 /*************************************************************************/ 4 /* This file is part of: */ 5 /* GODOT ENGINE */ 6 /* https://godotengine.org */ 7 /*************************************************************************/ 8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ 9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ 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/math/math_defs.h" 35 #include "core/math/random_pcg.h" 36 #include "core/typedefs.h" 37 38 #include "thirdparty/misc/pcg.h" 39 40 #include <float.h> 41 #include <math.h> 42 43 class Math { 44 45 static RandomPCG default_rand; 46 47 public: Math()48 Math() {} // useless to instance 49 50 static const uint64_t RANDOM_MAX = 0xFFFFFFFF; 51 sin(double p_x)52 static _ALWAYS_INLINE_ double sin(double p_x) { return ::sin(p_x); } sin(float p_x)53 static _ALWAYS_INLINE_ float sin(float p_x) { return ::sinf(p_x); } 54 cos(double p_x)55 static _ALWAYS_INLINE_ double cos(double p_x) { return ::cos(p_x); } cos(float p_x)56 static _ALWAYS_INLINE_ float cos(float p_x) { return ::cosf(p_x); } 57 tan(double p_x)58 static _ALWAYS_INLINE_ double tan(double p_x) { return ::tan(p_x); } tan(float p_x)59 static _ALWAYS_INLINE_ float tan(float p_x) { return ::tanf(p_x); } 60 sinh(double p_x)61 static _ALWAYS_INLINE_ double sinh(double p_x) { return ::sinh(p_x); } sinh(float p_x)62 static _ALWAYS_INLINE_ float sinh(float p_x) { return ::sinhf(p_x); } 63 sinc(float p_x)64 static _ALWAYS_INLINE_ float sinc(float p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } sinc(double p_x)65 static _ALWAYS_INLINE_ double sinc(double p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } 66 sincn(float p_x)67 static _ALWAYS_INLINE_ float sincn(float p_x) { return sinc(Math_PI * p_x); } sincn(double p_x)68 static _ALWAYS_INLINE_ double sincn(double p_x) { return sinc(Math_PI * p_x); } 69 cosh(double p_x)70 static _ALWAYS_INLINE_ double cosh(double p_x) { return ::cosh(p_x); } cosh(float p_x)71 static _ALWAYS_INLINE_ float cosh(float p_x) { return ::coshf(p_x); } 72 tanh(double p_x)73 static _ALWAYS_INLINE_ double tanh(double p_x) { return ::tanh(p_x); } tanh(float p_x)74 static _ALWAYS_INLINE_ float tanh(float p_x) { return ::tanhf(p_x); } 75 asin(double p_x)76 static _ALWAYS_INLINE_ double asin(double p_x) { return ::asin(p_x); } asin(float p_x)77 static _ALWAYS_INLINE_ float asin(float p_x) { return ::asinf(p_x); } 78 acos(double p_x)79 static _ALWAYS_INLINE_ double acos(double p_x) { return ::acos(p_x); } acos(float p_x)80 static _ALWAYS_INLINE_ float acos(float p_x) { return ::acosf(p_x); } 81 atan(double p_x)82 static _ALWAYS_INLINE_ double atan(double p_x) { return ::atan(p_x); } atan(float p_x)83 static _ALWAYS_INLINE_ float atan(float p_x) { return ::atanf(p_x); } 84 atan2(double p_y,double p_x)85 static _ALWAYS_INLINE_ double atan2(double p_y, double p_x) { return ::atan2(p_y, p_x); } atan2(float p_y,float p_x)86 static _ALWAYS_INLINE_ float atan2(float p_y, float p_x) { return ::atan2f(p_y, p_x); } 87 sqrt(double p_x)88 static _ALWAYS_INLINE_ double sqrt(double p_x) { return ::sqrt(p_x); } sqrt(float p_x)89 static _ALWAYS_INLINE_ float sqrt(float p_x) { return ::sqrtf(p_x); } 90 fmod(double p_x,double p_y)91 static _ALWAYS_INLINE_ double fmod(double p_x, double p_y) { return ::fmod(p_x, p_y); } fmod(float p_x,float p_y)92 static _ALWAYS_INLINE_ float fmod(float p_x, float p_y) { return ::fmodf(p_x, p_y); } 93 floor(double p_x)94 static _ALWAYS_INLINE_ double floor(double p_x) { return ::floor(p_x); } floor(float p_x)95 static _ALWAYS_INLINE_ float floor(float p_x) { return ::floorf(p_x); } 96 ceil(double p_x)97 static _ALWAYS_INLINE_ double ceil(double p_x) { return ::ceil(p_x); } ceil(float p_x)98 static _ALWAYS_INLINE_ float ceil(float p_x) { return ::ceilf(p_x); } 99 pow(double p_x,double p_y)100 static _ALWAYS_INLINE_ double pow(double p_x, double p_y) { return ::pow(p_x, p_y); } pow(float p_x,float p_y)101 static _ALWAYS_INLINE_ float pow(float p_x, float p_y) { return ::powf(p_x, p_y); } 102 log(double p_x)103 static _ALWAYS_INLINE_ double log(double p_x) { return ::log(p_x); } log(float p_x)104 static _ALWAYS_INLINE_ float log(float p_x) { return ::logf(p_x); } 105 exp(double p_x)106 static _ALWAYS_INLINE_ double exp(double p_x) { return ::exp(p_x); } exp(float p_x)107 static _ALWAYS_INLINE_ float exp(float p_x) { return ::expf(p_x); } 108 is_nan(double p_val)109 static _ALWAYS_INLINE_ bool is_nan(double p_val) { 110 #ifdef _MSC_VER 111 return _isnan(p_val); 112 #elif defined(__GNUC__) && __GNUC__ < 6 113 union { 114 uint64_t u; 115 double f; 116 } ieee754; 117 ieee754.f = p_val; 118 // (unsigned)(0x7ff0000000000001 >> 32) : 0x7ff00000 119 return ((((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0)) > 0x7ff00000); 120 #else 121 return isnan(p_val); 122 #endif 123 } 124 is_nan(float p_val)125 static _ALWAYS_INLINE_ bool is_nan(float p_val) { 126 #ifdef _MSC_VER 127 return _isnan(p_val); 128 #elif defined(__GNUC__) && __GNUC__ < 6 129 union { 130 uint32_t u; 131 float f; 132 } ieee754; 133 ieee754.f = p_val; 134 // ----------------------------------- 135 // (single-precision floating-point) 136 // NaN : s111 1111 1xxx xxxx xxxx xxxx xxxx xxxx 137 // : (> 0x7f800000) 138 // where, 139 // s : sign 140 // x : non-zero number 141 // ----------------------------------- 142 return ((ieee754.u & 0x7fffffff) > 0x7f800000); 143 #else 144 return isnan(p_val); 145 #endif 146 } 147 is_inf(double p_val)148 static _ALWAYS_INLINE_ bool is_inf(double p_val) { 149 #ifdef _MSC_VER 150 return !_finite(p_val); 151 // use an inline implementation of isinf as a workaround for problematic libstdc++ versions from gcc 5.x era 152 #elif defined(__GNUC__) && __GNUC__ < 6 153 union { 154 uint64_t u; 155 double f; 156 } ieee754; 157 ieee754.f = p_val; 158 return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && 159 ((unsigned)ieee754.u == 0); 160 #else 161 return isinf(p_val); 162 #endif 163 } 164 is_inf(float p_val)165 static _ALWAYS_INLINE_ bool is_inf(float p_val) { 166 #ifdef _MSC_VER 167 return !_finite(p_val); 168 // use an inline implementation of isinf as a workaround for problematic libstdc++ versions from gcc 5.x era 169 #elif defined(__GNUC__) && __GNUC__ < 6 170 union { 171 uint32_t u; 172 float f; 173 } ieee754; 174 ieee754.f = p_val; 175 return (ieee754.u & 0x7fffffff) == 0x7f800000; 176 #else 177 return isinf(p_val); 178 #endif 179 } 180 abs(double g)181 static _ALWAYS_INLINE_ double abs(double g) { return absd(g); } abs(float g)182 static _ALWAYS_INLINE_ float abs(float g) { return absf(g); } abs(int g)183 static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; } 184 fposmod(double p_x,double p_y)185 static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) { 186 double value = Math::fmod(p_x, p_y); 187 if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { 188 value += p_y; 189 } 190 value += 0.0; 191 return value; 192 } fposmod(float p_x,float p_y)193 static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) { 194 float value = Math::fmod(p_x, p_y); 195 if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { 196 value += p_y; 197 } 198 value += 0.0; 199 return value; 200 } posmod(int p_x,int p_y)201 static _ALWAYS_INLINE_ int posmod(int p_x, int p_y) { 202 int value = p_x % p_y; 203 if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { 204 value += p_y; 205 } 206 return value; 207 } 208 deg2rad(double p_y)209 static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * Math_PI / 180.0; } deg2rad(float p_y)210 static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * Math_PI / 180.0; } 211 rad2deg(double p_y)212 static _ALWAYS_INLINE_ double rad2deg(double p_y) { return p_y * 180.0 / Math_PI; } rad2deg(float p_y)213 static _ALWAYS_INLINE_ float rad2deg(float p_y) { return p_y * 180.0 / Math_PI; } 214 lerp(double p_from,double p_to,double p_weight)215 static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; } lerp(float p_from,float p_to,float p_weight)216 static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; } 217 lerp_angle(double p_from,double p_to,double p_weight)218 static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) { 219 double difference = fmod(p_to - p_from, Math_TAU); 220 double distance = fmod(2.0 * difference, Math_TAU) - difference; 221 return p_from + distance * p_weight; 222 } lerp_angle(float p_from,float p_to,float p_weight)223 static _ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) { 224 float difference = fmod(p_to - p_from, (float)Math_TAU); 225 float distance = fmod(2.0f * difference, (float)Math_TAU) - difference; 226 return p_from + distance * p_weight; 227 } 228 inverse_lerp(double p_from,double p_to,double p_value)229 static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) { return (p_value - p_from) / (p_to - p_from); } inverse_lerp(float p_from,float p_to,float p_value)230 static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) { return (p_value - p_from) / (p_to - p_from); } 231 range_lerp(double p_value,double p_istart,double p_istop,double p_ostart,double p_ostop)232 static _ALWAYS_INLINE_ double range_lerp(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } range_lerp(float p_value,float p_istart,float p_istop,float p_ostart,float p_ostop)233 static _ALWAYS_INLINE_ float range_lerp(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } 234 smoothstep(double p_from,double p_to,double p_weight)235 static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_weight) { 236 if (is_equal_approx(p_from, p_to)) return p_from; 237 double x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0, 1.0); 238 return x * x * (3.0 - 2.0 * x); 239 } smoothstep(float p_from,float p_to,float p_weight)240 static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_weight) { 241 if (is_equal_approx(p_from, p_to)) return p_from; 242 float x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f); 243 return x * x * (3.0f - 2.0f * x); 244 } move_toward(double p_from,double p_to,double p_delta)245 static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } move_toward(float p_from,float p_to,float p_delta)246 static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } 247 linear2db(double p_linear)248 static _ALWAYS_INLINE_ double linear2db(double p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } linear2db(float p_linear)249 static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } 250 db2linear(double p_db)251 static _ALWAYS_INLINE_ double db2linear(double p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); } db2linear(float p_db)252 static _ALWAYS_INLINE_ float db2linear(float p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); } 253 round(double p_val)254 static _ALWAYS_INLINE_ double round(double p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } round(float p_val)255 static _ALWAYS_INLINE_ float round(float p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } 256 wrapi(int64_t value,int64_t min,int64_t max)257 static _ALWAYS_INLINE_ int64_t wrapi(int64_t value, int64_t min, int64_t max) { 258 int64_t range = max - min; 259 return range == 0 ? min : min + ((((value - min) % range) + range) % range); 260 } wrapf(double value,double min,double max)261 static _ALWAYS_INLINE_ double wrapf(double value, double min, double max) { 262 double range = max - min; 263 return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); 264 } wrapf(float value,float min,float max)265 static _ALWAYS_INLINE_ float wrapf(float value, float min, float max) { 266 float range = max - min; 267 return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); 268 } 269 270 // double only, as these functions are mainly used by the editor and not performance-critical, 271 static double ease(double p_x, double p_c); 272 static int step_decimals(double p_step); 273 static int range_step_decimals(double p_step); 274 static double stepify(double p_value, double p_step); 275 static double dectime(double p_value, double p_amount, double p_step); 276 277 static uint32_t larger_prime(uint32_t p_val); 278 279 static void seed(uint64_t x); 280 static void randomize(); 281 static uint32_t rand_from_seed(uint64_t *seed); 282 static uint32_t rand(); randd()283 static _ALWAYS_INLINE_ double randd() { return (double)rand() / (double)Math::RANDOM_MAX; } randf()284 static _ALWAYS_INLINE_ float randf() { return (float)rand() / (float)Math::RANDOM_MAX; } 285 286 static double random(double from, double to); 287 static float random(float from, float to); random(int from,int to)288 static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); } 289 290 static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON, real_t min_epsilon = CMP_EPSILON) { 291 // this is an approximate way to check that numbers are close, as a ratio of their average size 292 // helps compare approximate numbers that may be very big or very small 293 real_t diff = abs(a - b); 294 if (diff == 0.0 || diff < min_epsilon) { 295 return true; 296 } 297 real_t avg_size = (abs(a) + abs(b)) / 2.0; 298 diff /= avg_size; 299 return diff < epsilon; 300 } 301 is_equal_approx(real_t a,real_t b)302 static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b) { 303 // Check for exact equality first, required to handle "infinity" values. 304 if (a == b) { 305 return true; 306 } 307 // Then check for approximate equality. 308 real_t tolerance = CMP_EPSILON * abs(a); 309 if (tolerance < CMP_EPSILON) { 310 tolerance = CMP_EPSILON; 311 } 312 return abs(a - b) < tolerance; 313 } 314 is_equal_approx(real_t a,real_t b,real_t tolerance)315 static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b, real_t tolerance) { 316 // Check for exact equality first, required to handle "infinity" values. 317 if (a == b) { 318 return true; 319 } 320 // Then check for approximate equality. 321 return abs(a - b) < tolerance; 322 } 323 is_zero_approx(real_t s)324 static _ALWAYS_INLINE_ bool is_zero_approx(real_t s) { 325 return abs(s) < CMP_EPSILON; 326 } 327 absf(float g)328 static _ALWAYS_INLINE_ float absf(float g) { 329 330 union { 331 float f; 332 uint32_t i; 333 } u; 334 335 u.f = g; 336 u.i &= 2147483647u; 337 return u.f; 338 } 339 absd(double g)340 static _ALWAYS_INLINE_ double absd(double g) { 341 342 union { 343 double d; 344 uint64_t i; 345 } u; 346 u.d = g; 347 u.i &= (uint64_t)9223372036854775807ll; 348 return u.d; 349 } 350 351 //this function should be as fast as possible and rounding mode should not matter fast_ftoi(float a)352 static _ALWAYS_INLINE_ int fast_ftoi(float a) { 353 354 static int b; 355 356 #if (defined(_WIN32_WINNT) && _WIN32_WINNT >= 0x0603) || WINAPI_FAMILY == WINAPI_FAMILY_PHONE_APP // windows 8 phone? 357 b = (int)((a > 0.0) ? (a + 0.5) : (a - 0.5)); 358 359 #elif defined(_MSC_VER) && _MSC_VER < 1800 360 __asm fld a __asm fistp b 361 /*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) 362 // use AT&T inline assembly style, document that 363 // we use memory as output (=m) and input (m) 364 __asm__ __volatile__ ( 365 "flds %1 \n\t" 366 "fistpl %0 \n\t" 367 : "=m" (b) 368 : "m" (a));*/ 369 370 #else 371 b = lrintf(a); //assuming everything but msvc 2012 or earlier has lrint 372 #endif 373 return b; 374 } 375 halfbits_to_floatbits(uint16_t h)376 static _ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t h) { 377 uint16_t h_exp, h_sig; 378 uint32_t f_sgn, f_exp, f_sig; 379 380 h_exp = (h & 0x7c00u); 381 f_sgn = ((uint32_t)h & 0x8000u) << 16; 382 switch (h_exp) { 383 case 0x0000u: /* 0 or subnormal */ 384 h_sig = (h & 0x03ffu); 385 /* Signed zero */ 386 if (h_sig == 0) { 387 return f_sgn; 388 } 389 /* Subnormal */ 390 h_sig <<= 1; 391 while ((h_sig & 0x0400u) == 0) { 392 h_sig <<= 1; 393 h_exp++; 394 } 395 f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; 396 f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13; 397 return f_sgn + f_exp + f_sig; 398 case 0x7c00u: /* inf or NaN */ 399 /* All-ones exponent and a copy of the significand */ 400 return f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13); 401 default: /* normalized */ 402 /* Just need to adjust the exponent and shift */ 403 return f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13); 404 } 405 } 406 halfptr_to_float(const uint16_t * h)407 static _ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *h) { 408 409 union { 410 uint32_t u32; 411 float f32; 412 } u; 413 414 u.u32 = halfbits_to_floatbits(*h); 415 return u.f32; 416 } 417 half_to_float(const uint16_t h)418 static _ALWAYS_INLINE_ float half_to_float(const uint16_t h) { 419 return halfptr_to_float(&h); 420 } 421 make_half_float(float f)422 static _ALWAYS_INLINE_ uint16_t make_half_float(float f) { 423 424 union { 425 float fv; 426 uint32_t ui; 427 } ci; 428 ci.fv = f; 429 430 uint32_t x = ci.ui; 431 uint32_t sign = (unsigned short)(x >> 31); 432 uint32_t mantissa; 433 uint32_t exp; 434 uint16_t hf; 435 436 // get mantissa 437 mantissa = x & ((1 << 23) - 1); 438 // get exponent bits 439 exp = x & (0xFF << 23); 440 if (exp >= 0x47800000) { 441 // check if the original single precision float number is a NaN 442 if (mantissa && (exp == (0xFF << 23))) { 443 // we have a single precision NaN 444 mantissa = (1 << 23) - 1; 445 } else { 446 // 16-bit half-float representation stores number as Inf 447 mantissa = 0; 448 } 449 hf = (((uint16_t)sign) << 15) | (uint16_t)((0x1F << 10)) | 450 (uint16_t)(mantissa >> 13); 451 } 452 // check if exponent is <= -15 453 else if (exp <= 0x38000000) { 454 455 /*// store a denorm half-float value or zero 456 exp = (0x38000000 - exp) >> 23; 457 mantissa >>= (14 + exp); 458 459 hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa); 460 */ 461 hf = 0; //denormals do not work for 3D, convert to zero 462 } else { 463 hf = (((uint16_t)sign) << 15) | 464 (uint16_t)((exp - 0x38000000) >> 13) | 465 (uint16_t)(mantissa >> 13); 466 } 467 468 return hf; 469 } 470 snap_scalar(float p_offset,float p_step,float p_target)471 static _ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) { 472 return p_step != 0 ? Math::stepify(p_target - p_offset, p_step) + p_offset : p_target; 473 } 474 snap_scalar_separation(float p_offset,float p_step,float p_target,float p_separation)475 static _ALWAYS_INLINE_ float snap_scalar_separation(float p_offset, float p_step, float p_target, float p_separation) { 476 if (p_step != 0) { 477 float a = Math::stepify(p_target - p_offset, p_step + p_separation) + p_offset; 478 float b = a; 479 if (p_target >= 0) 480 b -= p_separation; 481 else 482 b += p_step; 483 return (Math::abs(p_target - a) < Math::abs(p_target - b)) ? a : b; 484 } 485 return p_target; 486 } 487 }; 488 489 #endif // MATH_FUNCS_H 490