summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star.cpp85
-rw-r--r--core/math/a_star.h11
-rw-r--r--core/math/camera_matrix.cpp2
-rw-r--r--core/math/math_funcs.cpp15
-rw-r--r--core/math/math_funcs.h62
-rw-r--r--core/math/matrix3.cpp38
-rw-r--r--core/math/matrix3.h15
7 files changed, 170 insertions, 58 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index d1afcec18f..7e26761abf 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -42,8 +42,10 @@ int AStar::get_available_point_id() const {
}
void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) {
+
ERR_FAIL_COND(p_id < 0);
ERR_FAIL_COND(p_weight_scale < 1);
+
if (!points.has(p_id)) {
Point *pt = memnew(Point);
pt->id = p_id;
@@ -58,18 +60,35 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) {
}
}
-Vector3 AStar::get_point_pos(int p_id) const {
+Vector3 AStar::get_point_position(int p_id) const {
ERR_FAIL_COND_V(!points.has(p_id), Vector3());
return points[p_id]->pos;
}
+
+void AStar::set_point_position(int p_id, const Vector3 &p_pos) {
+
+ ERR_FAIL_COND(!points.has(p_id));
+
+ points[p_id]->pos = p_pos;
+}
+
real_t AStar::get_point_weight_scale(int p_id) const {
ERR_FAIL_COND_V(!points.has(p_id), 0);
return points[p_id]->weight_scale;
}
+
+void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) {
+
+ ERR_FAIL_COND(!points.has(p_id));
+ ERR_FAIL_COND(p_weight_scale < 1);
+
+ points[p_id]->weight_scale = p_weight_scale;
+}
+
void AStar::remove_point(int p_id) {
ERR_FAIL_COND(!points.has(p_id));
@@ -130,6 +149,7 @@ bool AStar::has_point(int p_id) const {
}
Array AStar::get_points() {
+
Array point_list;
for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) {
@@ -139,6 +159,21 @@ Array AStar::get_points() {
return point_list;
}
+PoolVector<int> AStar::get_point_connections(int p_id) {
+
+ ERR_FAIL_COND_V(!points.has(p_id), PoolVector<int>());
+
+ PoolVector<int> point_list;
+
+ Point *p = points[p_id];
+
+ for (int i = 0; i < p->neighbours.size(); i++) {
+ point_list.push_back(p->neighbours[i]->id);
+ }
+
+ return point_list;
+}
+
bool AStar::are_points_connected(int p_id, int p_with_id) const {
Segment s(p_id, p_with_id);
@@ -171,7 +206,8 @@ int AStar::get_closest_point(const Vector3 &p_point) const {
return closest_id;
}
-Vector3 AStar::get_closest_pos_in_segment(const Vector3 &p_point) const {
+
+Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
real_t closest_dist = 1e20;
bool found = false;
@@ -222,15 +258,15 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
while (!found_route) {
if (open_list.first() == NULL) {
- //could not find path sadly
+ // No path found
break;
}
- //check open list
+ // Check open list
SelfList<Point> *least_cost_point = NULL;
real_t least_cost = 1e30;
- //this could be faster (cache previous results)
+ // TODO: Cache previous results
for (SelfList<Point> *E = open_list.first(); E; E = E->next()) {
Point *p = E->self();
@@ -246,7 +282,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
}
Point *p = least_cost_point->self();
- //open the neighbours for search
+ // Open the neighbours for search
int es = p->neighbours.size();
for (int i = 0; i < es; i++) {
@@ -256,7 +292,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance;
if (e->last_pass == pass) {
- //oh this was visited already, can we win the cost?
+ // Already visited, is this cheaper?
if (e->distance > distance) {
@@ -264,15 +300,15 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
e->distance = distance;
}
} else {
- //add to open neighbours
+ // Add to open neighbours
e->prev_point = p;
e->distance = distance;
- e->last_pass = pass; //mark as used
+ e->last_pass = pass; // Mark as used
open_list.add(&e->list);
if (e == end_point) {
- //oh my reached end! stop algorithm
+ // End reached; stop algorithm
found_route = true;
break;
}
@@ -285,7 +321,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
open_list.remove(least_cost_point);
}
- //clear the openf list
+ // Clear the openf list
while (open_list.first()) {
open_list.remove(open_list.first());
}
@@ -294,6 +330,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
}
float AStar::_estimate_cost(int p_from_id, int p_to_id) {
+
if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost))
return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id);
@@ -301,6 +338,7 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) {
}
float AStar::_compute_cost(int p_from_id, int p_to_id) {
+
if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost))
return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id);
@@ -331,9 +369,9 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
if (!found_route)
return PoolVector<Vector3>();
- //midpoints
+ // Midpoints
Point *p = end_point;
- int pc = 1; //begin point
+ int pc = 1; // Begin point
while (p != begin_point) {
pc++;
p = p->prev_point;
@@ -352,7 +390,7 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
p = p->prev_point;
}
- w[0] = p->pos; //assign first
+ w[0] = p->pos; // Assign first
}
return path;
@@ -382,9 +420,9 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
if (!found_route)
return PoolVector<int>();
- //midpoints
+ // Midpoints
Point *p = end_point;
- int pc = 1; //begin point
+ int pc = 1; // Begin point
while (p != begin_point) {
pc++;
p = p->prev_point;
@@ -403,7 +441,7 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
p = p->prev_point;
}
- w[0] = p->id; //assign first
+ w[0] = p->id; // Assign first
}
return path;
@@ -412,21 +450,25 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
void AStar::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar::get_available_point_id);
- ClassDB::bind_method(D_METHOD("add_point", "id", "pos", "weight_scale"), &AStar::add_point, DEFVAL(1.0));
- ClassDB::bind_method(D_METHOD("get_point_pos", "id"), &AStar::get_point_pos);
+ ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar::add_point, DEFVAL(1.0));
+ ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar::get_point_position);
+ ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar::set_point_position);
ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar::get_point_weight_scale);
+ ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar::set_point_weight_scale);
ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point);
ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar::has_point);
ClassDB::bind_method(D_METHOD("get_points"), &AStar::get_points);
+ ClassDB::bind_method(D_METHOD("get_point_connections"), &AStar::get_point_connections);
+
ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true));
ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points);
ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected);
ClassDB::bind_method(D_METHOD("clear"), &AStar::clear);
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_pos"), &AStar::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_pos_in_segment", "to_pos"), &AStar::get_closest_pos_in_segment);
+ ClassDB::bind_method(D_METHOD("get_closest_point", "to_position"), &AStar::get_closest_point);
+ ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar::get_closest_position_in_segment);
ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar::get_point_path);
ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar::get_id_path);
@@ -443,4 +485,5 @@ AStar::AStar() {
AStar::~AStar() {
pass = 1;
+ clear();
}
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 38d13d510b..b7b7e54125 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -33,6 +33,8 @@
#include "reference.h"
#include "self_list.h"
/**
+ A* pathfinding algorithm
+
@author Juan Linietsky <reduzio@gmail.com>
*/
@@ -53,7 +55,7 @@ class AStar : public Reference {
Vector<Point *> neighbours;
- //used for pathfinding
+ // Used for pathfinding
Point *prev_point;
real_t distance;
@@ -101,10 +103,13 @@ public:
int get_available_point_id() const;
void add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale = 1);
- Vector3 get_point_pos(int p_id) const;
+ Vector3 get_point_position(int p_id) const;
+ void set_point_position(int p_id, const Vector3 &p_pos);
real_t get_point_weight_scale(int p_id) const;
+ void set_point_weight_scale(int p_id, real_t p_weight_scale);
void remove_point(int p_id);
bool has_point(int p_id) const;
+ PoolVector<int> get_point_connections(int p_id);
Array get_points();
void connect_points(int p_id, int p_with_id, bool bidirectional = true);
@@ -114,7 +119,7 @@ public:
void clear();
int get_closest_point(const Vector3 &p_point) const;
- Vector3 get_closest_pos_in_segment(const Vector3 &p_point) const;
+ Vector3 get_closest_position_in_segment(const Vector3 &p_point) const;
PoolVector<Vector3> get_point_path(int p_from_id, int p_to_id);
PoolVector<int> get_id_path(int p_from_id, int p_to_id);
diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp
index 7132b6573e..2c587762e8 100644
--- a/core/math/camera_matrix.cpp
+++ b/core/math/camera_matrix.cpp
@@ -577,7 +577,7 @@ real_t CameraMatrix::get_fov() const {
if ((matrix[8] == 0) && (matrix[9] == 0)) {
return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))) * 2.0;
} else {
- // our frustum is asymetrical need to calculate the left planes angle seperately..
+ // our frustum is asymmetrical need to calculate the left planes angle separately..
Plane left_plane = Plane(matrix[3] + matrix[0],
matrix[7] + matrix[4],
matrix[11] + matrix[8],
diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp
index 6fb688f16b..64e01e5841 100644
--- a/core/math/math_funcs.cpp
+++ b/core/math/math_funcs.cpp
@@ -176,3 +176,18 @@ float Math::random(float from, float to) {
float ret = (float)r / (float)RANDOM_MAX;
return (ret) * (to - from) + from;
}
+
+int Math::wrapi(int value, int min, int max) {
+ --max;
+ int rng = max - min + 1;
+ value = ((value - min) % rng);
+ if (value < 0)
+ return max + 1 + value;
+ else
+ return min + value;
+}
+
+float Math::wrapf(float value, float min, float max) {
+ float rng = max - min;
+ return min + (value - min) - (rng * floor((value - min) / rng));
+}
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 9651e37f3e..7715e5d6e5 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -104,8 +104,44 @@ public:
static _ALWAYS_INLINE_ double exp(double p_x) { return ::exp(p_x); }
static _ALWAYS_INLINE_ float exp(float p_x) { return ::expf(p_x); }
- static _ALWAYS_INLINE_ bool is_nan(double p_val) { return (p_val != p_val); }
- static _ALWAYS_INLINE_ bool is_nan(float p_val) { return (p_val != p_val); }
+ static _ALWAYS_INLINE_ bool is_nan(double p_val) {
+#ifdef _MSC_VER
+ return _isnan(p_val);
+#elif defined(__GNUC__) && __GNUC__ < 6
+ union {
+ uint64_t u;
+ double f;
+ } ieee754;
+ ieee754.f = p_val;
+ // (unsigned)(0x7ff0000000000001 >> 32) : 0x7ff00000
+ return ((((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0)) > 0x7ff00000);
+#else
+ return isnan(p_val);
+#endif
+ }
+
+ static _ALWAYS_INLINE_ bool is_nan(float p_val) {
+#ifdef _MSC_VER
+ return _isnan(p_val);
+#elif defined(__GNUC__) && __GNUC__ < 6
+ union {
+ uint32_t u;
+ float f;
+ } ieee754;
+ ieee754.f = p_val;
+ // -----------------------------------
+ // (single-precision floating-point)
+ // NaN : s111 1111 1xxx xxxx xxxx xxxx xxxx xxxx
+ // : (> 0x7f800000)
+ // where,
+ // s : sign
+ // x : non-zero number
+ // -----------------------------------
+ return ((ieee754.u & 0x7fffffff) > 0x7f800000);
+#else
+ return isnan(p_val);
+#endif
+ }
static _ALWAYS_INLINE_ bool is_inf(double p_val) {
#ifdef _MSC_VER
@@ -171,6 +207,9 @@ public:
static _ALWAYS_INLINE_ double round(double p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); }
static _ALWAYS_INLINE_ float round(float p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); }
+ static int wrapi(int value, int min, int max);
+ static float wrapf(float value, float min, float max);
+
// double only, as these functions are mainly used by the editor and not performance-critical,
static double ease(double p_x, double p_c);
static int step_decimals(double p_step);
@@ -232,7 +271,7 @@ public:
#elif defined(_MSC_VER) && _MSC_VER < 1800
__asm fld a __asm fistp b
-/*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) )
+ /*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) )
// use AT&T inline assembly style, document that
// we use memory as output (=m) and input (m)
__asm__ __volatile__ (
@@ -351,6 +390,23 @@ public:
return hf;
}
+
+ static _ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) {
+ return p_step != 0 ? Math::stepify(p_target - p_offset, p_step) + p_offset : p_target;
+ }
+
+ static _ALWAYS_INLINE_ float snap_scalar_seperation(float p_offset, float p_step, float p_target, float p_separation) {
+ if (p_step != 0) {
+ float a = Math::stepify(p_target - p_offset, p_step + p_separation) + p_offset;
+ float b = a;
+ if (p_target >= 0)
+ b -= p_separation;
+ else
+ b += p_step;
+ return (Math::abs(p_target - a) < Math::abs(p_target - b)) ? a : b;
+ }
+ return p_target;
+ }
};
#endif // MATH_FUNCS_H
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index 4051de7afb..ab3bca79ae 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -279,7 +279,7 @@ Vector3 Basis::get_signed_scale() const {
// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S.
// Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3.
-// This (internal) function is too specıfıc and named too ugly to expose to users, and probably there's no need to do so.
+// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so.
Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(determinant() == 0, Vector3());
@@ -371,31 +371,30 @@ Vector3 Basis::get_euler_xyz() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(is_rotation() == false, euler);
#endif
- euler.y = Math::asin(elements[0][2]);
- if (euler.y < Math_PI * 0.5) {
- if (euler.y > -Math_PI * 0.5) {
+ real_t sy = elements[0][2];
+ if (sy < 1.0) {
+ if (sy > -1.0) {
// is this a pure Y rotation?
if (elements[1][0] == 0.0 && elements[0][1] == 0.0 && elements[1][2] == 0 && elements[2][1] == 0 && elements[1][1] == 1) {
- // return the simplest form
+ // return the simplest form (human friendlier in editor and scripts)
euler.x = 0;
euler.y = atan2(elements[0][2], elements[0][0]);
euler.z = 0;
} else {
euler.x = Math::atan2(-elements[1][2], elements[2][2]);
+ euler.y = Math::asin(sy);
euler.z = Math::atan2(-elements[0][1], elements[0][0]);
}
-
} else {
- real_t r = Math::atan2(elements[1][0], elements[1][1]);
+ euler.x = -Math::atan2(elements[0][1], elements[1][1]);
+ euler.y = -Math_PI / 2.0;
euler.z = 0.0;
- euler.x = euler.z - r;
}
} else {
- real_t r = Math::atan2(elements[0][1], elements[1][1]);
- euler.z = 0;
- euler.x = r - euler.z;
+ euler.x = Math::atan2(elements[0][1], elements[1][1]);
+ euler.y = Math_PI / 2.0;
+ euler.z = 0.0;
}
-
return euler;
}
@@ -445,7 +444,7 @@ Vector3 Basis::get_euler_yxz() const {
if (m12 > -1) {
// is this a pure X rotation?
if (elements[1][0] == 0 && elements[0][1] == 0 && elements[0][2] == 0 && elements[2][0] == 0 && elements[0][0] == 1) {
- // return the simplest form
+ // return the simplest form (human friendlier in editor and scripts)
euler.x = atan2(-m12, elements[1][1]);
euler.y = 0;
euler.z = 0;
@@ -538,7 +537,7 @@ Basis::operator String() const {
return mtx;
}
-Basis::operator Quat() const {
+Quat Basis::get_quat() const {
//commenting this check because precision issues cause it to fail when it shouldn't
//#ifdef MATH_CHECKS
//ERR_FAIL_COND_V(is_rotation() == false, Quat());
@@ -710,12 +709,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
r_angle = angle;
}
-Basis::Basis(const Vector3 &p_euler) {
-
- set_euler(p_euler);
-}
-
-Basis::Basis(const Quat &p_quat) {
+void Basis::set_quat(const Quat &p_quat) {
real_t d = p_quat.length_squared();
real_t s = 2.0 / d;
@@ -750,7 +744,3 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine;
elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
}
-
-Basis::Basis(const Vector3 &p_axis, real_t p_phi) {
- set_axis_angle(p_axis, p_phi);
-}
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 23429888e0..9a33b8203d 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -88,8 +88,11 @@ public:
Vector3 get_euler_yxz() const;
void set_euler_yxz(const Vector3 &p_euler);
- Vector3 get_euler() const { return get_euler_yxz(); };
- void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); };
+ Quat get_quat() const;
+ void set_quat(const Quat &p_quat);
+
+ Vector3 get_euler() const { return get_euler_yxz(); }
+ void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
void set_axis_angle(const Vector3 &p_axis, real_t p_phi);
@@ -205,11 +208,11 @@ public:
bool is_symmetric() const;
Basis diagonalize();
- operator Quat() const;
+ operator Quat() const { return get_quat(); }
- Basis(const Quat &p_quat); // euler
- Basis(const Vector3 &p_euler); // euler
- Basis(const Vector3 &p_axis, real_t p_phi);
+ Basis(const Quat &p_quat) { set_quat(p_quat); };
+ Basis(const Vector3 &p_euler) { set_euler(p_euler); }
+ Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); }
_FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) {
elements[0] = row0;
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-23 13:22:14 +0000