summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star.cpp56
-rw-r--r--core/math/a_star.h6
-rw-r--r--core/math/math_funcs.h17
-rw-r--r--core/math/matrix3.cpp38
-rw-r--r--core/math/matrix3.h15
5 files changed, 86 insertions, 46 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 4f80fb2491..f43af49754 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;
@@ -64,12 +66,29 @@ Vector3 AStar::get_point_position(int p_id) const {
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()) {
@@ -171,6 +191,7 @@ int AStar::get_closest_point(const Vector3 &p_point) const {
return closest_id;
}
+
Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
real_t closest_dist = 1e20;
@@ -222,15 +243,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 +267,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 +277,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 +285,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 +306,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 +315,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 +323,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 +354,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 +375,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 +405,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 +426,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;
@@ -414,7 +437,9 @@ 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", "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);
@@ -443,4 +468,5 @@ AStar::AStar() {
AStar::~AStar() {
pass = 1;
+ clear();
}
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 2c1e2e2cf7..23773e82e2 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;
@@ -102,7 +104,9 @@ public:
void add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale = 1);
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;
Array get_points();
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 4644a00d7c..7715e5d6e5 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -390,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-07-20 00:49:09 +0000