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/aabb.h8
-rw-r--r--core/math/basis.cpp51
-rw-r--r--core/math/math_funcs.h4
-rw-r--r--core/math/rect2.h4
-rw-r--r--core/math/rect2i.h4
6 files changed, 42 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/aabb.h b/core/math/aabb.h
index e88ba33531..acf903eeba 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -47,12 +47,12 @@ struct _NO_DISCARD_ AABB {
Vector3 size;
real_t get_volume() const;
- _FORCE_INLINE_ bool has_no_volume() const {
- return (size.x <= 0 || size.y <= 0 || size.z <= 0);
+ _FORCE_INLINE_ bool has_volume() const {
+ return size.x > 0.0f && size.y > 0.0f && size.z > 0.0f;
}
- _FORCE_INLINE_ bool has_no_surface() const {
- return (size.x <= 0 && size.y <= 0 && size.z <= 0);
+ _FORCE_INLINE_ bool has_surface() const {
+ return size.x > 0.0f || size.y > 0.0f || size.z > 0.0f;
}
const Vector3 &get_position() const { return position; }
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 cae76b182a..656fc9f798 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -371,8 +371,8 @@ public:
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 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)); }
- 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)); }
+ 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)) {
diff --git a/core/math/rect2.h b/core/math/rect2.h
index 679af933c2..2d1be3d4f3 100644
--- a/core/math/rect2.h
+++ b/core/math/rect2.h
@@ -140,8 +140,8 @@ struct _NO_DISCARD_ Rect2 {
((p_rect.position.y + p_rect.size.y) <= (position.y + size.y));
}
- _FORCE_INLINE_ bool has_no_area() const {
- return (size.x <= 0 || size.y <= 0);
+ _FORCE_INLINE_ bool has_area() const {
+ return size.x > 0.0f && size.y > 0.0f;
}
// Returns the instersection between two Rect2s or an empty Rect2 if there is no intersection
diff --git a/core/math/rect2i.h b/core/math/rect2i.h
index db1459a3e6..2b58dcdd98 100644
--- a/core/math/rect2i.h
+++ b/core/math/rect2i.h
@@ -83,8 +83,8 @@ struct _NO_DISCARD_ Rect2i {
((p_rect.position.y + p_rect.size.y) <= (position.y + size.y));
}
- _FORCE_INLINE_ bool has_no_area() const {
- return (size.x <= 0 || size.y <= 0);
+ _FORCE_INLINE_ bool has_area() const {
+ return size.x > 0 && size.y > 0;
}
// Returns the instersection between two Rect2is or an empty Rect2i if there is no intersection
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-02 09:56:41 +0000