summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star_grid_2d.cpp12
-rw-r--r--core/math/basis.cpp51
-rw-r--r--core/math/math_funcs.h44
3 files changed, 66 insertions, 41 deletions
diff --git a/core/math/a_star_grid_2d.cpp b/core/math/a_star_grid_2d.cpp
index 23d7e379ee..ad67cfa852 100644
--- a/core/math/a_star_grid_2d.cpp
+++ b/core/math/a_star_grid_2d.cpp
@@ -30,16 +30,16 @@
#include "a_star_grid_2d.h"
-static real_t heuristic_manhattan(const Vector2i &p_from, const Vector2i &p_to) {
+static real_t heuristic_euclidian(const Vector2i &p_from, const Vector2i &p_to) {
real_t dx = (real_t)ABS(p_to.x - p_from.x);
real_t dy = (real_t)ABS(p_to.y - p_from.y);
- return dx + dy;
+ return (real_t)Math::sqrt(dx * dx + dy * dy);
}
-static real_t heuristic_euclidian(const Vector2i &p_from, const Vector2i &p_to) {
+static real_t heuristic_manhattan(const Vector2i &p_from, const Vector2i &p_to) {
real_t dx = (real_t)ABS(p_to.x - p_from.x);
real_t dy = (real_t)ABS(p_to.y - p_from.y);
- return (real_t)Math::sqrt(dx * dx + dy * dy);
+ return dx + dy;
}
static real_t heuristic_octile(const Vector2i &p_from, const Vector2i &p_to) {
@@ -55,7 +55,7 @@ static real_t heuristic_chebyshev(const Vector2i &p_from, const Vector2i &p_to)
return MAX(dx, dy);
}
-static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_manhattan, heuristic_euclidian, heuristic_octile, heuristic_chebyshev };
+static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_euclidian, heuristic_manhattan, heuristic_octile, heuristic_chebyshev };
void AStarGrid2D::set_size(const Size2i &p_size) {
ERR_FAIL_COND(p_size.x < 0 || p_size.y < 0);
@@ -572,7 +572,7 @@ void AStarGrid2D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "cell_size"), "set_cell_size", "get_cell_size");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "jumping_enabled"), "set_jumping_enabled", "is_jumping_enabled");
- ADD_PROPERTY(PropertyInfo(Variant::INT, "default_heuristic", PROPERTY_HINT_ENUM, "Manhattan,Euclidean,Octile,Chebyshev,Max"), "set_default_heuristic", "get_default_heuristic");
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "default_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev,Max"), "set_default_heuristic", "get_default_heuristic");
ADD_PROPERTY(PropertyInfo(Variant::INT, "diagonal_mode", PROPERTY_HINT_ENUM, "Never,Always,At Least One Walkable,Only If No Obstacles,Max"), "set_diagonal_mode", "get_diagonal_mode");
BIND_ENUM_CONSTANT(HEURISTIC_EUCLIDEAN);
diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 0eb6320ac6..4b163409ce 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -754,29 +754,28 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND(!is_rotation());
#endif
-*/
- real_t angle, x, y, z; // variables for result
- real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees
-
- if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[1][2]) < CMP_EPSILON)) {
- // singularity found
- // first check for identity matrix which must have +1 for all terms
- // in leading diagonal and zero in other terms
- if ((Math::abs(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_epsilon)) {
- // this singularity is identity matrix so angle = 0
+ */
+
+ // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
+ real_t x, y, z; // Variables for result.
+ if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[2][1])) {
+ // Singularity found.
+ // First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms.
+ if (is_diagonal() && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) {
+ // This singularity is identity matrix so angle = 0.
r_axis = Vector3(0, 1, 0);
r_angle = 0;
return;
}
- // otherwise this singularity is angle = 180
- angle = Math_PI;
+ // Otherwise this singularity is angle = 180.
real_t xx = (rows[0][0] + 1) / 2;
real_t yy = (rows[1][1] + 1) / 2;
real_t zz = (rows[2][2] + 1) / 2;
- real_t xy = (rows[1][0] + rows[0][1]) / 4;
- real_t xz = (rows[2][0] + rows[0][2]) / 4;
- real_t yz = (rows[2][1] + rows[1][2]) / 4;
- if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term
+ real_t xy = (rows[0][1] + rows[1][0]) / 4;
+ real_t xz = (rows[0][2] + rows[2][0]) / 4;
+ real_t yz = (rows[1][2] + rows[2][1]) / 4;
+
+ if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term.
if (xx < CMP_EPSILON) {
x = 0;
y = Math_SQRT12;
@@ -786,7 +785,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
y = xy / x;
z = xz / x;
}
- } else if (yy > zz) { // rows[1][1] is the largest diagonal term
+ } else if (yy > zz) { // rows[1][1] is the largest diagonal term.
if (yy < CMP_EPSILON) {
x = Math_SQRT12;
y = 0;
@@ -796,7 +795,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
x = xy / y;
z = yz / y;
}
- } else { // rows[2][2] is the largest diagonal term so base result on this
+ } else { // rows[2][2] is the largest diagonal term so base result on this.
if (zz < CMP_EPSILON) {
x = Math_SQRT12;
y = Math_SQRT12;
@@ -808,22 +807,24 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
}
}
r_axis = Vector3(x, y, z);
- r_angle = angle;
+ r_angle = Math_PI;
return;
}
- // as we have reached here there are no singularities so we can handle normally
- real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise
+ // As we have reached here there are no singularities so we can handle normally.
+ double s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise.
- angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2);
- if (angle < 0) {
- s = -s;
+ if (Math::abs(s) < CMP_EPSILON) {
+ // Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above.
+ s = 1;
}
+
x = (rows[2][1] - rows[1][2]) / s;
y = (rows[0][2] - rows[2][0]) / s;
z = (rows[1][0] - rows[0][1]) / s;
r_axis = Vector3(x, y, z);
- r_angle = angle;
+ // CLAMP to avoid NaN if the value passed to acos is not in [0,1].
+ r_angle = Math::acos(CLAMP((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2, (real_t)0.0, (real_t)1.0));
}
void Basis::set_quaternion(const Quaternion &p_quaternion) {
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 656fc9f798..7fa674a23d 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -267,6 +267,7 @@ public:
return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
}
+
static _ALWAYS_INLINE_ float cubic_interpolate_angle(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
float from_rot = fmod(p_from, (float)Math_TAU);
@@ -293,6 +294,7 @@ public:
double b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t);
return Math::lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t);
}
+
static _ALWAYS_INLINE_ float cubic_interpolate_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
float p_to_t, float p_pre_t, float p_post_t) {
/* Barry-Goldman method */
@@ -320,6 +322,7 @@ public:
return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
}
+
static _ALWAYS_INLINE_ float cubic_interpolate_angle_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
float p_to_t, float p_pre_t, float p_post_t) {
float from_rot = fmod(p_from, (float)Math_TAU);
@@ -346,6 +349,7 @@ public:
return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
}
+
static _ALWAYS_INLINE_ float bezier_interpolate(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) {
/* Formula from Wikipedia article on Bezier curves. */
float omt = (1.0f - p_t);
@@ -368,11 +372,19 @@ public:
return p_from + distance * p_weight;
}
- static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) { return (p_value - p_from) / (p_to - p_from); }
- static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) { return (p_value - p_from) / (p_to - p_from); }
+ static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) {
+ return (p_value - p_from) / (p_to - p_from);
+ }
+ static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) {
+ return (p_value - p_from) / (p_to - p_from);
+ }
- static _ALWAYS_INLINE_ double remap(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)); }
- static _ALWAYS_INLINE_ float remap(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)); }
+ static _ALWAYS_INLINE_ double remap(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));
+ }
+ static _ALWAYS_INLINE_ float remap(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));
+ }
static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_s) {
if (is_equal_approx(p_from, p_to)) {
@@ -388,14 +400,26 @@ public:
float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f);
return s * s * (3.0f - 2.0f * s);
}
- 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 + SIGN(p_to - p_from) * p_delta; }
- 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 + SIGN(p_to - p_from) * p_delta; }
+ 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 + SIGN(p_to - p_from) * p_delta;
+ }
+ 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 + SIGN(p_to - p_from) * p_delta;
+ }
- static _ALWAYS_INLINE_ double linear_to_db(double p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; }
- static _ALWAYS_INLINE_ float linear_to_db(float p_linear) { return Math::log(p_linear) * (float)8.6858896380650365530225783783321; }
+ static _ALWAYS_INLINE_ double linear_to_db(double p_linear) {
+ return Math::log(p_linear) * 8.6858896380650365530225783783321;
+ }
+ static _ALWAYS_INLINE_ float linear_to_db(float p_linear) {
+ return Math::log(p_linear) * (float)8.6858896380650365530225783783321;
+ }
- static _ALWAYS_INLINE_ double db_to_linear(double p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); }
- static _ALWAYS_INLINE_ float db_to_linear(float p_db) { return Math::exp(p_db * (float)0.11512925464970228420089957273422); }
+ static _ALWAYS_INLINE_ double db_to_linear(double p_db) {
+ return Math::exp(p_db * 0.11512925464970228420089957273422);
+ }
+ static _ALWAYS_INLINE_ float db_to_linear(float p_db) {
+ return Math::exp(p_db * (float)0.11512925464970228420089957273422);
+ }
static _ALWAYS_INLINE_ double round(double p_val) { return ::round(p_val); }
static _ALWAYS_INLINE_ float round(float p_val) { return ::roundf(p_val); }
generated by cgit v1.2.3 (git 2.39.1) at 2026-02-01 13:15:34 +0000