From 0d1511695d5235fcdc8c566504a4fe6896a29900 Mon Sep 17 00:00:00 2001 From: Marc Gilleron Date: Sun, 30 Aug 2020 23:15:07 +0100 Subject: [PATCH] Added Godot's math functions --- include/core/Defs.hpp | 1 + include/core/Math.hpp | 250 +++++++++++++++++++++++++++++++++++++++ include/core/Vector2.hpp | 24 +++- include/core/Vector3.hpp | 2 +- src/core/Vector3.cpp | 11 +- 5 files changed, 275 insertions(+), 13 deletions(-) create mode 100644 include/core/Math.hpp diff --git a/include/core/Defs.hpp b/include/core/Defs.hpp index 9981afe..67e3279 100644 --- a/include/core/Defs.hpp +++ b/include/core/Defs.hpp @@ -73,6 +73,7 @@ typedef float real_t; #define CMP_EPSILON 0.00001 #define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON) #define Math_PI 3.14159265358979323846 +#define Math_TAU 6.2831853071795864769252867666 #define _PLANE_EQ_DOT_EPSILON 0.999 #define _PLANE_EQ_D_EPSILON 0.0001 diff --git a/include/core/Math.hpp b/include/core/Math.hpp new file mode 100644 index 0000000..4d0d32e --- /dev/null +++ b/include/core/Math.hpp @@ -0,0 +1,250 @@ +#ifndef GODOT_MATH_H +#define GODOT_MATH_H + +#include "Defs.hpp" +#include + +namespace godot { +namespace Math { + +// Functions reproduced as in Godot's source code `math_funcs.h`. +// Some are overloads to automatically support changing real_t into either double or float in the way Godot does. + +inline double fmod(double p_x, double p_y) { + return ::fmod(p_x, p_y); +} +inline float fmod(float p_x, float p_y) { + return ::fmodf(p_x, p_y); +} + +inline double floor(double p_x) { + return ::floor(p_x); +} +inline float floor(float p_x) { + return ::floorf(p_x); +} + +inline double exp(double p_x) { + return ::exp(p_x); +} +inline float exp(float p_x) { + return ::expf(p_x); +} + +inline double sin(double p_x) { + return ::sin(p_x); +} +inline float sin(float p_x) { + return ::sinf(p_x); +} + +inline double cos(double p_x) { + return ::cos(p_x); +} +inline float cos(float p_x) { + return ::cosf(p_x); +} + +inline double tan(double p_x) { + return ::tan(p_x); +} +inline float tan(float p_x) { + return ::tanf(p_x); +} + +inline double atan2(double p_y, double p_x) { + return ::atan2(p_y, p_x); +} +inline float atan2(float p_y, float p_x) { + return ::atan2f(p_y, p_x); +} + +inline double sqrt(double p_x) { + return ::sqrt(p_x); +} +inline float sqrt(float p_x) { + return ::sqrtf(p_x); +} + +inline float lerp(float minv, float maxv, float t) { + return minv + t * (maxv - minv); +} +inline double lerp(double minv, double maxv, double t) { + return minv + t * (maxv - minv); +} + +inline double lerp_angle(double p_from, double p_to, double p_weight) { + double difference = fmod(p_to - p_from, Math_TAU); + double distance = fmod(2.0 * difference, Math_TAU) - difference; + return p_from + distance * p_weight; +} +inline float lerp_angle(float p_from, float p_to, float p_weight) { + float difference = fmod(p_to - p_from, (float)Math_TAU); + float distance = fmod(2.0f * difference, (float)Math_TAU) - difference; + return p_from + distance * p_weight; +} + +template +inline T clamp(T x, T minv, T maxv) { + if (x < minv) { + return minv; + } + if (x > maxv) { + return maxv; + } + return x; +} + +template +inline T min(T a, T b) { + return a < b ? a : b; +} + +template +inline T max(T a, T b) { + return a > b ? a : b; +} + +template +inline T sign(T x) { + return x < 0 ? -1 : 1; +} + +inline double deg2rad(double p_y) { + return p_y * Math_PI / 180.0; +} +inline float deg2rad(float p_y) { + return p_y * Math_PI / 180.0; +} + +inline double rad2deg(double p_y) { + return p_y * 180.0 / Math_PI; +} +inline float rad2deg(float p_y) { + return p_y * 180.0 / Math_PI; +} + +inline double inverse_lerp(double p_from, double p_to, double p_value) { + return (p_value - p_from) / (p_to - p_from); +} +inline float inverse_lerp(float p_from, float p_to, float p_value) { + return (p_value - p_from) / (p_to - p_from); +} + +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)); +} +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)); +} + +inline bool is_equal_approx(real_t a, real_t b) { + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { + return true; + } + // Then check for approximate equality. + real_t tolerance = CMP_EPSILON * std::abs(a); + if (tolerance < CMP_EPSILON) { + tolerance = CMP_EPSILON; + } + return std::abs(a - b) < tolerance; +} + +inline bool is_equal_approx(real_t a, real_t b, real_t tolerance) { + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { + return true; + } + // Then check for approximate equality. + return std::abs(a - b) < tolerance; +} + +inline bool is_zero_approx(real_t s) { + return std::abs(s) < CMP_EPSILON; +} + +inline double smoothstep(double p_from, double p_to, double p_weight) { + if (is_equal_approx(p_from, p_to)) { + return p_from; + } + double x = clamp((p_weight - p_from) / (p_to - p_from), 0.0, 1.0); + return x * x * (3.0 - 2.0 * x); +} +inline float smoothstep(float p_from, float p_to, float p_weight) { + if (is_equal_approx(p_from, p_to)) { + return p_from; + } + float x = clamp((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f); + return x * x * (3.0f - 2.0f * x); +} + +inline double move_toward(double p_from, double p_to, double p_delta) { + return std::abs(p_to - p_from) <= p_delta ? p_to : p_from + sign(p_to - p_from) * p_delta; +} + +inline float move_toward(float p_from, float p_to, float p_delta) { + return std::abs(p_to - p_from) <= p_delta ? p_to : p_from + sign(p_to - p_from) * p_delta; +} + +inline double linear2db(double p_linear) { + return log(p_linear) * 8.6858896380650365530225783783321; +} +inline float linear2db(float p_linear) { + return log(p_linear) * 8.6858896380650365530225783783321f; +} + +inline double db2linear(double p_db) { + return exp(p_db * 0.11512925464970228420089957273422); +} +inline float db2linear(float p_db) { + return exp(p_db * 0.11512925464970228420089957273422f); +} + +inline double round(double p_val) { + return (p_val >= 0) ? floor(p_val + 0.5) : -floor(-p_val + 0.5); +} +inline float round(float p_val) { + return (p_val >= 0) ? floor(p_val + 0.5) : -floor(-p_val + 0.5); +} + +inline int64_t wrapi(int64_t value, int64_t min, int64_t max) { + int64_t range = max - min; + return range == 0 ? min : min + ((((value - min) % range) + range) % range); +} + +inline double wrapf(double value, double min, double max) { + double range = max - min; + return is_zero_approx(range) ? min : value - (range * floor((value - min) / range)); +} +inline float wrapf(float value, float min, float max) { + float range = max - min; + return is_zero_approx(range) ? min : value - (range * floor((value - min) / range)); +} + +inline real_t stepify(real_t p_value, real_t p_step) { + if (p_step != 0) { + p_value = floor(p_value / p_step + 0.5) * p_step; + } + return p_value; +} + +inline unsigned int next_power_of_2(unsigned int x) { + + if (x == 0) + return 0; + + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + x |= x >> 8; + x |= x >> 16; + + return ++x; +} + +} // namespace Math +} // namespace godot + +#endif // GODOT_MATH_H diff --git a/include/core/Vector2.hpp b/include/core/Vector2.hpp index b3ef032..031d47f 100644 --- a/include/core/Vector2.hpp +++ b/include/core/Vector2.hpp @@ -5,7 +5,7 @@ #include "Defs.hpp" -#include +#include namespace godot { @@ -222,13 +222,13 @@ struct Vector2 { } inline Vector2 floor() const { - return Vector2(::floor(x), ::floor(y)); + return Vector2(Math::floor(x), Math::floor(y)); } inline Vector2 snapped(const Vector2 &p_by) const { return Vector2( - p_by.x != 0 ? ::floor(x / p_by.x + 0.5) * p_by.x : x, - p_by.y != 0 ? ::floor(y / p_by.y + 0.5) * p_by.y : y); + Math::stepify(x, p_by.x), + Math::stepify(y, p_by.y)); } inline real_t aspect() const { return width / height; } @@ -240,6 +240,22 @@ inline Vector2 operator*(real_t p_scalar, const Vector2 &p_vec) { return p_vec * p_scalar; } +namespace Math { + +// Convenience, since they exist in GDScript + +inline Vector2 cartesian2polar(Vector2 v) { + return Vector2(Math::sqrt(v.x * v.x + v.y * v.y), Math::atan2(v.y, v.x)); +} + +inline Vector2 polar2cartesian(Vector2 v) { + // x == radius + // y == angle + return Vector2(v.x * Math::cos(v.y), v.x * Math::sin(v.y)); +} + +} // namespace Math + } // namespace godot #endif // VECTOR2_H diff --git a/include/core/Vector3.hpp b/include/core/Vector3.hpp index 0bfa2d9..d879097 100644 --- a/include/core/Vector3.hpp +++ b/include/core/Vector3.hpp @@ -7,7 +7,7 @@ #include "String.hpp" -#include +#include namespace godot { diff --git a/src/core/Vector3.cpp b/src/core/Vector3.cpp index cf95792..879de0b 100644 --- a/src/core/Vector3.cpp +++ b/src/core/Vector3.cpp @@ -67,17 +67,12 @@ void Vector3::rotate(const Vector3 &p_axis, real_t p_phi) { *this = Basis(p_axis, p_phi).xform(*this); } -// this is ugly as well, but hey, I'm a simple man -#define _ugly_stepify(val, step) (step != 0 ? ::floor(val / step + 0.5) * step : val) - void Vector3::snap(real_t p_val) { - x = _ugly_stepify(x, p_val); - y = _ugly_stepify(y, p_val); - z = _ugly_stepify(z, p_val); + x = Math::stepify(x, p_val); + y = Math::stepify(y, p_val); + z = Math::stepify(z, p_val); } -#undef _ugly_stepify - Vector3::operator String() const { return String::num(x) + ", " + String::num(y) + ", " + String::num(z); }