summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star.cpp88
-rw-r--r--core/math/a_star.h39
-rw-r--r--core/math/aabb.cpp5
-rw-r--r--core/math/aabb.h1
-rw-r--r--core/math/basis.cpp23
-rw-r--r--core/math/basis.h8
-rw-r--r--core/math/bsp_tree.cpp4
-rw-r--r--core/math/delaunay.h4
-rw-r--r--core/math/disjoint_set.cpp31
-rw-r--r--core/math/disjoint_set.h155
-rw-r--r--core/math/geometry.cpp7
-rw-r--r--core/math/math_funcs.cpp2
-rw-r--r--core/math/math_funcs.h2
-rw-r--r--core/math/plane.cpp9
-rw-r--r--core/math/plane.h6
-rw-r--r--core/math/quat.cpp5
-rw-r--r--core/math/quat.h1
-rw-r--r--core/math/quick_hull.cpp2
-rw-r--r--core/math/rect2.cpp5
-rw-r--r--core/math/rect2.h5
-rw-r--r--core/math/transform.cpp5
-rw-r--r--core/math/transform.h1
-rw-r--r--core/math/transform_2d.cpp6
-rw-r--r--core/math/transform_2d.h1
-rw-r--r--core/math/vector2.cpp4
-rw-r--r--core/math/vector2.h6
-rw-r--r--core/math/vector3.cpp20
-rw-r--r--core/math/vector3.h29
28 files changed, 366 insertions, 108 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 60b7326c29..810e290922 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -164,23 +164,23 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) {
}
Segment s(p_id, p_with_id);
- if (s.from == p_id) {
- s.from_point = a;
- s.to_point = b;
- } else {
- s.from_point = b;
- s.to_point = a;
+ if (bidirectional) s.direction = Segment::BIDIRECTIONAL;
+
+ Set<Segment>::Element *element = segments.find(s);
+ if (element != NULL) {
+ s.direction |= element->get().direction;
+ if (s.direction == Segment::BIDIRECTIONAL) {
+ // Both are neighbours of each other now
+ a->unlinked_neighbours.remove(b->id);
+ b->unlinked_neighbours.remove(a->id);
+ }
+ segments.erase(element);
}
segments.insert(s);
}
-void AStar::disconnect_points(int p_id, int p_with_id) {
-
- Segment s(p_id, p_with_id);
- ERR_FAIL_COND(!segments.has(s));
-
- segments.erase(s);
+void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) {
Point *a;
bool a_exists = points.lookup(p_id, a);
@@ -190,10 +190,33 @@ void AStar::disconnect_points(int p_id, int p_with_id) {
bool b_exists = points.lookup(p_with_id, b);
CRASH_COND(!b_exists);
- a->neighbours.remove(b->id);
- a->unlinked_neighbours.remove(b->id);
- b->neighbours.remove(a->id);
- b->unlinked_neighbours.remove(a->id);
+ Segment s(p_id, p_with_id);
+ int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : s.direction;
+
+ Set<Segment>::Element *element = segments.find(s);
+ if (element != NULL) {
+ // s is the new segment
+ // Erase the directions to be removed
+ s.direction = (element->get().direction & ~remove_direction);
+
+ a->neighbours.remove(b->id);
+ if (bidirectional) {
+ b->neighbours.remove(a->id);
+ if (element->get().direction != Segment::BIDIRECTIONAL) {
+ a->unlinked_neighbours.remove(b->id);
+ b->unlinked_neighbours.remove(a->id);
+ }
+ } else {
+ if (s.direction == Segment::NONE)
+ b->unlinked_neighbours.remove(a->id);
+ else
+ a->unlinked_neighbours.set(b->id, b);
+ }
+
+ segments.erase(element);
+ if (s.direction != Segment::NONE)
+ segments.insert(s);
+ }
}
bool AStar::has_point(int p_id) const {
@@ -227,10 +250,13 @@ PoolVector<int> AStar::get_point_connections(int p_id) {
return point_list;
}
-bool AStar::are_points_connected(int p_id, int p_with_id) const {
+bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) const {
Segment s(p_id, p_with_id);
- return segments.has(s);
+ const Set<Segment>::Element *element = segments.find(s);
+
+ return element != NULL &&
+ (bidirectional || (element->get().direction & s.direction) == s.direction);
}
void AStar::clear() {
@@ -257,14 +283,14 @@ void AStar::reserve_space(int p_num_nodes) {
points.reserve(p_num_nodes);
}
-int AStar::get_closest_point(const Vector3 &p_point) const {
+int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const {
int closest_id = -1;
real_t closest_dist = 1e20;
for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
- if (!(*it.value)->enabled) continue; // Disabled points should not be considered.
+ if (!p_include_disabled && !(*it.value)->enabled) continue; // Disabled points should not be considered.
real_t d = p_point.distance_squared_to((*it.value)->pos);
if (closest_id < 0 || d < closest_dist) {
@@ -284,13 +310,17 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) {
- if (!(E->get().from_point->enabled && E->get().to_point->enabled)) {
+ Point *from_point = nullptr, *to_point = nullptr;
+ points.lookup(E->get().u, from_point);
+ points.lookup(E->get().v, to_point);
+
+ if (!(from_point->enabled && to_point->enabled)) {
continue;
}
Vector3 segment[2] = {
- E->get().from_point->pos,
- E->get().to_point->pos,
+ from_point->pos,
+ to_point->pos,
};
Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment);
@@ -532,15 +562,15 @@ void AStar::_bind_methods() {
ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar::is_point_disabled);
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("disconnect_points", "id", "to_id", "bidirectional"), &AStar::disconnect_points, DEFVAL(true));
+ ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar::are_points_connected, DEFVAL(true));
ClassDB::bind_method(D_METHOD("get_point_count"), &AStar::get_point_count);
ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar::get_point_capacity);
ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar::reserve_space);
ClassDB::bind_method(D_METHOD("clear"), &AStar::clear);
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_position"), &AStar::get_closest_point);
+ ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar::get_closest_point, DEFVAL(false));
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);
@@ -638,8 +668,8 @@ void AStar2D::reserve_space(int p_num_nodes) {
astar.reserve_space(p_num_nodes);
}
-int AStar2D::get_closest_point(const Vector2 &p_point) const {
- return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0));
+int AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const {
+ return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0), p_include_disabled);
}
Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
@@ -693,7 +723,7 @@ void AStar2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar2D::reserve_space);
ClassDB::bind_method(D_METHOD("clear"), &AStar2D::clear);
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_position"), &AStar2D::get_closest_point);
+ ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar2D::get_closest_point, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment);
ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path);
diff --git a/core/math/a_star.h b/core/math/a_star.h
index ec2a06f07f..8ff62e646b 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -81,24 +81,35 @@ class AStar : public Reference {
struct Segment {
union {
struct {
- int32_t from;
- int32_t to;
+ int32_t u;
+ int32_t v;
};
uint64_t key;
};
- Point *from_point;
- Point *to_point;
+ enum {
+ NONE = 0,
+ FORWARD = 1,
+ BACKWARD = 2,
+ BIDIRECTIONAL = FORWARD | BACKWARD
+ };
+ unsigned char direction;
bool operator<(const Segment &p_s) const { return key < p_s.key; }
- Segment() { key = 0; }
+ Segment() {
+ key = 0;
+ direction = NONE;
+ }
Segment(int p_from, int p_to) {
- if (p_from > p_to) {
- SWAP(p_from, p_to);
+ if (p_from < p_to) {
+ u = p_from;
+ v = p_to;
+ direction = FORWARD;
+ } else {
+ u = p_to;
+ v = p_from;
+ direction = BACKWARD;
}
-
- from = p_from;
- to = p_to;
}
};
@@ -133,15 +144,15 @@ public:
bool is_point_disabled(int p_id) const;
void connect_points(int p_id, int p_with_id, bool bidirectional = true);
- void disconnect_points(int p_id, int p_with_id);
- bool are_points_connected(int p_id, int p_with_id) const;
+ void disconnect_points(int p_id, int p_with_id, bool bidirectional = true);
+ bool are_points_connected(int p_id, int p_with_id, bool bidirectional = true) const;
int get_point_count() const;
int get_point_capacity() const;
void reserve_space(int p_num_nodes);
void clear();
- int get_closest_point(const Vector3 &p_point) const;
+ int get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) 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);
@@ -183,7 +194,7 @@ public:
void reserve_space(int p_num_nodes);
void clear();
- int get_closest_point(const Vector2 &p_point) const;
+ int get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const;
Vector2 get_closest_position_in_segment(const Vector2 &p_point) const;
PoolVector<Vector2> get_point_path(int p_from_id, int p_to_id);
diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp
index a4eb1fe2a5..27da061274 100644
--- a/core/math/aabb.cpp
+++ b/core/math/aabb.cpp
@@ -69,6 +69,11 @@ void AABB::merge_with(const AABB &p_aabb) {
size = max - min;
}
+bool AABB::is_equal_approx(const AABB &p_aabb) const {
+
+ return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size);
+}
+
AABB AABB::intersection(const AABB &p_aabb) const {
Vector3 src_min = position;
diff --git a/core/math/aabb.h b/core/math/aabb.h
index 52e5ed3626..c3ce33c6f4 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -64,6 +64,7 @@ public:
bool operator==(const AABB &p_rval) const;
bool operator!=(const AABB &p_rval) const;
+ bool is_equal_approx(const AABB &p_aabb) const;
_FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap
_FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap
_FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this
diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 2985959113..d77501c0f6 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -106,17 +106,17 @@ Basis Basis::orthonormalized() const {
}
bool Basis::is_orthogonal() const {
- Basis id;
+ Basis identity;
Basis m = (*this) * transposed();
- return is_equal_approx(id, m);
+ return m.is_equal_approx(identity);
}
bool Basis::is_diagonal() const {
return (
- Math::is_equal_approx(elements[0][1], 0) && Math::is_equal_approx(elements[0][2], 0) &&
- Math::is_equal_approx(elements[1][0], 0) && Math::is_equal_approx(elements[1][2], 0) &&
- Math::is_equal_approx(elements[2][0], 0) && Math::is_equal_approx(elements[2][1], 0));
+ Math::is_zero_approx(elements[0][1]) && Math::is_zero_approx(elements[0][2]) &&
+ Math::is_zero_approx(elements[1][0]) && Math::is_zero_approx(elements[1][2]) &&
+ Math::is_zero_approx(elements[2][0]) && Math::is_zero_approx(elements[2][1]));
}
bool Basis::is_rotation() const {
@@ -557,16 +557,9 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) {
*this = ymat * xmat * zmat;
}
-bool Basis::is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon) const {
+bool Basis::is_equal_approx(const Basis &p_basis) const {
- for (int i = 0; i < 3; i++) {
- for (int j = 0; j < 3; j++) {
- if (!Math::is_equal_approx(a.elements[i][j], b.elements[i][j], p_epsilon))
- return false;
- }
- }
-
- return true;
+ return elements[0].is_equal_approx(p_basis.elements[0]) && elements[1].is_equal_approx(p_basis.elements[1]) && elements[2].is_equal_approx(p_basis.elements[2]);
}
bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon) const {
@@ -807,7 +800,7 @@ void Basis::set_quat(const Quat &p_quat) {
void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle
#ifdef MATH_CHECKS
- ERR_FAIL_COND(!p_axis.is_normalized());
+ ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "Axis must be normalized.");
#endif
Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z);
real_t cosine = Math::cos(p_phi);
diff --git a/core/math/basis.h b/core/math/basis.h
index 053effda69..9b2e38b3d3 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -28,13 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
-// Circular dependency between Vector3 and Basis :/
-#include "core/math/vector3.h"
-
#ifndef BASIS_H
#define BASIS_H
#include "core/math/quat.h"
+#include "core/math/vector3.h"
class Basis {
public:
@@ -129,7 +127,9 @@ public:
return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
}
- bool is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon = CMP_EPSILON) const;
+ bool is_equal_approx(const Basis &p_basis) const;
+ // TODO: Break compatibility in 4.0 by getting rid of this so that it's only an instance method. See also TODO in variant_call.cpp
+ bool is_equal_approx(const Basis &a, const Basis &b) const { return a.is_equal_approx(b); }
bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon = UNIT_EPSILON) const;
bool operator==(const Basis &p_matrix) const;
diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp
index cfa698282e..ece293d036 100644
--- a/core/math/bsp_tree.cpp
+++ b/core/math/bsp_tree.cpp
@@ -364,7 +364,7 @@ static int _bsp_create_node(const Face3 *p_faces, const Vector<int> &p_indices,
const Face3 &f = p_faces[indices[i]];
/*
- if (f.get_plane().is_almost_like(divisor_plane))
+ if (f.get_plane().is_equal_approx(divisor_plane))
continue;
*/
@@ -412,7 +412,7 @@ static int _bsp_create_node(const Face3 *p_faces, const Vector<int> &p_indices,
for (int i = 0; i < p_planes.size(); i++) {
- if (p_planes[i].is_almost_like(divisor_plane)) {
+ if (p_planes[i].is_equal_approx(divisor_plane)) {
divisor_plane_idx = i;
break;
}
diff --git a/core/math/delaunay.h b/core/math/delaunay.h
index 3f8013a3e6..89a34de082 100644
--- a/core/math/delaunay.h
+++ b/core/math/delaunay.h
@@ -80,11 +80,11 @@ public:
}
static bool edge_compare(const Vector<Vector2> &p_vertices, const Edge &p_a, const Edge &p_b) {
- if (p_vertices[p_a.edge[0]] == p_vertices[p_b.edge[0]] && p_vertices[p_a.edge[1]] == p_vertices[p_b.edge[1]]) {
+ if (p_vertices[p_a.edge[0]].is_equal_approx(p_vertices[p_b.edge[0]]) && p_vertices[p_a.edge[1]].is_equal_approx(p_vertices[p_b.edge[1]])) {
return true;
}
- if (p_vertices[p_a.edge[0]] == p_vertices[p_b.edge[1]] && p_vertices[p_a.edge[1]] == p_vertices[p_b.edge[0]]) {
+ if (p_vertices[p_a.edge[0]].is_equal_approx(p_vertices[p_b.edge[1]]) && p_vertices[p_a.edge[1]].is_equal_approx(p_vertices[p_b.edge[0]])) {
return true;
}
diff --git a/core/math/disjoint_set.cpp b/core/math/disjoint_set.cpp
new file mode 100644
index 0000000000..c9d47aa0ae
--- /dev/null
+++ b/core/math/disjoint_set.cpp
@@ -0,0 +1,31 @@
+/*************************************************************************/
+/* disjoint_set.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "disjoint_set.h"
diff --git a/core/math/disjoint_set.h b/core/math/disjoint_set.h
new file mode 100644
index 0000000000..c9b3d0b65d
--- /dev/null
+++ b/core/math/disjoint_set.h
@@ -0,0 +1,155 @@
+/*************************************************************************/
+/* disjoint_set.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef DISJOINT_SET_H
+#define DISJOINT_SET_H
+
+#include "core/map.h"
+#include "core/vector.h"
+
+/**
+ @author Marios Staikopoulos <marios@staik.net>
+*/
+
+/* This DisjointSet class uses Find with path compression and Union by rank */
+template <typename T, class C = Comparator<T>, class AL = DefaultAllocator>
+class DisjointSet {
+
+ struct Element {
+ T object;
+ Element *parent = nullptr;
+ int rank = 0;
+ };
+
+ typedef Map<T, Element *, C, AL> MapT;
+
+ MapT elements;
+
+ Element *get_parent(Element *element);
+
+ _FORCE_INLINE_ Element *insert_or_get(T object);
+
+public:
+ ~DisjointSet();
+
+ _FORCE_INLINE_ void insert(T object) { (void)insert_or_get(object); }
+
+ void create_union(T a, T b);
+
+ void get_representatives(Vector<T> &out_roots);
+
+ void get_members(Vector<T> &out_members, T representative);
+};
+
+/* FUNCTIONS */
+
+template <typename T, class C, class AL>
+DisjointSet<T, C, AL>::~DisjointSet() {
+ for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) {
+ memdelete_allocator<Element, AL>(itr->value());
+ }
+}
+
+template <typename T, class C, class AL>
+typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::get_parent(Element *element) {
+ if (element->parent != element) {
+ element->parent = get_parent(element->parent);
+ }
+
+ return element->parent;
+}
+
+template <typename T, class C, class AL>
+typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::insert_or_get(T object) {
+ typename MapT::Element *itr = elements.find(object);
+ if (itr != nullptr) {
+ return itr->value();
+ }
+
+ Element *new_element = memnew_allocator(Element, AL);
+ new_element->object = object;
+ new_element->parent = new_element;
+ elements.insert(object, new_element);
+
+ return new_element;
+}
+
+template <typename T, class C, class AL>
+void DisjointSet<T, C, AL>::create_union(T a, T b) {
+
+ Element *x = insert_or_get(a);
+ Element *y = insert_or_get(b);
+
+ Element *x_root = get_parent(x);
+ Element *y_root = get_parent(y);
+
+ // Already in the same set
+ if (x_root == y_root)
+ return;
+
+ // Not in the same set, merge
+ if (x_root->rank < y_root->rank) {
+ SWAP(x_root, y_root);
+ }
+
+ // Merge y_root into x_root
+ y_root->parent = x_root;
+ if (x_root->rank == y_root->rank) {
+ ++x_root->rank;
+ }
+}
+
+template <typename T, class C, class AL>
+void DisjointSet<T, C, AL>::get_representatives(Vector<T> &out_representatives) {
+ for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) {
+ Element *element = itr->value();
+ if (element->parent == element) {
+ out_representatives.push_back(element->object);
+ }
+ }
+}
+
+template <typename T, class C, class AL>
+void DisjointSet<T, C, AL>::get_members(Vector<T> &out_members, T representative) {
+ typename MapT::Element *rep_itr = elements.find(representative);
+ ERR_FAIL_COND(rep_itr == nullptr);
+
+ Element *rep_element = rep_itr->value();
+ ERR_FAIL_COND(rep_element->parent != rep_element);
+
+ for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) {
+ Element *parent = get_parent(itr->value());
+ if (parent == rep_element) {
+ out_members.push_back(itr->key());
+ }
+ }
+}
+
+#endif
diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp
index f37db90929..e0ead8446f 100644
--- a/core/math/geometry.cpp
+++ b/core/math/geometry.cpp
@@ -241,10 +241,7 @@ PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_ar
bool error = _connect_faces(_fcptr, len, -1);
- if (error) {
-
- ERR_FAIL_COND_V(error, PoolVector<PoolVector<Face3> >()); // Invalid geometry.
- }
+ ERR_FAIL_COND_V_MSG(error, PoolVector<PoolVector<Face3> >(), "Invalid geometry.");
// Group connected faces in separate objects.
@@ -715,7 +712,7 @@ Vector<Vector<Vector2> > Geometry::decompose_polygon_in_convex(Vector<Point2> po
decomp.write[idx].resize(tp.GetNumPoints());
- for (int i = 0; i < tp.GetNumPoints(); i++) {
+ for (int64_t i = 0; i < tp.GetNumPoints(); i++) {
decomp.write[idx].write[i] = tp.GetPoint(i);
}
diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp
index f04e40cb6c..50fcdb2c13 100644
--- a/core/math/math_funcs.cpp
+++ b/core/math/math_funcs.cpp
@@ -30,6 +30,8 @@
#include "math_funcs.h"
+#include "core/error_macros.h"
+
RandomPCG Math::default_rand(RandomPCG::DEFAULT_SEED, RandomPCG::DEFAULT_INC);
#define PHI 0x9e3779b9
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 9078abea68..a94b27fcc5 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -472,7 +472,7 @@ public:
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) {
+ static _ALWAYS_INLINE_ float snap_scalar_separation(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;
diff --git a/core/math/plane.cpp b/core/math/plane.cpp
index b01853c4ac..d55957cd0a 100644
--- a/core/math/plane.cpp
+++ b/core/math/plane.cpp
@@ -32,9 +32,6 @@
#include "core/math/math_funcs.h"
-#define _PLANE_EQ_DOT_EPSILON 0.999
-#define _PLANE_EQ_D_EPSILON 0.0001
-
void Plane::set_normal(const Vector3 &p_normal) {
normal = p_normal;
@@ -91,7 +88,7 @@ bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r
real_t denom = vec3_cross(normal0, normal1).dot(normal2);
- if (ABS(denom) <= CMP_EPSILON)
+ if (Math::is_zero_approx(denom))
return false;
if (r_result) {
@@ -156,9 +153,9 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec
/* misc */
-bool Plane::is_almost_like(const Plane &p_plane) const {
+bool Plane::is_equal_approx(const Plane &p_plane) const {
- return (normal.dot(p_plane.normal) > _PLANE_EQ_DOT_EPSILON && Math::absd(d - p_plane.d) < _PLANE_EQ_D_EPSILON);
+ return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d);
}
Plane::operator String() const {
diff --git a/core/math/plane.h b/core/math/plane.h
index ec817edd2c..9abf24fbba 100644
--- a/core/math/plane.h
+++ b/core/math/plane.h
@@ -68,7 +68,7 @@ public:
/* misc */
Plane operator-() const { return Plane(-normal, -d); }
- bool is_almost_like(const Plane &p_plane) const;
+ bool is_equal_approx(const Plane &p_plane) const;
_FORCE_INLINE_ bool operator==(const Plane &p_plane) const;
_FORCE_INLINE_ bool operator!=(const Plane &p_plane) const;
@@ -125,12 +125,12 @@ Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_
bool Plane::operator==(const Plane &p_plane) const {
- return normal == p_plane.normal && Math::is_equal_approx(d, p_plane.d);
+ return normal == p_plane.normal && d == p_plane.d;
}
bool Plane::operator!=(const Plane &p_plane) const {
- return normal != p_plane.normal || !Math::is_equal_approx(d, p_plane.d);
+ return normal != p_plane.normal || d != p_plane.d;
}
#endif // PLANE_H
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 1a67be7384..a4f91844b9 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -121,6 +121,11 @@ Quat Quat::operator*(const Quat &q) const {
return r;
}
+bool Quat::is_equal_approx(const Quat &p_quat) const {
+
+ return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w);
+}
+
real_t Quat::length() const {
return Math::sqrt(length_squared());
diff --git a/core/math/quat.h b/core/math/quat.h
index 3d6602e466..27885f4152 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -43,6 +43,7 @@ public:
real_t x, y, z, w;
_FORCE_INLINE_ real_t length_squared() const;
+ bool is_equal_approx(const Quat &p_quat) const;
real_t length() const;
void normalize();
Quat normalized() const;
diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp
index fc2eb1454d..f71f00afd6 100644
--- a/core/math/quick_hull.cpp
+++ b/core/math/quick_hull.cpp
@@ -401,7 +401,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me
ERR_CONTINUE(O == E);
ERR_CONTINUE(O == NULL);
- if (O->get().plane.is_almost_like(f.plane)) {
+ if (O->get().plane.is_equal_approx(f.plane)) {
//merge and delete edge and contiguous face, while repointing edges (uuugh!)
int ois = O->get().indices.size();
int merged = 0;
diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp
index fea128afbd..e31776672e 100644
--- a/core/math/rect2.cpp
+++ b/core/math/rect2.cpp
@@ -30,6 +30,11 @@
#include "core/math/transform_2d.h" // Includes rect2.h but Rect2 needs Transform2D
+bool Rect2::is_equal_approx(const Rect2 &p_rect) const {
+
+ return position.is_equal_approx(p_rect.position) && size.is_equal_approx(p_rect.size);
+}
+
bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const {
real_t min = 0, max = 1;
diff --git a/core/math/rect2.h b/core/math/rect2.h
index d636aa223f..71221ffb1b 100644
--- a/core/math/rect2.h
+++ b/core/math/rect2.h
@@ -99,8 +99,8 @@ struct Rect2 {
inline bool encloses(const Rect2 &p_rect) const {
return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) &&
- ((p_rect.position.x + p_rect.size.x) < (position.x + size.x)) &&
- ((p_rect.position.y + p_rect.size.y) < (position.y + size.y));
+ ((p_rect.position.x + p_rect.size.x) <= (position.x + size.x)) &&
+ ((p_rect.position.y + p_rect.size.y) <= (position.y + size.y));
}
_FORCE_INLINE_ bool has_no_area() const {
@@ -153,6 +153,7 @@ struct Rect2 {
return true;
}
+ bool is_equal_approx(const Rect2 &p_rect) const;
bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; }
bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; }
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index 4056975da8..5dcc6ab9f0 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -182,6 +182,11 @@ Transform Transform::orthonormalized() const {
return _copy;
}
+bool Transform::is_equal_approx(const Transform &p_transform) const {
+
+ return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
+}
+
bool Transform::operator==(const Transform &p_transform) const {
return (basis == p_transform.basis && origin == p_transform.origin);
diff --git a/core/math/transform.h b/core/math/transform.h
index 90e2b07583..da65a183cf 100644
--- a/core/math/transform.h
+++ b/core/math/transform.h
@@ -70,6 +70,7 @@ public:
void orthonormalize();
Transform orthonormalized() const;
+ bool is_equal_approx(const Transform &p_transform) const;
bool operator==(const Transform &p_transform) const;
bool operator!=(const Transform &p_transform) const;
diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp
index 1d0387bd45..a1c0814637 100644
--- a/core/math/transform_2d.cpp
+++ b/core/math/transform_2d.cpp
@@ -147,6 +147,7 @@ void Transform2D::orthonormalize() {
elements[0] = x;
elements[1] = y;
}
+
Transform2D Transform2D::orthonormalized() const {
Transform2D on = *this;
@@ -154,6 +155,11 @@ Transform2D Transform2D::orthonormalized() const {
return on;
}
+bool Transform2D::is_equal_approx(const Transform2D &p_transform) const {
+
+ return elements[0].is_equal_approx(p_transform.elements[0]) && elements[1].is_equal_approx(p_transform.elements[1]) && elements[2].is_equal_approx(p_transform.elements[2]);
+}
+
bool Transform2D::operator==(const Transform2D &p_transform) const {
for (int i = 0; i < 3; i++) {
diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h
index e8b44ab197..0ec39a1765 100644
--- a/core/math/transform_2d.h
+++ b/core/math/transform_2d.h
@@ -96,6 +96,7 @@ struct Transform2D {
void orthonormalize();
Transform2D orthonormalized() const;
+ bool is_equal_approx(const Transform2D &p_transform) const;
bool operator==(const Transform2D &p_transform) const;
bool operator!=(const Transform2D &p_transform) const;
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index 972bccc0ac..fbedeb8eb2 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -203,6 +203,10 @@ Vector2 Vector2::reflect(const Vector2 &p_normal) const {
return 2.0 * p_normal * this->dot(p_normal) - *this;
}
+bool Vector2::is_equal_approx(const Vector2 &p_v) const {
+ return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y);
+}
+
/* Vector2i */
Vector2i Vector2i::operator+(const Vector2i &p_v) const {
diff --git a/core/math/vector2.h b/core/math/vector2.h
index 1a73831891..7fcaadab00 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -92,6 +92,8 @@ struct Vector2 {
Vector2 bounce(const Vector2 &p_normal) const;
Vector2 reflect(const Vector2 &p_normal) const;
+ bool is_equal_approx(const Vector2 &p_v) const;
+
Vector2 operator+(const Vector2 &p_v) const;
void operator+=(const Vector2 &p_v);
Vector2 operator-(const Vector2 &p_v) const;
@@ -221,11 +223,11 @@ _FORCE_INLINE_ Vector2 Vector2::operator-() const {
_FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const {
- return Math::is_equal_approx(x, p_vec2.x) && Math::is_equal_approx(y, p_vec2.y);
+ return x == p_vec2.x && y == p_vec2.y;
}
_FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const {
- return !Math::is_equal_approx(x, p_vec2.x) || !Math::is_equal_approx(y, p_vec2.y);
+ return x != p_vec2.x || y != p_vec2.y;
}
Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const {
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index 73927821cf..e3211c8fb1 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -134,6 +134,26 @@ Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const {
return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta;
}
+Basis Vector3::outer(const Vector3 &p_b) const {
+
+ Vector3 row0(x * p_b.x, x * p_b.y, x * p_b.z);
+ Vector3 row1(y * p_b.x, y * p_b.y, y * p_b.z);
+ Vector3 row2(z * p_b.x, z * p_b.y, z * p_b.z);
+
+ return Basis(row0, row1, row2);
+}
+
+Basis Vector3::to_diagonal_matrix() const {
+ return Basis(x, 0, 0,
+ 0, y, 0,
+ 0, 0, z);
+}
+
+bool Vector3::is_equal_approx(const Vector3 &p_v) const {
+
+ return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z);
+}
+
Vector3::operator String() const {
return (rtos(x) + ", " + rtos(y) + ", " + rtos(z));
diff --git a/core/math/vector3.h b/core/math/vector3.h
index c68b075613..43fa09ffac 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -96,8 +96,8 @@ struct Vector3 {
_FORCE_INLINE_ Vector3 cross(const Vector3 &p_b) const;
_FORCE_INLINE_ real_t dot(const Vector3 &p_b) const;
- _FORCE_INLINE_ Basis outer(const Vector3 &p_b) const;
- _FORCE_INLINE_ Basis to_diagonal_matrix() const;
+ Basis outer(const Vector3 &p_b) const;
+ Basis to_diagonal_matrix() const;
_FORCE_INLINE_ Vector3 abs() const;
_FORCE_INLINE_ Vector3 floor() const;
@@ -119,6 +119,8 @@ struct Vector3 {
_FORCE_INLINE_ Vector3 bounce(const Vector3 &p_normal) const;
_FORCE_INLINE_ Vector3 reflect(const Vector3 &p_normal) const;
+ bool is_equal_approx(const Vector3 &p_v) const;
+
/* Operators */
_FORCE_INLINE_ Vector3 &operator+=(const Vector3 &p_v);
@@ -154,9 +156,6 @@ struct Vector3 {
_FORCE_INLINE_ Vector3() { x = y = z = 0; }
};
-// Should be included after class definition, otherwise we get circular refs
-#include "core/math/basis.h"
-
Vector3 Vector3::cross(const Vector3 &p_b) const {
Vector3 ret(
@@ -172,21 +171,6 @@ real_t Vector3::dot(const Vector3 &p_b) const {
return x * p_b.x + y * p_b.y + z * p_b.z;
}
-Basis Vector3::outer(const Vector3 &p_b) const {
-
- Vector3 row0(x * p_b.x, x * p_b.y, x * p_b.z);
- Vector3 row1(y * p_b.x, y * p_b.y, y * p_b.z);
- Vector3 row2(z * p_b.x, z * p_b.y, z * p_b.z);
-
- return Basis(row0, row1, row2);
-}
-
-Basis Vector3::to_diagonal_matrix() const {
- return Basis(x, 0, 0,
- 0, y, 0,
- 0, 0, z);
-}
-
Vector3 Vector3::abs() const {
return Vector3(Math::abs(x), Math::abs(y), Math::abs(z));
@@ -348,11 +332,12 @@ Vector3 Vector3::operator-() const {
bool Vector3::operator==(const Vector3 &p_v) const {
- return (Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z));
+ return x == p_v.x && y == p_v.y && z == p_v.z;
}
bool Vector3::operator!=(const Vector3 &p_v) const {
- return (!Math::is_equal_approx(x, p_v.x) || !Math::is_equal_approx(y, p_v.y) || !Math::is_equal_approx(z, p_v.z));
+
+ return x != p_v.x || y != p_v.y || z != p_v.z;
}
bool Vector3::operator<(const Vector3 &p_v) const {
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-29 10:34:14 +0000