Math.hpp 6.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272
  1. #ifndef GODOT_MATH_H
  2. #define GODOT_MATH_H
  3. #include "Defs.hpp"
  4. #include <cmath>
  5. namespace godot {
  6. namespace Math {
  7. // Functions reproduced as in Godot's source code `math_funcs.h`.
  8. // Some are overloads to automatically support changing real_t into either double or float in the way Godot does.
  9. inline double fmod(double p_x, double p_y) {
  10. return ::fmod(p_x, p_y);
  11. }
  12. inline float fmod(float p_x, float p_y) {
  13. return ::fmodf(p_x, p_y);
  14. }
  15. inline double floor(double p_x) {
  16. return ::floor(p_x);
  17. }
  18. inline float floor(float p_x) {
  19. return ::floorf(p_x);
  20. }
  21. inline double exp(double p_x) {
  22. return ::exp(p_x);
  23. }
  24. inline float exp(float p_x) {
  25. return ::expf(p_x);
  26. }
  27. inline double sin(double p_x) {
  28. return ::sin(p_x);
  29. }
  30. inline float sin(float p_x) {
  31. return ::sinf(p_x);
  32. }
  33. inline double cos(double p_x) {
  34. return ::cos(p_x);
  35. }
  36. inline float cos(float p_x) {
  37. return ::cosf(p_x);
  38. }
  39. inline double tan(double p_x) {
  40. return ::tan(p_x);
  41. }
  42. inline float tan(float p_x) {
  43. return ::tanf(p_x);
  44. }
  45. inline double asin(double p_x) {
  46. return ::asin(p_x);
  47. }
  48. inline float asin(float p_x) {
  49. return ::asinf(p_x);
  50. }
  51. inline double acos(double p_x) {
  52. return ::acos(p_x);
  53. }
  54. inline float acos(float p_x) {
  55. return ::acosf(p_x);
  56. }
  57. inline double atan(double p_x) {
  58. return ::atan(p_x);
  59. }
  60. inline float atan(float p_x) {
  61. return ::atanf(p_x);
  62. }
  63. inline double atan2(double p_y, double p_x) {
  64. return ::atan2(p_y, p_x);
  65. }
  66. inline float atan2(float p_y, float p_x) {
  67. return ::atan2f(p_y, p_x);
  68. }
  69. inline double sqrt(double p_x) {
  70. return ::sqrt(p_x);
  71. }
  72. inline float sqrt(float p_x) {
  73. return ::sqrtf(p_x);
  74. }
  75. inline float lerp(float minv, float maxv, float t) {
  76. return minv + t * (maxv - minv);
  77. }
  78. inline double lerp(double minv, double maxv, double t) {
  79. return minv + t * (maxv - minv);
  80. }
  81. inline double lerp_angle(double p_from, double p_to, double p_weight) {
  82. double difference = fmod(p_to - p_from, Math_TAU);
  83. double distance = fmod(2.0 * difference, Math_TAU) - difference;
  84. return p_from + distance * p_weight;
  85. }
  86. inline float lerp_angle(float p_from, float p_to, float p_weight) {
  87. float difference = fmod(p_to - p_from, (float)Math_TAU);
  88. float distance = fmod(2.0f * difference, (float)Math_TAU) - difference;
  89. return p_from + distance * p_weight;
  90. }
  91. template <typename T>
  92. inline T clamp(T x, T minv, T maxv) {
  93. if (x < minv) {
  94. return minv;
  95. }
  96. if (x > maxv) {
  97. return maxv;
  98. }
  99. return x;
  100. }
  101. template <typename T>
  102. inline T min(T a, T b) {
  103. return a < b ? a : b;
  104. }
  105. template <typename T>
  106. inline T max(T a, T b) {
  107. return a > b ? a : b;
  108. }
  109. template <typename T>
  110. inline T sign(T x) {
  111. return static_cast<T>(x < 0 ? -1 : 1);
  112. }
  113. inline double deg2rad(double p_y) {
  114. return p_y * Math_PI / 180.0;
  115. }
  116. inline float deg2rad(float p_y) {
  117. return p_y * static_cast<float>(Math_PI) / 180.f;
  118. }
  119. inline double rad2deg(double p_y) {
  120. return p_y * 180.0 / Math_PI;
  121. }
  122. inline float rad2deg(float p_y) {
  123. return p_y * 180.f / static_cast<float>(Math_PI);
  124. }
  125. inline double inverse_lerp(double p_from, double p_to, double p_value) {
  126. return (p_value - p_from) / (p_to - p_from);
  127. }
  128. inline float inverse_lerp(float p_from, float p_to, float p_value) {
  129. return (p_value - p_from) / (p_to - p_from);
  130. }
  131. inline double range_lerp(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) {
  132. return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value));
  133. }
  134. inline float range_lerp(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) {
  135. return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value));
  136. }
  137. inline bool is_equal_approx(real_t a, real_t b) {
  138. // Check for exact equality first, required to handle "infinity" values.
  139. if (a == b) {
  140. return true;
  141. }
  142. // Then check for approximate equality.
  143. real_t tolerance = CMP_EPSILON * std::abs(a);
  144. if (tolerance < CMP_EPSILON) {
  145. tolerance = CMP_EPSILON;
  146. }
  147. return std::abs(a - b) < tolerance;
  148. }
  149. inline bool is_equal_approx(real_t a, real_t b, real_t tolerance) {
  150. // Check for exact equality first, required to handle "infinity" values.
  151. if (a == b) {
  152. return true;
  153. }
  154. // Then check for approximate equality.
  155. return std::abs(a - b) < tolerance;
  156. }
  157. inline bool is_zero_approx(real_t s) {
  158. return std::abs(s) < CMP_EPSILON;
  159. }
  160. inline double smoothstep(double p_from, double p_to, double p_weight) {
  161. if (is_equal_approx(static_cast<real_t>(p_from), static_cast<real_t>(p_to))) {
  162. return p_from;
  163. }
  164. double x = clamp((p_weight - p_from) / (p_to - p_from), 0.0, 1.0);
  165. return x * x * (3.0 - 2.0 * x);
  166. }
  167. inline float smoothstep(float p_from, float p_to, float p_weight) {
  168. if (is_equal_approx(p_from, p_to)) {
  169. return p_from;
  170. }
  171. float x = clamp((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f);
  172. return x * x * (3.0f - 2.0f * x);
  173. }
  174. inline double move_toward(double p_from, double p_to, double p_delta) {
  175. return std::abs(p_to - p_from) <= p_delta ? p_to : p_from + sign(p_to - p_from) * p_delta;
  176. }
  177. inline float move_toward(float p_from, float p_to, float p_delta) {
  178. return std::abs(p_to - p_from) <= p_delta ? p_to : p_from + sign(p_to - p_from) * p_delta;
  179. }
  180. inline double linear2db(double p_linear) {
  181. return log(p_linear) * 8.6858896380650365530225783783321;
  182. }
  183. inline float linear2db(float p_linear) {
  184. return log(p_linear) * 8.6858896380650365530225783783321f;
  185. }
  186. inline double db2linear(double p_db) {
  187. return exp(p_db * 0.11512925464970228420089957273422);
  188. }
  189. inline float db2linear(float p_db) {
  190. return exp(p_db * 0.11512925464970228420089957273422f);
  191. }
  192. inline double round(double p_val) {
  193. return (p_val >= 0) ? floor(p_val + 0.5) : -floor(-p_val + 0.5);
  194. }
  195. inline float round(float p_val) {
  196. return (p_val >= 0) ? floor(p_val + 0.5f) : -floor(-p_val + 0.5f);
  197. }
  198. inline int64_t wrapi(int64_t value, int64_t min, int64_t max) {
  199. int64_t range = max - min;
  200. return range == 0 ? min : min + ((((value - min) % range) + range) % range);
  201. }
  202. inline float wrapf(real_t value, real_t min, real_t max) {
  203. const real_t range = max - min;
  204. return is_zero_approx(range) ? min : value - (range * floor((value - min) / range));
  205. }
  206. inline float stepify(float p_value, float p_step) {
  207. if (p_step != 0) {
  208. p_value = floor(p_value / p_step + 0.5f) * p_step;
  209. }
  210. return p_value;
  211. }
  212. inline double stepify(double p_value, double p_step) {
  213. if (p_step != 0) {
  214. p_value = floor(p_value / p_step + 0.5) * p_step;
  215. }
  216. return p_value;
  217. }
  218. inline unsigned int next_power_of_2(unsigned int x) {
  219. if (x == 0)
  220. return 0;
  221. --x;
  222. x |= x >> 1;
  223. x |= x >> 2;
  224. x |= x >> 4;
  225. x |= x >> 8;
  226. x |= x >> 16;
  227. return ++x;
  228. }
  229. } // namespace Math
  230. } // namespace godot
  231. #endif // GODOT_MATH_H