summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star.cpp24
-rw-r--r--core/math/a_star.h2
-rw-r--r--core/math/aabb.cpp4
-rw-r--r--core/math/bsp_tree.cpp10
-rw-r--r--core/math/math_funcs.h18
-rw-r--r--core/math/matrix3.cpp24
-rw-r--r--core/math/matrix3.h4
-rw-r--r--core/math/quat.cpp16
-rw-r--r--core/math/quat.h4
-rw-r--r--core/math/transform.cpp4
10 files changed, 68 insertions, 42 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 6908d7831d..021391da83 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -96,11 +96,11 @@ void AStar::remove_point(int p_id) {
Point *p = points[p_id];
- for (int i = 0; i < p->neighbours.size(); i++) {
+ for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
- Segment s(p_id, p->neighbours[i]->id);
+ Segment s(p_id, E->get()->id);
segments.erase(s);
- p->neighbours[i]->neighbours.erase(p);
+ E->get()->neighbours.erase(p);
}
memdelete(p);
@@ -115,10 +115,10 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) {
Point *a = points[p_id];
Point *b = points[p_with_id];
- a->neighbours.push_back(b);
+ a->neighbours.insert(b);
if (bidirectional)
- b->neighbours.push_back(a);
+ b->neighbours.insert(a);
Segment s(p_id, p_with_id);
if (s.from == p_id) {
@@ -168,8 +168,8 @@ PoolVector<int> AStar::get_point_connections(int p_id) {
Point *p = points[p_id];
- for (int i = 0; i < p->neighbours.size(); i++) {
- point_list.push_back(p->neighbours[i]->id);
+ for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
+ point_list.push_back(E->get()->id);
}
return point_list;
@@ -242,9 +242,9 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
bool found_route = false;
- for (int i = 0; i < begin_point->neighbours.size(); i++) {
+ for (Set<Point *>::Element *E = begin_point->neighbours.front(); E; E = E->next()) {
- Point *n = begin_point->neighbours[i];
+ Point *n = E->get();
n->prev_point = begin_point;
n->distance = _compute_cost(begin_point->id, n->id) * n->weight_scale;
n->last_pass = pass;
@@ -283,12 +283,10 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
}
Point *p = least_cost_point->self();
- // Open the neighbours for search
- int es = p->neighbours.size();
- for (int i = 0; i < es; i++) {
+ for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
- Point *e = p->neighbours[i];
+ Point *e = E->get();
real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance;
diff --git a/core/math/a_star.h b/core/math/a_star.h
index f89e17c7bb..8c1b5f64cb 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -54,7 +54,7 @@ class AStar : public Reference {
real_t weight_scale;
uint64_t last_pass;
- Vector<Point *> neighbours;
+ Set<Point *> neighbours;
// Used for pathfinding
Point *prev_point;
diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp
index cff19f990c..e2e71dda92 100644
--- a/core/math/aabb.cpp
+++ b/core/math/aabb.cpp
@@ -245,7 +245,6 @@ Vector3 AABB::get_longest_axis() const {
if (size.z > max_size) {
axis = Vector3(0, 0, 1);
- max_size = size.z;
}
return axis;
@@ -262,7 +261,6 @@ int AABB::get_longest_axis_index() const {
if (size.z > max_size) {
axis = 2;
- max_size = size.z;
}
return axis;
@@ -280,7 +278,6 @@ Vector3 AABB::get_shortest_axis() const {
if (size.z < max_size) {
axis = Vector3(0, 0, 1);
- max_size = size.z;
}
return axis;
@@ -297,7 +294,6 @@ int AABB::get_shortest_axis_index() const {
if (size.z < max_size) {
axis = 2;
- max_size = size.z;
}
return axis;
diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp
index b1424e1d78..2e184f7a88 100644
--- a/core/math/bsp_tree.cpp
+++ b/core/math/bsp_tree.cpp
@@ -244,10 +244,8 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const {
const Node *nodesptr = &nodes[0];
const Plane *planesptr = &planes[0];
- int plane_count = planes.size();
int idx = node_count - 1;
- int steps = 0;
while (true) {
@@ -259,21 +257,19 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const {
return true;
}
- uint16_t plane = nodesptr[idx].plane;
#ifdef DEBUG_ENABLED
-
+ int plane_count = planes.size();
+ uint16_t plane = nodesptr[idx].plane;
ERR_FAIL_INDEX_V(plane, plane_count, false);
#endif
+
bool over = planesptr[nodesptr[idx].plane].is_point_over(p_point);
idx = over ? nodes[idx].over : nodes[idx].under;
#ifdef DEBUG_ENABLED
-
ERR_FAIL_COND_V(idx < MAX_NODES && idx >= node_count, false);
#endif
-
- steps++;
}
return false;
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 20001bb9a6..f0c0268f31 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -182,8 +182,22 @@ public:
static _ALWAYS_INLINE_ float abs(float g) { return absf(g); }
static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; }
- static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) { return (p_x >= 0) ? Math::fmod(p_x, p_y) : p_y - Math::fmod(-p_x, p_y); }
- static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) { return (p_x >= 0) ? Math::fmod(p_x, p_y) : p_y - Math::fmod(-p_x, p_y); }
+ static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) {
+ double value = Math::fmod(p_x, p_y);
+ if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) {
+ value += p_y;
+ }
+ value += 0.0;
+ return value;
+ }
+ static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) {
+ float value = Math::fmod(p_x, p_y);
+ if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) {
+ value += p_y;
+ }
+ value += 0.0;
+ return value;
+ }
static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * Math_PI / 180.0; }
static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * Math_PI / 180.0; }
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index 202115e2ca..2371f49561 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -356,8 +356,7 @@ void Basis::rotate(const Quat &p_quat) {
*this = rotated(p_quat);
}
-// TODO: rename this to get_rotation_euler
-Vector3 Basis::get_rotation() const {
+Vector3 Basis::get_rotation_euler() const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
// See the comment in get_scale() for further information.
@@ -371,6 +370,20 @@ Vector3 Basis::get_rotation() const {
return m.get_euler();
}
+Quat Basis::get_rotation_quat() const {
+ // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
+ // and returns the Euler angles corresponding to the rotation part, complementing get_scale().
+ // See the comment in get_scale() for further information.
+ Basis m = orthonormalized();
+ real_t det = m.determinant();
+ if (det < 0) {
+ // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
+ m.scale(Vector3(-1, -1, -1));
+ }
+
+ return m.get_quat();
+}
+
void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
@@ -591,10 +604,9 @@ Basis::operator String() 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());
- //#endif
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_rotation() == false, Quat());
+#endif
real_t trace = elements[0][0] + elements[1][1] + elements[2][2];
real_t temp[4];
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 63d4f5d79d..cd1b51baa6 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -84,9 +84,11 @@ public:
void rotate(const Quat &p_quat);
Basis rotated(const Quat &p_quat) const;
- Vector3 get_rotation() const;
+ Vector3 get_rotation_euler() const;
void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
+ Quat get_rotation_quat() const;
+ Vector3 get_rotation() const { return get_rotation_euler(); };
Vector3 rotref_posscale_decomposition(Basis &rotref) const;
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index b938fc3cfd..67c9048a41 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -139,15 +139,15 @@ bool Quat::is_normalized() const {
Quat Quat::inverse() const {
#ifdef MATH_CHECKS
- ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
+ ERR_FAIL_COND_V(is_normalized() == false, Quat());
#endif
return Quat(-x, -y, -z, w);
}
Quat Quat::slerp(const Quat &q, const real_t &t) const {
#ifdef MATH_CHECKS
- ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
- ERR_FAIL_COND_V(q.is_normalized() == false, Quat(0, 0, 0, 0));
+ ERR_FAIL_COND_V(is_normalized() == false, Quat());
+ ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
#endif
Quat to1;
real_t omega, cosom, sinom, scale0, scale1;
@@ -192,7 +192,10 @@ Quat Quat::slerp(const Quat &q, const real_t &t) const {
}
Quat Quat::slerpni(const Quat &q, const real_t &t) const {
-
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Quat());
+ ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
+#endif
const Quat &from = *this;
real_t dot = from.dot(q);
@@ -211,7 +214,10 @@ Quat Quat::slerpni(const Quat &q, const real_t &t) const {
}
Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const {
-
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Quat());
+ ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
+#endif
//the only way to do slerp :|
real_t t2 = (1.0 - t) * t * 2;
Quat sp = this->slerp(q, t);
diff --git a/core/math/quat.h b/core/math/quat.h
index 3e1344a913..6dc8d66f60 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -84,7 +84,9 @@ public:
}
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
-
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, v);
+#endif
Vector3 u(x, y, z);
Vector3 uv = u.cross(v);
return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index 7cd186ca60..d1e190f4b9 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -120,11 +120,11 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c)
/* not sure if very "efficient" but good enough? */
Vector3 src_scale = basis.get_scale();
- Quat src_rot = basis.orthonormalized();
+ Quat src_rot = basis.get_rotation_quat();
Vector3 src_loc = origin;
Vector3 dst_scale = p_transform.basis.get_scale();
- Quat dst_rot = p_transform.basis;
+ Quat dst_rot = p_transform.basis.get_rotation_quat();
Vector3 dst_loc = p_transform.origin;
Transform dst; //this could be made faster by using a single function in Basis..
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-03 22:01:43 +0000