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/delaunay.cpp1
-rw-r--r--core/math/delaunay.h145
-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, 211 insertions, 31 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/delaunay.cpp b/core/math/delaunay.cpp
new file mode 100644
index 0000000000..8cae92b7c0
--- /dev/null
+++ b/core/math/delaunay.cpp
@@ -0,0 +1 @@
+#include "delaunay.h"
diff --git a/core/math/delaunay.h b/core/math/delaunay.h
new file mode 100644
index 0000000000..09aebc773f
--- /dev/null
+++ b/core/math/delaunay.h
@@ -0,0 +1,145 @@
+#ifndef DELAUNAY_H
+#define DELAUNAY_H
+
+#include "math_2d.h"
+
+class Delaunay2D {
+public:
+ struct Triangle {
+
+ int points[3];
+ bool bad;
+ Triangle() { bad = false; }
+ Triangle(int p_a, int p_b, int p_c) {
+ points[0] = p_a;
+ points[1] = p_b;
+ points[2] = p_c;
+ bad = false;
+ }
+ };
+
+ struct Edge {
+ int edge[2];
+ bool bad;
+ Edge() { bad = false; }
+ Edge(int p_a, int p_b) {
+ bad = false;
+ edge[0] = p_a;
+ edge[1] = p_b;
+ }
+ };
+
+ static bool circum_circle_contains(const Vector<Vector2> &p_vertices, const Triangle &p_triangle, int p_vertex) {
+
+ Vector2 p1 = p_vertices[p_triangle.points[0]];
+ Vector2 p2 = p_vertices[p_triangle.points[1]];
+ Vector2 p3 = p_vertices[p_triangle.points[2]];
+
+ real_t ab = p1.x * p1.x + p1.y * p1.y;
+ real_t cd = p2.x * p2.x + p2.y * p2.y;
+ real_t ef = p3.x * p3.x + p3.y * p3.y;
+
+ Vector2 circum(
+ (ab * (p3.y - p2.y) + cd * (p1.y - p3.y) + ef * (p2.y - p1.y)) / (p1.x * (p3.y - p2.y) + p2.x * (p1.y - p3.y) + p3.x * (p2.y - p1.y)),
+ (ab * (p3.x - p2.x) + cd * (p1.x - p3.x) + ef * (p2.x - p1.x)) / (p1.y * (p3.x - p2.x) + p2.y * (p1.x - p3.x) + p3.y * (p2.x - p1.x)));
+
+ circum *= 0.5;
+ float r = p1.distance_squared_to(circum);
+ float d = p_vertices[p_vertex].distance_squared_to(circum);
+ return d <= r;
+ }
+
+ static bool edge_compare(const Vector<Vector2> &p_vertices, const Edge &p_a, const Edge &p_b) {
+ if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON) {
+ return true;
+ }
+
+ if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON) {
+ return true;
+ }
+
+ return false;
+ }
+
+ static Vector<Triangle> triangulate(const Vector<Vector2> &p_points) {
+
+ Vector<Vector2> points = p_points;
+ Vector<Triangle> triangles;
+
+ Rect2 rect;
+ for (int i = 0; i < p_points.size(); i++) {
+ if (i == 0) {
+ rect.position = p_points[i];
+ } else {
+ rect.expand_to(p_points[i]);
+ }
+ }
+
+ float delta_max = MAX(rect.size.width, rect.size.height);
+ Vector2 center = rect.position + rect.size * 0.5;
+
+ points.push_back(Vector2(center.x - 20 * delta_max, center.y - delta_max));
+ points.push_back(Vector2(center.x, center.y + 20 * delta_max));
+ points.push_back(Vector2(center.x + 20 * delta_max, center.y - delta_max));
+
+ triangles.push_back(Triangle(p_points.size() + 0, p_points.size() + 1, p_points.size() + 2));
+
+ for (int i = 0; i < p_points.size(); i++) {
+ //std::cout << "Traitement du point " << *p << std::endl;
+ //std::cout << "_triangles contains " << _triangles.size() << " elements" << std::endl;
+
+ Vector<Edge> polygon;
+
+ for (int j = 0; j < triangles.size(); j++) {
+ if (circum_circle_contains(points, triangles[j], i)) {
+ triangles[j].bad = true;
+ polygon.push_back(Edge(triangles[j].points[0], triangles[j].points[1]));
+ polygon.push_back(Edge(triangles[j].points[1], triangles[j].points[2]));
+ polygon.push_back(Edge(triangles[j].points[2], triangles[j].points[0]));
+ }
+ }
+
+ for (int j = 0; j < triangles.size(); j++) {
+ if (triangles[j].bad) {
+ triangles.remove(j);
+ j--;
+ }
+ }
+
+ for (int j = 0; j < polygon.size(); j++) {
+ for (int k = j + 1; k < polygon.size(); k++) {
+ if (edge_compare(points, polygon[j], polygon[k])) {
+ polygon[j].bad = true;
+ polygon[k].bad = true;
+ }
+ }
+ }
+
+ for (int j = 0; j < polygon.size(); j++) {
+
+ if (polygon[j].bad) {
+ continue;
+ }
+ triangles.push_back(Triangle(polygon[j].edge[0], polygon[j].edge[1], i));
+ }
+ }
+
+ for (int i = 0; i < triangles.size(); i++) {
+ bool invalid = false;
+ for (int j = 0; j < 3; j++) {
+ if (triangles[i].points[j] >= p_points.size()) {
+ invalid = true;
+ break;
+ }
+ }
+ if (invalid) {
+ triangles.remove(i);
+ i--;
+ }
+ }
+
+ return triangles;
+ }
+};
+
+#endif // DELAUNAY_H
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-01-25 07:17:45 +0000