diff options
Diffstat (limited to 'core/math')
49 files changed, 3133 insertions, 2432 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 3e3e6c50a7..30f712b2c3 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -30,12 +30,11 @@ #include "a_star.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/script_language.h" #include "scene/scene_string_names.h" int AStar::get_available_point_id() const { - if (points.empty()) { return 1; } @@ -54,7 +53,6 @@ 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); @@ -78,7 +76,6 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { } Vector3 AStar::get_point_position(int p_id) const { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V(!p_exists, Vector3()); @@ -87,7 +84,6 @@ Vector3 AStar::get_point_position(int p_id) const { } void AStar::set_point_position(int p_id, const Vector3 &p_pos) { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND(!p_exists); @@ -96,7 +92,6 @@ void AStar::set_point_position(int p_id, const Vector3 &p_pos) { } real_t AStar::get_point_weight_scale(int p_id) const { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V(!p_exists, 0); @@ -105,7 +100,6 @@ real_t AStar::get_point_weight_scale(int p_id) const { } void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND(!p_exists); @@ -115,13 +109,11 @@ void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) { } void AStar::remove_point(int p_id) { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND(!p_exists); for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - Segment s(p_id, (*it.key)); segments.erase(s); @@ -130,7 +122,6 @@ void AStar::remove_point(int p_id) { } for (OAHashMap<int, Point *>::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) { - Segment s(p_id, (*it.key)); segments.erase(s); @@ -144,7 +135,6 @@ void AStar::remove_point(int p_id) { } void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { - ERR_FAIL_COND(p_id == p_with_id); Point *a; @@ -164,7 +154,9 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { } Segment s(p_id, p_with_id); - if (bidirectional) s.direction = Segment::BIDIRECTIONAL; + if (bidirectional) { + s.direction = Segment::BIDIRECTIONAL; + } Set<Segment>::Element *element = segments.find(s); if (element != nullptr) { @@ -181,7 +173,6 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { } void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { - Point *a; bool a_exists = points.lookup(p_id, a); ERR_FAIL_COND(!a_exists); @@ -207,25 +198,25 @@ void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { b->unlinked_neighbours.remove(a->id); } } else { - if (s.direction == Segment::NONE) + if (s.direction == Segment::NONE) { b->unlinked_neighbours.remove(a->id); - else + } else { a->unlinked_neighbours.set(b->id, b); + } } segments.erase(element); - if (s.direction != Segment::NONE) + if (s.direction != Segment::NONE) { segments.insert(s); + } } } bool AStar::has_point(int p_id) const { - return points.has(p_id); } Array AStar::get_points() { - Array point_list; for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { @@ -236,7 +227,6 @@ Array AStar::get_points() { } Vector<int> AStar::get_point_connections(int p_id) { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V(!p_exists, Vector<int>()); @@ -251,7 +241,6 @@ Vector<int> AStar::get_point_connections(int p_id) { } bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) const { - Segment s(p_id, p_with_id); const Set<Segment>::Element *element = segments.find(s); @@ -260,7 +249,6 @@ bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) co } void AStar::clear() { - last_free_id = 0; for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { memdelete(*(it.value)); @@ -284,18 +272,24 @@ void AStar::reserve_space(int p_num_nodes) { } 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 (!p_include_disabled && !(*it.value)->enabled) { + continue; // Disabled points should not be considered. + } - if (!p_include_disabled && !(*it.value)->enabled) continue; // Disabled points should not be considered. - + // Keep the closest point's ID, and in case of multiple closest IDs, + // the smallest one (makes it deterministic). real_t d = p_point.distance_squared_to((*it.value)->pos); - if (closest_id < 0 || d < closest_dist) { + int id = *(it.key); + if (d <= closest_dist) { + if (d == closest_dist && id > closest_id) { // Keep lowest ID. + continue; + } closest_dist = d; - closest_id = *(it.key); + closest_id = id; } } @@ -303,13 +297,10 @@ int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) co } Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { - - bool found = false; real_t closest_dist = 1e20; Vector3 closest_point; for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) { - Point *from_point = nullptr, *to_point = nullptr; points.lookup(E->get().u, from_point); points.lookup(E->get().v, to_point); @@ -323,13 +314,11 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { to_point->pos, }; - Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment); + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, segment); real_t d = p_point.distance_squared_to(p); - if (!found || d < closest_dist) { - + if (d < closest_dist) { closest_point = p; closest_dist = d; - found = true; } } @@ -337,10 +326,11 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { } bool AStar::_solve(Point *begin_point, Point *end_point) { - pass++; - if (!end_point->enabled) return false; + if (!end_point->enabled) { + return false; + } bool found_route = false; @@ -352,7 +342,6 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { open_list.push_back(begin_point); while (!open_list.empty()) { - Point *p = open_list[0]; // The currently processed point if (p == end_point) { @@ -365,7 +354,6 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { p->closed_pass = pass; // Mark the point as closed for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - Point *e = *(it.value); // The neighbour point if (!e->enabled || e->closed_pass == pass) { @@ -400,9 +388,9 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { } real_t 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)) + 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); + } Point *from_point; bool from_exists = points.lookup(p_from_id, from_point); @@ -416,9 +404,9 @@ real_t AStar::_estimate_cost(int p_from_id, int p_to_id) { } real_t 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)) + 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); + } Point *from_point; bool from_exists = points.lookup(p_from_id, from_point); @@ -432,7 +420,6 @@ real_t AStar::_compute_cost(int p_from_id, int p_to_id) { } Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { - Point *a; bool from_exists = points.lookup(p_from_id, a); ERR_FAIL_COND_V(!from_exists, Vector<Vector3>()); @@ -451,7 +438,9 @@ Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); - if (!found_route) return Vector<Vector3>(); + if (!found_route) { + return Vector<Vector3>(); + } Point *p = end_point; int pc = 1; // Begin point @@ -480,7 +469,6 @@ Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { } Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { - Point *a; bool from_exists = points.lookup(p_from_id, a); ERR_FAIL_COND_V(!from_exists, Vector<int>()); @@ -499,7 +487,9 @@ Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); - if (!found_route) return Vector<int>(); + if (!found_route) { + return Vector<int>(); + } Point *p = end_point; int pc = 1; // Begin point @@ -528,7 +518,6 @@ Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { } void AStar::set_point_disabled(int p_id, bool p_disabled) { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND(!p_exists); @@ -537,7 +526,6 @@ void AStar::set_point_disabled(int p_id, bool p_disabled) { } bool AStar::is_point_disabled(int p_id) const { - Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V(!p_exists, false); @@ -546,7 +534,6 @@ bool AStar::is_point_disabled(int p_id) const { } 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); @@ -580,11 +567,6 @@ void AStar::_bind_methods() { BIND_VMETHOD(MethodInfo(Variant::FLOAT, "_compute_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id"))); } -AStar::AStar() { - last_free_id = 0; - pass = 1; -} - AStar::~AStar() { clear(); } @@ -678,9 +660,9 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { } real_t AStar2D::_estimate_cost(int p_from_id, int p_to_id) { - - if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost)) + 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); + } AStar::Point *from_point; bool from_exists = astar.points.lookup(p_from_id, from_point); @@ -694,9 +676,9 @@ real_t AStar2D::_estimate_cost(int p_from_id, int p_to_id) { } real_t AStar2D::_compute_cost(int p_from_id, int p_to_id) { - - if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost)) + 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); + } AStar::Point *from_point; bool from_exists = astar.points.lookup(p_from_id, from_point); @@ -710,7 +692,6 @@ real_t AStar2D::_compute_cost(int p_from_id, int p_to_id) { } Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { - AStar::Point *a; bool from_exists = astar.points.lookup(p_from_id, a); ERR_FAIL_COND_V(!from_exists, Vector<Vector2>()); @@ -729,7 +710,9 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { AStar::Point *end_point = b; bool found_route = _solve(begin_point, end_point); - if (!found_route) return Vector<Vector2>(); + if (!found_route) { + return Vector<Vector2>(); + } AStar::Point *p = end_point; int pc = 1; // Begin point @@ -758,7 +741,6 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { } Vector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { - AStar::Point *a; bool from_exists = astar.points.lookup(p_from_id, a); ERR_FAIL_COND_V(!from_exists, Vector<int>()); @@ -777,7 +759,9 @@ Vector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { AStar::Point *end_point = b; bool found_route = _solve(begin_point, end_point); - if (!found_route) return Vector<int>(); + if (!found_route) { + return Vector<int>(); + } AStar::Point *p = end_point; int pc = 1; // Begin point @@ -806,10 +790,11 @@ Vector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { } bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { - astar.pass++; - if (!end_point->enabled) return false; + if (!end_point->enabled) { + return false; + } bool found_route = false; @@ -821,7 +806,6 @@ bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { open_list.push_back(begin_point); while (!open_list.empty()) { - AStar::Point *p = open_list[0]; // The currently processed point if (p == end_point) { @@ -834,7 +818,6 @@ bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { p->closed_pass = astar.pass; // Mark the point as closed for (OAHashMap<int, AStar::Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - AStar::Point *e = *(it.value); // The neighbour point if (!e->enabled || e->closed_pass == astar.pass) { @@ -869,7 +852,6 @@ bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { } void AStar2D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar2D::get_available_point_id); ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar2D::add_point, DEFVAL(1.0)); ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar2D::get_point_position); @@ -902,9 +884,3 @@ void AStar2D::_bind_methods() { BIND_VMETHOD(MethodInfo(Variant::FLOAT, "_estimate_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id"))); BIND_VMETHOD(MethodInfo(Variant::FLOAT, "_compute_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id"))); } - -AStar2D::AStar2D() { -} - -AStar2D::~AStar2D() { -} diff --git a/core/math/a_star.h b/core/math/a_star.h index 8c10ace33c..ba1c3033b8 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -41,23 +41,19 @@ */ class AStar : public Reference { - GDCLASS(AStar, Reference); friend class AStar2D; struct Point { - - Point() : - neighbours(4u), - unlinked_neighbours(4u) {} + Point() {} int id; Vector3 pos; real_t weight_scale; bool enabled; - OAHashMap<int, Point *> neighbours; - OAHashMap<int, Point *> unlinked_neighbours; + OAHashMap<int, Point *> neighbours = 4u; + OAHashMap<int, Point *> unlinked_neighbours = 4u; // Used for pathfinding. Point *prev_point; @@ -85,7 +81,7 @@ class AStar : public Reference { int32_t u; int32_t v; }; - uint64_t key; + uint64_t key = 0; }; enum { @@ -94,13 +90,11 @@ class AStar : public Reference { BACKWARD = 2, BIDIRECTIONAL = FORWARD | BACKWARD }; - unsigned char direction; + unsigned char direction = NONE; bool operator<(const Segment &p_s) const { return key < p_s.key; } - Segment() { - key = 0; - direction = NONE; - } + + Segment() {} Segment(int p_from, int p_to) { if (p_from < p_to) { u = p_from; @@ -114,8 +108,8 @@ class AStar : public Reference { } }; - int last_free_id; - uint64_t pass; + int last_free_id = 0; + uint64_t pass = 1; OAHashMap<int, Point *> points; Set<Segment> segments; @@ -159,7 +153,7 @@ public: Vector<Vector3> get_point_path(int p_from_id, int p_to_id); Vector<int> get_id_path(int p_from_id, int p_to_id); - AStar(); + AStar() {} ~AStar(); }; @@ -206,8 +200,8 @@ public: Vector<Vector2> get_point_path(int p_from_id, int p_to_id); Vector<int> get_id_path(int p_from_id, int p_to_id); - AStar2D(); - ~AStar2D(); + AStar2D() {} + ~AStar2D() {} }; #endif // A_STAR_H diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 19d60fea72..f5c667dab0 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -33,21 +33,18 @@ #include "core/print_string.h" real_t AABB::get_area() const { - return size.x * size.y * size.z; } bool AABB::operator==(const AABB &p_rval) const { - return ((position == p_rval.position) && (size == p_rval.size)); } -bool AABB::operator!=(const AABB &p_rval) const { +bool AABB::operator!=(const AABB &p_rval) const { return ((position != p_rval.position) || (size != p_rval.size)); } void AABB::merge_with(const AABB &p_aabb) { - Vector3 beg_1, beg_2; Vector3 end_1, end_2; Vector3 min, max; @@ -70,12 +67,10 @@ void AABB::merge_with(const AABB &p_aabb) { } 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; Vector3 src_max = position + size; Vector3 dst_min = p_aabb.position; @@ -83,26 +78,23 @@ AABB AABB::intersection(const AABB &p_aabb) const { Vector3 min, max; - if (src_min.x > dst_max.x || src_max.x < dst_min.x) + if (src_min.x > dst_max.x || src_max.x < dst_min.x) { return AABB(); - else { - + } else { min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x; max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x; } - if (src_min.y > dst_max.y || src_max.y < dst_min.y) + if (src_min.y > dst_max.y || src_max.y < dst_min.y) { return AABB(); - else { - + } else { min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y; max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y; } - if (src_min.z > dst_max.z || src_max.z < dst_min.z) + if (src_min.z > dst_max.z || src_max.z < dst_min.z) { return AABB(); - else { - + } else { min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z; max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z; } @@ -111,7 +103,6 @@ AABB AABB::intersection(const AABB &p_aabb) const { } bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const { - Vector3 c1, c2; Vector3 end = position + size; real_t near = -1e20; @@ -143,8 +134,9 @@ bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 * } } - if (r_clip) + if (r_clip) { *r_clip = c1; + } if (r_normal) { *r_normal = Vector3(); (*r_normal)[axis] = p_dir[axis] ? -1 : 1; @@ -154,7 +146,6 @@ bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 * } bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const { - real_t min = 0, max = 1; int axis = 0; real_t sign = 0; @@ -168,18 +159,18 @@ bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector real_t csign; if (seg_from < seg_to) { - - if (seg_from > box_end || seg_to < box_begin) + if (seg_from > box_end || seg_to < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; csign = -1.0; } else { - - if (seg_to > box_end || seg_from < box_begin) + if (seg_to > box_end || seg_from < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; @@ -191,10 +182,12 @@ bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector axis = i; sign = csign; } - if (cmax < max) + if (cmax < max) { max = cmax; - if (max < min) + } + if (max < min) { return false; + } } Vector3 rel = p_to - p_from; @@ -205,14 +198,14 @@ bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector *r_normal = normal; } - if (r_clip) + if (r_clip) { *r_clip = p_from + rel * min; + } return true; } bool AABB::intersects_plane(const Plane &p_plane) const { - Vector3 points[8] = { Vector3(position.x, position.y, position.z), Vector3(position.x, position.y, position.z + size.z), @@ -228,18 +221,17 @@ bool AABB::intersects_plane(const Plane &p_plane) const { bool under = false; for (int i = 0; i < 8; i++) { - - if (p_plane.distance_to(points[i]) > 0) + if (p_plane.distance_to(points[i]) > 0) { over = true; - else + } else { under = true; + } } return under && over; } Vector3 AABB::get_longest_axis() const { - Vector3 axis(1, 0, 0); real_t max_size = size.x; @@ -254,8 +246,8 @@ Vector3 AABB::get_longest_axis() const { return axis; } -int AABB::get_longest_axis_index() const { +int AABB::get_longest_axis_index() const { int axis = 0; real_t max_size = size.x; @@ -272,7 +264,6 @@ int AABB::get_longest_axis_index() const { } Vector3 AABB::get_shortest_axis() const { - Vector3 axis(1, 0, 0); real_t max_size = size.x; @@ -287,8 +278,8 @@ Vector3 AABB::get_shortest_axis() const { return axis; } -int AABB::get_shortest_axis_index() const { +int AABB::get_shortest_axis_index() const { int axis = 0; real_t max_size = size.x; @@ -305,35 +296,31 @@ int AABB::get_shortest_axis_index() const { } AABB AABB::merge(const AABB &p_with) const { - AABB aabb = *this; aabb.merge_with(p_with); return aabb; } + AABB AABB::expand(const Vector3 &p_vector) const { AABB aabb = *this; aabb.expand_to(p_vector); return aabb; } -AABB AABB::grow(real_t p_by) const { +AABB AABB::grow(real_t p_by) const { AABB aabb = *this; aabb.grow_by(p_by); return aabb; } void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { - ERR_FAIL_INDEX(p_edge, 12); switch (p_edge) { - case 0: { - r_from = Vector3(position.x + size.x, position.y, position.z); r_to = Vector3(position.x, position.y, position.z); } break; case 1: { - r_from = Vector3(position.x + size.x, position.y, position.z + size.z); r_to = Vector3(position.x + size.x, position.y, position.z); } break; @@ -343,18 +330,15 @@ void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { } break; case 3: { - r_from = Vector3(position.x, position.y, position.z); r_to = Vector3(position.x, position.y, position.z + size.z); } break; case 4: { - r_from = Vector3(position.x, position.y + size.y, position.z); r_to = Vector3(position.x + size.x, position.y + size.y, position.z); } break; case 5: { - r_from = Vector3(position.x + size.x, position.y + size.y, position.z); r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z); } break; @@ -364,31 +348,26 @@ void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { } break; case 7: { - r_from = Vector3(position.x, position.y + size.y, position.z + size.z); r_to = Vector3(position.x, position.y + size.y, position.z); } break; case 8: { - r_from = Vector3(position.x, position.y, position.z + size.z); r_to = Vector3(position.x, position.y + size.y, position.z + size.z); } break; case 9: { - r_from = Vector3(position.x, position.y, position.z); r_to = Vector3(position.x, position.y + size.y, position.z); } break; case 10: { - r_from = Vector3(position.x + size.x, position.y, position.z); r_to = Vector3(position.x + size.x, position.y + size.y, position.z); } break; case 11: { - r_from = Vector3(position.x + size.x, position.y, position.z + size.z); r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z); @@ -397,6 +376,5 @@ void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { } AABB::operator String() const { - return String() + position + " - " + size; } diff --git a/core/math/aabb.h b/core/math/aabb.h index 7fdad07c89..4106fbb93c 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -47,12 +47,10 @@ public: real_t get_area() const; /// get area _FORCE_INLINE_ bool has_no_area() const { - return (size.x <= 0 || size.y <= 0 || size.z <= 0); } _FORCE_INLINE_ bool has_no_surface() const { - return (size.x <= 0 && size.y <= 0 && size.z <= 0); } @@ -111,43 +109,52 @@ public: }; inline bool AABB::intersects(const AABB &p_aabb) const { - - if (position.x >= (p_aabb.position.x + p_aabb.size.x)) + if (position.x >= (p_aabb.position.x + p_aabb.size.x)) { return false; - if ((position.x + size.x) <= p_aabb.position.x) + } + if ((position.x + size.x) <= p_aabb.position.x) { return false; - if (position.y >= (p_aabb.position.y + p_aabb.size.y)) + } + if (position.y >= (p_aabb.position.y + p_aabb.size.y)) { return false; - if ((position.y + size.y) <= p_aabb.position.y) + } + if ((position.y + size.y) <= p_aabb.position.y) { return false; - if (position.z >= (p_aabb.position.z + p_aabb.size.z)) + } + if (position.z >= (p_aabb.position.z + p_aabb.size.z)) { return false; - if ((position.z + size.z) <= p_aabb.position.z) + } + if ((position.z + size.z) <= p_aabb.position.z) { return false; + } return true; } inline bool AABB::intersects_inclusive(const AABB &p_aabb) const { - - if (position.x > (p_aabb.position.x + p_aabb.size.x)) + if (position.x > (p_aabb.position.x + p_aabb.size.x)) { return false; - if ((position.x + size.x) < p_aabb.position.x) + } + if ((position.x + size.x) < p_aabb.position.x) { return false; - if (position.y > (p_aabb.position.y + p_aabb.size.y)) + } + if (position.y > (p_aabb.position.y + p_aabb.size.y)) { return false; - if ((position.y + size.y) < p_aabb.position.y) + } + if ((position.y + size.y) < p_aabb.position.y) { return false; - if (position.z > (p_aabb.position.z + p_aabb.size.z)) + } + if (position.z > (p_aabb.position.z + p_aabb.size.z)) { return false; - if ((position.z + size.z) < p_aabb.position.z) + } + if ((position.z + size.z) < p_aabb.position.z) { return false; + } return true; } inline bool AABB::encloses(const AABB &p_aabb) const { - Vector3 src_min = position; Vector3 src_max = position + size; Vector3 dst_min = p_aabb.position; @@ -163,7 +170,6 @@ inline bool AABB::encloses(const AABB &p_aabb) const { } Vector3 AABB::get_support(const Vector3 &p_normal) const { - Vector3 half_extents = size * 0.5; Vector3 ofs = position + half_extents; @@ -175,23 +181,29 @@ Vector3 AABB::get_support(const Vector3 &p_normal) const { } Vector3 AABB::get_endpoint(int p_point) const { - switch (p_point) { - case 0: return Vector3(position.x, position.y, position.z); - case 1: return Vector3(position.x, position.y, position.z + size.z); - case 2: return Vector3(position.x, position.y + size.y, position.z); - case 3: return Vector3(position.x, position.y + size.y, position.z + size.z); - case 4: return Vector3(position.x + size.x, position.y, position.z); - case 5: return Vector3(position.x + size.x, position.y, position.z + size.z); - case 6: return Vector3(position.x + size.x, position.y + size.y, position.z); - case 7: return Vector3(position.x + size.x, position.y + size.y, position.z + size.z); - }; + case 0: + return Vector3(position.x, position.y, position.z); + case 1: + return Vector3(position.x, position.y, position.z + size.z); + case 2: + return Vector3(position.x, position.y + size.y, position.z); + case 3: + return Vector3(position.x, position.y + size.y, position.z + size.z); + case 4: + return Vector3(position.x + size.x, position.y, position.z); + case 5: + return Vector3(position.x + size.x, position.y, position.z + size.z); + case 6: + return Vector3(position.x + size.x, position.y + size.y, position.z); + case 7: + return Vector3(position.x + size.x, position.y + size.y, position.z + size.z); + } ERR_FAIL_V(Vector3()); } bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const { - Vector3 half_extents = size * 0.5; Vector3 ofs = position + half_extents; @@ -202,8 +214,9 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con (p.normal.y > 0) ? -half_extents.y : half_extents.y, (p.normal.z > 0) ? -half_extents.z : half_extents.z); point += ofs; - if (p.is_point_over(point)) + if (p.is_point_over(point)) { return false; + } } // Make sure all points in the shape aren't fully separated from the AABB on @@ -212,7 +225,6 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con int bad_point_counts_negative[3] = { 0 }; for (int k = 0; k < 3; k++) { - for (int i = 0; i < p_point_count; i++) { if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) { bad_point_counts_positive[k]++; @@ -234,7 +246,6 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con } bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const { - Vector3 half_extents = size * 0.5; Vector3 ofs = position + half_extents; @@ -245,56 +256,66 @@ bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const { (p.normal.y < 0) ? -half_extents.y : half_extents.y, (p.normal.z < 0) ? -half_extents.z : half_extents.z); point += ofs; - if (p.is_point_over(point)) + if (p.is_point_over(point)) { return false; + } } return true; } bool AABB::has_point(const Vector3 &p_point) const { - - if (p_point.x < position.x) + if (p_point.x < position.x) { return false; - if (p_point.y < position.y) + } + if (p_point.y < position.y) { return false; - if (p_point.z < position.z) + } + if (p_point.z < position.z) { return false; - if (p_point.x > position.x + size.x) + } + if (p_point.x > position.x + size.x) { return false; - if (p_point.y > position.y + size.y) + } + if (p_point.y > position.y + size.y) { return false; - if (p_point.z > position.z + size.z) + } + if (p_point.z > position.z + size.z) { return false; + } return true; } inline void AABB::expand_to(const Vector3 &p_vector) { - Vector3 begin = position; Vector3 end = position + size; - if (p_vector.x < begin.x) + if (p_vector.x < begin.x) { begin.x = p_vector.x; - if (p_vector.y < begin.y) + } + if (p_vector.y < begin.y) { begin.y = p_vector.y; - if (p_vector.z < begin.z) + } + if (p_vector.z < begin.z) { begin.z = p_vector.z; + } - if (p_vector.x > end.x) + if (p_vector.x > end.x) { end.x = p_vector.x; - if (p_vector.y > end.y) + } + if (p_vector.y > end.y) { end.y = p_vector.y; - if (p_vector.z > end.z) + } + if (p_vector.z > end.z) { end.z = p_vector.z; + } position = begin; size = end - begin; } void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const { - Vector3 half_extents(size.x * 0.5, size.y * 0.5, size.z * 0.5); Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z); @@ -305,7 +326,6 @@ void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r } inline real_t AABB::get_longest_axis_size() const { - real_t max_size = size.x; if (size.y > max_size) { @@ -320,7 +340,6 @@ inline real_t AABB::get_longest_axis_size() const { } inline real_t AABB::get_shortest_axis_size() const { - real_t max_size = size.x; if (size.y < max_size) { @@ -335,7 +354,6 @@ inline real_t AABB::get_shortest_axis_size() const { } bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const { - real_t divx = 1.0 / p_dir.x; real_t divy = 1.0 / p_dir.y; real_t divz = 1.0 / p_dir.z; @@ -356,12 +374,15 @@ bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real tymin = (upbound.y - p_from.y) * divy; tymax = (position.y - p_from.y) * divy; } - if ((tmin > tymax) || (tymin > tmax)) + if ((tmin > tymax) || (tymin > tmax)) { return false; - if (tymin > tmin) + } + if (tymin > tmin) { tmin = tymin; - if (tymax < tmax) + } + if (tymax < tmax) { tmax = tymax; + } if (p_dir.z >= 0) { tzmin = (position.z - p_from.z) * divz; tzmax = (upbound.z - p_from.z) * divz; @@ -369,17 +390,19 @@ bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real tzmin = (upbound.z - p_from.z) * divz; tzmax = (position.z - p_from.z) * divz; } - if ((tmin > tzmax) || (tzmin > tmax)) + if ((tmin > tzmax) || (tzmin > tmax)) { return false; - if (tzmin > tmin) + } + if (tzmin > tmin) { tmin = tzmin; - if (tzmax < tmax) + } + if (tzmax < tmax) { tmax = tzmax; + } return ((tmin < t1) && (tmax > t0)); } void AABB::grow_by(real_t p_amount) { - position.x -= p_amount; position.y -= p_amount; position.z -= p_amount; diff --git a/core/math/audio_frame.cpp b/core/math/audio_frame.cpp deleted file mode 100644 index c565ea9b13..0000000000 --- a/core/math/audio_frame.cpp +++ /dev/null @@ -1,31 +0,0 @@ -/*************************************************************************/ -/* audio_frame.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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 "audio_frame.h" diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index 4665311059..91f533eafb 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -48,7 +48,6 @@ static inline float undenormalise(volatile float f) { } struct AudioFrame { - //left and right samples float l, r; @@ -105,7 +104,6 @@ struct AudioFrame { } _FORCE_INLINE_ AudioFrame lerp(const AudioFrame &p_b, float p_t) const { - AudioFrame res = *this; res.l += (p_t * (p_b.l - l)); diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 0f519a20d8..dd38e25bb1 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -38,16 +38,13 @@ (elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1]) void Basis::from_z(const Vector3 &p_z) { - if (Math::abs(p_z.z) > Math_SQRT12) { - // choose p in y-z plane real_t a = p_z[1] * p_z[1] + p_z[2] * p_z[2]; real_t k = 1.0 / Math::sqrt(a); elements[0] = Vector3(0, -p_z[2] * k, p_z[1] * k); elements[1] = Vector3(a * k, -p_z[0] * elements[0][2], p_z[0] * elements[0][1]); } else { - // choose p in x-y plane real_t a = p_z.x * p_z.x + p_z.y * p_z.y; real_t k = 1.0 / Math::sqrt(a); @@ -58,7 +55,6 @@ void Basis::from_z(const Vector3 &p_z) { } void Basis::invert() { - real_t co[3] = { cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) }; @@ -76,7 +72,6 @@ void Basis::invert() { } void Basis::orthonormalize() { - // Gram-Schmidt Process Vector3 x = get_axis(0); @@ -95,7 +90,6 @@ void Basis::orthonormalize() { } Basis Basis::orthonormalized() const { - Basis c = *this; c.orthonormalize(); return c; @@ -120,19 +114,20 @@ bool Basis::is_rotation() const { } bool Basis::is_symmetric() const { - - if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) + if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) { return false; - if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) + } + if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) { return false; - if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) + } + if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) { return false; + } return true; } Basis Basis::diagonalize() { - //NOTE: only implemented for symmetric matrices //with the Jacobi iterative method method #ifdef MATH_CHECKS @@ -193,21 +188,18 @@ Basis Basis::diagonalize() { } Basis Basis::inverse() const { - Basis inv = *this; inv.invert(); return inv; } void Basis::transpose() { - SWAP(elements[0][1], elements[1][0]); SWAP(elements[0][2], elements[2][0]); SWAP(elements[1][2], elements[2][1]); } Basis Basis::transposed() const { - Basis tr = *this; tr.transpose(); return tr; @@ -216,7 +208,6 @@ Basis Basis::transposed() const { // Multiplies the matrix from left by the scaling matrix: M -> S.M // See the comment for Basis::rotated for further explanation. void Basis::scale(const Vector3 &p_scale) { - elements[0][0] *= p_scale.x; elements[0][1] *= p_scale.x; elements[0][2] *= p_scale.x; @@ -260,7 +251,6 @@ Basis Basis::scaled_local(const Vector3 &p_scale) const { } Vector3 Basis::get_scale_abs() const { - return Vector3( Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), Vector3(elements[0][1], elements[1][1], elements[2][1]).length(), @@ -340,8 +330,8 @@ void Basis::rotate_local(const Vector3 &p_axis, real_t p_phi) { // M -> (M.R.Minv).M = M.R. *this = rotated_local(p_axis, p_phi); } -Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const { +Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const { return (*this) * Basis(p_axis, p_phi); } @@ -430,7 +420,6 @@ void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) cons // the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates // around the z-axis by a and so on. Vector3 Basis::get_euler_xyz() const { - // Euler angles in XYZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -439,12 +428,9 @@ Vector3 Basis::get_euler_xyz() const { // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy Vector3 euler; -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(!is_rotation(), euler); -#endif real_t sy = elements[0][2]; - if (sy < 1.0) { - if (sy > -1.0) { + if (sy < (1.0 - CMP_EPSILON)) { + if (sy > -(1.0 - CMP_EPSILON)) { // 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 (human friendlier in editor and scripts) @@ -457,12 +443,12 @@ Vector3 Basis::get_euler_xyz() const { euler.z = Math::atan2(-elements[0][1], elements[0][0]); } } else { - euler.x = -Math::atan2(elements[0][1], elements[1][1]); + euler.x = Math::atan2(elements[2][1], elements[1][1]); euler.y = -Math_PI / 2.0; euler.z = 0.0; } } else { - euler.x = Math::atan2(elements[0][1], elements[1][1]); + euler.x = Math::atan2(elements[2][1], elements[1][1]); euler.y = Math_PI / 2.0; euler.z = 0.0; } @@ -474,7 +460,6 @@ Vector3 Basis::get_euler_xyz() const { // and similar for other axes. // The current implementation uses XYZ convention (Z is the first rotation). void Basis::set_euler_xyz(const Vector3 &p_euler) { - real_t c, s; c = Math::cos(p_euler.x); @@ -493,16 +478,106 @@ void Basis::set_euler_xyz(const Vector3 &p_euler) { *this = xmat * (ymat * zmat); } +Vector3 Basis::get_euler_xzy() const { + // Euler angles in XZY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy -sz cz*sy + // sx*sy+cx*cy*sz cx*cz cx*sz*sy-cy*sx + // cy*sx*sz cz*sx cx*cy+sx*sz*sy + + Vector3 euler; + real_t sz = elements[0][1]; + if (sz < (1.0 - CMP_EPSILON)) { + if (sz > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(elements[2][1], elements[1][1]); + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = Math::asin(-sz); + } else { + // It's -1 + euler.x = -Math::atan2(elements[1][2], elements[2][2]); + euler.y = 0.0; + euler.z = Math_PI / 2.0; + } + } else { + // It's 1 + euler.x = -Math::atan2(elements[1][2], elements[2][2]); + euler.y = 0.0; + euler.z = -Math_PI / 2.0; + } + return euler; +} + +void Basis::set_euler_xzy(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = xmat * zmat * ymat; +} + +Vector3 Basis::get_euler_yzx() const { + // Euler angles in YZX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + Vector3 euler; + real_t sz = elements[1][0]; + if (sz < (1.0 - CMP_EPSILON)) { + if (sz > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(-elements[1][2], elements[1][1]); + euler.y = Math::atan2(-elements[2][0], elements[0][0]); + euler.z = Math::asin(sz); + } else { + // It's -1 + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = 0.0; + euler.z = -Math_PI / 2.0; + } + } else { + // It's 1 + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = 0.0; + euler.z = Math_PI / 2.0; + } + return euler; +} + +void Basis::set_euler_yzx(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = ymat * zmat * xmat; +} + // get_euler_yxz returns a vector containing the Euler angles in the YXZ convention, // as in first-Z, then-X, last-Y. The angles for X, Y, and Z rotations are returned // as the x, y, and z components of a Vector3 respectively. Vector3 Basis::get_euler_yxz() const { - - /* checking this is a bad idea, because obtaining from scaled transform is a valid use case -#ifdef MATH_CHECKS - ERR_FAIL_COND(!is_rotation()); -#endif -*/ // Euler angles in YXZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -514,8 +589,8 @@ Vector3 Basis::get_euler_yxz() const { real_t m12 = elements[1][2]; - if (m12 < 1) { - if (m12 > -1) { + if (m12 < (1 - CMP_EPSILON)) { + if (m12 > -(1 - CMP_EPSILON)) { // 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 (human friendlier in editor and scripts) @@ -529,12 +604,12 @@ Vector3 Basis::get_euler_yxz() const { } } else { // m12 == -1 euler.x = Math_PI * 0.5; - euler.y = -atan2(-elements[0][1], elements[0][0]); + euler.y = atan2(elements[0][1], elements[0][0]); euler.z = 0; } } else { // m12 == 1 euler.x = -Math_PI * 0.5; - euler.y = -atan2(-elements[0][1], elements[0][0]); + euler.y = -atan2(elements[0][1], elements[0][0]); euler.z = 0; } @@ -546,7 +621,6 @@ Vector3 Basis::get_euler_yxz() const { // and similar for other axes. // The current implementation uses YXZ convention (Z is the first rotation). void Basis::set_euler_yxz(const Vector3 &p_euler) { - real_t c, s; c = Math::cos(p_euler.x); @@ -565,17 +639,110 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) { *this = ymat * xmat * zmat; } -bool Basis::is_equal_approx(const Basis &p_basis) const { +Vector3 Basis::get_euler_zxy() const { + // Euler angles in ZXY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*cy*sx + // -cx*sy sx cx*cy + Vector3 euler; + real_t sx = elements[2][1]; + if (sx < (1.0 - CMP_EPSILON)) { + if (sx > -(1.0 - CMP_EPSILON)) { + euler.x = Math::asin(sx); + euler.y = Math::atan2(-elements[2][0], elements[2][2]); + euler.z = Math::atan2(-elements[0][1], elements[1][1]); + } else { + // It's -1 + euler.x = -Math_PI / 2.0; + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = 0; + } + } else { + // It's 1 + euler.x = Math_PI / 2.0; + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = 0; + } + return euler; +} + +void Basis::set_euler_zxy(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = zmat * xmat * ymat; +} + +Vector3 Basis::get_euler_zyx() const { + // Euler angles in ZYX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*cy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + Vector3 euler; + real_t sy = elements[2][0]; + if (sy < (1.0 - CMP_EPSILON)) { + if (sy > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = Math::asin(-sy); + euler.z = Math::atan2(elements[1][0], elements[0][0]); + } else { + // It's -1 + euler.x = 0; + euler.y = Math_PI / 2.0; + euler.z = -Math::atan2(elements[0][1], elements[1][1]); + } + } else { + // It's 1 + euler.x = 0; + euler.y = -Math_PI / 2.0; + euler.z = -Math::atan2(elements[0][1], elements[1][1]); + } + return euler; +} + +void Basis::set_euler_zyx(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = zmat * ymat * xmat; +} + +bool Basis::is_equal_approx(const Basis &p_basis) const { 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 { - for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon)) + if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon)) { return false; + } } } @@ -583,11 +750,11 @@ bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsil } bool Basis::operator==(const Basis &p_matrix) const { - for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - if (elements[i][j] != p_matrix.elements[i][j]) + if (elements[i][j] != p_matrix.elements[i][j]) { return false; + } } } @@ -595,21 +762,18 @@ bool Basis::operator==(const Basis &p_matrix) const { } bool Basis::operator!=(const Basis &p_matrix) const { - return (!(*this == p_matrix)); } Basis::operator String() const { - String mtx; for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - - if (i != 0 || j != 0) + if (i != 0 || j != 0) { mtx += ", "; + } - mtx += rtos(elements[i][j]); + mtx += rtos(elements[j][i]); //matrix is stored transposed for performance, so print it transposed } } @@ -617,7 +781,6 @@ Basis::operator String() const { } Quat Basis::get_quat() const { - #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_rotation(), Quat(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead."); #endif @@ -681,35 +844,33 @@ static const Basis _ortho_bases[24] = { }; int Basis::get_orthogonal_index() const { - //could be sped up if i come up with a way Basis orth = *this; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - real_t v = orth[i][j]; - if (v > 0.5) + if (v > 0.5) { v = 1.0; - else if (v < -0.5) + } else if (v < -0.5) { v = -1.0; - else + } else { v = 0; + } orth[i][j] = v; } } for (int i = 0; i < 24; i++) { - - if (_ortho_bases[i] == orth) + if (_ortho_bases[i] == orth) { return i; + } } return 0; } void Basis::set_orthogonal_index(int p_index) { - //there only exist 24 orthogonal bases in r3 ERR_FAIL_INDEX(p_index, 24); @@ -783,7 +944,9 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { real_t s = Math::sqrt((elements[1][2] - elements[2][1]) * (elements[1][2] - elements[2][1]) + (elements[2][0] - elements[0][2]) * (elements[2][0] - elements[0][2]) + (elements[0][1] - elements[1][0]) * (elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise angle = Math::acos((elements[0][0] + elements[1][1] + elements[2][2] - 1) / 2); - if (angle < 0) s = -s; + if (angle < 0) { + s = -s; + } x = (elements[2][1] - elements[1][2]) / s; y = (elements[0][2] - elements[2][0]) / s; z = (elements[1][0] - elements[0][1]) / s; @@ -793,7 +956,6 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { } void Basis::set_quat(const Quat &p_quat) { - real_t d = p_quat.length_squared(); real_t s = 2.0 / d; real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s; @@ -865,7 +1027,6 @@ void Basis::set_diagonal(const Vector3 &p_diag) { } Basis Basis::slerp(const Basis &target, const real_t &t) const { - //consider scale Quat from(*this); Quat to(target); @@ -877,3 +1038,113 @@ Basis Basis::slerp(const Basis &target, const real_t &t) const { return b; } + +void Basis::rotate_sh(real_t *p_values) { + // code by John Hable + // http://filmicworlds.com/blog/simple-and-fast-spherical-harmonic-rotation/ + // this code is Public Domain + + const static real_t s_c3 = 0.94617469575; // (3*sqrt(5))/(4*sqrt(pi)) + const static real_t s_c4 = -0.31539156525; // (-sqrt(5))/(4*sqrt(pi)) + const static real_t s_c5 = 0.54627421529; // (sqrt(15))/(4*sqrt(pi)) + + const static real_t s_c_scale = 1.0 / 0.91529123286551084; + const static real_t s_c_scale_inv = 0.91529123286551084; + + const static real_t s_rc2 = 1.5853309190550713 * s_c_scale; + const static real_t s_c4_div_c3 = s_c4 / s_c3; + const static real_t s_c4_div_c3_x2 = (s_c4 / s_c3) * 2.0; + + const static real_t s_scale_dst2 = s_c3 * s_c_scale_inv; + const static real_t s_scale_dst4 = s_c5 * s_c_scale_inv; + + real_t src[9] = { p_values[0], p_values[1], p_values[2], p_values[3], p_values[4], p_values[5], p_values[6], p_values[7], p_values[8] }; + + real_t m00 = elements[0][0]; + real_t m01 = elements[0][1]; + real_t m02 = elements[0][2]; + real_t m10 = elements[1][0]; + real_t m11 = elements[1][1]; + real_t m12 = elements[1][2]; + real_t m20 = elements[2][0]; + real_t m21 = elements[2][1]; + real_t m22 = elements[2][2]; + + p_values[0] = src[0]; + p_values[1] = m11 * src[1] - m12 * src[2] + m10 * src[3]; + p_values[2] = -m21 * src[1] + m22 * src[2] - m20 * src[3]; + p_values[3] = m01 * src[1] - m02 * src[2] + m00 * src[3]; + + real_t sh0 = src[7] + src[8] + src[8] - src[5]; + real_t sh1 = src[4] + s_rc2 * src[6] + src[7] + src[8]; + real_t sh2 = src[4]; + real_t sh3 = -src[7]; + real_t sh4 = -src[5]; + + // Rotations. R0 and R1 just use the raw matrix columns + real_t r2x = m00 + m01; + real_t r2y = m10 + m11; + real_t r2z = m20 + m21; + + real_t r3x = m00 + m02; + real_t r3y = m10 + m12; + real_t r3z = m20 + m22; + + real_t r4x = m01 + m02; + real_t r4y = m11 + m12; + real_t r4z = m21 + m22; + + // dense matrix multiplication one column at a time + + // column 0 + real_t sh0_x = sh0 * m00; + real_t sh0_y = sh0 * m10; + real_t d0 = sh0_x * m10; + real_t d1 = sh0_y * m20; + real_t d2 = sh0 * (m20 * m20 + s_c4_div_c3); + real_t d3 = sh0_x * m20; + real_t d4 = sh0_x * m00 - sh0_y * m10; + + // column 1 + real_t sh1_x = sh1 * m02; + real_t sh1_y = sh1 * m12; + d0 += sh1_x * m12; + d1 += sh1_y * m22; + d2 += sh1 * (m22 * m22 + s_c4_div_c3); + d3 += sh1_x * m22; + d4 += sh1_x * m02 - sh1_y * m12; + + // column 2 + real_t sh2_x = sh2 * r2x; + real_t sh2_y = sh2 * r2y; + d0 += sh2_x * r2y; + d1 += sh2_y * r2z; + d2 += sh2 * (r2z * r2z + s_c4_div_c3_x2); + d3 += sh2_x * r2z; + d4 += sh2_x * r2x - sh2_y * r2y; + + // column 3 + real_t sh3_x = sh3 * r3x; + real_t sh3_y = sh3 * r3y; + d0 += sh3_x * r3y; + d1 += sh3_y * r3z; + d2 += sh3 * (r3z * r3z + s_c4_div_c3_x2); + d3 += sh3_x * r3z; + d4 += sh3_x * r3x - sh3_y * r3y; + + // column 4 + real_t sh4_x = sh4 * r4x; + real_t sh4_y = sh4 * r4y; + d0 += sh4_x * r4y; + d1 += sh4_y * r4z; + d2 += sh4 * (r4z * r4z + s_c4_div_c3_x2); + d3 += sh4_x * r4z; + d4 += sh4_x * r4x - sh4_y * r4y; + + // extra multipliers + p_values[4] = d0; + p_values[5] = -d1; + p_values[6] = d2 * s_scale_dst2; + p_values[7] = -d3; + p_values[8] = d4 * s_scale_dst4; +} diff --git a/core/math/basis.h b/core/math/basis.h index 0261cf67c6..985fb0e44f 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -39,11 +39,9 @@ public: Vector3 elements[3]; _FORCE_INLINE_ const Vector3 &operator[](int axis) const { - return elements[axis]; } _FORCE_INLINE_ Vector3 &operator[](int axis) { - return elements[axis]; } @@ -90,9 +88,22 @@ public: Vector3 get_euler_xyz() const; void set_euler_xyz(const Vector3 &p_euler); + + Vector3 get_euler_xzy() const; + void set_euler_xzy(const Vector3 &p_euler); + + Vector3 get_euler_yzx() const; + void set_euler_yzx(const Vector3 &p_euler); + Vector3 get_euler_yxz() const; void set_euler_yxz(const Vector3 &p_euler); + Vector3 get_euler_zxy() const; + void set_euler_zxy(const Vector3 &p_euler); + + Vector3 get_euler_zyx() const; + void set_euler_zyx(const Vector3 &p_euler); + Quat get_quat() const; void set_quat(const Quat &p_quat); @@ -159,13 +170,13 @@ public: bool is_rotation() const; Basis slerp(const Basis &target, const real_t &t) const; + void rotate_sh(real_t *p_values); operator String() const; /* create / set */ _FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { - elements[0][0] = xx; elements[0][1] = xy; elements[0][2] = xz; @@ -177,18 +188,15 @@ public: elements[2][2] = zz; } _FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { - set_axis(0, p_x); set_axis(1, p_y); set_axis(2, p_z); } _FORCE_INLINE_ Vector3 get_column(int i) const { - return Vector3(elements[0][i], elements[1][i], elements[2][i]); } _FORCE_INLINE_ Vector3 get_row(int i) const { - return Vector3(elements[i][0], elements[i][1], elements[i][2]); } _FORCE_INLINE_ Vector3 get_main_diagonal() const { @@ -220,7 +228,6 @@ public: elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); } Basis(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { - set(xx, xy, xz, yx, yy, yz, zx, zy, zz); } @@ -248,7 +255,6 @@ public: } _FORCE_INLINE_ Basis() { - elements[0][0] = 1; elements[0][1] = 0; elements[0][2] = 0; @@ -262,7 +268,6 @@ public: }; _FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) { - set( p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), @@ -270,7 +275,6 @@ _FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) { } _FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const { - return Basis( p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), @@ -278,49 +282,42 @@ _FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const { } _FORCE_INLINE_ void Basis::operator+=(const Basis &p_matrix) { - elements[0] += p_matrix.elements[0]; elements[1] += p_matrix.elements[1]; elements[2] += p_matrix.elements[2]; } _FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const { - Basis ret(*this); ret += p_matrix; return ret; } _FORCE_INLINE_ void Basis::operator-=(const Basis &p_matrix) { - elements[0] -= p_matrix.elements[0]; elements[1] -= p_matrix.elements[1]; elements[2] -= p_matrix.elements[2]; } _FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const { - Basis ret(*this); ret -= p_matrix; return ret; } _FORCE_INLINE_ void Basis::operator*=(real_t p_val) { - elements[0] *= p_val; elements[1] *= p_val; elements[2] *= p_val; } _FORCE_INLINE_ Basis Basis::operator*(real_t p_val) const { - Basis ret(*this); ret *= p_val; return ret; } Vector3 Basis::xform(const Vector3 &p_vector) const { - return Vector3( elements[0].dot(p_vector), elements[1].dot(p_vector), @@ -328,7 +325,6 @@ Vector3 Basis::xform(const Vector3 &p_vector) const { } Vector3 Basis::xform_inv(const Vector3 &p_vector) const { - return Vector3( (elements[0][0] * p_vector.x) + (elements[1][0] * p_vector.y) + (elements[2][0] * p_vector.z), (elements[0][1] * p_vector.x) + (elements[1][1] * p_vector.y) + (elements[2][1] * p_vector.z), @@ -336,7 +332,6 @@ Vector3 Basis::xform_inv(const Vector3 &p_vector) const { } real_t Basis::determinant() const { - return elements[0][0] * (elements[1][1] * elements[2][2] - elements[2][1] * elements[1][2]) - elements[1][0] * (elements[0][1] * elements[2][2] - elements[2][1] * elements[0][2]) + elements[2][0] * (elements[0][1] * elements[1][2] - elements[1][1] * elements[0][2]); diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index c36070e47f..22ab83f358 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -33,30 +33,38 @@ #include "core/math/math_funcs.h" #include "core/print_string.h" -void CameraMatrix::set_identity() { +float CameraMatrix::determinant() const { + return matrix[0][3] * matrix[1][2] * matrix[2][1] * matrix[3][0] - matrix[0][2] * matrix[1][3] * matrix[2][1] * matrix[3][0] - + matrix[0][3] * matrix[1][1] * matrix[2][2] * matrix[3][0] + matrix[0][1] * matrix[1][3] * matrix[2][2] * matrix[3][0] + + matrix[0][2] * matrix[1][1] * matrix[2][3] * matrix[3][0] - matrix[0][1] * matrix[1][2] * matrix[2][3] * matrix[3][0] - + matrix[0][3] * matrix[1][2] * matrix[2][0] * matrix[3][1] + matrix[0][2] * matrix[1][3] * matrix[2][0] * matrix[3][1] + + matrix[0][3] * matrix[1][0] * matrix[2][2] * matrix[3][1] - matrix[0][0] * matrix[1][3] * matrix[2][2] * matrix[3][1] - + matrix[0][2] * matrix[1][0] * matrix[2][3] * matrix[3][1] + matrix[0][0] * matrix[1][2] * matrix[2][3] * matrix[3][1] + + matrix[0][3] * matrix[1][1] * matrix[2][0] * matrix[3][2] - matrix[0][1] * matrix[1][3] * matrix[2][0] * matrix[3][2] - + matrix[0][3] * matrix[1][0] * matrix[2][1] * matrix[3][2] + matrix[0][0] * matrix[1][3] * matrix[2][1] * matrix[3][2] + + matrix[0][1] * matrix[1][0] * matrix[2][3] * matrix[3][2] - matrix[0][0] * matrix[1][1] * matrix[2][3] * matrix[3][2] - + matrix[0][2] * matrix[1][1] * matrix[2][0] * matrix[3][3] + matrix[0][1] * matrix[1][2] * matrix[2][0] * matrix[3][3] + + matrix[0][2] * matrix[1][0] * matrix[2][1] * matrix[3][3] - matrix[0][0] * matrix[1][2] * matrix[2][1] * matrix[3][3] - + matrix[0][1] * matrix[1][0] * matrix[2][2] * matrix[3][3] + matrix[0][0] * matrix[1][1] * matrix[2][2] * matrix[3][3]; +} +void CameraMatrix::set_identity() { for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - matrix[i][j] = (i == j) ? 1 : 0; } } } void CameraMatrix::set_zero() { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - matrix[i][j] = 0; } } } Plane CameraMatrix::xform4(const Plane &p_vec4) const { - Plane ret; ret.normal.x = matrix[0][0] * p_vec4.normal.x + matrix[1][0] * p_vec4.normal.y + matrix[2][0] * p_vec4.normal.z + matrix[3][0] * p_vec4.d; @@ -67,7 +75,6 @@ Plane CameraMatrix::xform4(const Plane &p_vec4) const { } void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) { - if (p_flip_fov) { p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect); } @@ -109,18 +116,18 @@ void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_ left = -xmax + frustumshift; right = xmax + frustumshift; modeltranslation = p_intraocular_dist / 2.0; - }; break; + } break; case 2: { // right eye left = -xmax - frustumshift; right = xmax - frustumshift; modeltranslation = -p_intraocular_dist / 2.0; - }; break; + } break; default: { // mono, should give the same result as set_perspective(p_fovy_degrees,p_aspect,p_z_near,p_z_far,p_flip_fov) left = -xmax; right = xmax; modeltranslation = 0.0; - }; break; - }; + } break; + } set_frustum(left, right, -ymax, ymax, p_z_near, p_z_far); @@ -150,17 +157,16 @@ void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_ switch (p_eye) { case 1: { // left eye set_frustum(-f2 * p_z_near, f1 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far); - }; break; + } break; case 2: { // right eye set_frustum(-f1 * p_z_near, f2 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far); - }; break; + } break; default: { // mono, does not apply here! - }; break; - }; -}; + } break; + } +} void CameraMatrix::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar) { - set_identity(); matrix[0][0] = 2.0 / (p_right - p_left); @@ -173,7 +179,6 @@ void CameraMatrix::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom } void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) { - if (!p_flip_fov) { p_size *= p_aspect; } @@ -182,7 +187,6 @@ void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear } void CameraMatrix::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far) { - ERR_FAIL_COND(p_right <= p_left); ERR_FAIL_COND(p_top <= p_bottom); ERR_FAIL_COND(p_far <= p_near); @@ -223,7 +227,6 @@ void CameraMatrix::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, } real_t CameraMatrix::get_z_far() const { - const real_t *matrix = (const real_t *)this->matrix; Plane new_plane = Plane(matrix[3] - matrix[2], matrix[7] - matrix[6], @@ -235,8 +238,8 @@ real_t CameraMatrix::get_z_far() const { return new_plane.d; } -real_t CameraMatrix::get_z_near() const { +real_t CameraMatrix::get_z_near() const { const real_t *matrix = (const real_t *)this->matrix; Plane new_plane = Plane(matrix[3] + matrix[2], matrix[7] + matrix[6], @@ -248,7 +251,6 @@ real_t CameraMatrix::get_z_near() const { } Vector2 CameraMatrix::get_viewport_half_extents() const { - const real_t *matrix = (const real_t *)this->matrix; ///////--- Near Plane ---/////// Plane near_plane = Plane(matrix[3] + matrix[2], @@ -277,7 +279,6 @@ Vector2 CameraMatrix::get_viewport_half_extents() const { } void CameraMatrix::get_far_plane_size(real_t &r_width, real_t &r_height) const { - const real_t *matrix = (const real_t *)this->matrix; ///////--- Far Plane ---/////// Plane far_plane = Plane(matrix[3] - matrix[2], @@ -307,7 +308,6 @@ void CameraMatrix::get_far_plane_size(real_t &r_width, real_t &r_height) const { } bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8points) const { - Vector<Plane> planes = get_projection_planes(Transform()); const Planes intersections[8][3] = { { PLANE_FAR, PLANE_LEFT, PLANE_TOP }, @@ -321,7 +321,6 @@ bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8point }; for (int i = 0; i < 8; i++) { - Vector3 point; bool res = planes[intersections[i][0]].intersect_3(planes[intersections[i][1]], planes[intersections[i][2]], &point); ERR_FAIL_COND_V(!res, false); @@ -332,7 +331,6 @@ bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8point } Vector<Plane> CameraMatrix::get_projection_planes(const Transform &p_transform) const { - /** Fast Plane Extraction from combined modelview/projection matrices. * References: * https://web.archive.org/web/20011221205252/http://www.markmorley.com/opengl/frustumculling.html @@ -415,14 +413,12 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform &p_transform) } CameraMatrix CameraMatrix::inverse() const { - CameraMatrix cm = *this; cm.invert(); return cm; } void CameraMatrix::invert() { - int i, j, k; int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */ real_t pvt_val; /* Value of current pivot element */ @@ -473,20 +469,26 @@ void CameraMatrix::invert() { /** Divide column by minus pivot value **/ for (i = 0; i < 4; i++) { - if (i != k) matrix[i][k] /= (-pvt_val); + if (i != k) { + matrix[i][k] /= (-pvt_val); + } } /** Reduce the matrix **/ for (i = 0; i < 4; i++) { hold = matrix[i][k]; for (j = 0; j < 4; j++) { - if (i != k && j != k) matrix[i][j] += hold * matrix[k][j]; + if (i != k && j != k) { + matrix[i][j] += hold * matrix[k][j]; + } } } /** Divide row by pivot **/ for (j = 0; j < 4; j++) { - if (j != k) matrix[k][j] /= pvt_val; + if (j != k) { + matrix[k][j] /= pvt_val; + } } /** Replace pivot by reciprocal (at last we can touch it). **/ @@ -506,12 +508,13 @@ void CameraMatrix::invert() { } j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */ - if (j != k) /* If columns are different */ + if (j != k) { /* If columns are different */ for (i = 0; i < 4; i++) { hold = matrix[i][k]; matrix[i][k] = -matrix[i][j]; matrix[i][j] = hold; } + } } } @@ -522,19 +525,18 @@ void CameraMatrix::flip_y() { } CameraMatrix::CameraMatrix() { - set_identity(); } CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { - CameraMatrix new_matrix; for (int j = 0; j < 4; j++) { for (int i = 0; i < 4; i++) { real_t ab = 0; - for (int k = 0; k < 4; k++) + for (int k = 0; k < 4; k++) { ab += matrix[k][i] * p_matrix.matrix[j][k]; + } new_matrix.matrix[j][i] = ab; } } @@ -543,7 +545,6 @@ CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { } void CameraMatrix::set_depth_correction(bool p_flip_y) { - real_t *m = &matrix[0][0]; m[0] = 1; @@ -565,7 +566,6 @@ void CameraMatrix::set_depth_correction(bool p_flip_y) { } void CameraMatrix::set_light_bias() { - real_t *m = &matrix[0][0]; m[0] = 0.5; @@ -587,7 +587,6 @@ void CameraMatrix::set_light_bias() { } void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { - real_t *m = &matrix[0][0]; m[0] = p_rect.size.width; @@ -609,30 +608,28 @@ void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { } CameraMatrix::operator String() const { - String str; - for (int i = 0; i < 4; i++) - for (int j = 0; j < 4; j++) + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { str += String((j > 0) ? ", " : "\n") + rtos(matrix[i][j]); + } + } return str; } real_t CameraMatrix::get_aspect() const { - Vector2 vp_he = get_viewport_half_extents(); return vp_he.x / vp_he.y; } int CameraMatrix::get_pixels_per_meter(int p_for_pixel_width) const { - Vector3 result = xform(Vector3(1, 0, -1)); return int((result.x * 0.5 + 0.5) * p_for_pixel_width); } bool CameraMatrix::is_orthogonal() const { - return matrix[3][3] == 1.0; } @@ -660,7 +657,6 @@ real_t CameraMatrix::get_fov() const { } void CameraMatrix::make_scale(const Vector3 &p_scale) { - set_identity(); matrix[0][0] = p_scale.x; matrix[1][1] = p_scale.y; @@ -668,7 +664,6 @@ void CameraMatrix::make_scale(const Vector3 &p_scale) { } void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { - Vector3 min = p_aabb.position; Vector3 max = p_aabb.position + p_aabb.size; @@ -694,7 +689,6 @@ void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { } CameraMatrix::operator Transform() const { - Transform tr; const real_t *m = &matrix[0][0]; @@ -718,7 +712,6 @@ CameraMatrix::operator Transform() const { } CameraMatrix::CameraMatrix(const Transform &p_transform) { - const Transform &tr = p_transform; real_t *m = &matrix[0][0]; diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index c10193bc84..49fdecae02 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -35,7 +35,6 @@ #include "core/math/transform.h" struct CameraMatrix { - enum Planes { PLANE_NEAR, PLANE_FAR, @@ -47,6 +46,7 @@ struct CameraMatrix { real_t matrix[4][4]; + float determinant() const; void set_identity(); void set_zero(); void set_light_bias(); @@ -61,7 +61,6 @@ struct CameraMatrix { void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false); static real_t get_fovy(real_t p_fovx, real_t p_aspect) { - return Math::rad2deg(Math::atan(p_aspect * Math::tan(Math::deg2rad(p_fovx) * 0.5)) * 2.0); } @@ -115,7 +114,6 @@ struct CameraMatrix { }; Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const { - Vector3 ret; ret.x = matrix[0][0] * p_vec3.x + matrix[1][0] * p_vec3.y + matrix[2][0] * p_vec3.z + matrix[3][0]; ret.y = matrix[0][1] * p_vec3.x + matrix[1][1] * p_vec3.y + matrix[2][1] * p_vec3.z + matrix[3][1]; diff --git a/core/math/delaunay.h b/core/math/delaunay_2d.h index 29f84210d2..d637671686 100644 --- a/core/math/delaunay.h +++ b/core/math/delaunay_2d.h @@ -1,5 +1,5 @@ /*************************************************************************/ -/* delaunay.h */ +/* delaunay_2d.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,39 +28,35 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef DELAUNAY_H -#define DELAUNAY_H +#ifndef DELAUNAY_2D_H +#define DELAUNAY_2D_H #include "core/math/rect2.h" class Delaunay2D { public: struct Triangle { - int points[3]; - bool bad; - Triangle() { bad = false; } + bool bad = false; + Triangle() {} 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; } + bool bad = false; + Edge() {} 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]]; @@ -92,7 +88,6 @@ public: } static Vector<Triangle> triangulate(const Vector<Vector2> &p_points) { - Vector<Vector2> points = p_points; Vector<Triangle> triangles; @@ -115,9 +110,6 @@ public: 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++) { @@ -146,7 +138,6 @@ public: } for (int j = 0; j < polygon.size(); j++) { - if (polygon[j].bad) { continue; } @@ -172,4 +163,4 @@ public: } }; -#endif // DELAUNAY_H +#endif // DELAUNAY_2D_H diff --git a/core/math/delaunay_3d.h b/core/math/delaunay_3d.h new file mode 100644 index 0000000000..014b4c4621 --- /dev/null +++ b/core/math/delaunay_3d.h @@ -0,0 +1,408 @@ +/*************************************************************************/ +/* delaunay_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 DELAUNAY_3D_H +#define DELAUNAY_3D_H + +#include "core/local_vector.h" +#include "core/math/aabb.h" +#include "core/math/camera_matrix.h" +#include "core/math/vector3.h" +#include "core/oa_hash_map.h" +#include "core/os/file_access.h" +#include "core/print_string.h" +#include "core/variant.h" +#include "core/vector.h" + +#include "thirdparty/misc/r128.h" + +class Delaunay3D { + struct Simplex; + + enum { + ACCEL_GRID_SIZE = 16 + }; + struct GridPos { + Vector3i pos; + List<Simplex *>::Element *E = nullptr; + }; + + struct Simplex { + uint32_t points[4]; + R128 circum_center_x; + R128 circum_center_y; + R128 circum_center_z; + R128 circum_r2; + LocalVector<GridPos> grid_positions; + List<Simplex *>::Element *SE = nullptr; + + _FORCE_INLINE_ Simplex() {} + _FORCE_INLINE_ Simplex(uint32_t p_a, uint32_t p_b, uint32_t p_c, uint32_t p_d) { + points[0] = p_a; + points[1] = p_b; + points[2] = p_c; + points[3] = p_d; + } + }; + + struct Triangle { + uint32_t triangle[3]; + bool bad = false; + _FORCE_INLINE_ bool operator==(const Triangle &p_triangle) const { + return triangle[0] == p_triangle.triangle[0] && triangle[1] == p_triangle.triangle[1] && triangle[2] == p_triangle.triangle[2]; + } + + _FORCE_INLINE_ Triangle() {} + _FORCE_INLINE_ Triangle(uint32_t p_a, uint32_t p_b, uint32_t p_c) { + if (p_a > p_b) { + SWAP(p_a, p_b); + } + if (p_b > p_c) { + SWAP(p_b, p_c); + } + if (p_a > p_b) { + SWAP(p_a, p_b); + } + + triangle[0] = p_a; + triangle[1] = p_b; + triangle[2] = p_c; + } + }; + + struct TriangleHasher { + _FORCE_INLINE_ static uint32_t hash(const Triangle &p_triangle) { + uint32_t h = hash_djb2_one_32(p_triangle.triangle[0]); + h = hash_djb2_one_32(p_triangle.triangle[1], h); + return hash_djb2_one_32(p_triangle.triangle[2], h); + } + }; + + _FORCE_INLINE_ static void circum_sphere_compute(const Vector3 *p_points, Simplex *p_simplex) { + // the only part in the algorithm where there may be precision errors is this one, so ensure that + // we do it as maximum precision as possible + + R128 v0_x = p_points[p_simplex->points[0]].x; + R128 v0_y = p_points[p_simplex->points[0]].y; + R128 v0_z = p_points[p_simplex->points[0]].z; + R128 v1_x = p_points[p_simplex->points[1]].x; + R128 v1_y = p_points[p_simplex->points[1]].y; + R128 v1_z = p_points[p_simplex->points[1]].z; + R128 v2_x = p_points[p_simplex->points[2]].x; + R128 v2_y = p_points[p_simplex->points[2]].y; + R128 v2_z = p_points[p_simplex->points[2]].z; + R128 v3_x = p_points[p_simplex->points[3]].x; + R128 v3_y = p_points[p_simplex->points[3]].y; + R128 v3_z = p_points[p_simplex->points[3]].z; + + //Create the rows of our "unrolled" 3x3 matrix + R128 row1_x = v1_x - v0_x; + R128 row1_y = v1_y - v0_y; + R128 row1_z = v1_z - v0_z; + + R128 row2_x = v2_x - v0_x; + R128 row2_y = v2_y - v0_y; + R128 row2_z = v2_z - v0_z; + + R128 row3_x = v3_x - v0_x; + R128 row3_y = v3_y - v0_y; + R128 row3_z = v3_z - v0_z; + + R128 sq_lenght1 = row1_x * row1_x + row1_y * row1_y + row1_z * row1_z; + R128 sq_lenght2 = row2_x * row2_x + row2_y * row2_y + row2_z * row2_z; + R128 sq_lenght3 = row3_x * row3_x + row3_y * row3_y + row3_z * row3_z; + + //Compute the determinant of said matrix + R128 determinant = row1_x * (row2_y * row3_z - row3_y * row2_z) - row2_x * (row1_y * row3_z - row3_y * row1_z) + row3_x * (row1_y * row2_z - row2_y * row1_z); + + // Compute the volume of the tetrahedron, and precompute a scalar quantity for re-use in the formula + R128 volume = determinant / R128(6.f); + R128 i12volume = R128(1.f) / (volume * R128(12.f)); + + R128 center_x = v0_x + i12volume * ((row2_y * row3_z - row3_y * row2_z) * sq_lenght1 - (row1_y * row3_z - row3_y * row1_z) * sq_lenght2 + (row1_y * row2_z - row2_y * row1_z) * sq_lenght3); + R128 center_y = v0_y + i12volume * (-(row2_x * row3_z - row3_x * row2_z) * sq_lenght1 + (row1_x * row3_z - row3_x * row1_z) * sq_lenght2 - (row1_x * row2_z - row2_x * row1_z) * sq_lenght3); + R128 center_z = v0_z + i12volume * ((row2_x * row3_y - row3_x * row2_y) * sq_lenght1 - (row1_x * row3_y - row3_x * row1_y) * sq_lenght2 + (row1_x * row2_y - row2_x * row1_y) * sq_lenght3); + + //Once we know the center, the radius is clearly the distance to any vertex + + R128 rel1_x = center_x - v0_x; + R128 rel1_y = center_y - v0_y; + R128 rel1_z = center_z - v0_z; + + R128 radius1 = rel1_x * rel1_x + rel1_y * rel1_y + rel1_z * rel1_z; + + p_simplex->circum_center_x = center_x; + p_simplex->circum_center_y = center_y; + p_simplex->circum_center_z = center_z; + p_simplex->circum_r2 = radius1; + } + + _FORCE_INLINE_ static bool simplex_contains(const Vector3 *p_points, const Simplex &p_simplex, uint32_t p_vertex) { + R128 v_x = p_points[p_vertex].x; + R128 v_y = p_points[p_vertex].y; + R128 v_z = p_points[p_vertex].z; + + R128 rel2_x = p_simplex.circum_center_x - v_x; + R128 rel2_y = p_simplex.circum_center_y - v_y; + R128 rel2_z = p_simplex.circum_center_z - v_z; + + R128 radius2 = rel2_x * rel2_x + rel2_y * rel2_y + rel2_z * rel2_z; + + return radius2 < (p_simplex.circum_r2 - R128(0.00001)); + } + + static bool simplex_is_coplanar(const Vector3 *p_points, const Simplex &p_simplex) { + Plane p(p_points[p_simplex.points[0]], p_points[p_simplex.points[1]], p_points[p_simplex.points[2]]); + if (ABS(p.distance_to(p_points[p_simplex.points[3]])) < CMP_EPSILON) { + return true; + } + + CameraMatrix cm; + + cm.matrix[0][0] = p_points[p_simplex.points[0]].x; + cm.matrix[0][1] = p_points[p_simplex.points[1]].x; + cm.matrix[0][2] = p_points[p_simplex.points[2]].x; + cm.matrix[0][3] = p_points[p_simplex.points[3]].x; + + cm.matrix[1][0] = p_points[p_simplex.points[0]].y; + cm.matrix[1][1] = p_points[p_simplex.points[1]].y; + cm.matrix[1][2] = p_points[p_simplex.points[2]].y; + cm.matrix[1][3] = p_points[p_simplex.points[3]].y; + + cm.matrix[2][0] = p_points[p_simplex.points[0]].z; + cm.matrix[2][1] = p_points[p_simplex.points[1]].z; + cm.matrix[2][2] = p_points[p_simplex.points[2]].z; + cm.matrix[2][3] = p_points[p_simplex.points[3]].z; + + cm.matrix[3][0] = 1.0; + cm.matrix[3][1] = 1.0; + cm.matrix[3][2] = 1.0; + cm.matrix[3][3] = 1.0; + + return ABS(cm.determinant()) <= CMP_EPSILON; + } + +public: + struct OutputSimplex { + uint32_t points[4]; + }; + + static Vector<OutputSimplex> tetrahedralize(const Vector<Vector3> &p_points) { + uint32_t point_count = p_points.size(); + Vector3 *points = (Vector3 *)memalloc(sizeof(Vector3) * (point_count + 4)); + + { + const Vector3 *src_points = p_points.ptr(); + AABB rect; + for (uint32_t i = 0; i < point_count; i++) { + Vector3 point = src_points[i]; + if (i == 0) { + rect.position = point; + } else { + rect.expand_to(point); + } + points[i] = point; + } + + for (uint32_t i = 0; i < point_count; i++) { + points[i] = (points[i] - rect.position) / rect.size; + } + + float delta_max = Math::sqrt(2.0) * 20.0; + Vector3 center = Vector3(0.5, 0.5, 0.5); + + // any simplex that contains everything is good + points[point_count + 0] = center + Vector3(0, 1, 0) * delta_max; + points[point_count + 1] = center + Vector3(0, -1, 1) * delta_max; + points[point_count + 2] = center + Vector3(1, -1, -1) * delta_max; + points[point_count + 3] = center + Vector3(-1, -1, -1) * delta_max; + } + + List<Simplex *> acceleration_grid[ACCEL_GRID_SIZE][ACCEL_GRID_SIZE][ACCEL_GRID_SIZE]; + + List<Simplex *> simplex_list; + { + //create root simplex + Simplex *root = memnew(Simplex(point_count + 0, point_count + 1, point_count + 2, point_count + 3)); + root->SE = simplex_list.push_back(root); + + for (uint32_t i = 0; i < ACCEL_GRID_SIZE; i++) { + for (uint32_t j = 0; j < ACCEL_GRID_SIZE; j++) { + for (uint32_t k = 0; k < ACCEL_GRID_SIZE; k++) { + GridPos gp; + gp.E = acceleration_grid[i][j][k].push_back(root); + gp.pos = Vector3i(i, j, k); + root->grid_positions.push_back(gp); + } + } + } + + circum_sphere_compute(points, root); + } + + OAHashMap<Triangle, uint32_t, TriangleHasher> triangles_inserted; + LocalVector<Triangle> triangles; + + for (uint32_t i = 0; i < point_count; i++) { + bool unique = true; + for (uint32_t j = i + 1; j < point_count; j++) { + if (points[i].is_equal_approx(points[j])) { + unique = false; + break; + } + } + if (!unique) { + continue; + } + + Vector3i grid_pos = Vector3i(points[i] * ACCEL_GRID_SIZE); + grid_pos.x = CLAMP(grid_pos.x, 0, ACCEL_GRID_SIZE - 1); + grid_pos.y = CLAMP(grid_pos.y, 0, ACCEL_GRID_SIZE - 1); + grid_pos.z = CLAMP(grid_pos.z, 0, ACCEL_GRID_SIZE - 1); + + for (List<Simplex *>::Element *E = acceleration_grid[grid_pos.x][grid_pos.y][grid_pos.z].front(); E;) { + List<Simplex *>::Element *N = E->next(); //may be deleted + + Simplex *simplex = E->get(); + + if (simplex_contains(points, *simplex, i)) { + static const uint32_t triangle_order[4][3] = { + { 0, 1, 2 }, + { 0, 1, 3 }, + { 0, 2, 3 }, + { 1, 2, 3 }, + }; + + for (uint32_t k = 0; k < 4; k++) { + Triangle t = Triangle(simplex->points[triangle_order[k][0]], simplex->points[triangle_order[k][1]], simplex->points[triangle_order[k][2]]); + uint32_t *p = triangles_inserted.lookup_ptr(t); + if (p) { + triangles[*p].bad = true; + } else { + triangles_inserted.insert(t, triangles.size()); + triangles.push_back(t); + } + } + + //remove simplex and continue + simplex_list.erase(simplex->SE); + + for (uint32_t k = 0; k < simplex->grid_positions.size(); k++) { + Vector3i p = simplex->grid_positions[k].pos; + acceleration_grid[p.x][p.y][p.z].erase(simplex->grid_positions[k].E); + } + memdelete(simplex); + } + E = N; + } + + uint32_t good_triangles = 0; + for (uint32_t j = 0; j < triangles.size(); j++) { + if (triangles[j].bad) { + continue; + } + Simplex *new_simplex = memnew(Simplex(triangles[j].triangle[0], triangles[j].triangle[1], triangles[j].triangle[2], i)); + circum_sphere_compute(points, new_simplex); + new_simplex->SE = simplex_list.push_back(new_simplex); + { + Vector3 center; + center.x = double(new_simplex->circum_center_x); + center.y = double(new_simplex->circum_center_y); + center.z = double(new_simplex->circum_center_z); + + float radius2 = Math::sqrt(double(new_simplex->circum_r2)); + radius2 += 0.0001; // + Vector3 extents = Vector3(radius2, radius2, radius2); + Vector3i from = Vector3i((center - extents) * ACCEL_GRID_SIZE); + Vector3i to = Vector3i((center + extents) * ACCEL_GRID_SIZE); + from.x = CLAMP(from.x, 0, ACCEL_GRID_SIZE - 1); + from.y = CLAMP(from.y, 0, ACCEL_GRID_SIZE - 1); + from.z = CLAMP(from.z, 0, ACCEL_GRID_SIZE - 1); + to.x = CLAMP(to.x, 0, ACCEL_GRID_SIZE - 1); + to.y = CLAMP(to.y, 0, ACCEL_GRID_SIZE - 1); + to.z = CLAMP(to.z, 0, ACCEL_GRID_SIZE - 1); + + for (int32_t x = from.x; x <= to.x; x++) { + for (int32_t y = from.y; y <= to.y; y++) { + for (int32_t z = from.z; z <= to.z; z++) { + GridPos gp; + gp.pos = Vector3(x, y, z); + gp.E = acceleration_grid[x][y][z].push_back(new_simplex); + new_simplex->grid_positions.push_back(gp); + } + } + } + } + + good_triangles++; + } + + //print_line("at point " + itos(i) + "/" + itos(point_count) + " simplices added " + itos(good_triangles) + "/" + itos(simplex_list.size()) + " - triangles: " + itos(triangles.size())); + triangles.clear(); + triangles_inserted.clear(); + } + + //print_line("end with simplices: " + itos(simplex_list.size())); + Vector<OutputSimplex> ret_simplices; + ret_simplices.resize(simplex_list.size()); + OutputSimplex *ret_simplicesw = ret_simplices.ptrw(); + uint32_t simplices_written = 0; + + for (List<Simplex *>::Element *E = simplex_list.front(); E; E = E->next()) { + Simplex *simplex = E->get(); + bool invalid = false; + for (int j = 0; j < 4; j++) { + if (simplex->points[j] >= point_count) { + invalid = true; + break; + } + } + if (invalid || simplex_is_coplanar(points, *simplex)) { + memdelete(simplex); + continue; + } + + ret_simplicesw[simplices_written].points[0] = simplex->points[0]; + ret_simplicesw[simplices_written].points[1] = simplex->points[1]; + ret_simplicesw[simplices_written].points[2] = simplex->points[2]; + ret_simplicesw[simplices_written].points[3] = simplex->points[3]; + simplices_written++; + memdelete(simplex); + } + + ret_simplices.resize(simplices_written); + + memfree(points); + + return ret_simplices; + } +}; + +#endif // DELAUNAY_3D_H diff --git a/core/math/disjoint_set.cpp b/core/math/disjoint_set.cpp deleted file mode 100644 index a508151ad3..0000000000 --- a/core/math/disjoint_set.cpp +++ /dev/null @@ -1,31 +0,0 @@ -/*************************************************************************/ -/* disjoint_set.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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 index 32b9875e4c..198f46e111 100644 --- a/core/math/disjoint_set.h +++ b/core/math/disjoint_set.h @@ -41,7 +41,6 @@ /* 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; @@ -103,7 +102,6 @@ typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::insert_or_get(T 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); @@ -111,8 +109,9 @@ void DisjointSet<T, C, AL>::create_union(T a, T b) { Element *y_root = get_parent(y); // Already in the same set - if (x_root == y_root) + if (x_root == y_root) { return; + } // Not in the same set, merge if (x_root->rank < y_root->rank) { diff --git a/core/math/expression.cpp b/core/math/expression.cpp index 859b9be8c5..13a49feb6b 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -110,25 +110,22 @@ const char *Expression::func_name[Expression::FUNC_MAX] = { }; Expression::BuiltinFunc Expression::find_function(const String &p_string) { - for (int i = 0; i < FUNC_MAX; i++) { - if (p_string == func_name[i]) + if (p_string == func_name[i]) { return BuiltinFunc(i); + } } return FUNC_MAX; } String Expression::get_func_name(BuiltinFunc p_func) { - ERR_FAIL_INDEX_V(p_func, FUNC_MAX, String()); return func_name[p_func]; } int Expression::get_func_argument_count(BuiltinFunc p_func) { - switch (p_func) { - case MATH_RANDOMIZE: case MATH_RAND: case MATH_RANDF: @@ -220,194 +217,157 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant r_error.error = Callable::CallError::CALL_OK; switch (p_func) { case MATH_SIN: { - VALIDATE_ARG_NUM(0); *r_return = Math::sin((double)*p_inputs[0]); } break; case MATH_COS: { - VALIDATE_ARG_NUM(0); *r_return = Math::cos((double)*p_inputs[0]); } break; case MATH_TAN: { - VALIDATE_ARG_NUM(0); *r_return = Math::tan((double)*p_inputs[0]); } break; case MATH_SINH: { - VALIDATE_ARG_NUM(0); *r_return = Math::sinh((double)*p_inputs[0]); } break; case MATH_COSH: { - VALIDATE_ARG_NUM(0); *r_return = Math::cosh((double)*p_inputs[0]); } break; case MATH_TANH: { - VALIDATE_ARG_NUM(0); *r_return = Math::tanh((double)*p_inputs[0]); } break; case MATH_ASIN: { - VALIDATE_ARG_NUM(0); *r_return = Math::asin((double)*p_inputs[0]); } break; case MATH_ACOS: { - VALIDATE_ARG_NUM(0); *r_return = Math::acos((double)*p_inputs[0]); } break; case MATH_ATAN: { - VALIDATE_ARG_NUM(0); *r_return = Math::atan((double)*p_inputs[0]); } break; case MATH_ATAN2: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::atan2((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_SQRT: { - VALIDATE_ARG_NUM(0); *r_return = Math::sqrt((double)*p_inputs[0]); } break; case MATH_FMOD: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::fmod((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_FPOSMOD: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::fposmod((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_POSMOD: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::posmod((int)*p_inputs[0], (int)*p_inputs[1]); } break; case MATH_FLOOR: { - VALIDATE_ARG_NUM(0); *r_return = Math::floor((double)*p_inputs[0]); } break; case MATH_CEIL: { - VALIDATE_ARG_NUM(0); *r_return = Math::ceil((double)*p_inputs[0]); } break; case MATH_ROUND: { - VALIDATE_ARG_NUM(0); *r_return = Math::round((double)*p_inputs[0]); } break; case MATH_ABS: { - if (p_inputs[0]->get_type() == Variant::INT) { - int64_t i = *p_inputs[0]; *r_return = ABS(i); } else if (p_inputs[0]->get_type() == Variant::FLOAT) { - real_t r = *p_inputs[0]; *r_return = Math::abs(r); } else { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; r_error.expected = Variant::FLOAT; } } break; case MATH_SIGN: { - if (p_inputs[0]->get_type() == Variant::INT) { - int64_t i = *p_inputs[0]; *r_return = i < 0 ? -1 : (i > 0 ? +1 : 0); } else if (p_inputs[0]->get_type() == Variant::FLOAT) { - real_t r = *p_inputs[0]; *r_return = r < 0.0 ? -1.0 : (r > 0.0 ? +1.0 : 0.0); } else { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; r_error.expected = Variant::FLOAT; } } break; case MATH_POW: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::pow((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_LOG: { - VALIDATE_ARG_NUM(0); *r_return = Math::log((double)*p_inputs[0]); } break; case MATH_EXP: { - VALIDATE_ARG_NUM(0); *r_return = Math::exp((double)*p_inputs[0]); } break; case MATH_ISNAN: { - VALIDATE_ARG_NUM(0); *r_return = Math::is_nan((double)*p_inputs[0]); } break; case MATH_ISINF: { - VALIDATE_ARG_NUM(0); *r_return = Math::is_inf((double)*p_inputs[0]); } break; case MATH_EASE: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::ease((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_STEP_DECIMALS: { - VALIDATE_ARG_NUM(0); *r_return = Math::step_decimals((double)*p_inputs[0]); } break; case MATH_STEPIFY: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::stepify((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_LERP: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); *r_return = Math::lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case MATH_LERP_ANGLE: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); *r_return = Math::lerp_angle((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case MATH_INVERSE_LERP: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); *r_return = Math::inverse_lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case MATH_RANGE_LERP: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); @@ -422,14 +382,12 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return = Math::smoothstep((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case MATH_MOVE_TOWARD: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); *r_return = Math::move_toward((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case MATH_DECTIME: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); VALIDATE_ARG_NUM(2); @@ -446,20 +404,17 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return = Math::randf(); } break; case MATH_RANDOM: { - VALIDATE_ARG_NUM(0); VALIDATE_ARG_NUM(1); *r_return = Math::random((double)*p_inputs[0], (double)*p_inputs[1]); } break; case MATH_SEED: { - VALIDATE_ARG_NUM(0); uint64_t seed = *p_inputs[0]; Math::seed(seed); } break; case MATH_RANDSEED: { - VALIDATE_ARG_NUM(0); uint64_t seed = *p_inputs[0]; int ret = Math::rand_from_seed(&seed); @@ -470,22 +425,18 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case MATH_DEG2RAD: { - VALIDATE_ARG_NUM(0); *r_return = Math::deg2rad((double)*p_inputs[0]); } break; case MATH_RAD2DEG: { - VALIDATE_ARG_NUM(0); *r_return = Math::rad2deg((double)*p_inputs[0]); } break; case MATH_LINEAR2DB: { - VALIDATE_ARG_NUM(0); *r_return = Math::linear2db((double)*p_inputs[0]); } break; case MATH_DB2LINEAR: { - VALIDATE_ARG_NUM(0); *r_return = Math::db2linear((double)*p_inputs[0]); } break; @@ -516,9 +467,7 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return = Math::wrapf((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; case LOGIC_MAX: { - if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT) { - int64_t a = *p_inputs[0]; int64_t b = *p_inputs[1]; *r_return = MAX(a, b); @@ -534,9 +483,7 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case LOGIC_MIN: { - if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT) { - int64_t a = *p_inputs[0]; int64_t b = *p_inputs[1]; *r_return = MIN(a, b); @@ -551,9 +498,7 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } } break; case LOGIC_CLAMP: { - if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT && p_inputs[2]->get_type() == Variant::INT) { - int64_t a = *p_inputs[0]; int64_t b = *p_inputs[1]; int64_t c = *p_inputs[2]; @@ -571,15 +516,12 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } } break; case LOGIC_NEAREST_PO2: { - VALIDATE_ARG_NUM(0); int64_t num = *p_inputs[0]; *r_return = next_power_of_2(num); } break; case OBJ_WEAKREF: { - if (p_inputs[0]->get_type() != Variant::OBJECT) { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; r_error.expected = Variant::OBJECT; @@ -588,10 +530,8 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } if (p_inputs[0]->is_ref()) { - REF r = *p_inputs[0]; if (!r.is_valid()) { - return; } @@ -601,7 +541,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } else { Object *obj = *p_inputs[0]; if (!obj) { - return; } Ref<WeakRef> wref = memnew(WeakRef); @@ -611,9 +550,7 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case FUNC_FUNCREF: { - if (p_inputs[0]->get_type() != Variant::OBJECT) { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; r_error.expected = Variant::OBJECT; @@ -621,7 +558,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant return; } if (p_inputs[1]->get_type() != Variant::STRING && p_inputs[1]->get_type() != Variant::NODE_PATH) { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 1; r_error.expected = Variant::STRING; @@ -638,11 +574,9 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case TYPE_CONVERT: { - VALIDATE_ARG_NUM(1); int type = *p_inputs[1]; if (type < 0 || type >= Variant::VARIANT_MAX) { - r_error_str = RTR("Invalid type argument to convert(), use TYPE_* constants."); r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; @@ -650,31 +584,25 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant return; } else { - *r_return = Variant::construct(Variant::Type(type), p_inputs, 1, r_error); } } break; case TYPE_OF: { - *r_return = p_inputs[0]->get_type(); } break; case TYPE_EXISTS: { - *r_return = ClassDB::class_exists(*p_inputs[0]); } break; case TEXT_CHAR: { - CharType result[2] = { *p_inputs[0], 0 }; *r_return = String(result); } break; case TEXT_ORD: { - if (p_inputs[0]->get_type() != Variant::STRING) { - r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; r_error.expected = Variant::STRING; @@ -685,7 +613,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant String str = *p_inputs[0]; if (str.length() != 1) { - r_error_str = RTR("Expected a string of length 1 (a character)."); r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; @@ -698,39 +625,33 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case TEXT_STR: { - String str = *p_inputs[0]; *r_return = str; } break; case TEXT_PRINT: { - String str = *p_inputs[0]; print_line(str); } break; case TEXT_PRINTERR: { - String str = *p_inputs[0]; print_error(str); } break; case TEXT_PRINTRAW: { - String str = *p_inputs[0]; OS::get_singleton()->print("%s", str.utf8().get_data()); } break; case VAR_TO_STR: { - String vars; VariantWriter::write_to_string(*p_inputs[0], vars); *r_return = vars; } break; case STR_TO_VAR: { - if (p_inputs[0]->get_type() != Variant::STRING) { r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; @@ -756,7 +677,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case VAR_TO_BYTES: { - PackedByteArray barr; bool full_objects = *p_inputs[1]; int len; @@ -777,7 +697,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return = barr; } break; case BYTES_TO_VAR: { - if (p_inputs[0]->get_type() != Variant::PACKED_BYTE_ARRAY) { r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; @@ -805,7 +724,6 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } break; case COLORN: { - VALIDATE_ARG_NUM(1); Color color = Color::named(*p_inputs[0]); @@ -826,60 +744,49 @@ static bool _is_number(CharType c) { } Error Expression::_get_token(Token &r_token) { - while (true) { #define GET_CHAR() (str_ofs >= expression.length() ? 0 : expression[str_ofs++]) CharType cchar = GET_CHAR(); switch (cchar) { - case 0: { r_token.type = TK_EOF; return OK; - }; + } case '{': { - r_token.type = TK_CURLY_BRACKET_OPEN; return OK; - }; + } case '}': { - r_token.type = TK_CURLY_BRACKET_CLOSE; return OK; - }; + } case '[': { - r_token.type = TK_BRACKET_OPEN; return OK; - }; + } case ']': { - r_token.type = TK_BRACKET_CLOSE; return OK; - }; + } case '(': { - r_token.type = TK_PARENTHESIS_OPEN; return OK; - }; + } case ')': { - r_token.type = TK_PARENTHESIS_CLOSE; return OK; - }; + } case ',': { - r_token.type = TK_COMMA; return OK; - }; + } case ':': { - r_token.type = TK_COLON; return OK; - }; + } case '$': { - r_token.type = TK_INPUT; int index = 0; do { @@ -896,9 +803,8 @@ Error Expression::_get_token(Token &r_token) { r_token.value = index; return OK; - }; + } case '=': { - cchar = GET_CHAR(); if (cchar == '=') { r_token.type = TK_OP_EQUAL; @@ -908,9 +814,8 @@ Error Expression::_get_token(Token &r_token) { return ERR_PARSE_ERROR; } return OK; - }; + } case '!': { - if (expression[str_ofs] == '=') { r_token.type = TK_OP_NOT_EQUAL; str_ofs++; @@ -918,9 +823,8 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_OP_NOT; } return OK; - }; + } case '>': { - if (expression[str_ofs] == '=') { r_token.type = TK_OP_GREATER_EQUAL; str_ofs++; @@ -931,9 +835,8 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_OP_GREATER; } return OK; - }; + } case '<': { - if (expression[str_ofs] == '=') { r_token.type = TK_OP_LESS_EQUAL; str_ofs++; @@ -944,29 +847,28 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_OP_LESS; } return OK; - }; + } case '+': { r_token.type = TK_OP_ADD; return OK; - }; + } case '-': { r_token.type = TK_OP_SUB; return OK; - }; + } case '/': { r_token.type = TK_OP_DIV; return OK; - }; + } case '*': { r_token.type = TK_OP_MUL; return OK; - }; + } case '%': { r_token.type = TK_OP_MOD; return OK; - }; + } case '&': { - if (expression[str_ofs] == '&') { r_token.type = TK_OP_AND; str_ofs++; @@ -974,9 +876,8 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_OP_BIT_AND; } return OK; - }; + } case '|': { - if (expression[str_ofs] == '|') { r_token.type = TK_OP_OR; str_ofs++; @@ -984,31 +885,29 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_OP_BIT_OR; } return OK; - }; + } case '^': { - r_token.type = TK_OP_BIT_XOR; return OK; - }; + } case '~': { - r_token.type = TK_OP_BIT_INVERT; return OK; - }; + } + case '\'': case '"': { - String str; while (true) { - CharType ch = GET_CHAR(); if (ch == 0) { _set_error("Unterminated String"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; - } else if (ch == '"') { + } else if (ch == cchar) { + // cchar contain a corresponding quote symbol break; } else if (ch == '\\') { //escaped characters... @@ -1022,12 +921,21 @@ Error Expression::_get_token(Token &r_token) { CharType res = 0; switch (next) { - - case 'b': res = 8; break; - case 't': res = 9; break; - case 'n': res = 10; break; - case 'f': res = 12; break; - case 'r': res = 13; break; + case 'b': + res = 8; + break; + case 't': + res = 9; + break; + case 'n': + res = 10; + break; + case 'f': + res = 12; + break; + case 'r': + res = 13; + break; case 'u': { // hex number for (int j = 0; j < 4; j++) { @@ -1039,7 +947,6 @@ Error Expression::_get_token(Token &r_token) { return ERR_PARSE_ERROR; } if (!(_is_number(c) || (c >= 'a' && c <= 'f') || (c >= 'A' && c <= 'F'))) { - _set_error("Malformed hex constant in string"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; @@ -1081,7 +988,6 @@ Error Expression::_get_token(Token &r_token) { } break; default: { - if (cchar <= 32) { break; } @@ -1104,10 +1010,8 @@ Error Expression::_get_token(Token &r_token) { bool is_float = false; while (true) { - switch (reading) { case READING_INT: { - if (_is_number(c)) { //pass } else if (c == '.') { @@ -1121,9 +1025,7 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_DEC: { - if (_is_number(c)) { - } else if (c == 'e') { reading = READING_EXP; @@ -1133,13 +1035,13 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_EXP: { - if (_is_number(c)) { exp_beg = true; } else if ((c == '-' || c == '+') && !exp_sign && !exp_beg) { - if (c == '-') + if (c == '-') { is_float = true; + } exp_sign = true; } else { @@ -1148,8 +1050,9 @@ Error Expression::_get_token(Token &r_token) { } break; } - if (reading == READING_DONE) + if (reading == READING_DONE) { break; + } num += String::chr(c); c = GET_CHAR(); } @@ -1158,19 +1061,18 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_CONSTANT; - if (is_float) + if (is_float) { r_token.value = num.to_double(); - else - r_token.value = num.to_int64(); + } else { + r_token.value = num.to_int(); + } return OK; } else if ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_') { - String id; bool first = true; while ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_' || (!first && _is_number(cchar))) { - id += String::chr(cchar); cchar = GET_CHAR(); first = false; @@ -1210,7 +1112,6 @@ Error Expression::_get_token(Token &r_token) { } else if (id == "self") { r_token.type = TK_SELF; } else { - for (int i = 0; i < Variant::VARIANT_MAX; i++) { if (id == Variant::get_type_name(Variant::Type(i))) { r_token.type = TK_BASIC_TYPE; @@ -1293,7 +1194,6 @@ const char *Expression::token_name[TK_MAX] = { }; Expression::ENode *Expression::_parse_expression() { - Vector<ExpressionNode> expression; while (true) { @@ -1302,8 +1202,9 @@ Expression::ENode *Expression::_parse_expression() { Token tk; _get_token(tk); - if (error_set) + if (error_set) { return nullptr; + } switch (tk.type) { case TK_CURLY_BRACKET_OPEN: { @@ -1311,7 +1212,6 @@ Expression::ENode *Expression::_parse_expression() { DictionaryNode *dn = alloc_node<DictionaryNode>(); while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_CURLY_BRACKET_CLOSE) { @@ -1320,8 +1220,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } dn->dict.push_back(subexpr); _get_token(tk); @@ -1331,8 +1232,9 @@ Expression::ENode *Expression::_parse_expression() { } subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } dn->dict.push_back(subexpr); @@ -1355,7 +1257,6 @@ Expression::ENode *Expression::_parse_expression() { ArrayNode *an = alloc_node<ArrayNode>(); while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_BRACKET_CLOSE) { @@ -1364,8 +1265,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } an->array.push_back(subexpr); cofs = str_ofs; @@ -1384,8 +1286,9 @@ Expression::ENode *Expression::_parse_expression() { case TK_PARENTHESIS_OPEN: { //a suexpression ENode *e = _parse_expression(); - if (error_set) + if (error_set) { return nullptr; + } _get_token(tk); if (tk.type != TK_PARENTHESIS_CLOSE) { _set_error("Expected ')'"); @@ -1396,7 +1299,6 @@ Expression::ENode *Expression::_parse_expression() { } break; case TK_IDENTIFIER: { - String identifier = tk.value; int cofs = str_ofs; @@ -1409,7 +1311,6 @@ Expression::ENode *Expression::_parse_expression() { func_call->base = self_node; while (true) { - int cofs2 = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1418,8 +1319,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs2; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } func_call->arguments.push_back(subexpr); @@ -1452,7 +1354,6 @@ Expression::ENode *Expression::_parse_expression() { input->index = input_index; expr = input; } else { - NamedIndexNode *index = alloc_node<NamedIndexNode>(); SelfNode *self_node = alloc_node<SelfNode>(); index->base = self_node; @@ -1462,13 +1363,11 @@ Expression::ENode *Expression::_parse_expression() { } } break; case TK_INPUT: { - InputNode *input = alloc_node<InputNode>(); input->index = tk.value; expr = input; } break; case TK_SELF: { - SelfNode *self = alloc_node<SelfNode>(); expr = self; } break; @@ -1491,7 +1390,6 @@ Expression::ENode *Expression::_parse_expression() { constructor->data_type = bt; while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1500,8 +1398,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } constructor->arguments.push_back(subexpr); @@ -1532,7 +1431,6 @@ Expression::ENode *Expression::_parse_expression() { bifunc->func = BuiltinFunc(int(tk.value)); while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1541,8 +1439,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } bifunc->arguments.push_back(subexpr); @@ -1566,7 +1465,6 @@ Expression::ENode *Expression::_parse_expression() { } break; case TK_OP_SUB: { - ExpressionNode e; e.is_op = true; e.op = Variant::OP_NEGATE; @@ -1574,7 +1472,6 @@ Expression::ENode *Expression::_parse_expression() { continue; } break; case TK_OP_NOT: { - ExpressionNode e; e.is_op = true; e.op = Variant::OP_NOT; @@ -1593,8 +1490,9 @@ Expression::ENode *Expression::_parse_expression() { while (true) { int cofs2 = str_ofs; _get_token(tk); - if (error_set) + if (error_set) { return nullptr; + } bool done = false; @@ -1606,8 +1504,9 @@ Expression::ENode *Expression::_parse_expression() { index->base = expr; ENode *what = _parse_expression(); - if (!what) + if (!what) { return nullptr; + } index->index = what; @@ -1638,7 +1537,6 @@ Expression::ENode *Expression::_parse_expression() { func_call->base = expr; while (true) { - int cofs3 = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1647,8 +1545,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs3; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) + if (!subexpr) { return nullptr; + } func_call->arguments.push_back(subexpr); @@ -1681,8 +1580,9 @@ Expression::ENode *Expression::_parse_expression() { } break; } - if (done) + if (done) { break; + } } //push expression @@ -1697,35 +1597,78 @@ Expression::ENode *Expression::_parse_expression() { int cofs = str_ofs; _get_token(tk); - if (error_set) + if (error_set) { return nullptr; + } Variant::Operator op = Variant::OP_MAX; switch (tk.type) { - case TK_OP_IN: op = Variant::OP_IN; break; - case TK_OP_EQUAL: op = Variant::OP_EQUAL; break; - case TK_OP_NOT_EQUAL: op = Variant::OP_NOT_EQUAL; break; - case TK_OP_LESS: op = Variant::OP_LESS; break; - case TK_OP_LESS_EQUAL: op = Variant::OP_LESS_EQUAL; break; - case TK_OP_GREATER: op = Variant::OP_GREATER; break; - case TK_OP_GREATER_EQUAL: op = Variant::OP_GREATER_EQUAL; break; - case TK_OP_AND: op = Variant::OP_AND; break; - case TK_OP_OR: op = Variant::OP_OR; break; - case TK_OP_NOT: op = Variant::OP_NOT; break; - case TK_OP_ADD: op = Variant::OP_ADD; break; - case TK_OP_SUB: op = Variant::OP_SUBTRACT; break; - case TK_OP_MUL: op = Variant::OP_MULTIPLY; break; - case TK_OP_DIV: op = Variant::OP_DIVIDE; break; - case TK_OP_MOD: op = Variant::OP_MODULE; break; - case TK_OP_SHIFT_LEFT: op = Variant::OP_SHIFT_LEFT; break; - case TK_OP_SHIFT_RIGHT: op = Variant::OP_SHIFT_RIGHT; break; - case TK_OP_BIT_AND: op = Variant::OP_BIT_AND; break; - case TK_OP_BIT_OR: op = Variant::OP_BIT_OR; break; - case TK_OP_BIT_XOR: op = Variant::OP_BIT_XOR; break; - case TK_OP_BIT_INVERT: op = Variant::OP_BIT_NEGATE; break; + case TK_OP_IN: + op = Variant::OP_IN; + break; + case TK_OP_EQUAL: + op = Variant::OP_EQUAL; + break; + case TK_OP_NOT_EQUAL: + op = Variant::OP_NOT_EQUAL; + break; + case TK_OP_LESS: + op = Variant::OP_LESS; + break; + case TK_OP_LESS_EQUAL: + op = Variant::OP_LESS_EQUAL; + break; + case TK_OP_GREATER: + op = Variant::OP_GREATER; + break; + case TK_OP_GREATER_EQUAL: + op = Variant::OP_GREATER_EQUAL; + break; + case TK_OP_AND: + op = Variant::OP_AND; + break; + case TK_OP_OR: + op = Variant::OP_OR; + break; + case TK_OP_NOT: + op = Variant::OP_NOT; + break; + case TK_OP_ADD: + op = Variant::OP_ADD; + break; + case TK_OP_SUB: + op = Variant::OP_SUBTRACT; + break; + case TK_OP_MUL: + op = Variant::OP_MULTIPLY; + break; + case TK_OP_DIV: + op = Variant::OP_DIVIDE; + break; + case TK_OP_MOD: + op = Variant::OP_MODULE; + break; + case TK_OP_SHIFT_LEFT: + op = Variant::OP_SHIFT_LEFT; + break; + case TK_OP_SHIFT_RIGHT: + op = Variant::OP_SHIFT_RIGHT; + break; + case TK_OP_BIT_AND: + op = Variant::OP_BIT_AND; + break; + case TK_OP_BIT_OR: + op = Variant::OP_BIT_OR; + break; + case TK_OP_BIT_XOR: + op = Variant::OP_BIT_XOR; + break; + case TK_OP_BIT_INVERT: + op = Variant::OP_BIT_NEGATE; + break; default: { - }; + } } if (op == Variant::OP_MAX) { //stop appending stuff @@ -1745,15 +1688,12 @@ Expression::ENode *Expression::_parse_expression() { /* Reduce the set set of expressions and place them in an operator tree, respecting precedence */ while (expression.size() > 1) { - int next_op = -1; int min_priority = 0xFFFFF; bool is_unary = false; for (int i = 0; i < expression.size(); i++) { - if (!expression[i].is_op) { - continue; } @@ -1762,7 +1702,6 @@ Expression::ENode *Expression::_parse_expression() { bool unary = false; switch (expression[i].op) { - case Variant::OP_BIT_NEGATE: priority = 0; unary = true; @@ -1772,36 +1711,74 @@ Expression::ENode *Expression::_parse_expression() { unary = true; break; - case Variant::OP_MULTIPLY: priority = 2; break; - case Variant::OP_DIVIDE: priority = 2; break; - case Variant::OP_MODULE: priority = 2; break; + case Variant::OP_MULTIPLY: + priority = 2; + break; + case Variant::OP_DIVIDE: + priority = 2; + break; + case Variant::OP_MODULE: + priority = 2; + break; - case Variant::OP_ADD: priority = 3; break; - case Variant::OP_SUBTRACT: priority = 3; break; + case Variant::OP_ADD: + priority = 3; + break; + case Variant::OP_SUBTRACT: + priority = 3; + break; - case Variant::OP_SHIFT_LEFT: priority = 4; break; - case Variant::OP_SHIFT_RIGHT: priority = 4; break; + case Variant::OP_SHIFT_LEFT: + priority = 4; + break; + case Variant::OP_SHIFT_RIGHT: + priority = 4; + break; - case Variant::OP_BIT_AND: priority = 5; break; - case Variant::OP_BIT_XOR: priority = 6; break; - case Variant::OP_BIT_OR: priority = 7; break; + case Variant::OP_BIT_AND: + priority = 5; + break; + case Variant::OP_BIT_XOR: + priority = 6; + break; + case Variant::OP_BIT_OR: + priority = 7; + break; - case Variant::OP_LESS: priority = 8; break; - case Variant::OP_LESS_EQUAL: priority = 8; break; - case Variant::OP_GREATER: priority = 8; break; - case Variant::OP_GREATER_EQUAL: priority = 8; break; + case Variant::OP_LESS: + priority = 8; + break; + case Variant::OP_LESS_EQUAL: + priority = 8; + break; + case Variant::OP_GREATER: + priority = 8; + break; + case Variant::OP_GREATER_EQUAL: + priority = 8; + break; - case Variant::OP_EQUAL: priority = 8; break; - case Variant::OP_NOT_EQUAL: priority = 8; break; + case Variant::OP_EQUAL: + priority = 8; + break; + case Variant::OP_NOT_EQUAL: + priority = 8; + break; - case Variant::OP_IN: priority = 10; break; + case Variant::OP_IN: + priority = 10; + break; case Variant::OP_NOT: priority = 11; unary = true; break; - case Variant::OP_AND: priority = 12; break; - case Variant::OP_OR: priority = 13; break; + case Variant::OP_AND: + priority = 12; + break; + case Variant::OP_OR: + priority = 13; + break; default: { _set_error("Parser bug, invalid operator in expression: " + itos(expression[i].op)); @@ -1820,17 +1797,14 @@ Expression::ENode *Expression::_parse_expression() { } if (next_op == -1) { - _set_error("Yet another parser bug...."); ERR_FAIL_V(nullptr); } // OK! create operator.. if (is_unary) { - int expr_pos = next_op; while (expression[expr_pos].is_op) { - expr_pos++; if (expr_pos == expression.size()) { //can happen.. @@ -1841,7 +1815,6 @@ Expression::ENode *Expression::_parse_expression() { //consecutively do unary operators for (int i = expr_pos - 1; i >= next_op; i--) { - OperatorNode *op = alloc_node<OperatorNode>(); op->op = expression[i].op; op->nodes[0] = expression[i + 1].node; @@ -1852,7 +1825,6 @@ Expression::ENode *Expression::_parse_expression() { } } else { - if (next_op < 1 || next_op >= (expression.size() - 1)) { _set_error("Parser bug..."); ERR_FAIL_V(nullptr); @@ -1862,7 +1834,6 @@ Expression::ENode *Expression::_parse_expression() { op->op = expression[next_op].op; if (expression[next_op - 1].is_op) { - _set_error("Parser bug..."); ERR_FAIL_V(nullptr); } @@ -1891,9 +1862,9 @@ Expression::ENode *Expression::_parse_expression() { } bool Expression::_compile_expression() { - - if (!expression_dirty) + if (!expression_dirty) { return error_set; + } if (nodes) { memdelete(nodes); @@ -1921,10 +1892,8 @@ bool Expression::_compile_expression() { } bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str) { - switch (p_node->type) { case Expression::ENode::TYPE_INPUT: { - const Expression::InputNode *in = static_cast<const Expression::InputNode *>(p_node); if (in->index < 0 || in->index >= p_inputs.size()) { r_error_str = vformat(RTR("Invalid input %i (not passed) in expression"), in->index); @@ -1933,13 +1902,11 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: r_ret = p_inputs[in->index]; } break; case Expression::ENode::TYPE_CONSTANT: { - const Expression::ConstantNode *c = static_cast<const Expression::ConstantNode *>(p_node); r_ret = c->value; } break; case Expression::ENode::TYPE_SELF: { - if (!p_instance) { r_error_str = RTR("self can't be used because instance is null (not passed)"); return true; @@ -1947,20 +1914,21 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: r_ret = p_instance; } break; case Expression::ENode::TYPE_OPERATOR: { - const Expression::OperatorNode *op = static_cast<const Expression::OperatorNode *>(p_node); Variant a; bool ret = _execute(p_inputs, p_instance, op->nodes[0], a, r_error_str); - if (ret) + if (ret) { return true; + } Variant b; if (op->nodes[1]) { ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str); - if (ret) + if (ret) { return true; + } } bool valid = true; @@ -1972,19 +1940,20 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } break; case Expression::ENode::TYPE_INDEX: { - const Expression::IndexNode *index = static_cast<const Expression::IndexNode *>(p_node); Variant base; bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str); - if (ret) + if (ret) { return true; + } Variant idx; ret = _execute(p_inputs, p_instance, index->index, idx, r_error_str); - if (ret) + if (ret) { return true; + } bool valid; r_ret = base.get(idx, &valid); @@ -1995,13 +1964,13 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } break; case Expression::ENode::TYPE_NAMED_INDEX: { - const Expression::NamedIndexNode *index = static_cast<const Expression::NamedIndexNode *>(p_node); Variant base; bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str); - if (ret) + if (ret) { return true; + } bool valid; r_ret = base.get_named(index->name, &valid); @@ -2017,12 +1986,12 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: Array arr; arr.resize(array->array.size()); for (int i = 0; i < array->array.size(); i++) { - Variant value; bool ret = _execute(p_inputs, p_instance, array->array[i], value, r_error_str); - if (ret) + if (ret) { return true; + } arr[i] = value; } @@ -2034,17 +2003,18 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: Dictionary d; for (int i = 0; i < dictionary->dict.size(); i += 2) { - Variant key; bool ret = _execute(p_inputs, p_instance, dictionary->dict[i + 0], key, r_error_str); - if (ret) + if (ret) { return true; + } Variant value; ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, r_error_str); - if (ret) + if (ret) { return true; + } d[key] = value; } @@ -2052,7 +2022,6 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: r_ret = d; } break; case Expression::ENode::TYPE_CONSTRUCTOR: { - const Expression::ConstructorNode *constructor = static_cast<const Expression::ConstructorNode *>(p_node); Vector<Variant> arr; @@ -2061,12 +2030,12 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: argp.resize(constructor->arguments.size()); for (int i = 0; i < constructor->arguments.size(); i++) { - Variant value; bool ret = _execute(p_inputs, p_instance, constructor->arguments[i], value, r_error_str); - if (ret) + if (ret) { return true; + } arr.write[i] = value; argp.write[i] = &arr[i]; } @@ -2081,7 +2050,6 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } break; case Expression::ENode::TYPE_BUILTIN_FUNC: { - const Expression::BuiltinFuncNode *bifunc = static_cast<const Expression::BuiltinFuncNode *>(p_node); Vector<Variant> arr; @@ -2090,11 +2058,11 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: argp.resize(bifunc->arguments.size()); for (int i = 0; i < bifunc->arguments.size(); i++) { - Variant value; bool ret = _execute(p_inputs, p_instance, bifunc->arguments[i], value, r_error_str); - if (ret) + if (ret) { return true; + } arr.write[i] = value; argp.write[i] = &arr[i]; } @@ -2109,14 +2077,14 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } break; case Expression::ENode::TYPE_CALL: { - const Expression::CallNode *call = static_cast<const Expression::CallNode *>(p_node); Variant base; bool ret = _execute(p_inputs, p_instance, call->base, base, r_error_str); - if (ret) + if (ret) { return true; + } Vector<Variant> arr; Vector<const Variant *> argp; @@ -2124,12 +2092,12 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: argp.resize(call->arguments.size()); for (int i = 0; i < call->arguments.size(); i++) { - Variant value; ret = _execute(p_inputs, p_instance, call->arguments[i], value, r_error_str); - if (ret) + if (ret) { return true; + } arr.write[i] = value; argp.write[i] = &arr[i]; } @@ -2148,7 +2116,6 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } Error Expression::parse(const String &p_expression, const Vector<String> &p_input_names) { - if (nodes) { memdelete(nodes); nodes = nullptr; @@ -2176,7 +2143,6 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu } Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) { - ERR_FAIL_COND_V_MSG(error_set, Variant(), "There was previously a parse error: " + error_str + "."); execution_error = false; @@ -2201,26 +2167,13 @@ String Expression::get_error_text() const { } void Expression::_bind_methods() { - ClassDB::bind_method(D_METHOD("parse", "expression", "input_names"), &Expression::parse, DEFVAL(Vector<String>())); ClassDB::bind_method(D_METHOD("execute", "inputs", "base_instance", "show_error"), &Expression::execute, DEFVAL(Array()), DEFVAL(Variant()), DEFVAL(true)); ClassDB::bind_method(D_METHOD("has_execute_failed"), &Expression::has_execute_failed); ClassDB::bind_method(D_METHOD("get_error_text"), &Expression::get_error_text); } -Expression::Expression() : - output_type(Variant::NIL), - sequenced(false), - error_set(true), - root(nullptr), - nodes(nullptr), - execution_error(false) { - str_ofs = 0; - expression_dirty = false; -} - Expression::~Expression() { - if (nodes) { memdelete(nodes); } diff --git a/core/math/expression.h b/core/math/expression.h index 78de225ebf..59a9a2f4ed 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -118,23 +118,20 @@ private: static const char *func_name[FUNC_MAX]; struct Input { - - Variant::Type type; + Variant::Type type = Variant::NIL; String name; - Input() : - type(Variant::NIL) { - } + Input() {} }; Vector<Input> inputs; - Variant::Type output_type; + Variant::Type output_type = Variant::NIL; String expression; - bool sequenced; - int str_ofs; - bool expression_dirty; + bool sequenced = false; + int str_ofs = 0; + bool expression_dirty = false; bool _compile_expression(); @@ -182,14 +179,14 @@ private: static const char *token_name[TK_MAX]; struct Token { - TokenType type; Variant value; }; void _set_error(const String &p_err) { - if (error_set) + if (error_set) { return; + } error_str = p_err; error_set = true; } @@ -197,10 +194,9 @@ private: Error _get_token(Token &r_token); String error_str; - bool error_set; + bool error_set = true; struct ENode { - enum Type { TYPE_INPUT, TYPE_CONSTANT, @@ -215,11 +211,11 @@ private: TYPE_CALL }; - ENode *next; + ENode *next = nullptr; Type type; - ENode() { next = nullptr; } + ENode() {} virtual ~ENode() { if (next) { memdelete(next); @@ -228,7 +224,6 @@ private: }; struct ExpressionNode { - bool is_op; union { Variant::Operator op; @@ -239,7 +234,6 @@ private: ENode *_parse_expression(); struct InputNode : public ENode { - int index; InputNode() { type = TYPE_INPUT; @@ -247,7 +241,6 @@ private: }; struct ConstantNode : public ENode { - Variant value; ConstantNode() { type = TYPE_CONSTANT; @@ -255,7 +248,6 @@ private: }; struct OperatorNode : public ENode { - Variant::Operator op; ENode *nodes[2]; @@ -266,7 +258,6 @@ private: }; struct SelfNode : public ENode { - SelfNode() { type = TYPE_SELF; } @@ -339,12 +330,12 @@ private: return node; } - ENode *root; - ENode *nodes; + ENode *root = nullptr; + ENode *nodes = nullptr; Vector<String> input_names; - bool execution_error; + bool execution_error = false; bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str); protected: @@ -356,7 +347,7 @@ public: bool has_execute_failed() const; String get_error_text() const; - Expression(); + Expression() {} ~Expression(); }; diff --git a/core/math/face3.cpp b/core/math/face3.cpp index 74331b391f..db2bfaa58b 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -30,10 +30,9 @@ #include "face3.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_over[3]) const { - ERR_FAIL_COND_V(is_degenerate(), 0); Vector3 above[4]; @@ -43,7 +42,6 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ int below_count = 0; for (int i = 0; i < 3; i++) { - if (p_plane.has_point(vertex[i], CMP_EPSILON)) { // point is in plane ERR_FAIL_COND_V(above_count >= 4, 0); @@ -52,7 +50,6 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ below[below_count++] = vertex[i]; } else { - if (p_plane.is_point_over(vertex[i])) { //Point is over ERR_FAIL_COND_V(above_count >= 4, 0); @@ -67,8 +64,9 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ /* Check for Intersection between this and the next vertex*/ Vector3 inters; - if (!p_plane.intersects_segment(vertex[i], vertex[(i + 1) % 3], &inters)) + if (!p_plane.intersects_segment(vertex[i], vertex[(i + 1) % 3], &inters)) { continue; + } /* Intersection goes to both */ ERR_FAIL_COND_V(above_count >= 4, 0); @@ -83,13 +81,11 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ ERR_FAIL_COND_V(above_count >= 4 && below_count >= 4, 0); //bug in the algo if (above_count >= 3) { - p_res[polygons_created] = Face3(above[0], above[1], above[2]); p_is_point_over[polygons_created] = true; polygons_created++; if (above_count == 4) { - p_res[polygons_created] = Face3(above[2], above[3], above[0]); p_is_point_over[polygons_created] = true; polygons_created++; @@ -97,13 +93,11 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ } if (below_count >= 3) { - p_res[polygons_created] = Face3(below[0], below[1], below[2]); p_is_point_over[polygons_created] = false; polygons_created++; if (below_count == 4) { - p_res[polygons_created] = Face3(below[2], below[3], below[0]); p_is_point_over[polygons_created] = false; polygons_created++; @@ -114,52 +108,49 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ } bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { - - return Geometry::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); + return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); } bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { - - return Geometry::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); + return Geometry3D::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); } bool Face3::is_degenerate() const { - Vector3 normal = vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]); return (normal.length_squared() < CMP_EPSILON2); } Face3::Side Face3::get_side_of(const Face3 &p_face, ClockDirection p_clock_dir) const { - int over = 0, under = 0; Plane plane = get_plane(p_clock_dir); for (int i = 0; i < 3; i++) { - const Vector3 &v = p_face.vertex[i]; - if (plane.has_point(v)) //coplanar, don't bother + if (plane.has_point(v)) { //coplanar, don't bother continue; + } - if (plane.is_point_over(v)) + if (plane.is_point_over(v)) { over++; - else + } else { under++; + } } - if (over > 0 && under == 0) + if (over > 0 && under == 0) { return SIDE_OVER; - else if (under > 0 && over == 0) + } else if (under > 0 && over == 0) { return SIDE_UNDER; - else if (under == 0 && over == 0) + } else if (under == 0 && over == 0) { return SIDE_COPLANAR; - else + } else { return SIDE_SPANNING; + } } Vector3 Face3::get_random_point_inside() const { - real_t a = Math::random(0, 1); real_t b = Math::random(0, 1); if (a > b) { @@ -170,32 +161,28 @@ Vector3 Face3::get_random_point_inside() const { } Plane Face3::get_plane(ClockDirection p_dir) const { - return Plane(vertex[0], vertex[1], vertex[2], p_dir); } Vector3 Face3::get_median_point() const { - return (vertex[0] + vertex[1] + vertex[2]) / 3.0; } real_t Face3::get_area() const { - return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length(); } ClockDirection Face3::get_clock_dir() const { - Vector3 normal = vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]); //printf("normal is %g,%g,%g x %g,%g,%g- wtfu is %g\n",tofloat(normal.x),tofloat(normal.y),tofloat(normal.z),tofloat(vertex[0].x),tofloat(vertex[0].y),tofloat(vertex[0].z),tofloat( normal.dot( vertex[0] ) ) ); return (normal.dot(vertex[0]) >= 0) ? CLOCKWISE : COUNTERCLOCKWISE; } bool Face3::intersects_aabb(const AABB &p_aabb) const { - /** TEST PLANE **/ - if (!p_aabb.intersects_plane(get_plane())) + if (!p_aabb.intersects_plane(get_plane())) { return false; + } #define TEST_AXIS(m_ax) \ /** TEST FACE AXIS */ \ @@ -228,7 +215,6 @@ bool Face3::intersects_aabb(const AABB &p_aabb) const { }; for (int i = 0; i < 12; i++) { - Vector3 from, to; p_aabb.get_edge(i, from, to); Vector3 e1 = from - to; @@ -237,58 +223,57 @@ bool Face3::intersects_aabb(const AABB &p_aabb) const { Vector3 axis = vec3_cross(e1, e2); - if (axis.length_squared() < 0.0001) + if (axis.length_squared() < 0.0001) { continue; // coplanar + } axis.normalize(); real_t minA, maxA, minB, maxB; p_aabb.project_range_in_plane(Plane(axis, 0), minA, maxA); project_range(axis, Transform(), minB, maxB); - if (maxA < minB || maxB < minA) + if (maxA < minB || maxB < minA) { return false; + } } } return true; } Face3::operator String() const { - return String() + vertex[0] + ", " + vertex[1] + ", " + vertex[2]; } void Face3::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - for (int i = 0; i < 3; i++) { - Vector3 v = p_transform.xform(vertex[i]); real_t d = p_normal.dot(v); - if (i == 0 || d > r_max) + if (i == 0 || d > r_max) { r_max = d; + } - if (i == 0 || d < r_min) + if (i == 0 || d < r_min) { r_min = d; + } } } void Face3::get_support(const Vector3 &p_normal, const Transform &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const { - #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.98 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.05 - if (p_max <= 0) + if (p_max <= 0) { return; + } Vector3 n = p_transform.basis.xform_inv(p_normal); /** TEST FACE AS SUPPORT **/ if (get_plane().normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - *p_count = MIN(3, p_max); for (int i = 0; i < *p_count; i++) { - p_vertices[i] = p_transform.xform(vertex[i]); } @@ -301,7 +286,6 @@ void Face3::get_support(const Vector3 &p_normal, const Transform &p_transform, V real_t support_max = 0; for (int i = 0; i < 3; i++) { - real_t d = n.dot(vertex[i]); if (i == 0 || d > support_max) { @@ -313,19 +297,19 @@ void Face3::get_support(const Vector3 &p_normal, const Transform &p_transform, V /** TEST EDGES AS SUPPORT **/ for (int i = 0; i < 3; i++) { - - if (i != vert_support_idx && i + 1 != vert_support_idx) + if (i != vert_support_idx && i + 1 != vert_support_idx) { continue; + } // check if edge is valid as a support real_t dot = (vertex[i] - vertex[(i + 1) % 3]).normalized().dot(n); dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - *p_count = MIN(2, p_max); - for (int j = 0; j < *p_count; j++) + for (int j = 0; j < *p_count; j++) { p_vertices[j] = p_transform.xform(vertex[(j + i) % 3]); + } return; } @@ -336,7 +320,6 @@ void Face3::get_support(const Vector3 &p_normal, const Transform &p_transform, V } Vector3 Face3::get_closest_point_to(const Vector3 &p_point) const { - Vector3 edge0 = vertex[1] - vertex[0]; Vector3 edge1 = vertex[2] - vertex[0]; Vector3 v0 = vertex[0] - p_point; diff --git a/core/math/face3.h b/core/math/face3.h index f4b8721caa..fb40e8ab9e 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -69,8 +69,8 @@ public: Vector3 get_median_point() const; Vector3 get_closest_point_to(const Vector3 &p_point) const; - bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = 0) const; - bool intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = 0) const; + bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const; + bool intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const; ClockDirection get_clock_dir() const; ///< todo, test if this is returning the proper clockwisity @@ -78,7 +78,6 @@ public: void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; AABB get_aabb() const { - AABB aabb(vertex[0], Vector3()); aabb.expand_to(vertex[1]); aabb.expand_to(vertex[2]); @@ -98,7 +97,6 @@ public: }; bool Face3::intersects_aabb2(const AABB &p_aabb) const { - Vector3 perp = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]); Vector3 half_extents = p_aabb.size * 0.5; @@ -113,8 +111,9 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { real_t dist_a = perp.dot(ofs + sup) - d; real_t dist_b = perp.dot(ofs - sup) - d; - if (dist_a * dist_b > 0) + if (dist_a * dist_b > 0) { return false; //does not intersect the plane + } #define TEST_AXIS(m_ax) \ { \ @@ -145,17 +144,13 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { }; for (int i = 0; i < 12; i++) { - Vector3 from, to; switch (i) { - case 0: { - from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z); to = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z); } break; case 1: { - from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z); to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z); } break; @@ -165,18 +160,15 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { } break; case 3: { - from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z); to = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z); } break; case 4: { - from = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); } break; case 5: { - from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z); } break; @@ -186,31 +178,26 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { } break; case 7: { - from = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z); to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); } break; case 8: { - from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z); to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z); } break; case 9: { - from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z); to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); } break; case 10: { - from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z); to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z); } break; case 11: { - from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z); to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z); @@ -223,8 +210,9 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { Vector3 axis = vec3_cross(e1, e2); - if (axis.length_squared() < 0.0001) + if (axis.length_squared() < 0.0001) { continue; // coplanar + } //axis.normalize(); Vector3 sup2 = Vector3( @@ -240,18 +228,20 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { real_t minT = 1e20, maxT = -1e20; for (int k = 0; k < 3; k++) { - real_t vert_d = axis.dot(vertex[k]); - if (vert_d > maxT) + if (vert_d > maxT) { maxT = vert_d; + } - if (vert_d < minT) + if (vert_d < minT) { minT = vert_d; + } } - if (maxB < minT || maxT < minB) + if (maxB < minT || maxT < minB) { return false; + } } } return true; diff --git a/core/math/geometry_2d.cpp b/core/math/geometry_2d.cpp new file mode 100644 index 0000000000..4636e1c774 --- /dev/null +++ b/core/math/geometry_2d.cpp @@ -0,0 +1,384 @@ +/*************************************************************************/ +/* geometry_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 "geometry_2d.h" + +#include "thirdparty/misc/clipper.hpp" +#include "thirdparty/misc/triangulator.h" +#define STB_RECT_PACK_IMPLEMENTATION +#include "thirdparty/misc/stb_rect_pack.h" + +#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. + +Vector<Vector<Vector2>> Geometry2D::decompose_polygon_in_convex(Vector<Point2> polygon) { + Vector<Vector<Vector2>> decomp; + List<TriangulatorPoly> in_poly, out_poly; + + TriangulatorPoly inp; + inp.Init(polygon.size()); + for (int i = 0; i < polygon.size(); i++) { + inp.GetPoint(i) = polygon[i]; + } + inp.SetOrientation(TRIANGULATOR_CCW); + in_poly.push_back(inp); + TriangulatorPartition tpart; + if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed. + ERR_PRINT("Convex decomposing failed!"); + return decomp; + } + + decomp.resize(out_poly.size()); + int idx = 0; + for (List<TriangulatorPoly>::Element *I = out_poly.front(); I; I = I->next()) { + TriangulatorPoly &tp = I->get(); + + decomp.write[idx].resize(tp.GetNumPoints()); + + for (int64_t i = 0; i < tp.GetNumPoints(); i++) { + decomp.write[idx].write[i] = tp.GetPoint(i); + } + + idx++; + } + + return decomp; +} + +struct _AtlasWorkRect { + Size2i s; + Point2i p; + int idx; + _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }; +}; + +struct _AtlasWorkRectResult { + Vector<_AtlasWorkRect> result; + int max_w; + int max_h; +}; + +void Geometry2D::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size) { + // Super simple, almost brute force scanline stacking fitter. + // It's pretty basic for now, but it tries to make sure that the aspect ratio of the + // resulting atlas is somehow square. This is necessary because video cards have limits. + // On texture size (usually 2048 or 4096), so the more square a texture, the more chances. + // It will work in every hardware. + // For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a + // 256x8192 atlas (won't work anywhere). + + ERR_FAIL_COND(p_rects.size() == 0); + + Vector<_AtlasWorkRect> wrects; + wrects.resize(p_rects.size()); + for (int i = 0; i < p_rects.size(); i++) { + wrects.write[i].s = p_rects[i]; + wrects.write[i].idx = i; + } + wrects.sort(); + int widest = wrects[0].s.width; + + Vector<_AtlasWorkRectResult> results; + + for (int i = 0; i <= 12; i++) { + int w = 1 << i; + int max_h = 0; + int max_w = 0; + if (w < widest) { + continue; + } + + Vector<int> hmax; + hmax.resize(w); + for (int j = 0; j < w; j++) { + hmax.write[j] = 0; + } + + // Place them. + int ofs = 0; + int limit_h = 0; + for (int j = 0; j < wrects.size(); j++) { + if (ofs + wrects[j].s.width > w) { + ofs = 0; + } + + int from_y = 0; + for (int k = 0; k < wrects[j].s.width; k++) { + if (hmax[ofs + k] > from_y) { + from_y = hmax[ofs + k]; + } + } + + wrects.write[j].p.x = ofs; + wrects.write[j].p.y = from_y; + int end_h = from_y + wrects[j].s.height; + int end_w = ofs + wrects[j].s.width; + if (ofs == 0) { + limit_h = end_h; + } + + for (int k = 0; k < wrects[j].s.width; k++) { + hmax.write[ofs + k] = end_h; + } + + if (end_h > max_h) { + max_h = end_h; + } + + if (end_w > max_w) { + max_w = end_w; + } + + if (ofs == 0 || end_h > limit_h) { // While h limit not reached, keep stacking. + ofs += wrects[j].s.width; + } + } + + _AtlasWorkRectResult result; + result.result = wrects; + result.max_h = max_h; + result.max_w = max_w; + results.push_back(result); + } + + // Find the result with the best aspect ratio. + + int best = -1; + real_t best_aspect = 1e20; + + for (int i = 0; i < results.size(); i++) { + real_t h = next_power_of_2(results[i].max_h); + real_t w = next_power_of_2(results[i].max_w); + real_t aspect = h > w ? h / w : w / h; + if (aspect < best_aspect) { + best = i; + best_aspect = aspect; + } + } + + r_result.resize(p_rects.size()); + + for (int i = 0; i < p_rects.size(); i++) { + r_result.write[results[best].result[i].idx] = results[best].result[i].p; + } + + r_size = Size2(results[best].max_w, results[best].max_h); +} + +Vector<Vector<Point2>> Geometry2D::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open) { + using namespace ClipperLib; + + ClipType op = ctUnion; + + switch (p_op) { + case OPERATION_UNION: + op = ctUnion; + break; + case OPERATION_DIFFERENCE: + op = ctDifference; + break; + case OPERATION_INTERSECTION: + op = ctIntersection; + break; + case OPERATION_XOR: + op = ctXor; + break; + } + Path path_a, path_b; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath_a.size(); ++i) { + path_a << IntPoint(p_polypath_a[i].x * SCALE_FACTOR, p_polypath_a[i].y * SCALE_FACTOR); + } + for (int i = 0; i != p_polypath_b.size(); ++i) { + path_b << IntPoint(p_polypath_b[i].x * SCALE_FACTOR, p_polypath_b[i].y * SCALE_FACTOR); + } + Clipper clp; + clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. + clp.AddPath(path_b, ptClip, true); // Polylines cannot be set as clip. + + Paths paths; + + if (is_a_open) { + PolyTree tree; // Needed to populate polylines. + clp.Execute(op, tree); + OpenPathsFromPolyTree(tree, paths); + } else { + clp.Execute(op, paths); // Works on closed polygons only. + } + // Have to scale points down now. + Vector<Vector<Point2>> polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector<Vector2> polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} + +Vector<Vector<Point2>> Geometry2D::_polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + using namespace ClipperLib; + + JoinType jt = jtSquare; + + switch (p_join_type) { + case JOIN_SQUARE: + jt = jtSquare; + break; + case JOIN_ROUND: + jt = jtRound; + break; + case JOIN_MITER: + jt = jtMiter; + break; + } + + EndType et = etClosedPolygon; + + switch (p_end_type) { + case END_POLYGON: + et = etClosedPolygon; + break; + case END_JOINED: + et = etClosedLine; + break; + case END_BUTT: + et = etOpenButt; + break; + case END_SQUARE: + et = etOpenSquare; + break; + case END_ROUND: + et = etOpenRound; + break; + } + ClipperOffset co(2.0, 0.25 * SCALE_FACTOR); // Defaults from ClipperOffset. + Path path; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath.size(); ++i) { + path << IntPoint(p_polypath[i].x * SCALE_FACTOR, p_polypath[i].y * SCALE_FACTOR); + } + co.AddPath(path, jt, et); + + Paths paths; + co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. + + // Have to scale points down now. + Vector<Vector<Point2>> polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector<Vector2> polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} + +Vector<Point2i> Geometry2D::pack_rects(const Vector<Size2i> &p_sizes, const Size2i &p_atlas_size) { + Vector<stbrp_node> nodes; + nodes.resize(p_atlas_size.width); + + stbrp_context context; + stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); + + Vector<stbrp_rect> rects; + rects.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + rects.write[i].id = 0; + rects.write[i].w = p_sizes[i].width; + rects.write[i].h = p_sizes[i].height; + rects.write[i].x = 0; + rects.write[i].y = 0; + rects.write[i].was_packed = 0; + } + + int res = stbrp_pack_rects(&context, rects.ptrw(), rects.size()); + if (res == 0) { //pack failed + return Vector<Point2i>(); + } + + Vector<Point2i> ret; + ret.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + Point2i r(rects[i].x, rects[i].y); + ret.write[i] = r; + } + + return ret; +} + +Vector<Vector3i> Geometry2D::partial_pack_rects(const Vector<Vector2i> &p_sizes, const Size2i &p_atlas_size) { + Vector<stbrp_node> nodes; + nodes.resize(p_atlas_size.width); + zeromem(nodes.ptrw(), sizeof(stbrp_node) * nodes.size()); + + stbrp_context context; + stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); + + Vector<stbrp_rect> rects; + rects.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + rects.write[i].id = i; + rects.write[i].w = p_sizes[i].width; + rects.write[i].h = p_sizes[i].height; + rects.write[i].x = 0; + rects.write[i].y = 0; + rects.write[i].was_packed = 0; + } + + stbrp_pack_rects(&context, rects.ptrw(), rects.size()); + + Vector<Vector3i> ret; + ret.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + ret.write[rects[i].id] = Vector3i(rects[i].x, rects[i].y, rects[i].was_packed != 0 ? 1 : 0); + } + + return ret; +} diff --git a/core/math/geometry_2d.h b/core/math/geometry_2d.h new file mode 100644 index 0000000000..cfd7abfacb --- /dev/null +++ b/core/math/geometry_2d.h @@ -0,0 +1,398 @@ +/*************************************************************************/ +/* geometry_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 GEOMETRY_2D_H +#define GEOMETRY_2D_H + +#include "core/math/delaunay_2d.h" +#include "core/math/rect2.h" +#include "core/math/triangulate.h" +#include "core/object.h" +#include "core/vector.h" + +class Geometry2D { + Geometry2D(); + +public: + static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) { + Vector2 d1 = q1 - p1; // Direction vector of segment S1. + Vector2 d2 = q2 - p2; // Direction vector of segment S2. + Vector2 r = p1 - p2; + real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative. + real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative. + real_t f = d2.dot(r); + real_t s, t; + // Check if either or both segments degenerate into points. + if (a <= CMP_EPSILON && e <= CMP_EPSILON) { + // Both segments degenerate into points. + c1 = p1; + c2 = p2; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + if (a <= CMP_EPSILON) { + // First segment degenerates into a point. + s = 0.0; + t = f / e; // s = 0 => t = (b*s + f) / e = f / e + t = CLAMP(t, 0.0, 1.0); + } else { + real_t c = d1.dot(r); + if (e <= CMP_EPSILON) { + // Second segment degenerates into a point. + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a + } else { + // The general nondegenerate case starts here. + real_t b = d1.dot(d2); + real_t denom = a * e - b * b; // Always nonnegative. + // If segments not parallel, compute closest point on L1 to L2 and + // clamp to segment S1. Else pick arbitrary s (here 0). + if (denom != 0.0) { + s = CLAMP((b * f - c * e) / denom, 0.0, 1.0); + } else { + s = 0.0; + } + // Compute point on L2 closest to S1(s) using + // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e + t = (b * s + f) / e; + + //If t in [0,1] done. Else clamp t, recompute s for the new value + // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a + // and clamp s to [0, 1]. + if (t < 0.0) { + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); + } else if (t > 1.0) { + t = 1.0; + s = CLAMP((b - c) / a, 0.0, 1.0); + } + } + } + c1 = p1 + d1 * s; + c2 = p2 + d2 * t; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + + static Vector2 get_closest_point_to_segment(const Vector2 &p_point, const Vector2 *p_segment) { + Vector2 p = p_point - p_segment[0]; + Vector2 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + if (d <= 0.0) { + return p_segment[0]; // Before first point. + } else if (d >= 1.0) { + return p_segment[1]; // After first point. + } else { + return p_segment[0] + n * d; // Inside. + } + } + + static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { + Vector2 an = a - s; + Vector2 bn = b - s; + Vector2 cn = c - s; + + bool orientation = an.cross(bn) > 0; + + if ((bn.cross(cn) > 0) != orientation) { + return false; + } + + return (cn.cross(an) > 0) == orientation; + } + + static Vector2 get_closest_point_to_segment_uncapped(const Vector2 &p_point, const Vector2 *p_segment) { + Vector2 p = p_point - p_segment[0]; + Vector2 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + return p_segment[0] + n * d; // Inside. + } + + static bool line_intersects_line(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) { + // See http://paulbourke.net/geometry/pointlineplane/ + + const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y; + if (Math::is_zero_approx(denom)) { // Parallel? + return false; + } + + const Vector2 v = p_from_a - p_from_b; + const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom; + r_result = p_from_a + t * p_dir_a; + return true; + } + + static bool segment_intersects_segment(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) { + Vector2 B = p_to_a - p_from_a; + Vector2 C = p_from_b - p_from_a; + Vector2 D = p_to_b - p_from_a; + + real_t ABlen = B.dot(B); + if (ABlen <= 0) { + return false; + } + Vector2 Bn = B / ABlen; + C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y); + D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y); + + if ((C.y < 0 && D.y < 0) || (C.y >= 0 && D.y >= 0)) { + return false; + } + + real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y); + + // Fail if segment C-D crosses line A-B outside of segment A-B. + if (ABpos < 0 || ABpos > 1.0) { + return false; + } + + // (4) Apply the discovered position to line A-B in the original coordinate system. + if (r_result) { + *r_result = p_from_a + B * ABpos; + } + + return true; + } + + static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) { + return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius; + } + + static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) { + Vector2 line_vec = p_to - p_from; + Vector2 vec_to_line = p_from - p_circle_pos; + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * vec_to_line.dot(line_vec); + c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so it definitely won't be t in the range 0 to 1. + if (sqrtterm < 0) { + return -1; + } + + // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) + // then the following can be skipped and we can just return the equivalent of res1. + sqrtterm = Math::sqrt(sqrtterm); + real_t res1 = (-b - sqrtterm) / (2 * a); + real_t res2 = (-b + sqrtterm) / (2 * a); + + if (res1 >= 0 && res1 <= 1) { + return res1; + } + if (res2 >= 0 && res2 <= 1) { + return res2; + } + return -1; + } + + enum PolyBooleanOperation { + OPERATION_UNION, + OPERATION_DIFFERENCE, + OPERATION_INTERSECTION, + OPERATION_XOR + }; + enum PolyJoinType { + JOIN_SQUARE, + JOIN_ROUND, + JOIN_MITER + }; + enum PolyEndType { + END_POLYGON, + END_JOINED, + END_BUTT, + END_SQUARE, + END_ROUND + }; + + static Vector<Vector<Point2>> merge_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2>> clip_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2>> intersect_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2>> exclude_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2>> clip_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true); + } + + static Vector<Vector<Point2>> intersect_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true); + } + + static Vector<Vector<Point2>> offset_polygon(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type) { + return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON); + } + + static Vector<Vector<Point2>> offset_polyline(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector<Vector<Point2>>(), "Attempt to offset a polyline like a polygon (use offset_polygon instead)."); + + return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); + } + + static Vector<int> triangulate_delaunay(const Vector<Vector2> &p_points) { + Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points); + Vector<int> triangles; + + for (int i = 0; i < tr.size(); i++) { + triangles.push_back(tr[i].points[0]); + triangles.push_back(tr[i].points[1]); + triangles.push_back(tr[i].points[2]); + } + return triangles; + } + + static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) { + Vector<int> triangles; + if (!Triangulate::triangulate(p_polygon, triangles)) { + return Vector<int>(); //fail + } + return triangles; + } + + static bool is_polygon_clockwise(const Vector<Vector2> &p_polygon) { + int c = p_polygon.size(); + if (c < 3) { + return false; + } + const Vector2 *p = p_polygon.ptr(); + real_t sum = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + sum += (v2.x - v1.x) * (v2.y + v1.y); + } + + return sum > 0.0f; + } + + // Alternate implementation that should be faster. + static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) { + int c = p_polygon.size(); + if (c < 3) { + return false; + } + const Vector2 *p = p_polygon.ptr(); + Vector2 further_away(-1e20, -1e20); + Vector2 further_away_opposite(1e20, 1e20); + + for (int i = 0; i < c; i++) { + further_away.x = MAX(p[i].x, further_away.x); + further_away.y = MAX(p[i].y, further_away.y); + further_away_opposite.x = MIN(p[i].x, further_away_opposite.x); + further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); + } + + // Make point outside that won't intersect with points in segment from p_point. + further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); + + int intersections = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + if (segment_intersects_segment(v1, v2, p_point, further_away, nullptr)) { + intersections++; + } + } + + return (intersections & 1); + } + + static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) { + return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x); + } + + // Returns a list of points on the convex hull in counter-clockwise order. + // Note: the last point in the returned list is the same as the first one. + static Vector<Point2> convex_hull(Vector<Point2> P) { + int n = P.size(), k = 0; + Vector<Point2> H; + H.resize(2 * n); + + // Sort points lexicographically. + P.sort(); + + // Build lower hull. + for (int i = 0; i < n; ++i) { + while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { + k--; + } + H.write[k++] = P[i]; + } + + // Build upper hull. + for (int i = n - 2, t = k + 1; i >= 0; i--) { + while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { + k--; + } + H.write[k++] = P[i]; + } + + H.resize(k); + return H; + } + static Vector<Vector<Vector2>> decompose_polygon_in_convex(Vector<Point2> polygon); + + static void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size); + static Vector<Point2i> pack_rects(const Vector<Size2i> &p_sizes, const Size2i &p_atlas_size); + static Vector<Vector3i> partial_pack_rects(const Vector<Vector2i> &p_sizes, const Size2i &p_atlas_size); + +private: + static Vector<Vector<Point2>> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open = false); + static Vector<Vector<Point2>> _polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); +}; + +#endif // GEOMETRY_2D_H diff --git a/core/math/geometry.cpp b/core/math/geometry_3d.cpp index fa96fc4535..7807ab19a7 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry_3d.cpp @@ -1,5 +1,5 @@ /*************************************************************************/ -/* geometry.cpp */ +/* geometry_3d.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,36 +28,18 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "geometry.h" +#include "geometry_3d.h" #include "core/print_string.h" + #include "thirdparty/misc/clipper.hpp" #include "thirdparty/misc/triangulator.h" -#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. - -// This implementation is very inefficient, commenting unless bugs happen. See the other one. -/* -bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) { - - Vector<int> indices = Geometry::triangulate_polygon(p_polygon); - for (int j = 0; j + 3 <= indices.size(); j += 3) { - int i1 = indices[j], i2 = indices[j + 1], i3 = indices[j + 2]; - if (Geometry::is_point_in_triangle(p_point, p_polygon[i1], p_polygon[i2], p_polygon[i3])) - return true; - } - return false; -} -*/ - -void Geometry::MeshData::optimize_vertices() { - +void Geometry3D::MeshData::optimize_vertices() { Map<int, int> vtx_remap; for (int i = 0; i < faces.size(); i++) { - for (int j = 0; j < faces[i].indices.size(); j++) { - int idx = faces[i].indices[j]; if (!vtx_remap.has(idx)) { int ni = vtx_remap.size(); @@ -69,7 +51,6 @@ void Geometry::MeshData::optimize_vertices() { } for (int i = 0; i < edges.size(); i++) { - int a = edges[i].a; int b = edges[i].b; @@ -90,36 +71,28 @@ void Geometry::MeshData::optimize_vertices() { new_vertices.resize(vtx_remap.size()); for (int i = 0; i < vertices.size(); i++) { - - if (vtx_remap.has(i)) + if (vtx_remap.has(i)) { new_vertices.write[vtx_remap[i]] = vertices[i]; + } } vertices = new_vertices; } struct _FaceClassify { - struct _Link { - - int face; - int edge; + int face = -1; + int edge = -1; void clear() { face = -1; edge = -1; } - _Link() { - face = -1; - edge = -1; - } + _Link() {} }; - bool valid; - int group; + bool valid = false; + int group = -1; _Link links[3]; Face3 face; - _FaceClassify() { - group = -1; - valid = false; - }; + _FaceClassify() {} }; static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { @@ -129,42 +102,36 @@ static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { bool error = false; for (int i = 0; i < len; i++) { - for (int j = 0; j < 3; j++) { - p_faces[i].links[j].clear(); } } for (int i = 0; i < len; i++) { - - if (p_faces[i].group != p_group) + if (p_faces[i].group != p_group) { continue; + } for (int j = i + 1; j < len; j++) { - - if (p_faces[j].group != p_group) + if (p_faces[j].group != p_group) { continue; + } for (int k = 0; k < 3; k++) { - Vector3 vi1 = p_faces[i].face.vertex[k]; Vector3 vi2 = p_faces[i].face.vertex[(k + 1) % 3]; for (int l = 0; l < 3; l++) { - Vector3 vj2 = p_faces[j].face.vertex[l]; Vector3 vj1 = p_faces[j].face.vertex[(l + 1) % 3]; if (vi1.distance_to(vj1) < 0.00001 && vi2.distance_to(vj2) < 0.00001) { if (p_faces[i].links[k].face != -1) { - ERR_PRINT("already linked\n"); error = true; break; } if (p_faces[j].links[l].face != -1) { - ERR_PRINT("already linked\n"); error = true; break; @@ -176,37 +143,38 @@ static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { p_faces[j].links[l].edge = k; } } - if (error) + if (error) { break; + } } - if (error) + if (error) { break; + } } - if (error) + if (error) { break; + } } for (int i = 0; i < len; i++) { - p_faces[i].valid = true; for (int j = 0; j < 3; j++) { - - if (p_faces[i].links[j].face == -1) + if (p_faces[i].links[j].face == -1) { p_faces[i].valid = false; + } } } return error; } static bool _group_face(_FaceClassify *p_faces, int len, int p_index, int p_group) { - - if (p_faces[p_index].group >= 0) + if (p_faces[p_index].group >= 0) { return false; + } p_faces[p_index].group = p_group; for (int i = 0; i < 3; i++) { - ERR_FAIL_INDEX_V(p_faces[p_index].links[i].face, len, true); _group_face(p_faces, len, p_faces[p_index].links[i].face, p_group); } @@ -214,8 +182,7 @@ static bool _group_face(_FaceClassify *p_faces, int len, int p_index, int p_grou return true; } -Vector<Vector<Face3>> Geometry::separate_objects(Vector<Face3> p_array) { - +Vector<Vector<Face3>> Geometry3D::separate_objects(Vector<Face3> p_array) { Vector<Vector<Face3>> objects; int len = p_array.size(); @@ -229,7 +196,6 @@ Vector<Vector<Face3>> Geometry::separate_objects(Vector<Face3> p_array) { _FaceClassify *_fcptr = fc.ptrw(); for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; } @@ -241,9 +207,9 @@ Vector<Vector<Face3>> Geometry::separate_objects(Vector<Face3> p_array) { int group = 0; for (int i = 0; i < len; i++) { - - if (!_fcptr[i].valid) + if (!_fcptr[i].valid) { continue; + } if (_group_face(_fcptr, len, i, group)) { group++; } @@ -252,20 +218,18 @@ Vector<Vector<Face3>> Geometry::separate_objects(Vector<Face3> p_array) { // Group connected faces in separate objects. for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; } if (group >= 0) { - objects.resize(group); Vector<Face3> *group_faces = objects.ptrw(); for (int i = 0; i < len; i++) { - if (!_fcptr[i].valid) + if (!_fcptr[i].valid) { continue; + } if (_fcptr[i].group >= 0 && _fcptr[i].group < group) { - group_faces[_fcptr[i].group].push_back(_fcptr[i].face); } } @@ -302,16 +266,15 @@ enum _CellFlags { }; static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, const Vector3 &voxelsize, const Face3 &p_face) { - AABB aabb(Vector3(x, y, z), Vector3(len_x, len_y, len_z)); aabb.position = aabb.position * voxelsize; aabb.size = aabb.size * voxelsize; - if (!p_face.intersects_aabb(aabb)) + if (!p_face.intersects_aabb(aabb)) { return; + } if (len_x == 1 && len_y == 1 && len_z == 1) { - p_cell_status[x][y][z] = _CELL_SOLID; return; } @@ -340,15 +303,12 @@ static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int int new_len_z; for (int i = 0; i < div_x; i++) { - _SPLIT(i, div_x, x, len_x, new_x, new_len_x); for (int j = 0; j < div_y; j++) { - _SPLIT(j, div_y, y, len_y, new_y, new_len_y); for (int k = 0; k < div_z; k++) { - _SPLIT(k, div_z, z, len_z, new_z, new_len_z); _plot_face(p_cell_status, new_x, new_y, new_z, new_len_x, new_len_y, new_len_z, voxelsize, p_face); @@ -358,14 +318,13 @@ static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int } static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) { - - if (p_cell_status[x][y][z] & 3) + if (p_cell_status[x][y][z] & 3) { return; // Nothing to do, already used and/or visited. + } p_cell_status[x][y][z] = _CELL_PREV_FIRST; while (true) { - uint8_t &c = p_cell_status[x][y][z]; if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) { @@ -419,9 +378,7 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, uint8_t prev = 0; switch (c & _CELL_STEP_MASK) { - case _CELL_STEP_Y_POS: { - next_y++; prev = _CELL_PREV_Y_NEG; } break; @@ -445,18 +402,23 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, next_z--; prev = _CELL_PREV_Z_POS; } break; - default: ERR_FAIL(); + default: + ERR_FAIL(); } - if (next_x < 0 || next_x >= len_x) + if (next_x < 0 || next_x >= len_x) { continue; - if (next_y < 0 || next_y >= len_y) + } + if (next_y < 0 || next_y >= len_y) { continue; - if (next_z < 0 || next_z >= len_z) + } + if (next_z < 0 || next_z >= len_z) { continue; + } - if (p_cell_status[next_x][next_y][next_z] & 3) + if (p_cell_status[next_x][next_y][next_z] & 3) { continue; + } x = next_x; y = next_y; @@ -466,13 +428,13 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, } static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, Vector<Face3> &p_faces) { - ERR_FAIL_INDEX(x, len_x); ERR_FAIL_INDEX(y, len_y); ERR_FAIL_INDEX(z, len_z); - if (p_cell_status[x][y][z] & _CELL_EXTERIOR) + if (p_cell_status[x][y][z] & _CELL_EXTERIOR) { return; + } #define vert(m_idx) Vector3(((m_idx)&4) >> 2, ((m_idx)&2) >> 1, (m_idx)&1) @@ -487,7 +449,6 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i }; for (int i = 0; i < 6; i++) { - Vector3 face_points[4]; int disp_x = x + ((i % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); int disp_y = y + (((i - 1) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); @@ -495,21 +456,27 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i bool plot = false; - if (disp_x < 0 || disp_x >= len_x) + if (disp_x < 0 || disp_x >= len_x) { plot = true; - if (disp_y < 0 || disp_y >= len_y) + } + if (disp_y < 0 || disp_y >= len_y) { plot = true; - if (disp_z < 0 || disp_z >= len_z) + } + if (disp_z < 0 || disp_z >= len_z) { plot = true; + } - if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) + if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) { plot = true; + } - if (!plot) + if (!plot) { continue; + } - for (int j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { face_points[j] = vert(indices[i][j]) + Vector3(x, y, z); + } p_faces.push_back( Face3( @@ -525,8 +492,7 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i } } -Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { - +Vector<Face3> Geometry3D::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { #define _MIN_SIZE 1.0 #define _MAX_LENGTH 20 @@ -536,12 +502,9 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { AABB global_aabb; for (int i = 0; i < face_count; i++) { - if (i == 0) { - global_aabb = faces[i].get_aabb(); } else { - global_aabb.merge_with(faces[i].get_aabb()); } } @@ -551,20 +514,23 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { // Determine amount of cells in grid axis. int div_x, div_y, div_z; - if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) + if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) { div_x = (int)(global_aabb.size.x / _MIN_SIZE) + 1; - else + } else { div_x = _MAX_LENGTH; + } - if (global_aabb.size.y / _MIN_SIZE < _MAX_LENGTH) + if (global_aabb.size.y / _MIN_SIZE < _MAX_LENGTH) { div_y = (int)(global_aabb.size.y / _MIN_SIZE) + 1; - else + } else { div_y = _MAX_LENGTH; + } - if (global_aabb.size.z / _MIN_SIZE < _MAX_LENGTH) + if (global_aabb.size.z / _MIN_SIZE < _MAX_LENGTH) { div_z = (int)(global_aabb.size.z / _MIN_SIZE) + 1; - else + } else { div_z = _MAX_LENGTH; + } Vector3 voxelsize = global_aabb.size; voxelsize.x /= div_x; @@ -575,15 +541,12 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { uint8_t ***cell_status = memnew_arr(uint8_t **, div_x); for (int i = 0; i < div_x; i++) { - cell_status[i] = memnew_arr(uint8_t *, div_y); for (int j = 0; j < div_y; j++) { - cell_status[i][j] = memnew_arr(uint8_t, div_z); for (int k = 0; k < div_z; k++) { - cell_status[i][j][k] = 0; } } @@ -592,10 +555,8 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { // Plot faces into cells. for (int i = 0; i < face_count; i++) { - Face3 f = faces[i]; for (int j = 0; j < 3; j++) { - f.vertex[j] -= global_aabb.position; } _plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f); @@ -604,27 +565,21 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { // Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking. for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - _mark_outside(cell_status, i, j, 0, div_x, div_y, div_z); _mark_outside(cell_status, i, j, div_z - 1, div_x, div_y, div_z); } } for (int i = 0; i < div_z; i++) { - for (int j = 0; j < div_y; j++) { - _mark_outside(cell_status, 0, j, i, div_x, div_y, div_z); _mark_outside(cell_status, div_x - 1, j, i, div_x, div_y, div_z); } } for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_z; j++) { - _mark_outside(cell_status, i, 0, j, div_x, div_y, div_z); _mark_outside(cell_status, i, div_y - 1, j, div_x, div_y, div_z); } @@ -635,11 +590,8 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { Vector<Face3> wrapped_faces; for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - for (int k = 0; k < div_z; k++) { - _build_faces(cell_status, i, j, k, div_x, div_y, div_z, wrapped_faces); } } @@ -651,9 +603,7 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { Face3 *wrapped_faces_ptr = wrapped_faces.ptrw(); for (int i = 0; i < wrapped_faces_count; i++) { - for (int j = 0; j < 3; j++) { - Vector3 &v = wrapped_faces_ptr[i].vertex[j]; v = v * voxelsize; v += global_aabb.position; @@ -663,9 +613,7 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { // clean up grid for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - memdelete_arr(cell_status[i][j]); } @@ -673,61 +621,27 @@ Vector<Face3> Geometry::wrap_geometry(Vector<Face3> p_array, real_t *p_error) { } memdelete_arr(cell_status); - if (p_error) + if (p_error) { *p_error = voxelsize.length(); - - return wrapped_faces; -} - -Vector<Vector<Vector2>> Geometry::decompose_polygon_in_convex(Vector<Point2> polygon) { - Vector<Vector<Vector2>> decomp; - List<TriangulatorPoly> in_poly, out_poly; - - TriangulatorPoly inp; - inp.Init(polygon.size()); - for (int i = 0; i < polygon.size(); i++) { - inp.GetPoint(i) = polygon[i]; - } - inp.SetOrientation(TRIANGULATOR_CCW); - in_poly.push_back(inp); - TriangulatorPartition tpart; - if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed. - ERR_PRINT("Convex decomposing failed!"); - return decomp; - } - - decomp.resize(out_poly.size()); - int idx = 0; - for (List<TriangulatorPoly>::Element *I = out_poly.front(); I; I = I->next()) { - TriangulatorPoly &tp = I->get(); - - decomp.write[idx].resize(tp.GetNumPoints()); - - for (int64_t i = 0; i < tp.GetNumPoints(); i++) { - decomp.write[idx].write[i] = tp.GetPoint(i); - } - - idx++; } - return decomp; + return wrapped_faces; } -Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { - +Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes) { MeshData mesh; #define SUBPLANE_SIZE 1024.0 real_t subplane_size = 1024.0; // Should compute this from the actual plane. for (int i = 0; i < p_planes.size(); i++) { - Plane p = p_planes[i]; Vector3 ref = Vector3(0.0, 1.0, 0.0); - if (ABS(p.normal.dot(ref)) > 0.95) + if (ABS(p.normal.dot(ref)) > 0.95) { ref = Vector3(0.0, 0.0, 1.0); // Change axis. + } Vector3 right = p.normal.cross(ref).normalized(); Vector3 up = p.normal.cross(right).normalized(); @@ -742,21 +656,22 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { vertices.push_back(center + up * subplane_size + right * subplane_size); for (int j = 0; j < p_planes.size(); j++) { - - if (j == i) + if (j == i) { continue; + } Vector<Vector3> new_vertices; Plane clip = p_planes[j]; - if (clip.normal.dot(p.normal) > 0.95) + if (clip.normal.dot(p.normal) > 0.95) { continue; + } - if (vertices.size() < 3) + if (vertices.size() < 3) { break; + } for (int k = 0; k < vertices.size(); k++) { - int k_n = (k + 1) % vertices.size(); Vector3 edge0_A = vertices[k]; @@ -772,13 +687,13 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { // Check for different sides and non coplanar. if ((dist0 * dist1) < 0) { - // Calculate intersection. Vector3 rel = edge1_A - edge0_A; real_t den = clip.normal.dot(rel); - if (Math::is_zero_approx(den)) + if (Math::is_zero_approx(den)) { continue; // Point too short. + } real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; Vector3 inters = edge0_A + rel * dist; @@ -789,8 +704,9 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { vertices = new_vertices; } - if (vertices.size() < 3) + if (vertices.size() < 3) { continue; + } // Result is a clockwise face. @@ -798,19 +714,15 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { // Add face indices. for (int j = 0; j < vertices.size(); j++) { - int idx = -1; for (int k = 0; k < mesh.vertices.size(); k++) { - if (mesh.vertices[k].distance_to(vertices[j]) < 0.001) { - idx = k; break; } } if (idx == -1) { - idx = mesh.vertices.size(); mesh.vertices.push_back(vertices[j]); } @@ -823,13 +735,11 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { // Add edge. for (int j = 0; j < face.indices.size(); j++) { - int a = face.indices[j]; int b = face.indices[(j + 1) % face.indices.size()]; bool found = false; for (int k = 0; k < mesh.edges.size(); k++) { - if (mesh.edges[k].a == a && mesh.edges[k].b == b) { found = true; break; @@ -840,8 +750,9 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { } } - if (found) + if (found) { continue; + } MeshData::Edge edge; edge.a = a; edge.b = b; @@ -852,8 +763,7 @@ Geometry::MeshData Geometry::build_convex_mesh(const Vector<Plane> &p_planes) { return mesh; } -Vector<Plane> Geometry::build_box_planes(const Vector3 &p_extents) { - +Vector<Plane> Geometry3D::build_box_planes(const Vector3 &p_extents) { Vector<Plane> planes; planes.push_back(Plane(Vector3(1, 0, 0), p_extents.x)); @@ -866,12 +776,10 @@ Vector<Plane> Geometry::build_box_planes(const Vector3 &p_extents) { return planes; } -Vector<Plane> Geometry::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { - +Vector<Plane> Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { Vector<Plane> planes; for (int i = 0; i < p_sides; i++) { - Vector3 normal; normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); @@ -888,8 +796,7 @@ Vector<Plane> Geometry::build_cylinder_planes(real_t p_radius, real_t p_height, return planes; } -Vector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) { - +Vector<Plane> Geometry3D::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) { Vector<Plane> planes; Vector3 axis; @@ -901,7 +808,6 @@ Vector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int p_l axis_neg[p_axis] = -1.0; for (int i = 0; i < p_lons; i++) { - Vector3 normal; normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_lons); normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_lons); @@ -909,7 +815,6 @@ Vector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int p_l planes.push_back(Plane(normal, p_radius)); for (int j = 1; j <= p_lats; j++) { - // FIXME: This is stupid. Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); Vector3 pos = angle * p_radius; @@ -921,8 +826,7 @@ Vector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int p_l return planes; } -Vector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { - +Vector<Plane> Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { Vector<Plane> planes; Vector3 axis; @@ -934,7 +838,6 @@ Vector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_height, i axis_neg[p_axis] = -1.0; for (int i = 0; i < p_sides; i++) { - Vector3 normal; normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); @@ -942,7 +845,6 @@ Vector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_height, i planes.push_back(Plane(normal, p_radius)); for (int j = 1; j <= p_lats; j++) { - Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); Vector3 pos = axis * p_height * 0.5 + angle * p_radius; planes.push_back(Plane(pos, angle)); @@ -953,267 +855,161 @@ Vector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_height, i return planes; } -struct _AtlasWorkRect { - - Size2i s; - Point2i p; - int idx; - _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }; -}; - -struct _AtlasWorkRectResult { - - Vector<_AtlasWorkRect> result; - int max_w; - int max_h; -}; - -void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size) { - - // Super simple, almost brute force scanline stacking fitter. - // It's pretty basic for now, but it tries to make sure that the aspect ratio of the - // resulting atlas is somehow square. This is necessary because video cards have limits. - // On texture size (usually 2048 or 4096), so the more square a texture, the more chances. - // It will work in every hardware. - // For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a - // 256x8192 atlas (won't work anywhere). - - ERR_FAIL_COND(p_rects.size() == 0); - - Vector<_AtlasWorkRect> wrects; - wrects.resize(p_rects.size()); - for (int i = 0; i < p_rects.size(); i++) { - wrects.write[i].s = p_rects[i]; - wrects.write[i].idx = i; - } - wrects.sort(); - int widest = wrects[0].s.width; - - Vector<_AtlasWorkRectResult> results; - - for (int i = 0; i <= 12; i++) { - - int w = 1 << i; - int max_h = 0; - int max_w = 0; - if (w < widest) - continue; - - Vector<int> hmax; - hmax.resize(w); - for (int j = 0; j < w; j++) - hmax.write[j] = 0; - - // Place them. - int ofs = 0; - int limit_h = 0; - for (int j = 0; j < wrects.size(); j++) { - - if (ofs + wrects[j].s.width > w) { - - ofs = 0; - } +Vector<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { + Vector<Vector3> points; - int from_y = 0; - for (int k = 0; k < wrects[j].s.width; k++) { + // Iterate through every unique combination of any three planes. + for (int i = p_plane_count - 1; i >= 0; i--) { + for (int j = i - 1; j >= 0; j--) { + for (int k = j - 1; k >= 0; k--) { + // Find the point where these planes all cross over (if they + // do at all). + Vector3 convex_shape_point; + if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) { + // See if any *other* plane excludes this point because it's + // on the wrong side. + bool excluded = false; + for (int n = 0; n < p_plane_count; n++) { + if (n != i && n != j && n != k) { + real_t dp = p_planes[n].normal.dot(convex_shape_point); + if (dp - p_planes[n].d > CMP_EPSILON) { + excluded = true; + break; + } + } + } - if (hmax[ofs + k] > from_y) - from_y = hmax[ofs + k]; + // Only add the point if it passed all tests. + if (!excluded) { + points.push_back(convex_shape_point); + } + } } + } + } - wrects.write[j].p.x = ofs; - wrects.write[j].p.y = from_y; - int end_h = from_y + wrects[j].s.height; - int end_w = ofs + wrects[j].s.width; - if (ofs == 0) - limit_h = end_h; - - for (int k = 0; k < wrects[j].s.width; k++) { - - hmax.write[ofs + k] = end_h; - } + return points; +} - if (end_h > max_h) - max_h = end_h; +#define square(m_s) ((m_s) * (m_s)) +#define INF 1e20 - if (end_w > max_w) - max_w = end_w; +/* dt of 1d function using squared distance */ +static void edt(float *f, int stride, int n) { + float *d = (float *)alloca(sizeof(float) * n + sizeof(int) * n + sizeof(float) * (n + 1)); + int *v = (int *)&(d[n]); + float *z = (float *)&v[n]; - if (ofs == 0 || end_h > limit_h) // While h limit not reached, keep stacking. - ofs += wrects[j].s.width; + int k = 0; + v[0] = 0; + z[0] = -INF; + z[1] = +INF; + for (int q = 1; q <= n - 1; q++) { + float s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); + while (s <= z[k]) { + k--; + s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); } + k++; + v[k] = q; - _AtlasWorkRectResult result; - result.result = wrects; - result.max_h = max_h; - result.max_w = max_w; - results.push_back(result); + z[k] = s; + z[k + 1] = +INF; } - // Find the result with the best aspect ratio. - - int best = -1; - real_t best_aspect = 1e20; - - for (int i = 0; i < results.size(); i++) { - - real_t h = next_power_of_2(results[i].max_h); - real_t w = next_power_of_2(results[i].max_w); - real_t aspect = h > w ? h / w : w / h; - if (aspect < best_aspect) { - best = i; - best_aspect = aspect; + k = 0; + for (int q = 0; q <= n - 1; q++) { + while (z[k + 1] < q) { + k++; } + d[q] = square(q - v[k]) + f[v[k] * stride]; } - r_result.resize(p_rects.size()); - - for (int i = 0; i < p_rects.size(); i++) { - - r_result.write[results[best].result[i].idx] = results[best].result[i].p; + for (int i = 0; i < n; i++) { + f[i * stride] = d[i]; } - - r_size = Size2(results[best].max_w, results[best].max_h); } -Vector<Vector<Point2>> Geometry::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open) { - - using namespace ClipperLib; - - ClipType op = ctUnion; - - switch (p_op) { - case OPERATION_UNION: op = ctUnion; break; - case OPERATION_DIFFERENCE: op = ctDifference; break; - case OPERATION_INTERSECTION: op = ctIntersection; break; - case OPERATION_XOR: op = ctXor; break; - } - Path path_a, path_b; +#undef square - // Need to scale points (Clipper's requirement for robust computation). - for (int i = 0; i != p_polypath_a.size(); ++i) { - path_a << IntPoint(p_polypath_a[i].x * SCALE_FACTOR, p_polypath_a[i].y * SCALE_FACTOR); - } - for (int i = 0; i != p_polypath_b.size(); ++i) { - path_b << IntPoint(p_polypath_b[i].x * SCALE_FACTOR, p_polypath_b[i].y * SCALE_FACTOR); - } - Clipper clp; - clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. - clp.AddPath(path_b, ptClip, true); // Polylines cannot be set as clip. +Vector<uint32_t> Geometry3D::generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative) { + uint32_t float_count = p_size.x * p_size.y * p_size.z; - Paths paths; + ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector<uint32_t>()); - if (is_a_open) { - PolyTree tree; // Needed to populate polylines. - clp.Execute(op, tree); - OpenPathsFromPolyTree(tree, paths); - } else { - clp.Execute(op, paths); // Works on closed polygons only. + float *work_memory = memnew_arr(float, float_count); + for (uint32_t i = 0; i < float_count; i++) { + work_memory[i] = INF; } - // Have to scale points down now. - Vector<Vector<Point2>> polypaths; - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector<Vector2> polypath; + uint32_t y_mult = p_size.x; + uint32_t z_mult = y_mult * p_size.y; - const Path &scaled_path = paths[i]; - - for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { - polypath.push_back(Point2( - static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, - static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + //plot solid cells + { + const bool *voxr = p_voxels.ptr(); + for (uint32_t i = 0; i < float_count; i++) { + bool plot = voxr[i]; + if (p_negative) { + plot = !plot; + } + if (plot) { + work_memory[i] = 0; + } } - polypaths.push_back(polypath); } - return polypaths; -} -Vector<Vector<Point2>> Geometry::_polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + //process in each direction - using namespace ClipperLib; + //xy->z - JoinType jt = jtSquare; - - switch (p_join_type) { - case JOIN_SQUARE: jt = jtSquare; break; - case JOIN_ROUND: jt = jtRound; break; - case JOIN_MITER: jt = jtMiter; break; + for (int i = 0; i < p_size.x; i++) { + for (int j = 0; j < p_size.y; j++) { + edt(&work_memory[i + j * y_mult], z_mult, p_size.z); + } } - EndType et = etClosedPolygon; + //xz->y - switch (p_end_type) { - case END_POLYGON: et = etClosedPolygon; break; - case END_JOINED: et = etClosedLine; break; - case END_BUTT: et = etOpenButt; break; - case END_SQUARE: et = etOpenSquare; break; - case END_ROUND: et = etOpenRound; break; + for (int i = 0; i < p_size.x; i++) { + for (int j = 0; j < p_size.z; j++) { + edt(&work_memory[i + j * z_mult], y_mult, p_size.y); + } } - ClipperOffset co(2.0, 0.25 * SCALE_FACTOR); // Defaults from ClipperOffset. - Path path; - // Need to scale points (Clipper's requirement for robust computation). - for (int i = 0; i != p_polypath.size(); ++i) { - path << IntPoint(p_polypath[i].x * SCALE_FACTOR, p_polypath[i].y * SCALE_FACTOR); + //yz->x + for (int i = 0; i < p_size.y; i++) { + for (int j = 0; j < p_size.z; j++) { + edt(&work_memory[i * y_mult + j * z_mult], 1, p_size.x); + } } - co.AddPath(path, jt, et); - Paths paths; - co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. - - // Have to scale points down now. - Vector<Vector<Point2>> polypaths; - - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector<Vector2> polypath; - - const Path &scaled_path = paths[i]; - - for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { - polypath.push_back(Point2( - static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, - static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + Vector<uint32_t> ret; + ret.resize(float_count); + { + uint32_t *w = ret.ptrw(); + for (uint32_t i = 0; i < float_count; i++) { + w[i] = uint32_t(Math::sqrt(work_memory[i])); } - polypaths.push_back(polypath); } - return polypaths; -} - -Vector<Vector3> Geometry::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { - - Vector<Vector3> points; - // Iterate through every unique combination of any three planes. - for (int i = p_plane_count - 1; i >= 0; i--) { - for (int j = i - 1; j >= 0; j--) { - for (int k = j - 1; k >= 0; k--) { + memdelete_arr(work_memory); - // Find the point where these planes all cross over (if they - // do at all). - Vector3 convex_shape_point; - if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) { + return ret; +} - // See if any *other* plane excludes this point because it's - // on the wrong side. - bool excluded = false; - for (int n = 0; n < p_plane_count; n++) { - if (n != i && n != j && n != k) { - real_t dp = p_planes[n].normal.dot(convex_shape_point); - if (dp - p_planes[n].d > CMP_EPSILON) { - excluded = true; - break; - } - } - } +Vector<int8_t> Geometry3D::generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative) { + ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector<int8_t>()); + Vector<int8_t> sdf8; + int s = p_positive.size(); + sdf8.resize(s); - // Only add the point if it passed all tests. - if (!excluded) { - points.push_back(convex_shape_point); - } - } - } - } + const uint32_t *rpos = p_positive.ptr(); + const uint32_t *rneg = p_negative.ptr(); + int8_t *wsdf = sdf8.ptrw(); + for (int i = 0; i < s; i++) { + int32_t diff = int32_t(rpos[i]) - int32_t(rneg[i]); + wsdf[i] = CLAMP(diff, -128, 127); } - - return points; + return sdf8; } diff --git a/core/math/geometry.h b/core/math/geometry_3d.h index ea063a8a59..6bbf518141 100644 --- a/core/math/geometry.h +++ b/core/math/geometry_3d.h @@ -1,5 +1,5 @@ /*************************************************************************/ -/* geometry.h */ +/* geometry_3d.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,83 +28,18 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef GEOMETRY_H -#define GEOMETRY_H +#ifndef GEOMETRY_3D_H +#define GEOMETRY_3D_H -#include "core/math/delaunay.h" #include "core/math/face3.h" -#include "core/math/rect2.h" -#include "core/math/triangulate.h" -#include "core/math/vector3.h" #include "core/object.h" - -#include "core/print_string.h" #include "core/vector.h" -class Geometry { - Geometry(); +class Geometry3D { + Geometry3D(); public: - static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) { - - Vector2 d1 = q1 - p1; // Direction vector of segment S1. - Vector2 d2 = q2 - p2; // Direction vector of segment S2. - Vector2 r = p1 - p2; - real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative. - real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative. - real_t f = d2.dot(r); - real_t s, t; - // Check if either or both segments degenerate into points. - if (a <= CMP_EPSILON && e <= CMP_EPSILON) { - // Both segments degenerate into points. - c1 = p1; - c2 = p2; - return Math::sqrt((c1 - c2).dot(c1 - c2)); - } - if (a <= CMP_EPSILON) { - // First segment degenerates into a point. - s = 0.0; - t = f / e; // s = 0 => t = (b*s + f) / e = f / e - t = CLAMP(t, 0.0, 1.0); - } else { - real_t c = d1.dot(r); - if (e <= CMP_EPSILON) { - // Second segment degenerates into a point. - t = 0.0; - s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a - } else { - // The general nondegenerate case starts here. - real_t b = d1.dot(d2); - real_t denom = a * e - b * b; // Always nonnegative. - // If segments not parallel, compute closest point on L1 to L2 and - // clamp to segment S1. Else pick arbitrary s (here 0). - if (denom != 0.0) { - s = CLAMP((b * f - c * e) / denom, 0.0, 1.0); - } else - s = 0.0; - // Compute point on L2 closest to S1(s) using - // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e - t = (b * s + f) / e; - - //If t in [0,1] done. Else clamp t, recompute s for the new value - // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a - // and clamp s to [0, 1]. - if (t < 0.0) { - t = 0.0; - s = CLAMP(-c / a, 0.0, 1.0); - } else if (t > 1.0) { - t = 1.0; - s = CLAMP((b - c) / a, 0.0, 1.0); - } - } - } - c1 = p1 + d1 * s; - c2 = p2 + d2 * t; - return Math::sqrt((c1 - c2).dot(c1 - c2)); - } - static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { - // Do the function 'd' as defined by pb. I think is is dot product of some sort. #define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) @@ -113,10 +48,18 @@ public: real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); // Clip the value between [0..1] constraining the solution to lie on the original curves. - if (mua < 0) mua = 0; - if (mub < 0) mub = 0; - if (mua > 1) mua = 1; - if (mub > 1) mub = 1; + if (mua < 0) { + mua = 0; + } + if (mub < 0) { + mub = 0; + } + if (mua > 1) { + mua = 1; + } + if (mub > 1) { + mub = 1; + } c1 = p1.lerp(p2, mua); c2 = q1.lerp(q2, mub); } @@ -157,22 +100,22 @@ public: if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. tN = 0.0; // Recompute sc for this edge. - if (-d < 0.0) + if (-d < 0.0) { sN = 0.0; - else if (-d > a) + } else if (-d > a) { sN = sD; - else { + } else { sN = -d; sD = a; } } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. tN = tD; // Recompute sc for this edge. - if ((-d + b) < 0.0) + if ((-d + b) < 0.0) { sN = 0; - else if ((-d + b) > a) + } else if ((-d + b) > a) { sN = sD; - else { + } else { sN = (-d + b); sD = a; } @@ -187,120 +130,134 @@ public: return dP.length(); // Return the closest distance. } - static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = 0) { + static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { Vector3 e1 = p_v1 - p_v0; Vector3 e2 = p_v2 - p_v0; Vector3 h = p_dir.cross(e2); real_t a = e1.dot(h); - if (Math::is_zero_approx(a)) // Parallel test. + if (Math::is_zero_approx(a)) { // Parallel test. return false; + } real_t f = 1.0 / a; Vector3 s = p_from - p_v0; real_t u = f * s.dot(h); - if (u < 0.0 || u > 1.0) + if (u < 0.0 || u > 1.0) { return false; + } Vector3 q = s.cross(e1); real_t v = f * p_dir.dot(q); - if (v < 0.0 || u + v > 1.0) + if (v < 0.0 || u + v > 1.0) { return false; + } // At this stage we can compute t to find out where // the intersection point is on the line. real_t t = f * e2.dot(q); if (t > 0.00001) { // ray intersection - if (r_res) + if (r_res) { *r_res = p_from + p_dir * t; + } return true; - } else // This means that there is a line intersection but not a ray intersection. + } else { // This means that there is a line intersection but not a ray intersection. return false; + } } - static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = 0) { - + static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { Vector3 rel = p_to - p_from; Vector3 e1 = p_v1 - p_v0; Vector3 e2 = p_v2 - p_v0; Vector3 h = rel.cross(e2); real_t a = e1.dot(h); - if (Math::is_zero_approx(a)) // Parallel test. + if (Math::is_zero_approx(a)) { // Parallel test. return false; + } real_t f = 1.0 / a; Vector3 s = p_from - p_v0; real_t u = f * s.dot(h); - if (u < 0.0 || u > 1.0) + if (u < 0.0 || u > 1.0) { return false; + } Vector3 q = s.cross(e1); real_t v = f * rel.dot(q); - if (v < 0.0 || u + v > 1.0) + if (v < 0.0 || u + v > 1.0) { return false; + } // At this stage we can compute t to find out where // the intersection point is on the line. real_t t = f * e2.dot(q); if (t > CMP_EPSILON && t <= 1.0) { // Ray intersection. - if (r_res) + if (r_res) { *r_res = p_from + rel * t; + } return true; - } else // This means that there is a line intersection but not a ray intersection. + } else { // This means that there is a line intersection but not a ray intersection. return false; + } } - static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = 0, Vector3 *r_norm = 0) { - + static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { Vector3 sphere_pos = p_sphere_pos - p_from; Vector3 rel = (p_to - p_from); real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) + if (rel_l < CMP_EPSILON) { return false; // Both points are the same. + } Vector3 normal = rel / rel_l; real_t sphere_d = normal.dot(sphere_pos); real_t ray_distance = sphere_pos.distance_to(normal * sphere_d); - if (ray_distance >= p_sphere_radius) + if (ray_distance >= p_sphere_radius) { return false; + } real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance; real_t inters_d = sphere_d; - if (inters_d2 >= CMP_EPSILON) + if (inters_d2 >= CMP_EPSILON) { inters_d -= Math::sqrt(inters_d2); + } // Check in segment. - if (inters_d < 0 || inters_d > rel_l) + if (inters_d < 0 || inters_d > rel_l) { return false; + } Vector3 result = p_from + normal * inters_d; - if (r_res) + if (r_res) { *r_res = result; - if (r_norm) + } + if (r_norm) { *r_norm = (result - p_sphere_pos).normalized(); + } return true; } - static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = 0, Vector3 *r_norm = 0) { - + static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { Vector3 rel = (p_to - p_from); real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) + if (rel_l < CMP_EPSILON) { return false; // Both points are the same. + } // First check if they are parallel. Vector3 normal = (rel / rel_l); @@ -317,13 +274,15 @@ public: real_t dist = z_dir.dot(p_from); - if (dist >= p_radius) + if (dist >= p_radius) { return false; // Too far away. + } // Convert to 2D. real_t w2 = p_radius * p_radius - dist * dist; - if (w2 < CMP_EPSILON) + if (w2 < CMP_EPSILON) { return false; // Avoid numerical error. + } Size2 size(Math::sqrt(w2), p_height * 0.5); Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized(); @@ -336,7 +295,6 @@ public: int axis = -1; for (int i = 0; i < 2; i++) { - real_t seg_from = from2D[i]; real_t seg_to = to2D[i]; real_t box_begin = -size[i]; @@ -344,17 +302,17 @@ public: real_t cmin, cmax; if (seg_from < seg_to) { - - if (seg_from > box_end || seg_to < box_begin) + if (seg_from > box_end || seg_to < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; } else { - - if (seg_to > box_end || seg_from < box_begin) + if (seg_to > box_end || seg_from < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; @@ -364,10 +322,12 @@ public: min = cmin; axis = i; } - if (cmax < max) + if (cmax < max) { max = cmax; - if (max < min) + } + if (max < min) { return false; + } } // Convert to 3D again. @@ -383,45 +343,47 @@ public: res_normal.normalize(); - if (r_res) + if (r_res) { *r_res = result; - if (r_norm) + } + if (r_norm) { *r_norm = res_normal; + } return true; } static bool segment_intersects_convex(const Vector3 &p_from, const Vector3 &p_to, const Plane *p_planes, int p_plane_count, Vector3 *p_res, Vector3 *p_norm) { - real_t min = -1e20, max = 1e20; Vector3 rel = p_to - p_from; real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) + if (rel_l < CMP_EPSILON) { return false; + } Vector3 dir = rel / rel_l; int min_index = -1; for (int i = 0; i < p_plane_count; i++) { - const Plane &p = p_planes[i]; real_t den = p.normal.dot(dir); - if (Math::abs(den) <= CMP_EPSILON) + if (Math::abs(den) <= CMP_EPSILON) { continue; // Ignore parallel plane. + } real_t dist = -p.distance_to(p_from) / den; if (den > 0) { // Backwards facing plane. - if (dist < max) + if (dist < max) { max = dist; + } } else { - // Front facing plane. if (dist > min) { min = dist; @@ -430,169 +392,89 @@ public: } } - if (max <= min || min < 0 || min > rel_l || min_index == -1) // Exit conditions. + if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions. return false; // No intersection. + } - if (p_res) + if (p_res) { *p_res = p_from + dir * min; - if (p_norm) + } + if (p_norm) { *p_norm = p_planes[min_index].normal; + } return true; } static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 *p_segment) { - Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) + if (l2 < 1e-20) { return p_segment[0]; // Both points are the same, just give any. + } real_t d = n.dot(p) / l2; - if (d <= 0.0) + if (d <= 0.0) { return p_segment[0]; // Before first point. - else if (d >= 1.0) + } else if (d >= 1.0) { return p_segment[1]; // After first point. - else + } else { return p_segment[0] + n * d; // Inside. + } } static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) { - Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) - return p_segment[0]; // Both points are the same, just give any. - - real_t d = n.dot(p) / l2; - - return p_segment[0] + n * d; // Inside. - } - - static Vector2 get_closest_point_to_segment_2d(const Vector2 &p_point, const Vector2 *p_segment) { - - Vector2 p = p_point - p_segment[0]; - Vector2 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) - return p_segment[0]; // Both points are the same, just give any. - - real_t d = n.dot(p) / l2; - - if (d <= 0.0) - return p_segment[0]; // Before first point. - else if (d >= 1.0) - return p_segment[1]; // After first point. - else - return p_segment[0] + n * d; // Inside. - } - - static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { - Vector2 an = a - s; - Vector2 bn = b - s; - Vector2 cn = c - s; - - bool orientation = an.cross(bn) > 0; - - if ((bn.cross(cn) > 0) != orientation) return false; - - return (cn.cross(an) > 0) == orientation; - } - - static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2 &p_point, const Vector2 *p_segment) { - - Vector2 p = p_point - p_segment[0]; - Vector2 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) + if (l2 < 1e-20) { return p_segment[0]; // Both points are the same, just give any. + } real_t d = n.dot(p) / l2; return p_segment[0] + n * d; // Inside. } - static bool line_intersects_line_2d(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) { - - // See http://paulbourke.net/geometry/pointlineplane/ - - const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y; - if (Math::is_zero_approx(denom)) { // Parallel? - return false; - } - - const Vector2 v = p_from_a - p_from_b; - const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom; - r_result = p_from_a + t * p_dir_a; - return true; - } - - static bool segment_intersects_segment_2d(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) { - - Vector2 B = p_to_a - p_from_a; - Vector2 C = p_from_b - p_from_a; - Vector2 D = p_to_b - p_from_a; - - real_t ABlen = B.dot(B); - if (ABlen <= 0) - return false; - Vector2 Bn = B / ABlen; - C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y); - D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y); - - if ((C.y < 0 && D.y < 0) || (C.y >= 0 && D.y >= 0)) - return false; - - real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y); - - // Fail if segment C-D crosses line A-B outside of segment A-B. - if (ABpos < 0 || ABpos > 1.0) - return false; - - // (4) Apply the discovered position to line A-B in the original coordinate system. - if (r_result) - *r_result = p_from_a + B * ABpos; - - return true; - } - static inline bool point_in_projected_triangle(const Vector3 &p_point, const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) { - Vector3 face_n = (p_v1 - p_v3).cross(p_v1 - p_v2); Vector3 n1 = (p_point - p_v3).cross(p_point - p_v2); - if (face_n.dot(n1) < 0) + if (face_n.dot(n1) < 0) { return false; + } Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point); - if (face_n.dot(n2) < 0) + if (face_n.dot(n2) < 0) { return false; + } Vector3 n3 = (p_v1 - p_point).cross(p_v1 - p_v2); - if (face_n.dot(n3) < 0) + if (face_n.dot(n3) < 0) { return false; + } return true; } static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) { - real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle[0]); - if (d > p_sphere_radius || d < -p_sphere_radius) // Not touching the plane of the face, return. + if (d > p_sphere_radius || d < -p_sphere_radius) { + // Not touching the plane of the face, return. return false; + } Vector3 contact = p_sphere_pos - (p_normal * d); /** 2nd) TEST INSIDE TRIANGLE **/ - if (Geometry::point_in_projected_triangle(contact, p_triangle[0], p_triangle[1], p_triangle[2])) { + if (Geometry3D::point_in_projected_triangle(contact, p_triangle[0], p_triangle[1], p_triangle[2])) { r_triangle_contact = contact; r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius; //printf("solved inside triangle\n"); @@ -604,7 +486,6 @@ public: const Vector3 verts[4] = { p_triangle[0], p_triangle[1], p_triangle[2], p_triangle[0] }; // for() friendly for (int i = 0; i < 3; i++) { - // Check edge cylinder. Vector3 n1 = verts[i] - verts[i + 1]; @@ -629,7 +510,6 @@ public: real_t sphere_at = n1.dot(n2); if (sphere_at >= 0 && sphere_at < n1.dot(n1)) { - r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2)); r_sphere_contact = p_sphere_pos - axis * p_sphere_radius; // Point inside here. @@ -639,7 +519,6 @@ public: real_t r2 = p_sphere_radius * p_sphere_radius; if (n2.length_squared() < r2) { - Vector3 n = (p_sphere_pos - verts[i + 1]).normalized(); r_triangle_contact = verts[i + 1]; @@ -661,51 +540,16 @@ public: return false; } - static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) { - - return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius; - } - - static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) { - - Vector2 line_vec = p_to - p_from; - Vector2 vec_to_line = p_from - p_circle_pos; - - // Create a quadratic formula of the form ax^2 + bx + c = 0 - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * vec_to_line.dot(line_vec); - c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius; - - // Solve for t. - real_t sqrtterm = b * b - 4 * a * c; - - // If the term we intend to square root is less than 0 then the answer won't be real, - // so it definitely won't be t in the range 0 to 1. - if (sqrtterm < 0) return -1; - - // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) - // then the following can be skipped and we can just return the equivalent of res1. - sqrtterm = Math::sqrt(sqrtterm); - real_t res1 = (-b - sqrtterm) / (2 * a); - real_t res2 = (-b + sqrtterm) / (2 * a); - - if (res1 >= 0 && res1 <= 1) return res1; - if (res2 >= 0 && res2 <= 1) return res2; - return -1; - } - static inline Vector<Vector3> clip_polygon(const Vector<Vector3> &polygon, const Plane &p_plane) { - enum LocationCache { LOC_INSIDE = 1, LOC_BOUNDARY = 0, LOC_OUTSIDE = -1 }; - if (polygon.size() == 0) + if (polygon.size() == 0) { return polygon; + } int *location_cache = (int *)alloca(sizeof(int) * polygon.size()); int inside_count = 0; @@ -727,11 +571,8 @@ public: } if (outside_count == 0) { - return polygon; // No changes. - } else if (inside_count == 0) { - return Vector<Vector3>(); // Empty. } @@ -771,141 +612,12 @@ public: return clipped; } - enum PolyBooleanOperation { - OPERATION_UNION, - OPERATION_DIFFERENCE, - OPERATION_INTERSECTION, - OPERATION_XOR - }; - enum PolyJoinType { - JOIN_SQUARE, - JOIN_ROUND, - JOIN_MITER - }; - enum PolyEndType { - END_POLYGON, - END_JOINED, - END_BUTT, - END_SQUARE, - END_ROUND - }; - - static Vector<Vector<Point2>> merge_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { - - return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b); - } - - static Vector<Vector<Point2>> clip_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { - - return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b); - } - - static Vector<Vector<Point2>> intersect_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { - - return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b); - } - - static Vector<Vector<Point2>> exclude_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { - - return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b); - } - - static Vector<Vector<Point2>> clip_polyline_with_polygon_2d(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { - - return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true); - } - - static Vector<Vector<Point2>> intersect_polyline_with_polygon_2d(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { - - return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true); - } - - static Vector<Vector<Point2>> offset_polygon_2d(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type) { - - return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON); - } - - static Vector<Vector<Point2>> offset_polyline_2d(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { - - ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector<Vector<Point2>>(), "Attempt to offset a polyline like a polygon (use offset_polygon_2d instead)."); - - return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); - } - - static Vector<int> triangulate_delaunay_2d(const Vector<Vector2> &p_points) { - - Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points); - Vector<int> triangles; - - for (int i = 0; i < tr.size(); i++) { - triangles.push_back(tr[i].points[0]); - triangles.push_back(tr[i].points[1]); - triangles.push_back(tr[i].points[2]); - } - return triangles; - } - - static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) { - - Vector<int> triangles; - if (!Triangulate::triangulate(p_polygon, triangles)) - return Vector<int>(); //fail - return triangles; - } - - static bool is_polygon_clockwise(const Vector<Vector2> &p_polygon) { - int c = p_polygon.size(); - if (c < 3) - return false; - const Vector2 *p = p_polygon.ptr(); - real_t sum = 0; - for (int i = 0; i < c; i++) { - const Vector2 &v1 = p[i]; - const Vector2 &v2 = p[(i + 1) % c]; - sum += (v2.x - v1.x) * (v2.y + v1.y); - } - - return sum > 0.0f; - } - - // Alternate implementation that should be faster. - static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) { - int c = p_polygon.size(); - if (c < 3) - return false; - const Vector2 *p = p_polygon.ptr(); - Vector2 further_away(-1e20, -1e20); - Vector2 further_away_opposite(1e20, 1e20); - - for (int i = 0; i < c; i++) { - further_away.x = MAX(p[i].x, further_away.x); - further_away.y = MAX(p[i].y, further_away.y); - further_away_opposite.x = MIN(p[i].x, further_away_opposite.x); - further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); - } - - // Make point outside that won't intersect with points in segment from p_point. - further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); - - int intersections = 0; - for (int i = 0; i < c; i++) { - const Vector2 &v1 = p[i]; - const Vector2 &v2 = p[(i + 1) % c]; - if (segment_intersects_segment_2d(v1, v2, p_point, further_away, nullptr)) { - intersections++; - } - } - - return (intersections & 1); - } - static Vector<Vector<Face3>> separate_objects(Vector<Face3> p_array); // Create a "wrap" that encloses the given geometry. static Vector<Face3> wrap_geometry(Vector<Face3> p_array, real_t *p_error = nullptr); struct MeshData { - struct Face { Plane plane; Vector<int> indices; @@ -914,7 +626,6 @@ public: Vector<Face> faces; struct Edge { - int a, b; }; @@ -926,7 +637,6 @@ public: }; _FORCE_INLINE_ static int get_uv84_normal_bit(const Vector3 &p_vector) { - int lat = Math::fast_ftoi(Math::floor(Math::acos(p_vector.dot(Vector3(0, 1, 0))) * 4.0 / Math_PI + 0.5)); if (lat == 0) { @@ -941,84 +651,310 @@ public: } _FORCE_INLINE_ static int get_uv84_normal_bit_neighbors(int p_idx) { - if (p_idx == 24) { return 1 | 2 | 4 | 8; } else if (p_idx == 25) { return (1 << 23) | (1 << 22) | (1 << 21) | (1 << 20); } else { - int ret = 0; - if ((p_idx % 8) == 0) + if ((p_idx % 8) == 0) { ret |= (1 << (p_idx + 7)); - else + } else { ret |= (1 << (p_idx - 1)); - if ((p_idx % 8) == 7) + } + if ((p_idx % 8) == 7) { ret |= (1 << (p_idx - 7)); - else + } else { ret |= (1 << (p_idx + 1)); + } int mask = ret | (1 << p_idx); - if (p_idx < 8) + if (p_idx < 8) { ret |= 24; - else + } else { ret |= mask >> 8; + } - if (p_idx >= 16) + if (p_idx >= 16) { ret |= 25; - else + } else { ret |= mask << 8; + } return ret; } } + static MeshData build_convex_mesh(const Vector<Plane> &p_planes); + static Vector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); + static Vector<Plane> build_box_planes(const Vector3 &p_extents); + static Vector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z); + static Vector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z); - static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) { - return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x); - } + static Vector<Vector3> compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); - // Returns a list of points on the convex hull in counter-clockwise order. - // Note: the last point in the returned list is the same as the first one. - static Vector<Point2> convex_hull_2d(Vector<Point2> P) { - int n = P.size(), k = 0; - Vector<Point2> H; - H.resize(2 * n); - - // Sort points lexicographically. - P.sort(); - - // Build lower hull. - for (int i = 0; i < n; ++i) { - while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) - k--; - H.write[k++] = P[i]; +#define FINDMINMAX(x0, x1, x2, min, max) \ + min = max = x0; \ + if (x1 < min) { \ + min = x1; \ + } \ + if (x1 > max) { \ + max = x1; \ + } \ + if (x2 < min) { \ + min = x2; \ + } \ + if (x2 > max) { \ + max = x2; \ + } + + _FORCE_INLINE_ static bool planeBoxOverlap(Vector3 normal, float d, Vector3 maxbox) { + int q; + Vector3 vmin, vmax; + for (q = 0; q <= 2; q++) { + if (normal[q] > 0.0f) { + vmin[q] = -maxbox[q]; + vmax[q] = maxbox[q]; + } else { + vmin[q] = maxbox[q]; + vmax[q] = -maxbox[q]; + } } - - // Build upper hull. - for (int i = n - 2, t = k + 1; i >= 0; i--) { - while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) - k--; - H.write[k++] = P[i]; + if (normal.dot(vmin) + d > 0.0f) { + return false; + } + if (normal.dot(vmax) + d >= 0.0f) { + return true; } - H.resize(k); - return H; + return false; } - static Vector<Vector<Vector2>> decompose_polygon_in_convex(Vector<Point2> polygon); - static MeshData build_convex_mesh(const Vector<Plane> &p_planes); - static Vector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); - static Vector<Plane> build_box_planes(const Vector3 &p_extents); - static Vector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z); - static Vector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z); +/*======================== X-tests ========================*/ +#define AXISTEST_X01(a, b, fa, fb) \ + p0 = a * v0.y - b * v0.z; \ + p2 = a * v2.y - b * v2.z; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_X2(a, b, fa, fb) \ + p0 = a * v0.y - b * v0.z; \ + p1 = a * v1.y - b * v1.z; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +/*======================== Y-tests ========================*/ +#define AXISTEST_Y02(a, b, fa, fb) \ + p0 = -a * v0.x + b * v0.z; \ + p2 = -a * v2.x + b * v2.z; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_Y1(a, b, fa, fb) \ + p0 = -a * v0.x + b * v0.z; \ + p1 = -a * v1.x + b * v1.z; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + + /*======================== Z-tests ========================*/ + +#define AXISTEST_Z12(a, b, fa, fb) \ + p1 = a * v1.x - b * v1.y; \ + p2 = a * v2.x - b * v2.y; \ + if (p2 < p1) { \ + min = p2; \ + max = p1; \ + } else { \ + min = p1; \ + max = p2; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_Z0(a, b, fa, fb) \ + p0 = a * v0.x - b * v0.y; \ + p1 = a * v1.x - b * v1.y; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ + if (min > rad || max < -rad) { \ + return false; \ + } + + _FORCE_INLINE_ static bool triangle_box_overlap(const Vector3 &boxcenter, const Vector3 boxhalfsize, const Vector3 *triverts) { + /* use separating axis theorem to test overlap between triangle and box */ + /* need to test for overlap in these directions: */ + /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ + /* we do not even need to test these) */ + /* 2) normal of the triangle */ + /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ + /* this gives 3x3=9 more tests */ + Vector3 v0, v1, v2; + float min, max, d, p0, p1, p2, rad, fex, fey, fez; + Vector3 normal, e0, e1, e2; + + /* This is the fastest branch on Sun */ + /* move everything so that the boxcenter is in (0,0,0) */ + + v0 = triverts[0] - boxcenter; + v1 = triverts[1] - boxcenter; + v2 = triverts[2] - boxcenter; + + /* compute triangle edges */ + e0 = v1 - v0; /* tri edge 0 */ + e1 = v2 - v1; /* tri edge 1 */ + e2 = v0 - v2; /* tri edge 2 */ + + /* Bullet 3: */ + /* test the 9 tests first (this was faster) */ + fex = Math::abs(e0.x); + fey = Math::abs(e0.y); + fez = Math::abs(e0.z); + AXISTEST_X01(e0.z, e0.y, fez, fey); + AXISTEST_Y02(e0.z, e0.x, fez, fex); + AXISTEST_Z12(e0.y, e0.x, fey, fex); + + fex = Math::abs(e1.x); + fey = Math::abs(e1.y); + fez = Math::abs(e1.z); + AXISTEST_X01(e1.z, e1.y, fez, fey); + AXISTEST_Y02(e1.z, e1.x, fez, fex); + AXISTEST_Z0(e1.y, e1.x, fey, fex); + + fex = Math::abs(e2.x); + fey = Math::abs(e2.y); + fez = Math::abs(e2.z); + AXISTEST_X2(e2.z, e2.y, fez, fey); + AXISTEST_Y1(e2.z, e2.x, fez, fex); + AXISTEST_Z12(e2.y, e2.x, fey, fex); + + /* Bullet 1: */ + /* first test overlap in the {x,y,z}-directions */ + /* find min, max of the triangle each direction, and test for overlap in */ + /* that direction -- this is equivalent to testing a minimal AABB around */ + /* the triangle against the AABB */ + + /* test in X-direction */ + FINDMINMAX(v0.x, v1.x, v2.x, min, max); + if (min > boxhalfsize.x || max < -boxhalfsize.x) { + return false; + } - static void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size); + /* test in Y-direction */ + FINDMINMAX(v0.y, v1.y, v2.y, min, max); + if (min > boxhalfsize.y || max < -boxhalfsize.y) { + return false; + } - static Vector<Vector3> compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); + /* test in Z-direction */ + FINDMINMAX(v0.z, v1.z, v2.z, min, max); + if (min > boxhalfsize.z || max < -boxhalfsize.z) { + return false; + } -private: - static Vector<Vector<Point2>> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open = false); - static Vector<Vector<Point2>> _polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); + /* Bullet 2: */ + /* test if the box intersects the plane of the triangle */ + /* compute plane equation of triangle: normal*x+d=0 */ + normal = e0.cross(e1); + d = -normal.dot(v0); /* plane eq: normal.x+d=0 */ + return planeBoxOverlap(normal, d, boxhalfsize); /* if true, box and triangle overlaps */ + } + + static Vector<uint32_t> generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative); + static Vector<int8_t> generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative); + + static Vector3 triangle_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_pos) { + Vector3 v0 = p_b - p_a; + Vector3 v1 = p_c - p_a; + Vector3 v2 = p_pos - p_a; + + float d00 = v0.dot(v0); + float d01 = v0.dot(v1); + float d11 = v1.dot(v1); + float d20 = v2.dot(v0); + float d21 = v2.dot(v1); + float denom = (d00 * d11 - d01 * d01); + if (denom == 0) { + return Vector3(); //invalid triangle, return empty + } + float v = (d11 * d20 - d01 * d21) / denom; + float w = (d00 * d21 - d01 * d20) / denom; + float u = 1.0f - v - w; + return Vector3(u, v, w); + } + + static Color tetrahedron_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_d, const Vector3 &p_pos) { + Vector3 vap = p_pos - p_a; + Vector3 vbp = p_pos - p_b; + + Vector3 vab = p_b - p_a; + Vector3 vac = p_c - p_a; + Vector3 vad = p_d - p_a; + + Vector3 vbc = p_c - p_b; + Vector3 vbd = p_d - p_b; + // ScTP computes the scalar triple product +#define STP(m_a, m_b, m_c) ((m_a).dot((m_b).cross((m_c)))) + float va6 = STP(vbp, vbd, vbc); + float vb6 = STP(vap, vac, vad); + float vc6 = STP(vap, vad, vab); + float vd6 = STP(vap, vab, vac); + float v6 = 1 / STP(vab, vac, vad); + return Color(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); +#undef STP + } + + _FORCE_INLINE_ static Vector3 octahedron_map_decode(const Vector2 &p_uv) { + // https://twitter.com/Stubbesaurus/status/937994790553227264 + Vector2 f = p_uv * 2.0 - Vector2(1.0, 1.0); + Vector3 n = Vector3(f.x, f.y, 1.0f - Math::abs(f.x) - Math::abs(f.y)); + float t = CLAMP(-n.z, 0.0, 1.0); + n.x += n.x >= 0 ? -t : t; + n.y += n.y >= 0 ? -t : t; + return n.normalized(); + } }; -#endif // GEOMETRY_H +#endif // GEOMETRY_3D_H diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index a47d4ef7ad..ef2a0c5339 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -41,7 +41,6 @@ } Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const String &p_field) { - ERR_FAIL_COND_V(p_target.get_type() != p_source.get_type(), p_target); /* clang-format makes a mess of this macro usage */ diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index 7417e64ac1..1585c96b38 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -94,16 +94,18 @@ double Math::dectime(double p_value, double p_amount, double p_step) { double sgn = p_value < 0 ? -1.0 : 1.0; double val = Math::abs(p_value); val -= p_amount * p_step; - if (val < 0.0) + if (val < 0.0) { val = 0.0; + } return val * sgn; } double Math::ease(double p_x, double p_c) { - if (p_x < 0) + if (p_x < 0) { p_x = 0; - else if (p_x > 1.0) + } else if (p_x > 1.0) { p_x = 1.0; + } if (p_c > 0) { if (p_c < 1.0) { return 1.0 - Math::pow(1.0 - p_x, 1.0 / p_c); @@ -118,8 +120,9 @@ double Math::ease(double p_x, double p_c) { } else { return (1.0 - Math::pow(1.0 - (p_x - 0.5) * 2.0, -p_c)) * 0.5 + 0.5; } - } else + } else { return 0; // no ease (raw) + } } double Math::stepify(double p_value, double p_step) { @@ -130,7 +133,6 @@ double Math::stepify(double p_value, double p_step) { } uint32_t Math::larger_prime(uint32_t p_val) { - static const uint32_t primes[] = { 5, 13, @@ -166,10 +168,10 @@ uint32_t Math::larger_prime(uint32_t p_val) { int idx = 0; while (true) { - ERR_FAIL_COND_V(primes[idx] == 0, 0); - if (primes[idx] > p_val) + if (primes[idx] > p_val) { return primes[idx]; + } idx++; } } diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 3e1eb14a6a..7a9fd60e23 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -41,7 +41,6 @@ #include <math.h> class Math { - static RandomPCG default_rand; public: @@ -233,12 +232,16 @@ public: static _ALWAYS_INLINE_ float range_lerp(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_weight) { - if (is_equal_approx(p_from, p_to)) return p_from; + if (is_equal_approx(p_from, p_to)) { + return p_from; + } double x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0, 1.0); return x * x * (3.0 - 2.0 * x); } static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_weight) { - if (is_equal_approx(p_from, p_to)) return p_from; + if (is_equal_approx(p_from, p_to)) { + return p_from; + } float x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f); return x * x * (3.0f - 2.0f * x); } @@ -326,7 +329,6 @@ public: } static _ALWAYS_INLINE_ float absf(float g) { - union { float f; uint32_t i; @@ -338,7 +340,6 @@ public: } static _ALWAYS_INLINE_ double absd(double g) { - union { double d; uint64_t i; @@ -350,7 +351,6 @@ public: //this function should be as fast as possible and rounding mode should not matter static _ALWAYS_INLINE_ int fast_ftoi(float a) { - static int b; #if (defined(_WIN32_WINNT) && _WIN32_WINNT >= 0x0603) || WINAPI_FAMILY == WINAPI_FAMILY_PHONE_APP // windows 8 phone? @@ -405,7 +405,6 @@ public: } static _ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *h) { - union { uint32_t u32; float f32; @@ -420,7 +419,6 @@ public: } static _ALWAYS_INLINE_ uint16_t make_half_float(float f) { - union { float fv; uint32_t ui; @@ -451,7 +449,6 @@ public: } // check if exponent is <= -15 else if (exp <= 0x38000000) { - /*// store a denorm half-float value or zero exp = (0x38000000 - exp) >> 23; mantissa >>= (14 + exp); @@ -476,10 +473,11 @@ public: 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) + if (p_target >= 0) { b -= p_separation; - else + } else { b += p_step; + } return (Math::abs(p_target - a) < Math::abs(p_target - b)) ? a : b; } return p_target; diff --git a/core/math/octree.h b/core/math/octree.h index 2060a61b4b..5d9688d442 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -34,7 +34,7 @@ #include "core/list.h" #include "core/map.h" #include "core/math/aabb.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/math/vector3.h" #include "core/print_string.h" #include "core/variant.h" @@ -52,7 +52,6 @@ public: private: enum { - NEG = 0, POS = 1, }; @@ -69,7 +68,6 @@ private: }; struct PairKey { - union { struct { OctreeElementID A; @@ -79,18 +77,14 @@ private: }; _FORCE_INLINE_ bool operator<(const PairKey &p_pair) const { - return key < p_pair.key; } _FORCE_INLINE_ PairKey(OctreeElementID p_A, OctreeElementID p_B) { - if (p_A < p_B) { - A = p_A; B = p_B; } else { - B = p_A; A = p_B; } @@ -102,53 +96,37 @@ private: struct Element; struct Octant { - // cached for FAST plane check AABB aabb; - uint64_t last_pass; - Octant *parent; - Octant *children[8]; + uint64_t last_pass = 0; + Octant *parent = nullptr; + Octant *children[8] = { nullptr }; - int children_count; // cache for amount of childrens (fast check for removal) - int parent_index; // cache for parent index (fast check for removal) + int children_count = 0; // cache for amount of childrens (fast check for removal) + int parent_index = -1; // cache for parent index (fast check for removal) List<Element *, AL> pairable_elements; List<Element *, AL> elements; - Octant() { - children_count = 0; - parent_index = -1; - last_pass = 0; - parent = nullptr; - for (int i = 0; i < 8; i++) - children[i] = nullptr; - } - - ~Octant() { - - /* - for (int i=0;i<8;i++) - memdelete_notnull(children[i]); - */ - } + Octant() {} + ~Octant() {} }; struct PairData; struct Element { + Octree *octree = nullptr; - Octree *octree; - - T *userdata; - int subindex; - bool pairable; - uint32_t pairable_mask; - uint32_t pairable_type; + T *userdata = nullptr; + int subindex = 0; + bool pairable = false; + uint32_t pairable_mask = 0; + uint32_t pairable_type = 0; - uint64_t last_pass; - OctreeElementID _id; - Octant *common_parent; + uint64_t last_pass = 0; + OctreeElementID _id = 0; + Octant *common_parent = nullptr; AABB aabb; AABB container_aabb; @@ -156,28 +134,16 @@ private: List<PairData *, AL> pair_list; struct OctantOwner { - Octant *octant; typename List<Element *, AL>::Element *E; }; // an element can be in max 8 octants List<OctantOwner, AL> octant_owners; - Element() { - last_pass = 0; - _id = 0; - pairable = false; - subindex = 0; - userdata = 0; - octree = 0; - pairable_mask = 0; - pairable_type = 0; - common_parent = nullptr; - } + Element() {} }; struct PairData { - int refcount; bool intersect; Element *A, *B; @@ -204,19 +170,15 @@ private: int pair_count; _FORCE_INLINE_ void _pair_check(PairData *p_pair) { - bool intersect = p_pair->A->aabb.intersects_inclusive(p_pair->B->aabb); if (intersect != p_pair->intersect) { - if (intersect) { - if (pair_callback) { p_pair->ud = pair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex); } pair_count++; } else { - if (unpair_callback) { unpair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex, p_pair->ud); } @@ -228,19 +190,19 @@ private: } _FORCE_INLINE_ void _pair_reference(Element *p_A, Element *p_B) { - - if (p_A == p_B || (p_A->userdata == p_B->userdata && p_A->userdata)) + if (p_A == p_B || (p_A->userdata == p_B->userdata && p_A->userdata)) { return; + } if (!(p_A->pairable_type & p_B->pairable_mask) && - !(p_B->pairable_type & p_A->pairable_mask)) + !(p_B->pairable_type & p_A->pairable_mask)) { return; // none can pair with none + } PairKey key(p_A->_id, p_B->_id); typename PairMap::Element *E = pair_map.find(key); if (!E) { - PairData pdata; pdata.refcount = 1; pdata.A = p_A; @@ -255,15 +217,14 @@ private: pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata); */ } else { - E->get().refcount++; } } _FORCE_INLINE_ void _pair_unreference(Element *p_A, Element *p_B) { - - if (p_A == p_B) + if (p_A == p_B) { return; + } PairKey key(p_A->_id, p_B->_id); typename PairMap::Element *E = pair_map.find(key); @@ -296,24 +257,18 @@ private: } _FORCE_INLINE_ void _element_check_pairs(Element *p_element) { - typename List<PairData *, AL>::Element *E = p_element->pair_list.front(); while (E) { - _pair_check(E->get()); E = E->next(); } } _FORCE_INLINE_ void _optimize() { - while (root && root->children_count < 2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) { - Octant *new_root = nullptr; if (root->children_count == 1) { - for (int i = 0; i < 8; i++) { - if (root->children[i]) { new_root = root->children[i]; root->children[i] = nullptr; @@ -339,7 +294,6 @@ private: void _unpair_element(Element *p_element, Octant *p_octant); struct _CullConvexData { - const Plane *planes; int plane_count; const Vector3 *points; @@ -356,14 +310,14 @@ private: void _cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); void _remove_tree(Octant *p_octant) { - - if (!p_octant) + if (!p_octant) { return; + } for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) + if (p_octant->children[i]) { _remove_tree(p_octant->children[i]); + } } memdelete_allocator<Octant, AL>(p_octant); @@ -405,7 +359,6 @@ T *Octree<T, use_pairs, AL>::get(OctreeElementID p_id) const { template <class T, bool use_pairs, class AL> bool Octree<T, use_pairs, AL>::is_pairable(OctreeElementID p_id) const { - const typename ElementMap::Element *E = element_map.find(p_id); ERR_FAIL_COND_V(!E, false); return E->get().pairable; @@ -413,7 +366,6 @@ bool Octree<T, use_pairs, AL>::is_pairable(OctreeElementID p_id) const { template <class T, bool use_pairs, class AL> int Octree<T, use_pairs, AL>::get_subindex(OctreeElementID p_id) const { - const typename ElementMap::Element *E = element_map.find(p_id); ERR_FAIL_COND_V(!E, -1); return E->get().subindex; @@ -423,7 +375,6 @@ int Octree<T, use_pairs, AL>::get_subindex(OctreeElementID p_id) const { template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_octant) { - real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues if (p_octant->aabb.size.x / OCTREE_DIVISOR < element_size) { @@ -434,11 +385,9 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct owner.octant = p_octant; if (use_pairs && p_element->pairable) { - p_octant->pairable_elements.push_back(p_element); owner.E = p_octant->pairable_elements.back(); } else { - p_octant->elements.push_back(p_element); owner.E = p_octant->elements.back(); } @@ -453,11 +402,9 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct } if (use_pairs && p_octant->children_count > 0) { - pass++; //elements below this only get ONE reference added for (int i = 0; i < 8; i++) { - if (p_octant->children[i]) { _pair_element(p_element, p_octant->children[i]); } @@ -469,7 +416,6 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct bool candidate = p_element->common_parent == nullptr; for (int i = 0; i < 8; i++) { - if (p_octant->children[i]) { /* element exists, go straight to it */ if (p_octant->children[i]->aabb.intersects_inclusive(p_element->aabb)) { @@ -482,12 +428,15 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct AABB aabb = p_octant->aabb; aabb.size *= 0.5; - if (i & 1) + if (i & 1) { aabb.position.x += aabb.size.x; - if (i & 2) + } + if (i & 2) { aabb.position.y += aabb.size.y; - if (i & 4) + } + if (i & 4) { aabb.position.z += aabb.size.z; + } if (aabb.intersects_inclusive(p_element->aabb)) { /* if actually intersects, create the child */ @@ -509,13 +458,11 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct } if (candidate && splits > 1) { - p_element->common_parent = p_octant; } } if (use_pairs) { - typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front(); while (E) { @@ -536,14 +483,12 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) { - if (!root) { // octre is empty AABB base(Vector3(), Vector3(1.0, 1.0, 1.0) * unit_size); while (!base.encloses(p_aabb)) { - if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { /* grow towards positive */ base.size *= 2.0; @@ -562,11 +507,9 @@ void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) { octant_count++; } else { - AABB base = root->aabb; while (!base.encloses(p_aabb)) { - ERR_FAIL_COND_MSG(base.size.x > OCTREE_SIZE_LIMIT, "Octree upper size limit reached, does the AABB supplied contain NAN?"); Octant *gp = memnew_allocator(Octant, AL); @@ -595,15 +538,14 @@ void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) { template <class T, bool use_pairs, class AL> bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit) { - bool octant_removed = false; while (true) { - // check all exit conditions - if (p_octant == p_limit) // reached limit, nothing to erase, exit + if (p_octant == p_limit) { // reached limit, nothing to erase, exit return octant_removed; + } bool unpaired = false; @@ -632,7 +574,6 @@ bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, O Octant *parent = p_octant->parent; if (p_octant->children_count == 0 && p_octant->elements.empty() && p_octant->pairable_elements.empty()) { - // erase octant if (p_octant == root) { // won't have a parent, just erase @@ -651,8 +592,9 @@ bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, O octant_removed = true; } - if (!removed && !unpaired) + if (!removed && !unpaired) { return octant_removed; // no reason to keep going up anymore! was already visited and was not removed + } p_octant = parent; } @@ -662,7 +604,6 @@ bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, O template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_unpair_element(Element *p_element, Octant *p_octant) { - // always test pairable typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front(); while (E) { @@ -687,25 +628,24 @@ void Octree<T, use_pairs, AL>::_unpair_element(Element *p_element, Octant *p_oct p_octant->last_pass = pass; - if (p_octant->children_count == 0) + if (p_octant->children_count == 0) { return; // small optimization for leafs + } for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) + if (p_octant->children[i]) { _unpair_element(p_element, p_octant->children[i]); + } } } template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_pair_element(Element *p_element, Octant *p_octant) { - // always test pairable typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front(); while (E) { - if (E->get()->last_pass != pass) { // only get ONE reference _pair_reference(p_element, E->get()); E->get()->last_pass = pass; @@ -726,30 +666,30 @@ void Octree<T, use_pairs, AL>::_pair_element(Element *p_element, Octant *p_octan } p_octant->last_pass = pass; - if (p_octant->children_count == 0) + if (p_octant->children_count == 0) { return; // small optimization for leafs + } for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) + if (p_octant->children[i]) { _pair_element(p_element, p_octant->children[i]); + } } } template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_remove_element(Element *p_element) { - pass++; // will do a new pass for this typename List<typename Element::OctantOwner, AL>::Element *I = p_element->octant_owners.front(); /* FIRST remove going up normally */ for (; I; I = I->next()) { - Octant *o = I->get().octant; - if (!use_pairs) // small speedup + if (!use_pairs) { // small speedup o->elements.erase(I->get().E); + } _remove_element_from_octant(p_element, o); } @@ -759,30 +699,28 @@ void Octree<T, use_pairs, AL>::_remove_element(Element *p_element) { I = p_element->octant_owners.front(); if (use_pairs) { - for (; I; I = I->next()) { - Octant *o = I->get().octant; // erase children pairs, they are erased ONCE even if repeated pass++; for (int i = 0; i < 8; i++) { - - if (o->children[i]) + if (o->children[i]) { _unpair_element(p_element, o->children[i]); + } } - if (p_element->pairable) + if (p_element->pairable) { o->pairable_elements.erase(I->get().E); - else + } else { o->elements.erase(I->get().E); + } } } p_element->octant_owners.clear(); if (use_pairs) { - int remaining = p_element->pair_list.size(); //p_element->pair_list.clear(); ERR_FAIL_COND(remaining); @@ -791,7 +729,6 @@ void Octree<T, use_pairs, AL>::_remove_element(Element *p_element) { template <class T, bool use_pairs, class AL> OctreeElementID Octree<T, use_pairs, AL>::create(T *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { - // check for AABB validity #ifdef DEBUG_ENABLED ERR_FAIL_COND_V(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15, 0); @@ -822,8 +759,9 @@ OctreeElementID Octree<T, use_pairs, AL>::create(T *p_userdata, const AABB &p_aa if (!e.aabb.has_no_surface()) { _ensure_valid_root(p_aabb); _insert_element(&e, root); - if (use_pairs) + if (use_pairs) { _element_check_pairs(&e); + } } return last_element_id - 1; @@ -831,7 +769,6 @@ OctreeElementID Octree<T, use_pairs, AL>::create(T *p_userdata, const AABB &p_aa template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { - #ifdef DEBUG_ENABLED // check for AABB validity ERR_FAIL_COND(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15); @@ -852,7 +789,6 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { bool new_has_surf = !p_aabb.has_no_surface(); if (old_has_surf != new_has_surf) { - if (old_has_surf) { _remove_element(&e); // removing e.common_parent = nullptr; @@ -863,22 +799,24 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { e.common_parent = nullptr; e.aabb = p_aabb; _insert_element(&e, root); - if (use_pairs) + if (use_pairs) { _element_check_pairs(&e); + } } return; } - if (!old_has_surf) // doing nothing + if (!old_has_surf) { // doing nothing return; + } // it still is enclosed in the same AABB it was assigned to if (e.container_aabb.encloses(p_aabb)) { - e.aabb = p_aabb; - if (use_pairs) + if (use_pairs) { _element_check_pairs(&e); // must check pairs anyway + } return; } @@ -898,8 +836,9 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { //src is now the place towards where insertion is going to happen pass++; - while (common_parent && !common_parent->aabb.encloses(p_aabb)) + while (common_parent && !common_parent->aabb.encloses(p_aabb)) { common_parent = common_parent->parent; + } ERR_FAIL_COND(!common_parent); @@ -913,7 +852,6 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { pass++; for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F;) { - Octant *o = F->get().octant; typename List<typename Element::OctantOwner, AL>::Element *N = F->next(); @@ -922,13 +860,13 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { o->elements.erase( F->get().E ); */ - if (use_pairs && e.pairable) + if (use_pairs && e.pairable) { o->pairable_elements.erase(F->get().E); - else + } else { o->elements.erase(F->get().E); + } if (_remove_element_from_octant(&e, o, common_parent->parent)) { - owners.erase(F); } @@ -938,15 +876,14 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { if (use_pairs) { //unpair child elements in anything that survived for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F; F = F->next()) { - Octant *o = F->get().octant; // erase children pairs, unref ONCE pass++; for (int i = 0; i < 8; i++) { - - if (o->children[i]) + if (o->children[i]) { _unpair_element(&e, o->children[i]); + } } } @@ -958,14 +895,14 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::set_pairable(OctreeElementID p_id, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { - typename ElementMap::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); Element &e = E->get(); - if (p_pairable == e.pairable && e.pairable_type == p_pairable_type && e.pairable_mask == p_pairable_mask) + if (p_pairable == e.pairable && e.pairable_type == p_pairable_type && e.pairable_mask == p_pairable_mask) { return; // no changes, return + } if (!e.aabb.has_no_surface()) { _remove_element(&e); @@ -979,21 +916,20 @@ void Octree<T, use_pairs, AL>::set_pairable(OctreeElementID p_id, bool p_pairabl if (!e.aabb.has_no_surface()) { _ensure_valid_root(e.aabb); _insert_element(&e, root); - if (use_pairs) + if (use_pairs) { _element_check_pairs(&e); + } } } template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::erase(OctreeElementID p_id) { - typename ElementMap::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); Element &e = E->get(); if (!e.aabb.has_no_surface()) { - _remove_element(&e); } @@ -1003,21 +939,20 @@ void Octree<T, use_pairs, AL>::erase(OctreeElementID p_id) { template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p_cull) { - - if (*p_cull->result_idx == p_cull->result_max) + if (*p_cull->result_idx == p_cull->result_max) { return; //pointless + } if (!p_octant->elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) { continue; + } e->last_pass = pass; if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { @@ -1025,7 +960,6 @@ void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p p_cull->result_array[*p_cull->result_idx] = e->userdata; (*p_cull->result_idx)++; } else { - return; // pointless to continue } } @@ -1033,26 +967,22 @@ void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p } if (use_pairs && !p_octant->pairable_elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->pairable_elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) { continue; + } e->last_pass = pass; if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - if (*p_cull->result_idx < p_cull->result_max) { - p_cull->result_array[*p_cull->result_idx] = e->userdata; (*p_cull->result_idx)++; } else { - return; // pointless to continue } } @@ -1060,7 +990,6 @@ void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p } for (int i = 0; i < 8; i++) { - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { _cull_convex(p_octant->children[i], p_cull); } @@ -1069,33 +998,30 @@ void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) + if (*p_result_idx == p_result_max) { return; //pointless + } if (!p_octant->elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (p_aabb.intersects_inclusive(e->aabb)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1103,27 +1029,24 @@ void Octree<T, use_pairs, AL>::_cull_aabb(Octant *p_octant, const AABB &p_aabb, } if (use_pairs && !p_octant->pairable_elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->pairable_elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (p_aabb.intersects_inclusive(e->aabb)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1131,7 +1054,6 @@ void Octree<T, use_pairs, AL>::_cull_aabb(Octant *p_octant, const AABB &p_aabb, } for (int i = 0; i < 8; i++) { - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_inclusive(p_aabb)) { _cull_aabb(p_octant->children[i], p_aabb, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); } @@ -1140,33 +1062,30 @@ void Octree<T, use_pairs, AL>::_cull_aabb(Octant *p_octant, const AABB &p_aabb, template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) + if (*p_result_idx == p_result_max) { return; //pointless + } if (!p_octant->elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (e->aabb.intersects_segment(p_from, p_to)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1174,30 +1093,27 @@ void Octree<T, use_pairs, AL>::_cull_segment(Octant *p_octant, const Vector3 &p_ } if (use_pairs && !p_octant->pairable_elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->pairable_elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (e->aabb.intersects_segment(p_from, p_to)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1205,7 +1121,6 @@ void Octree<T, use_pairs, AL>::_cull_segment(Octant *p_octant, const Vector3 &p_ } for (int i = 0; i < 8; i++) { - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from, p_to)) { _cull_segment(p_octant->children[i], p_from, p_to, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); } @@ -1214,33 +1129,30 @@ void Octree<T, use_pairs, AL>::_cull_segment(Octant *p_octant, const Vector3 &p_ template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::_cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) + if (*p_result_idx == p_result_max) { return; //pointless + } if (!p_octant->elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (e->aabb.has_point(p_point)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1248,30 +1160,27 @@ void Octree<T, use_pairs, AL>::_cull_point(Octant *p_octant, const Vector3 &p_po } if (use_pairs && !p_octant->pairable_elements.empty()) { - typename List<Element *, AL>::Element *I; I = p_octant->pairable_elements.front(); for (; I; I = I->next()) { - Element *e = I->get(); - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) { continue; + } e->last_pass = pass; if (e->aabb.has_point(p_point)) { - if (*p_result_idx < p_result_max) { - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) + if (p_subindex_array) { p_subindex_array[*p_result_idx] = e->subindex; + } (*p_result_idx)++; } else { - return; // pointless to continue } } @@ -1279,7 +1188,6 @@ void Octree<T, use_pairs, AL>::_cull_point(Octant *p_octant, const Vector3 &p_po } for (int i = 0; i < 8; i++) { - //could be optimized.. if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) { _cull_point(p_octant->children[i], p_point, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); @@ -1289,11 +1197,14 @@ void Octree<T, use_pairs, AL>::_cull_point(Octant *p_octant, const Vector3 &p_po template <class T, bool use_pairs, class AL> int Octree<T, use_pairs, AL>::cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) { - - if (!root) + if (!root || p_convex.size() == 0) { return 0; + } - Vector<Vector3> convex_points = Geometry::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + if (convex_points.size() == 0) { + return 0; + } int result_count = 0; pass++; @@ -1314,9 +1225,9 @@ int Octree<T, use_pairs, AL>::cull_convex(const Vector<Plane> &p_convex, T **p_r template <class T, bool use_pairs, class AL> int Octree<T, use_pairs, AL>::cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) + if (!root) { return 0; + } int result_count = 0; pass++; @@ -1327,9 +1238,9 @@ int Octree<T, use_pairs, AL>::cull_aabb(const AABB &p_aabb, T **p_result_array, template <class T, bool use_pairs, class AL> int Octree<T, use_pairs, AL>::cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) + if (!root) { return 0; + } int result_count = 0; pass++; @@ -1340,9 +1251,9 @@ int Octree<T, use_pairs, AL>::cull_segment(const Vector3 &p_from, const Vector3 template <class T, bool use_pairs, class AL> int Octree<T, use_pairs, AL>::cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) + if (!root) { return 0; + } int result_count = 0; pass++; @@ -1353,20 +1264,18 @@ int Octree<T, use_pairs, AL>::cull_point(const Vector3 &p_point, T **p_result_ar template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::set_pair_callback(PairCallback p_callback, void *p_userdata) { - pair_callback = p_callback; pair_callback_userdata = p_userdata; } + template <class T, bool use_pairs, class AL> void Octree<T, use_pairs, AL>::set_unpair_callback(UnpairCallback p_callback, void *p_userdata) { - unpair_callback = p_callback; unpair_callback_userdata = p_userdata; } template <class T, bool use_pairs, class AL> Octree<T, use_pairs, AL>::Octree(real_t p_unit_size) { - last_element_id = 1; pass = 1; unit_size = p_unit_size; diff --git a/core/math/plane.cpp b/core/math/plane.cpp index a3818698bc..df37ceb0e5 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -33,12 +33,10 @@ #include "core/math/math_funcs.h" void Plane::set_normal(const Vector3 &p_normal) { - normal = p_normal; } void Plane::normalize() { - real_t l = normal.length(); if (l == 0) { *this = Plane(0, 0, 0, 0); @@ -49,27 +47,25 @@ void Plane::normalize() { } Plane Plane::normalized() const { - Plane p = *this; p.normalize(); return p; } Vector3 Plane::get_any_point() const { - return get_normal() * d; } Vector3 Plane::get_any_perpendicular_normal() const { - static const Vector3 p1 = Vector3(1, 0, 0); static const Vector3 p2 = Vector3(0, 1, 0); Vector3 p; - if (ABS(normal.dot(p1)) > 0.99) // if too similar to p1 + if (ABS(normal.dot(p1)) > 0.99) { // if too similar to p1 p = p2; // use p2 - else + } else { p = p1; // use p1 + } p -= normal * normal.dot(p); p.normalize(); @@ -80,7 +76,6 @@ Vector3 Plane::get_any_perpendicular_normal() const { /* intersections */ bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const { - const Plane &p_plane0 = *this; Vector3 normal0 = p_plane0.normal; Vector3 normal1 = p_plane1.normal; @@ -88,8 +83,9 @@ bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r real_t denom = vec3_cross(normal0, normal1).dot(normal2); - if (Math::is_zero_approx(denom)) + if (Math::is_zero_approx(denom)) { return false; + } if (r_result) { *r_result = ((vec3_cross(normal1, normal2) * p_plane0.d) + @@ -102,13 +98,11 @@ bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r } bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { - Vector3 segment = p_dir; real_t den = normal.dot(segment); //printf("den is %i\n",den); if (Math::is_zero_approx(den)) { - return false; } @@ -127,13 +121,11 @@ bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 } bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const { - Vector3 segment = p_begin - p_end; real_t den = normal.dot(segment); //printf("den is %i\n",den); if (Math::is_zero_approx(den)) { - return false; } @@ -141,7 +133,6 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec //printf("dist is %i\n",dist); if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { - return false; } @@ -153,12 +144,14 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec /* misc */ -bool Plane::is_equal_approx(const Plane &p_plane) const { +bool Plane::is_equal_approx_any_side(const Plane &p_plane) const { + return (normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d)) || (normal.is_equal_approx(-p_plane.normal) && Math::is_equal_approx(d, -p_plane.d)); +} +bool Plane::is_equal_approx(const Plane &p_plane) const { return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d); } Plane::operator String() const { - return normal.operator String() + ", " + rtos(d); } diff --git a/core/math/plane.h b/core/math/plane.h index 771c8fc705..9a3e5a485f 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -36,7 +36,7 @@ class Plane { public: Vector3 normal; - real_t d; + real_t d = 0; void set_normal(const Vector3 &p_normal); _FORCE_INLINE_ Vector3 get_normal() const { return normal; }; ///Point is coplanar, CMP_EPSILON for precision @@ -56,12 +56,11 @@ public: /* intersections */ - bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result = 0) const; + bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result = nullptr) const; bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const; bool intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const; _FORCE_INLINE_ Vector3 project(const Vector3 &p_point) const { - return p_point - normal * distance_to(p_point); } @@ -69,13 +68,13 @@ public: Plane operator-() const { return Plane(-normal, -d); } bool is_equal_approx(const Plane &p_plane) const; + bool is_equal_approx_any_side(const Plane &p_plane) const; _FORCE_INLINE_ bool operator==(const Plane &p_plane) const; _FORCE_INLINE_ bool operator!=(const Plane &p_plane) const; operator String() const; - _FORCE_INLINE_ Plane() : - d(0) {} + _FORCE_INLINE_ Plane() {} _FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) : normal(p_a, p_b, p_c), d(p_d) {} @@ -86,17 +85,14 @@ public: }; bool Plane::is_point_over(const Vector3 &p_point) const { - return (normal.dot(p_point) > d); } real_t Plane::distance_to(const Vector3 &p_point) const { - return (normal.dot(p_point) - d); } bool Plane::has_point(const Vector3 &p_point, real_t _epsilon) const { - real_t dist = normal.dot(p_point) - d; dist = ABS(dist); return (dist <= _epsilon); @@ -113,23 +109,21 @@ Plane::Plane(const Vector3 &p_point, const Vector3 &p_normal) : } Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir) { - - if (p_dir == CLOCKWISE) + if (p_dir == CLOCKWISE) { normal = (p_point1 - p_point3).cross(p_point1 - p_point2); - else + } else { normal = (p_point1 - p_point2).cross(p_point1 - p_point3); + } normal.normalize(); d = normal.dot(p_point1); } bool Plane::operator==(const Plane &p_plane) const { - return normal == p_plane.normal && d == p_plane.d; } bool Plane::operator!=(const Plane &p_plane) const { - return normal != p_plane.normal || d != p_plane.d; } diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 61cd41b23d..c10f5da494 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -107,7 +107,6 @@ Vector3 Quat::get_euler_yxz() const { } void Quat::operator*=(const Quat &q) { - set(w * q.x + x * q.w + y * q.z - z * q.y, w * q.y + y * q.w + z * q.x - x * q.z, w * q.z + z * q.w + x * q.y - y * q.x, @@ -115,19 +114,16 @@ void Quat::operator*=(const Quat &q) { } Quat Quat::operator*(const Quat &q) const { - Quat r = *this; r *= q; 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()); } @@ -206,7 +202,9 @@ Quat Quat::slerpni(const Quat &q, const real_t &t) const { real_t dot = from.dot(q); - if (Math::absf(dot) > 0.9999) return from; + if (Math::absf(dot) > 0.9999) { + return from; + } real_t theta = Math::acos(dot), sinT = 1.0 / Math::sin(theta), @@ -232,7 +230,6 @@ Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const } Quat::operator String() const { - return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); } @@ -241,9 +238,9 @@ void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized."); #endif real_t d = axis.length(); - if (d == 0) + if (d == 0) { set(0, 0, 0, 0); - else { + } else { real_t sin_angle = Math::sin(angle * 0.5); real_t cos_angle = Math::cos(angle * 0.5); real_t s = sin_angle / d; diff --git a/core/math/quat.h b/core/math/quat.h index b3135ad1ca..64d0f00912 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -40,7 +40,7 @@ class Quat { public: - real_t x, y, z, w; + real_t x = 0, y = 0, z = 0, w = 1; _FORCE_INLINE_ real_t length_squared() const; bool is_equal_approx(const Quat &p_quat) const; @@ -112,7 +112,9 @@ public: z = p_z; w = p_w; } - inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : + + _FORCE_INLINE_ Quat() {} + _FORCE_INLINE_ Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : x(p_x), y(p_y), z(p_z), @@ -147,7 +149,6 @@ public: z = 0; w = 0; } else { - real_t s = Math::sqrt((1.0 + d) * 2.0); real_t rs = 1.0 / s; @@ -157,13 +158,6 @@ public: w = s * 0.5; } } - - inline Quat() : - x(0), - y(0), - z(0), - w(1) { - } }; real_t Quat::dot(const Quat &q) const { @@ -196,7 +190,6 @@ void Quat::operator*=(const real_t &s) { } void Quat::operator/=(const real_t &s) { - *this *= 1.0 / s; } diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index 7fbb26c377..8ba1ba9286 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -34,13 +34,11 @@ uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF; -Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_mesh) { - +Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) { /* CREATE AABB VOLUME */ AABB aabb; for (int i = 0; i < p_points.size(); i++) { - if (i == 0) { aabb.position = p_points[i]; } else { @@ -57,7 +55,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Set<Vector3> valid_cache; for (int i = 0; i < p_points.size(); i++) { - Vector3 sp = p_points[i].snapped(Vector3(0.0001, 0.0001, 0.0001)); if (valid_cache.has(sp)) { valid_points.write[i] = false; @@ -78,12 +75,11 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me real_t max = 0, min = 0; for (int i = 0; i < p_points.size(); i++) { - - if (!valid_points[i]) + if (!valid_points[i]) { continue; + } real_t d = p_points[i][longest_axis]; if (i == 0 || d < min) { - simplex[0] = i; min = d; } @@ -102,15 +98,14 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]]; for (int i = 0; i < p_points.size(); i++) { - - if (!valid_points[i]) + if (!valid_points[i]) { continue; + } Vector3 n = rel12.cross(p_points[simplex[0]] - p_points[i]).cross(rel12).normalized(); real_t d = Math::abs(n.dot(p_points[simplex[0]]) - n.dot(p_points[i])); if (i == 0 || d > maxd) { - maxd = d; simplex[2] = i; } @@ -124,14 +119,13 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Plane p(p_points[simplex[0]], p_points[simplex[1]], p_points[simplex[2]]); for (int i = 0; i < p_points.size(); i++) { - - if (!valid_points[i]) + if (!valid_points[i]) { continue; + } real_t d = Math::abs(p.distance_to(p_points[i])); if (i == 0 || d > maxd) { - maxd = d; simplex[3] = i; } @@ -152,7 +146,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me List<Face> faces; for (int i = 0; i < 4; i++) { - static const int face_order[4][3] = { { 0, 1, 2 }, { 0, 1, 3 }, @@ -183,22 +176,24 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me /* COMPUTE AVAILABLE VERTICES */ for (int i = 0; i < p_points.size(); i++) { - - if (i == simplex[0]) + if (i == simplex[0]) { continue; - if (i == simplex[1]) + } + if (i == simplex[1]) { continue; - if (i == simplex[2]) + } + if (i == simplex[2]) { continue; - if (i == simplex[3]) + } + if (i == simplex[3]) { continue; - if (!valid_points[i]) + } + if (!valid_points[i]) { continue; + } for (List<Face>::Element *E = faces.front(); E; E = E->next()) { - if (E->get().plane.distance_to(p_points[i]) > over_tolerance) { - E->get().points_over.push_back(i); break; } @@ -219,7 +214,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me uint32_t debug_stop = debug_stop_after; while (debug_stop > 0 && faces.back()->get().points_over.size()) { - debug_stop--; Face &f = faces.back()->get(); @@ -228,7 +222,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me real_t next_d = 0; for (int i = 0; i < f.points_over.size(); i++) { - real_t d = f.plane.distance_to(p_points[f.points_over[i]]); if (d > next_d) { @@ -247,9 +240,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Map<Edge, FaceConnect> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot for (List<Face>::Element *E = faces.front(); E; E = E->next()) { - if (E->get().plane.distance_to(v) > 0) { - lit_faces.push_back(E); for (int i = 0; i < 3; i++) { @@ -265,7 +256,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //left F->get().left = E; } else { - F->get().right = E; } } @@ -276,7 +266,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me List<List<Face>::Element *> new_faces; //new faces for (Map<Edge, FaceConnect>::Element *E = lit_edges.front(); E; E = E->next()) { - FaceConnect &fc = E->get(); if (fc.left && fc.right) { continue; //edge is uninteresting, not on horizont @@ -304,17 +293,15 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //distribute points into new faces for (List<List<Face>::Element *>::Element *F = lit_faces.front(); F; F = F->next()) { - Face &lf = F->get()->get(); for (int i = 0; i < lf.points_over.size(); i++) { - - if (lf.points_over[i] == f.points_over[next]) //do not add current one + if (lf.points_over[i] == f.points_over[next]) { //do not add current one continue; + } Vector3 p = p_points[lf.points_over[i]]; for (List<List<Face>::Element *>::Element *E = new_faces.front(); E; E = E->next()) { - Face &f2 = E->get()->get(); if (f2.plane.distance_to(p) > over_tolerance) { f2.points_over.push_back(lf.points_over[i]); @@ -327,7 +314,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //erase lit faces while (lit_faces.size()) { - faces.erase(lit_faces.front()->get()); lit_faces.pop_front(); } @@ -335,7 +321,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //put faces that contain no points on the front for (List<List<Face>::Element *>::Element *E = new_faces.front(); E; E = E->next()) { - Face &f2 = E->get()->get(); if (f2.points_over.size() == 0) { faces.move_to_front(E->get()); @@ -349,21 +334,19 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //make a map of edges again Map<Edge, RetFaceConnect> ret_edges; - List<Geometry::MeshData::Face> ret_faces; + List<Geometry3D::MeshData::Face> ret_faces; for (List<Face>::Element *E = faces.front(); E; E = E->next()) { - - Geometry::MeshData::Face f; + Geometry3D::MeshData::Face f; f.plane = E->get().plane; for (int i = 0; i < 3; i++) { f.indices.push_back(E->get().vertices[i]); } - List<Geometry::MeshData::Face>::Element *F = ret_faces.push_back(f); + List<Geometry3D::MeshData::Face>::Element *F = ret_faces.push_back(f); for (int i = 0; i < 3; i++) { - uint32_t a = E->get().vertices[i]; uint32_t b = E->get().vertices[(i + 1) % 3]; Edge e(a, b); @@ -376,7 +359,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //left G->get().left = F; } else { - G->get().right = F; } } @@ -384,12 +366,10 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //fill faces - for (List<Geometry::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { - - Geometry::MeshData::Face &f = E->get(); + for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { + Geometry3D::MeshData::Face &f = E->get(); for (int i = 0; i < f.indices.size(); i++) { - int a = E->get().indices[i]; int b = E->get().indices[(i + 1) % f.indices.size()]; Edge e(a, b); @@ -397,7 +377,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Map<Edge, RetFaceConnect>::Element *F = ret_edges.find(e); ERR_CONTINUE(!F); - List<Geometry::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left; + List<Geometry3D::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left; ERR_CONTINUE(O == E); ERR_CONTINUE(O == nullptr); @@ -411,7 +391,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me if (O->get().indices[j] == a) { //append the rest for (int k = 0; k < ois; k++) { - int idx = O->get().indices[(k + j) % ois]; int idxn = O->get().indices[(k + j + 1) % ois]; if (idx == b && idxn == a) { //already have b! @@ -427,10 +406,11 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Map<Edge, RetFaceConnect>::Element *F2 = ret_edges.find(e2); ERR_CONTINUE(!F2); //change faceconnect, point to this face instead - if (F2->get().left == O) + if (F2->get().left == O) { F2->get().left = E; - else if (F2->get().right == O) + } else if (F2->get().right == O) { F2->get().right = E; + } } break; @@ -439,11 +419,13 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me // remove all edge connections to this face for (Map<Edge, RetFaceConnect>::Element *G = ret_edges.front(); G; G = G->next()) { - if (G->get().left == O) + if (G->get().left == O) { G->get().left = nullptr; + } - if (G->get().right == O) + if (G->get().right == O) { G->get().right = nullptr; + } } ret_edges.erase(F); //remove the edge @@ -457,14 +439,13 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me r_mesh.faces.resize(ret_faces.size()); int idx = 0; - for (List<Geometry::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { + for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { r_mesh.faces.write[idx++] = E->get(); } r_mesh.edges.resize(ret_edges.size()); idx = 0; for (Map<Edge, RetFaceConnect>::Element *E = ret_edges.front(); E; E = E->next()) { - - Geometry::MeshData::Edge e; + Geometry3D::MeshData::Edge e; e.a = E->key().vertices[0]; e.b = E->key().vertices[1]; r_mesh.edges.write[idx++] = e; diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index 173f919a73..cac8e58d23 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -33,14 +33,12 @@ #include "core/list.h" #include "core/math/aabb.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/set.h" class QuickHull { - public: struct Edge { - union { uint32_t vertices[2]; uint64_t id; @@ -51,7 +49,6 @@ public: } Edge(int p_vtx_a = 0, int p_vtx_b = 0) { - if (p_vtx_a > p_vtx_b) { SWAP(p_vtx_a, p_vtx_b); } @@ -62,36 +59,28 @@ public: }; struct Face { - Plane plane; uint32_t vertices[3]; Vector<int> points_over; bool operator<(const Face &p_face) const { - return points_over.size() < p_face.points_over.size(); } }; private: struct FaceConnect { - List<Face>::Element *left, *right; - FaceConnect() { - left = nullptr; - right = nullptr; - } + List<Face>::Element *left, *right = nullptr; + FaceConnect() {} }; struct RetFaceConnect { - List<Geometry::MeshData::Face>::Element *left, *right; - RetFaceConnect() { - left = nullptr; - right = nullptr; - } + List<Geometry3D::MeshData::Face>::Element *left, *right = nullptr; + RetFaceConnect() {} }; public: static uint32_t debug_stop_after; - static Error build(const Vector<Vector3> &p_points, Geometry::MeshData &r_mesh); + static Error build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh); }; #endif // QUICK_HULL_H diff --git a/core/math/random_number_generator.cpp b/core/math/random_number_generator.cpp index 1a1bffb562..67f4c0b14a 100644 --- a/core/math/random_number_generator.cpp +++ b/core/math/random_number_generator.cpp @@ -30,8 +30,6 @@ #include "random_number_generator.h" -RandomNumberGenerator::RandomNumberGenerator() {} - void RandomNumberGenerator::_bind_methods() { ClassDB::bind_method(D_METHOD("set_seed", "seed"), &RandomNumberGenerator::set_seed); ClassDB::bind_method(D_METHOD("get_seed"), &RandomNumberGenerator::get_seed); diff --git a/core/math/random_number_generator.h b/core/math/random_number_generator.h index e7f188bb42..920308e597 100644 --- a/core/math/random_number_generator.h +++ b/core/math/random_number_generator.h @@ -59,13 +59,14 @@ public: _FORCE_INLINE_ int randi_range(int from, int to) { unsigned int ret = randbase.rand(); - if (to < from) + if (to < from) { return ret % (from - to + 1) + to; - else + } else { return ret % (to - from + 1) + from; + } } - RandomNumberGenerator(); + RandomNumberGenerator() {} }; #endif // RANDOM_NUMBER_GENERATOR_H diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index 12b9904c88..0cc3c4ca0f 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -31,12 +31,10 @@ #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; int axis = 0; real_t sign = 0; @@ -50,18 +48,18 @@ bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 real_t csign; if (seg_from < seg_to) { - - if (seg_from > box_end || seg_to < box_begin) + if (seg_from > box_end || seg_to < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; csign = -1.0; } else { - - if (seg_to > box_end || seg_from < box_begin) + if (seg_to > box_end || seg_from < box_begin) { return false; + } real_t length = seg_to - seg_from; cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; @@ -73,10 +71,12 @@ bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 axis = i; sign = csign; } - if (cmax < max) + if (cmax < max) { max = cmax; - if (max < min) + } + if (max < min) { return false; + } } Vector2 rel = p_to - p_from; @@ -87,14 +87,14 @@ bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_normal = normal; } - if (r_pos) + if (r_pos) { *r_pos = p_from + rel * min; + } return true; } bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const { - //SAT intersection between local and transformed rect2 Vector2 xf_points[4] = { @@ -108,14 +108,18 @@ bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_re //base rect2 first (faster) - if (xf_points[0].y > position.y) + if (xf_points[0].y > position.y) { goto next1; - if (xf_points[1].y > position.y) + } + if (xf_points[1].y > position.y) { goto next1; - if (xf_points[2].y > position.y) + } + if (xf_points[2].y > position.y) { goto next1; - if (xf_points[3].y > position.y) + } + if (xf_points[3].y > position.y) { goto next1; + } return false; @@ -123,27 +127,35 @@ next1: low_limit = position.y + size.y; - if (xf_points[0].y < low_limit) + if (xf_points[0].y < low_limit) { goto next2; - if (xf_points[1].y < low_limit) + } + if (xf_points[1].y < low_limit) { goto next2; - if (xf_points[2].y < low_limit) + } + if (xf_points[2].y < low_limit) { goto next2; - if (xf_points[3].y < low_limit) + } + if (xf_points[3].y < low_limit) { goto next2; + } return false; next2: - if (xf_points[0].x > position.x) + if (xf_points[0].x > position.x) { goto next3; - if (xf_points[1].x > position.x) + } + if (xf_points[1].x > position.x) { goto next3; - if (xf_points[2].x > position.x) + } + if (xf_points[2].x > position.x) { goto next3; - if (xf_points[3].x > position.x) + } + if (xf_points[3].x > position.x) { goto next3; + } return false; @@ -151,14 +163,18 @@ next3: low_limit = position.x + size.x; - if (xf_points[0].x < low_limit) + if (xf_points[0].x < low_limit) { goto next4; - if (xf_points[1].x < low_limit) + } + if (xf_points[1].x < low_limit) { goto next4; - if (xf_points[2].x < low_limit) + } + if (xf_points[2].x < low_limit) { goto next4; - if (xf_points[3].x < low_limit) + } + if (xf_points[3].x < low_limit) { goto next4; + } return false; @@ -201,10 +217,12 @@ next4: maxb = MAX(dp, maxb); minb = MIN(dp, minb); - if (mina > maxb) + if (mina > maxb) { return false; - if (minb > maxa) + } + if (minb > maxa) { return false; + } maxa = p_xform.elements[1].dot(xf_points2[0]); mina = maxa; @@ -236,10 +254,12 @@ next4: maxb = MAX(dp, maxb); minb = MIN(dp, minb); - if (mina > maxb) + if (mina > maxb) { return false; - if (minb > maxa) + } + if (minb > maxa) { return false; + } return true; } diff --git a/core/math/rect2.h b/core/math/rect2.h index 30dbfdbbe5..14393325ec 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -36,7 +36,6 @@ struct Transform2D; struct Rect2 { - Point2 position; Size2 size; @@ -49,30 +48,37 @@ struct Rect2 { inline bool intersects(const Rect2 &p_rect, const bool p_include_borders = false) const { if (p_include_borders) { - if (position.x > (p_rect.position.x + p_rect.size.width)) + if (position.x > (p_rect.position.x + p_rect.size.width)) { return false; - if ((position.x + size.width) < p_rect.position.x) + } + if ((position.x + size.width) < p_rect.position.x) { return false; - if (position.y > (p_rect.position.y + p_rect.size.height)) + } + if (position.y > (p_rect.position.y + p_rect.size.height)) { return false; - if ((position.y + size.height) < p_rect.position.y) + } + if ((position.y + size.height) < p_rect.position.y) { return false; + } } else { - if (position.x >= (p_rect.position.x + p_rect.size.width)) + if (position.x >= (p_rect.position.x + p_rect.size.width)) { return false; - if ((position.x + size.width) <= p_rect.position.x) + } + if ((position.x + size.width) <= p_rect.position.x) { return false; - if (position.y >= (p_rect.position.y + p_rect.size.height)) + } + if (position.y >= (p_rect.position.y + p_rect.size.height)) { return false; - if ((position.y + size.height) <= p_rect.position.y) + } + if ((position.y + size.height) <= p_rect.position.y) { return false; + } } return true; } inline real_t distance_to(const Vector2 &p_point) const { - real_t dist = 0.0; bool inside = true; @@ -97,10 +103,11 @@ struct Rect2 { inside = false; } - if (inside) + if (inside) { return 0; - else + } else { return dist; + } } bool intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const; @@ -108,22 +115,21 @@ struct Rect2 { bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = nullptr, Point2 *r_normal = nullptr) const; 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)); } _FORCE_INLINE_ bool has_no_area() const { - return (size.x <= 0 || size.y <= 0); } inline Rect2 clip(const Rect2 &p_rect) const { /// return a clipped rect Rect2 new_rect = p_rect; - if (!intersects(new_rect)) + if (!intersects(new_rect)) { return Rect2(); + } new_rect.position.x = MAX(p_rect.position.x, position.x); new_rect.position.y = MAX(p_rect.position.y, position.y); @@ -150,17 +156,21 @@ struct Rect2 { new_rect.size = new_rect.size - new_rect.position; //make relative again return new_rect; - }; + } inline bool has_point(const Point2 &p_point) const { - if (p_point.x < position.x) + if (p_point.x < position.x) { return false; - if (p_point.y < position.y) + } + if (p_point.y < position.y) { return false; + } - if (p_point.x >= (position.x + size.x)) + if (p_point.x >= (position.x + size.x)) { return false; - if (p_point.y >= (position.y + size.y)) + } + if (p_point.y >= (position.y + size.y)) { return false; + } return true; } @@ -170,7 +180,6 @@ struct Rect2 { bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } inline Rect2 grow(real_t p_by) const { - Rect2 g = *this; g.position.x -= p_by; g.position.y -= p_by; @@ -189,7 +198,6 @@ struct Rect2 { } inline Rect2 grow_individual(real_t p_left, real_t p_top, real_t p_right, real_t p_bottom) const { - Rect2 g = *this; g.position.x -= p_left; g.position.y -= p_top; @@ -200,7 +208,6 @@ struct Rect2 { } _FORCE_INLINE_ Rect2 expand(const Vector2 &p_vector) const { - Rect2 r = *this; r.expand_to(p_vector); return r; @@ -211,22 +218,25 @@ struct Rect2 { Vector2 begin = position; Vector2 end = position + size; - if (p_vector.x < begin.x) + if (p_vector.x < begin.x) { begin.x = p_vector.x; - if (p_vector.y < begin.y) + } + if (p_vector.y < begin.y) { begin.y = p_vector.y; + } - if (p_vector.x > end.x) + if (p_vector.x > end.x) { end.x = p_vector.x; - if (p_vector.y > end.y) + } + if (p_vector.y > end.y) { end.y = p_vector.y; + } position = begin; size = end - begin; } _FORCE_INLINE_ Rect2 abs() const { - return Rect2(Point2(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); } @@ -244,7 +254,6 @@ struct Rect2 { }; struct Rect2i { - Point2i position; Size2i size; @@ -256,35 +265,38 @@ struct Rect2i { int get_area() const { return size.width * size.height; } inline bool intersects(const Rect2i &p_rect) const { - if (position.x > (p_rect.position.x + p_rect.size.width)) + if (position.x > (p_rect.position.x + p_rect.size.width)) { return false; - if ((position.x + size.width) < p_rect.position.x) + } + if ((position.x + size.width) < p_rect.position.x) { return false; - if (position.y > (p_rect.position.y + p_rect.size.height)) + } + if (position.y > (p_rect.position.y + p_rect.size.height)) { return false; - if ((position.y + size.height) < p_rect.position.y) + } + if ((position.y + size.height) < p_rect.position.y) { return false; + } return true; } inline bool encloses(const Rect2i &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)); } _FORCE_INLINE_ bool has_no_area() const { - return (size.x <= 0 || size.y <= 0); } inline Rect2i clip(const Rect2i &p_rect) const { /// return a clipped rect Rect2i new_rect = p_rect; - if (!intersects(new_rect)) + if (!intersects(new_rect)) { return Rect2i(); + } new_rect.position.x = MAX(p_rect.position.x, position.x); new_rect.position.y = MAX(p_rect.position.y, position.y); @@ -311,17 +323,21 @@ struct Rect2i { new_rect.size = new_rect.size - new_rect.position; //make relative again return new_rect; - }; + } bool has_point(const Point2 &p_point) const { - if (p_point.x < position.x) + if (p_point.x < position.x) { return false; - if (p_point.y < position.y) + } + if (p_point.y < position.y) { return false; + } - if (p_point.x >= (position.x + size.x)) + if (p_point.x >= (position.x + size.x)) { return false; - if (p_point.y >= (position.y + size.y)) + } + if (p_point.y >= (position.y + size.y)) { return false; + } return true; } @@ -330,7 +346,6 @@ struct Rect2i { bool operator!=(const Rect2i &p_rect) const { return position != p_rect.position || size != p_rect.size; } Rect2i grow(int p_by) const { - Rect2i g = *this; g.position.x -= p_by; g.position.y -= p_by; @@ -349,7 +364,6 @@ struct Rect2i { } inline Rect2i grow_individual(int p_left, int p_top, int p_right, int p_bottom) const { - Rect2i g = *this; g.position.x -= p_left; g.position.y -= p_top; @@ -360,44 +374,46 @@ struct Rect2i { } _FORCE_INLINE_ Rect2i expand(const Vector2i &p_vector) const { - Rect2i r = *this; r.expand_to(p_vector); return r; } inline void expand_to(const Point2i &p_vector) { - Point2i begin = position; Point2i end = position + size; - if (p_vector.x < begin.x) + if (p_vector.x < begin.x) { begin.x = p_vector.x; - if (p_vector.y < begin.y) + } + if (p_vector.y < begin.y) { begin.y = p_vector.y; + } - if (p_vector.x > end.x) + if (p_vector.x > end.x) { end.x = p_vector.x; - if (p_vector.y > end.y) + } + if (p_vector.y > end.y) { end.y = p_vector.y; + } position = begin; size = end - begin; } _FORCE_INLINE_ Rect2i abs() const { - return Rect2i(Point2i(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); } operator String() const { return String(position) + ", " + String(size); } operator Rect2() const { return Rect2(position, size); } + + Rect2i() {} Rect2i(const Rect2 &p_r2) : position(p_r2.position), size(p_r2.size) { } - Rect2i() {} Rect2i(int p_x, int p_y, int p_width, int p_height) : position(Point2(p_x, p_y)), size(Size2(p_width, p_height)) { diff --git a/core/math/transform.cpp b/core/math/transform.cpp index 82e4005d3e..0274dd18af 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -35,20 +35,17 @@ #include "core/print_string.h" void Transform::affine_invert() { - basis.invert(); origin = basis.xform(-origin); } Transform Transform::affine_inverse() const { - Transform ret = *this; ret.affine_invert(); return ret; } void Transform::invert() { - basis.transpose(); origin = basis.xform(-origin); } @@ -62,22 +59,18 @@ Transform Transform::inverse() const { } void Transform::rotate(const Vector3 &p_axis, real_t p_phi) { - *this = rotated(p_axis, p_phi); } Transform Transform::rotated(const Vector3 &p_axis, real_t p_phi) const { - return Transform(Basis(p_axis, p_phi), Vector3()) * (*this); } void Transform::rotate_basis(const Vector3 &p_axis, real_t p_phi) { - basis.rotate(p_axis, p_phi); } Transform Transform::looking_at(const Vector3 &p_target, const Vector3 &p_up) const { - Transform t = *this; t.set_look_at(origin, p_target, p_up); return t; @@ -117,7 +110,6 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const } Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) const { - /* not sure if very "efficient" but good enough? */ Vector3 src_scale = basis.get_scale(); @@ -136,81 +128,70 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) } void Transform::scale(const Vector3 &p_scale) { - basis.scale(p_scale); origin *= p_scale; } Transform Transform::scaled(const Vector3 &p_scale) const { - Transform t = *this; t.scale(p_scale); return t; } void Transform::scale_basis(const Vector3 &p_scale) { - basis.scale(p_scale); } void Transform::translate(real_t p_tx, real_t p_ty, real_t p_tz) { translate(Vector3(p_tx, p_ty, p_tz)); } -void Transform::translate(const Vector3 &p_translation) { +void Transform::translate(const Vector3 &p_translation) { for (int i = 0; i < 3; i++) { origin[i] += basis[i].dot(p_translation); } } Transform Transform::translated(const Vector3 &p_translation) const { - Transform t = *this; t.translate(p_translation); return t; } void Transform::orthonormalize() { - basis.orthonormalize(); } Transform Transform::orthonormalized() const { - Transform _copy = *this; _copy.orthonormalize(); 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); } -bool Transform::operator!=(const Transform &p_transform) const { +bool Transform::operator!=(const Transform &p_transform) const { return (basis != p_transform.basis || origin != p_transform.origin); } void Transform::operator*=(const Transform &p_transform) { - origin = xform(p_transform.origin); basis *= p_transform.basis; } Transform Transform::operator*(const Transform &p_transform) const { - Transform t = *this; t *= p_transform; return t; } Transform::operator String() const { - return basis.operator String() + " - " + origin.operator String(); } diff --git a/core/math/transform.h b/core/math/transform.h index c6e3be4c70..71847d36ac 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -92,14 +92,12 @@ public: Transform interpolate_with(const Transform &p_transform, real_t p_c) const; _FORCE_INLINE_ Transform inverse_xform(const Transform &t) const { - Vector3 v = t.origin - origin; return Transform(basis.transpose_xform(t.basis), basis.xform(v)); } void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t tx, real_t ty, real_t tz) { - basis.set(xx, xy, xz, yx, yy, yz, zx, zy, zz); origin.x = tx; origin.y = ty; @@ -114,14 +112,13 @@ public: }; _FORCE_INLINE_ Vector3 Transform::xform(const Vector3 &p_vector) const { - return Vector3( basis[0].dot(p_vector) + origin.x, basis[1].dot(p_vector) + origin.y, basis[2].dot(p_vector) + origin.z); } -_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { +_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { Vector3 v = p_vector - origin; return Vector3( @@ -131,7 +128,6 @@ _FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { } _FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const { - Vector3 point = p_plane.normal * p_plane.d; Vector3 point_dir = point + p_plane.normal; point = xform(point); @@ -143,8 +139,8 @@ _FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const { return Plane(normal, d); } -_FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { +_FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { Vector3 point = p_plane.normal * p_plane.d; Vector3 point_dir = point + p_plane.normal; xform_inv(point); @@ -158,7 +154,6 @@ _FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { } _FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { - /* http://dev.theomader.com/transform-bounding-boxes/ */ Vector3 min = p_aabb.position; Vector3 max = p_aabb.position + p_aabb.size; @@ -184,7 +179,6 @@ _FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { } _FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { - /* define vertices */ Vector3 vertices[8] = { Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z), @@ -202,7 +196,6 @@ _FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { ret.position = xform_inv(vertices[0]); for (int i = 1; i < 8; i++) { - ret.expand_to(xform_inv(vertices[i])); } @@ -210,7 +203,6 @@ _FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { } Vector<Vector3> Transform::xform(const Vector<Vector3> &p_array) const { - Vector<Vector3> array; array.resize(p_array.size()); @@ -224,7 +216,6 @@ Vector<Vector3> Transform::xform(const Vector<Vector3> &p_array) const { } Vector<Vector3> Transform::xform_inv(const Vector<Vector3> &p_array) const { - Vector<Vector3> array; array.resize(p_array.size()); diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index 97a9216a5a..180aeaa0af 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -38,14 +38,12 @@ void Transform2D::invert() { } Transform2D Transform2D::inverse() const { - Transform2D inv = *this; inv.invert(); return inv; } void Transform2D::affine_invert() { - real_t det = basis_determinant(); #ifdef MATH_CHECKS ERR_FAIL_COND(det == 0); @@ -60,7 +58,6 @@ void Transform2D::affine_invert() { } Transform2D Transform2D::affine_inverse() const { - Transform2D inv = *this; inv.affine_invert(); return inv; @@ -70,13 +67,18 @@ void Transform2D::rotate(real_t p_phi) { *this = Transform2D(p_phi, Vector2()) * (*this); } -real_t Transform2D::get_rotation() const { +real_t Transform2D::get_skew() const { real_t det = basis_determinant(); - Transform2D m = orthonormalized(); - if (det < 0) { - m.scale_basis(Size2(1, -1)); // convention to separate rotation and reflection for 2D is to absorb a flip along y into scaling. - } - return Math::atan2(m[0].y, m[0].x); + return Math::acos(elements[0].normalized().dot(SGN(det) * elements[1].normalized())) - Math_PI * 0.5; +} + +void Transform2D::set_skew(float p_angle) { + real_t det = basis_determinant(); + elements[1] = SGN(det) * elements[0].rotated((Math_PI * 0.5 + p_angle)).normalized() * elements[1].length(); +} + +real_t Transform2D::get_rotation() const { + return Math::atan2(elements[0].y, elements[0].x); } void Transform2D::set_rotation(real_t p_rot) { @@ -91,7 +93,6 @@ void Transform2D::set_rotation(real_t p_rot) { } Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) { - real_t cr = Math::cos(p_rot); real_t sr = Math::sin(p_rot); elements[0][0] = cr; @@ -117,24 +118,23 @@ void Transform2D::scale(const Size2 &p_scale) { scale_basis(p_scale); elements[2] *= p_scale; } -void Transform2D::scale_basis(const Size2 &p_scale) { +void Transform2D::scale_basis(const Size2 &p_scale) { elements[0][0] *= p_scale.x; elements[0][1] *= p_scale.y; elements[1][0] *= p_scale.x; elements[1][1] *= p_scale.y; } -void Transform2D::translate(real_t p_tx, real_t p_ty) { +void Transform2D::translate(real_t p_tx, real_t p_ty) { translate(Vector2(p_tx, p_ty)); } -void Transform2D::translate(const Vector2 &p_translation) { +void Transform2D::translate(const Vector2 &p_translation) { elements[2] += basis_xform(p_translation); } void Transform2D::orthonormalize() { - // Gram-Schmidt Process Vector2 x = elements[0]; @@ -149,39 +149,36 @@ void Transform2D::orthonormalize() { } Transform2D Transform2D::orthonormalized() const { - Transform2D on = *this; on.orthonormalize(); 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++) { - if (elements[i] != p_transform.elements[i]) + if (elements[i] != p_transform.elements[i]) { return false; + } } return true; } bool Transform2D::operator!=(const Transform2D &p_transform) const { - for (int i = 0; i < 3; i++) { - if (elements[i] != p_transform.elements[i]) + if (elements[i] != p_transform.elements[i]) { return true; + } } return false; } void Transform2D::operator*=(const Transform2D &p_transform) { - elements[2] = xform(p_transform.elements[2]); real_t x0, x1, y0, y1; @@ -198,54 +195,46 @@ void Transform2D::operator*=(const Transform2D &p_transform) { } Transform2D Transform2D::operator*(const Transform2D &p_transform) const { - Transform2D t = *this; t *= p_transform; return t; } Transform2D Transform2D::scaled(const Size2 &p_scale) const { - Transform2D copy = *this; copy.scale(p_scale); return copy; } Transform2D Transform2D::basis_scaled(const Size2 &p_scale) const { - Transform2D copy = *this; copy.scale_basis(p_scale); return copy; } Transform2D Transform2D::untranslated() const { - Transform2D copy = *this; copy.elements[2] = Vector2(); return copy; } Transform2D Transform2D::translated(const Vector2 &p_offset) const { - Transform2D copy = *this; copy.translate(p_offset); return copy; } Transform2D Transform2D::rotated(real_t p_phi) const { - Transform2D copy = *this; copy.rotate(p_phi); return copy; } real_t Transform2D::basis_determinant() const { - return elements[0].x * elements[1].y - elements[0].y * elements[1].x; } Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t p_c) const { - //extract parameters Vector2 p1 = get_origin(); Vector2 p2 = p_transform.get_origin(); @@ -281,6 +270,5 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t } Transform2D::operator String() const { - return String(String() + elements[0] + ", " + elements[1] + ", " + elements[2]); } diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index fa43762aa4..46e97abaa7 100644 --- a/core/math/transform_2d.h +++ b/core/math/transform_2d.h @@ -70,7 +70,10 @@ struct Transform2D { void set_rotation(real_t p_rot); real_t get_rotation() const; + real_t get_skew() const; + void set_skew(float p_angle); _FORCE_INLINE_ void set_rotation_and_scale(real_t p_rot, const Size2 &p_scale); + _FORCE_INLINE_ void set_rotation_scale_and_skew(real_t p_rot, const Size2 &p_scale, float p_skew); void rotate(real_t p_phi); void scale(const Size2 &p_scale); @@ -117,7 +120,6 @@ struct Transform2D { operator String() const; Transform2D(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy) { - elements[0][0] = xx; elements[0][1] = xy; elements[1][0] = yx; @@ -134,36 +136,33 @@ struct Transform2D { }; Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const { - return Vector2( tdotx(p_vec), tdoty(p_vec)); } Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const { - return Vector2( elements[0].dot(p_vec), elements[1].dot(p_vec)); } Vector2 Transform2D::xform(const Vector2 &p_vec) const { - return Vector2( tdotx(p_vec), tdoty(p_vec)) + elements[2]; } -Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const { +Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const { Vector2 v = p_vec - elements[2]; return Vector2( elements[0].dot(v), elements[1].dot(v)); } -Rect2 Transform2D::xform(const Rect2 &p_rect) const { +Rect2 Transform2D::xform(const Rect2 &p_rect) const { Vector2 x = elements[0] * p_rect.size.x; Vector2 y = elements[1] * p_rect.size.y; Vector2 pos = xform(p_rect.position); @@ -177,15 +176,20 @@ Rect2 Transform2D::xform(const Rect2 &p_rect) const { } void Transform2D::set_rotation_and_scale(real_t p_rot, const Size2 &p_scale) { - elements[0][0] = Math::cos(p_rot) * p_scale.x; elements[1][1] = Math::cos(p_rot) * p_scale.y; elements[1][0] = -Math::sin(p_rot) * p_scale.y; elements[0][1] = Math::sin(p_rot) * p_scale.x; } -Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { +void Transform2D::set_rotation_scale_and_skew(real_t p_rot, const Size2 &p_scale, float p_skew) { + elements[0][0] = Math::cos(p_rot) * p_scale.x; + elements[1][1] = Math::cos(p_rot + p_skew) * p_scale.y; + elements[1][0] = -Math::sin(p_rot + p_skew) * p_scale.y; + elements[0][1] = Math::sin(p_rot) * p_scale.x; +} +Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { Vector2 ends[4] = { xform_inv(p_rect.position), xform_inv(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)), @@ -203,7 +207,6 @@ Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { } Vector<Vector2> Transform2D::xform(const Vector<Vector2> &p_array) const { - Vector<Vector2> array; array.resize(p_array.size()); @@ -217,7 +220,6 @@ Vector<Vector2> Transform2D::xform(const Vector<Vector2> &p_array) const { } Vector<Vector2> Transform2D::xform_inv(const Vector<Vector2> &p_array) const { - Vector<Vector2> array; array.resize(p_array.size()); diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 5c66721b9d..c9a546e385 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -33,30 +33,25 @@ #include "core/sort_array.h" int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) { - if (p_depth > max_depth) { max_depth = p_depth; } if (p_size == 1) { - return p_bb[p_from] - p_bvh; } else if (p_size == 0) { - return -1; } AABB aabb; aabb = p_bb[p_from]->aabb; for (int i = 1; i < p_size; i++) { - aabb.merge_with(p_bb[p_from + i]->aabb); } int li = aabb.get_longest_axis_index(); switch (li) { - case Vector3::AXIS_X: { SortArray<BVH *, BVHCmpX> sort_x; sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); @@ -90,9 +85,9 @@ int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, in } void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const { - - if (!valid) + if (!valid) { return; + } const int triangles_num = triangles.size(); @@ -110,7 +105,6 @@ void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const { } void TriangleMesh::create(const Vector<Vector3> &p_faces) { - valid = false; int fc = p_faces.size(); @@ -122,7 +116,6 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { BVH *bw = bvh.ptrw(); { - //create faces and indices and base bvh //except for the Set for repeated triangles, everything //goes in-place. @@ -132,12 +125,10 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { Map<Vector3, int> db; for (int i = 0; i < fc; i++) { - Triangle &f = w[i]; const Vector3 *v = &r[i * 3]; for (int j = 0; j < 3; j++) { - int vidx = -1; Vector3 vs = v[j].snapped(Vector3(0.0001, 0.0001, 0.0001)); Map<Vector3, int>::Element *E = db.find(vs); @@ -149,10 +140,11 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { } f.indices[j] = vidx; - if (j == 0) + if (j == 0) { bw[i].aabb.position = vs; - else + } else { bw[i].aabb.expand_to(vs); + } } f.normal = Face3(r[i * 3 + 0], r[i * 3 + 1], r[i * 3 + 2]).get_plane().get_normal(); @@ -174,7 +166,6 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { bwptrs.resize(fc); BVH **bwp = bwptrs.ptrw(); for (int i = 0; i < fc; i++) { - bwp[i] = &bw[i]; } @@ -188,7 +179,6 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { } Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -215,23 +205,18 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { stack[0] = pos; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &b = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects(p_aabb); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index >= 0) { - const Triangle &s = triangleptr[b.face_index]; n += s.normal; n_count++; @@ -239,49 +224,47 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; - } else + } else { level--; + } continue; } } - if (done) + if (done) { break; + } } - if (n_count > 0) + if (n_count > 0) { n /= n_count; + } return n; } bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -309,35 +292,28 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en stack[0] = pos; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &b = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_segment(p_begin, p_end); //bool valid = b.aabb.intersects(ray_aabb); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index >= 0) { - const Triangle &s = triangleptr[b.face_index]; Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]); Vector3 res; if (f3.intersects_segment(p_begin, p_end, &res)) { - real_t nd = n.dot(res); if (nd < d) { - d = nd; r_point = res; r_normal = f3.get_plane().get_normal(); @@ -348,52 +324,49 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; - } else + } else { level--; + } continue; } } - if (done) + if (done) { break; + } } if (inters) { - - if (n.dot(r_normal) > 0) + if (n.dot(r_normal) > 0) { r_normal = -r_normal; + } } return inters; } bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -421,33 +394,26 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V stack[0] = pos; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &b = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_ray(p_begin, p_dir); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index >= 0) { - const Triangle &s = triangleptr[b.face_index]; Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]); Vector3 res; if (f3.intersects_ray(p_begin, p_dir, &res)) { - real_t nd = n.dot(res); if (nd < d) { - d = nd; r_point = res; r_normal = f3.get_plane().get_normal(); @@ -458,45 +424,43 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; - } else + } else { level--; + } continue; } } - if (done) + if (done) { break; + } } if (inters) { - - if (n.dot(r_normal) > 0) + if (n.dot(r_normal) > 0) { r_normal = -r_normal; + } } return inters; @@ -528,23 +492,18 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou stack[0] = pos; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &b = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index >= 0) { - const Triangle &s = triangleptr[b.face_index]; for (int j = 0; j < 3; ++j) { @@ -558,14 +517,18 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou if (p.intersects_segment(point, next_point, &res)) { bool inisde = true; for (int k = 0; k < p_plane_count; k++) { - if (k == i) continue; + if (k == i) { + continue; + } const Plane &pp = p_planes[k]; if (pp.is_point_over(res)) { inisde = false; break; } } - if (inisde) return true; + if (inisde) { + return true; + } } if (p.is_point_over(point)) { @@ -573,45 +536,45 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou break; } } - if (over) return true; + if (over) { + return true; + } } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; - } else + } else { level--; + } continue; } } - if (done) + if (done) { break; + } } return false; @@ -643,84 +606,81 @@ bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, stack[0] = pos; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &b = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); - if (!intersects) return false; + if (!intersects) { + return false; + } bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count); if (inside) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index >= 0) { const Triangle &s = triangleptr[b.face_index]; for (int j = 0; j < 3; ++j) { Vector3 point = scale.xform(vertexptr[s.indices[j]]); for (int i = 0; i < p_plane_count; i++) { const Plane &p = p_planes[i]; - if (p.is_point_over(point)) return false; + if (p.is_point_over(point)) { + return false; + } } } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; - } else + } else { level--; + } continue; } } - if (done) + if (done) { break; + } } return true; } bool TriangleMesh::is_valid() const { - return valid; } Vector<Face3> TriangleMesh::get_faces() const { - - if (!valid) + if (!valid) { return Vector<Face3>(); + } Vector<Face3> faces; int ts = triangles.size(); @@ -740,7 +700,6 @@ Vector<Face3> TriangleMesh::get_faces() const { } TriangleMesh::TriangleMesh() { - valid = false; max_depth = 0; } diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 64704477cc..86412cf725 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -35,11 +35,9 @@ #include "core/reference.h" class TriangleMesh : public Reference { - GDCLASS(TriangleMesh, Reference); struct Triangle { - Vector3 normal; int indices[3]; }; @@ -48,7 +46,6 @@ class TriangleMesh : public Reference { Vector<Vector3> vertices; struct BVH { - AABB aabb; Vector3 center; //used for sorting int left; @@ -58,24 +55,18 @@ class TriangleMesh : public Reference { }; struct BVHCmpX { - bool operator()(const BVH *p_left, const BVH *p_right) const { - return p_left->center.x < p_right->center.x; } }; struct BVHCmpY { - bool operator()(const BVH *p_left, const BVH *p_right) const { - return p_left->center.y < p_right->center.y; } }; struct BVHCmpZ { - bool operator()(const BVH *p_left, const BVH *p_right) const { - return p_left->center.z < p_right->center.z; } }; diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index cbcb232745..12bd384c6a 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -31,7 +31,6 @@ #include "triangulate.h" real_t Triangulate::get_area(const Vector<Vector2> &contour) { - int n = contour.size(); const Vector2 *c = &contour[0]; @@ -80,7 +79,7 @@ bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, } else { return ((aCROSSbp >= 0.0) && (bCROSScp >= 0.0) && (cCROSSap >= 0.0)); } -}; +} bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed) { int p; @@ -103,13 +102,19 @@ bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, in // To avoid that we allow zero-area triangles if all else failed. float threshold = relaxed ? -CMP_EPSILON : CMP_EPSILON; - if (threshold > (((Bx - Ax) * (Cy - Ay)) - ((By - Ay) * (Cx - Ax)))) return false; + if (threshold > (((Bx - Ax) * (Cy - Ay)) - ((By - Ay) * (Cx - Ax)))) { + return false; + } for (p = 0; p < n; p++) { - if ((p == u) || (p == v) || (p == w)) continue; + if ((p == u) || (p == v) || (p == w)) { + continue; + } Px = contour[V[p]].x; Py = contour[V[p]].y; - if (is_inside_triangle(Ax, Ay, Bx, By, Cx, Cy, Px, Py, relaxed)) return false; + if (is_inside_triangle(Ax, Ay, Bx, By, Cx, Cy, Px, Py, relaxed)) { + return false; + } } return true; @@ -119,19 +124,24 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul /* allocate and initialize list of Vertices in polygon */ int n = contour.size(); - if (n < 3) return false; + if (n < 3) { + return false; + } Vector<int> V; V.resize(n); /* we want a counter-clockwise polygon in V */ - if (0.0 < get_area(contour)) - for (int v = 0; v < n; v++) + if (0.0 < get_area(contour)) { + for (int v = 0; v < n; v++) { V.write[v] = v; - else - for (int v = 0; v < n; v++) + } + } else { + for (int v = 0; v < n; v++) { V.write[v] = (n - 1) - v; + } + } bool relaxed = false; @@ -161,11 +171,17 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul /* three consecutive vertices in current polygon, <u,v,w> */ int u = v; - if (nv <= u) u = 0; /* previous */ + if (nv <= u) { + u = 0; /* previous */ + } v = u + 1; - if (nv <= v) v = 0; /* new v */ + if (nv <= v) { + v = 0; /* new v */ + } int w = v + 1; - if (nv <= w) w = 0; /* next */ + if (nv <= w) { + w = 0; /* next */ + } if (snip(contour, u, v, w, nv, V, relaxed)) { int a, b, c, s, t; @@ -181,8 +197,9 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul result.push_back(c); /* remove v from remaining polygon */ - for (s = v, t = v + 1; t < nv; s++, t++) + for (s = v, t = v + 1; t < nv; s++, t++) { V.write[s] = V[t]; + } nv--; diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index f46badd19e..233421e070 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -31,25 +31,20 @@ #include "vector2.h" real_t Vector2::angle() const { - return Math::atan2(y, x); } real_t Vector2::length() const { - return Math::sqrt(x * x + y * y); } real_t Vector2::length_squared() const { - return x * x + y * y; } void Vector2::normalize() { - real_t l = x * x + y * y; if (l != 0) { - l = Math::sqrt(l); x /= l; y /= l; @@ -57,7 +52,6 @@ void Vector2::normalize() { } Vector2 Vector2::normalized() const { - Vector2 v = *this; v.normalize(); return v; @@ -69,52 +63,42 @@ bool Vector2::is_normalized() const { } real_t Vector2::distance_to(const Vector2 &p_vector2) const { - return Math::sqrt((x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y)); } real_t Vector2::distance_squared_to(const Vector2 &p_vector2) const { - return (x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y); } real_t Vector2::angle_to(const Vector2 &p_vector2) const { - return Math::atan2(cross(p_vector2), dot(p_vector2)); } real_t Vector2::angle_to_point(const Vector2 &p_vector2) const { - return Math::atan2(y - p_vector2.y, x - p_vector2.x); } real_t Vector2::dot(const Vector2 &p_other) const { - return x * p_other.x + y * p_other.y; } real_t Vector2::cross(const Vector2 &p_other) const { - return x * p_other.y - y * p_other.x; } Vector2 Vector2::sign() const { - return Vector2(SGN(x), SGN(y)); } Vector2 Vector2::floor() const { - return Vector2(Math::floor(x), Math::floor(y)); } Vector2 Vector2::ceil() const { - return Vector2(Math::ceil(x), Math::ceil(y)); } Vector2 Vector2::round() const { - return Vector2(Math::round(x), Math::round(y)); } @@ -139,18 +123,15 @@ Vector2 Vector2::project(const Vector2 &p_b) const { } Vector2 Vector2::snapped(const Vector2 &p_by) const { - return Vector2( Math::stepify(x, p_by.x), Math::stepify(y, p_by.y)); } Vector2 Vector2::clamped(real_t p_len) const { - real_t l = length(); Vector2 v = *this; if (l > 0 && p_len < l) { - v /= l; v *= p_len; } @@ -159,7 +140,6 @@ Vector2 Vector2::clamped(real_t p_len) const { } Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const { - Vector2 p0 = p_pre_a; Vector2 p1 = *this; Vector2 p2 = p_b; @@ -210,65 +190,57 @@ bool Vector2::is_equal_approx(const Vector2 &p_v) const { /* Vector2i */ Vector2i Vector2i::operator+(const Vector2i &p_v) const { - return Vector2i(x + p_v.x, y + p_v.y); } -void Vector2i::operator+=(const Vector2i &p_v) { +void Vector2i::operator+=(const Vector2i &p_v) { x += p_v.x; y += p_v.y; } -Vector2i Vector2i::operator-(const Vector2i &p_v) const { +Vector2i Vector2i::operator-(const Vector2i &p_v) const { return Vector2i(x - p_v.x, y - p_v.y); } -void Vector2i::operator-=(const Vector2i &p_v) { +void Vector2i::operator-=(const Vector2i &p_v) { x -= p_v.x; y -= p_v.y; } Vector2i Vector2i::operator*(const Vector2i &p_v1) const { - return Vector2i(x * p_v1.x, y * p_v1.y); -}; +} Vector2i Vector2i::operator*(const int &rvalue) const { - return Vector2i(x * rvalue, y * rvalue); -}; -void Vector2i::operator*=(const int &rvalue) { +} +void Vector2i::operator*=(const int &rvalue) { x *= rvalue; y *= rvalue; -}; +} Vector2i Vector2i::operator/(const Vector2i &p_v1) const { - return Vector2i(x / p_v1.x, y / p_v1.y); -}; +} Vector2i Vector2i::operator/(const int &rvalue) const { - return Vector2i(x / rvalue, y / rvalue); -}; +} void Vector2i::operator/=(const int &rvalue) { - x /= rvalue; y /= rvalue; -}; +} Vector2i Vector2i::operator-() const { - return Vector2i(-x, -y); } bool Vector2i::operator==(const Vector2i &p_vec2) const { - return x == p_vec2.x && y == p_vec2.y; } -bool Vector2i::operator!=(const Vector2i &p_vec2) const { +bool Vector2i::operator!=(const Vector2i &p_vec2) const { return x != p_vec2.x || y != p_vec2.y; } diff --git a/core/math/vector2.h b/core/math/vector2.h index c0057f2543..8a08d3bf64 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -37,18 +37,17 @@ struct Vector2i; struct Vector2 { - enum Axis { AXIS_X, AXIS_Y, }; union { - real_t x; + real_t x = 0; real_t width; }; union { - real_t y; + real_t y = 0; real_t height; }; @@ -123,13 +122,11 @@ struct Vector2 { real_t angle() const; _FORCE_INLINE_ Vector2 abs() const { - return Vector2(Math::abs(x), Math::abs(y)); } Vector2 rotated(real_t p_by) const; Vector2 tangent() const { - return Vector2(y, -x); } @@ -142,89 +139,78 @@ struct Vector2 { operator String() const { return String::num(x) + ", " + String::num(y); } + _FORCE_INLINE_ Vector2() {} _FORCE_INLINE_ Vector2(real_t p_x, real_t p_y) { x = p_x; y = p_y; } - _FORCE_INLINE_ Vector2() { x = y = 0; } }; _FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2 &p_vec) const { - return p_vec - *this * (dot(p_vec) - p_d); } _FORCE_INLINE_ Vector2 operator*(real_t p_scalar, const Vector2 &p_vec) { - return p_vec * p_scalar; } _FORCE_INLINE_ Vector2 Vector2::operator+(const Vector2 &p_v) const { - return Vector2(x + p_v.x, y + p_v.y); } -_FORCE_INLINE_ void Vector2::operator+=(const Vector2 &p_v) { +_FORCE_INLINE_ void Vector2::operator+=(const Vector2 &p_v) { x += p_v.x; y += p_v.y; } -_FORCE_INLINE_ Vector2 Vector2::operator-(const Vector2 &p_v) const { +_FORCE_INLINE_ Vector2 Vector2::operator-(const Vector2 &p_v) const { return Vector2(x - p_v.x, y - p_v.y); } -_FORCE_INLINE_ void Vector2::operator-=(const Vector2 &p_v) { +_FORCE_INLINE_ void Vector2::operator-=(const Vector2 &p_v) { x -= p_v.x; y -= p_v.y; } _FORCE_INLINE_ Vector2 Vector2::operator*(const Vector2 &p_v1) const { - return Vector2(x * p_v1.x, y * p_v1.y); -}; +} _FORCE_INLINE_ Vector2 Vector2::operator*(const real_t &rvalue) const { - return Vector2(x * rvalue, y * rvalue); -}; -_FORCE_INLINE_ void Vector2::operator*=(const real_t &rvalue) { +} +_FORCE_INLINE_ void Vector2::operator*=(const real_t &rvalue) { x *= rvalue; y *= rvalue; -}; +} _FORCE_INLINE_ Vector2 Vector2::operator/(const Vector2 &p_v1) const { - return Vector2(x / p_v1.x, y / p_v1.y); -}; +} _FORCE_INLINE_ Vector2 Vector2::operator/(const real_t &rvalue) const { - return Vector2(x / rvalue, y / rvalue); -}; +} _FORCE_INLINE_ void Vector2::operator/=(const real_t &rvalue) { - x /= rvalue; y /= rvalue; -}; +} _FORCE_INLINE_ Vector2 Vector2::operator-() const { - return Vector2(-x, -y); } _FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const { - return x == p_vec2.x && y == p_vec2.y; } -_FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { +_FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { return x != p_vec2.x || y != p_vec2.y; } Vector2 Vector2::lerp(const Vector2 &p_b, real_t p_t) const { - Vector2 res = *this; res.x += (p_t * (p_b.x - x)); @@ -253,18 +239,17 @@ typedef Vector2 Point2; /* INTEGER STUFF */ struct Vector2i { - enum Axis { AXIS_X, AXIS_Y, }; union { - int x; + int x = 0; int width; }; union { - int y; + int y = 0; int height; }; @@ -307,6 +292,8 @@ struct Vector2i { operator String() const { return String::num(x) + ", " + String::num(y); } operator Vector2() const { return Vector2(x, y); } + + inline Vector2i() {} inline Vector2i(const Vector2 &p_vec2) { x = (int)p_vec2.x; y = (int)p_vec2.y; @@ -315,10 +302,6 @@ struct Vector2i { x = p_x; y = p_y; } - inline Vector2i() { - x = 0; - y = 0; - } }; typedef Vector2i Size2i; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index 353b2acd16..568df48c62 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -33,12 +33,10 @@ #include "core/math/basis.h" void Vector3::rotate(const Vector3 &p_axis, real_t p_phi) { - *this = Basis(p_axis, p_phi).xform(*this); } Vector3 Vector3::rotated(const Vector3 &p_axis, real_t p_phi) const { - Vector3 r = *this; r.rotate(p_axis, p_phi); return r; @@ -48,36 +46,33 @@ void Vector3::set_axis(int p_axis, real_t p_value) { ERR_FAIL_INDEX(p_axis, 3); coord[p_axis] = p_value; } -real_t Vector3::get_axis(int p_axis) const { +real_t Vector3::get_axis(int p_axis) const { ERR_FAIL_INDEX_V(p_axis, 3, 0); return operator[](p_axis); } int Vector3::min_axis() const { - return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); } -int Vector3::max_axis() const { +int Vector3::max_axis() const { return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); } void Vector3::snap(Vector3 p_val) { - x = Math::stepify(x, p_val.x); y = Math::stepify(y, p_val.y); z = Math::stepify(z, p_val.z); } -Vector3 Vector3::snapped(Vector3 p_val) const { +Vector3 Vector3::snapped(Vector3 p_val) const { Vector3 v = *this; v.snap(p_val); return v; } Vector3 Vector3::cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const { - Vector3 p0 = p_pre_a; Vector3 p1 = *this; Vector3 p2 = p_b; @@ -90,10 +85,12 @@ Vector3 Vector3::cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, real_t bc = p1.distance_to(p2); real_t cd = p2.distance_to(p3); - if (ab > 0) + if (ab > 0) { p0 = p1 + (p0 - p1) * (bc / ab); - if (cd > 0) + } + if (cd > 0) { p3 = p2 + (p3 - p2) * (bc / cd); + } } real_t t = p_t; @@ -109,7 +106,6 @@ Vector3 Vector3::cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, } Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const { - Vector3 p0 = p_pre_a; Vector3 p1 = *this; Vector3 p2 = p_b; @@ -135,7 +131,6 @@ Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const { } 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); @@ -150,11 +145,9 @@ Basis Vector3::to_diagonal_matrix() const { } 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 a5e9d09208..0bc1a467f2 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -38,7 +38,6 @@ class Basis; struct Vector3 { - enum Axis { AXIS_X, AXIS_Y, @@ -52,16 +51,14 @@ struct Vector3 { real_t z; }; - real_t coord[3]; + real_t coord[3] = { 0 }; }; _FORCE_INLINE_ const real_t &operator[](int p_axis) const { - return coord[p_axis]; } _FORCE_INLINE_ real_t &operator[](int p_axis) { - return coord[p_axis]; } @@ -152,22 +149,20 @@ struct Vector3 { return Vector3i(x, y, z); } + _FORCE_INLINE_ Vector3() {} _FORCE_INLINE_ Vector3(const Vector3i &p_ivec) { x = p_ivec.x; y = p_ivec.y; z = p_ivec.z; } - _FORCE_INLINE_ Vector3(real_t p_x, real_t p_y, real_t p_z) { x = p_x; y = p_y; z = p_z; } - _FORCE_INLINE_ Vector3() { x = y = z = 0; } }; Vector3 Vector3::cross(const Vector3 &p_b) const { - Vector3 ret( (y * p_b.z) - (z * p_b.y), (z * p_b.x) - (x * p_b.z), @@ -177,37 +172,30 @@ Vector3 Vector3::cross(const Vector3 &p_b) const { } real_t Vector3::dot(const Vector3 &p_b) const { - return x * p_b.x + y * p_b.y + z * p_b.z; } Vector3 Vector3::abs() const { - return Vector3(Math::abs(x), Math::abs(y), Math::abs(z)); } Vector3 Vector3::sign() const { - return Vector3(SGN(x), SGN(y), SGN(z)); } Vector3 Vector3::floor() const { - return Vector3(Math::floor(x), Math::floor(y), Math::floor(z)); } Vector3 Vector3::ceil() const { - return Vector3(Math::ceil(x), Math::ceil(y), Math::ceil(z)); } Vector3 Vector3::round() const { - return Vector3(Math::round(x), Math::round(y), Math::round(z)); } Vector3 Vector3::lerp(const Vector3 &p_b, real_t p_t) const { - return Vector3( x + (p_t * (p_b.x - x)), y + (p_t * (p_b.y - y)), @@ -220,12 +208,10 @@ Vector3 Vector3::slerp(const Vector3 &p_b, real_t p_t) const { } real_t Vector3::distance_to(const Vector3 &p_b) const { - return (p_b - *this).length(); } real_t Vector3::distance_squared_to(const Vector3 &p_b) const { - return (p_b - *this).length_squared(); } @@ -242,7 +228,6 @@ Vector3 Vector3::project(const Vector3 &p_b) const { } real_t Vector3::angle_to(const Vector3 &p_b) const { - return Math::atan2(cross(p_b).length(), dot(p_b)); } @@ -255,7 +240,6 @@ Vector3 Vector3::direction_to(const Vector3 &p_b) const { /* Operators */ Vector3 &Vector3::operator+=(const Vector3 &p_v) { - x += p_v.x; y += p_v.y; z += p_v.z; @@ -263,36 +247,32 @@ Vector3 &Vector3::operator+=(const Vector3 &p_v) { } Vector3 Vector3::operator+(const Vector3 &p_v) const { - return Vector3(x + p_v.x, y + p_v.y, z + p_v.z); } Vector3 &Vector3::operator-=(const Vector3 &p_v) { - x -= p_v.x; y -= p_v.y; z -= p_v.z; return *this; } -Vector3 Vector3::operator-(const Vector3 &p_v) const { +Vector3 Vector3::operator-(const Vector3 &p_v) const { return Vector3(x - p_v.x, y - p_v.y, z - p_v.z); } Vector3 &Vector3::operator*=(const Vector3 &p_v) { - x *= p_v.x; y *= p_v.y; z *= p_v.z; return *this; } -Vector3 Vector3::operator*(const Vector3 &p_v) const { +Vector3 Vector3::operator*(const Vector3 &p_v) const { return Vector3(x * p_v.x, y * p_v.y, z * p_v.z); } Vector3 &Vector3::operator/=(const Vector3 &p_v) { - x /= p_v.x; y /= p_v.y; z /= p_v.z; @@ -300,12 +280,10 @@ Vector3 &Vector3::operator/=(const Vector3 &p_v) { } Vector3 Vector3::operator/(const Vector3 &p_v) const { - return Vector3(x / p_v.x, y / p_v.y, z / p_v.z); } Vector3 &Vector3::operator*=(real_t p_scalar) { - x *= p_scalar; y *= p_scalar; z *= p_scalar; @@ -313,17 +291,14 @@ Vector3 &Vector3::operator*=(real_t p_scalar) { } _FORCE_INLINE_ Vector3 operator*(real_t p_scalar, const Vector3 &p_vec) { - return p_vec * p_scalar; } Vector3 Vector3::operator*(real_t p_scalar) const { - return Vector3(x * p_scalar, y * p_scalar, z * p_scalar); } Vector3 &Vector3::operator/=(real_t p_scalar) { - x /= p_scalar; y /= p_scalar; z /= p_scalar; @@ -331,85 +306,78 @@ Vector3 &Vector3::operator/=(real_t p_scalar) { } Vector3 Vector3::operator/(real_t p_scalar) const { - return Vector3(x / p_scalar, y / p_scalar, z / p_scalar); } Vector3 Vector3::operator-() const { - return Vector3(-x, -y, -z); } bool Vector3::operator==(const Vector3 &p_v) const { - return x == p_v.x && y == p_v.y && z == p_v.z; } bool Vector3::operator!=(const Vector3 &p_v) const { - return x != p_v.x || y != p_v.y || z != p_v.z; } bool Vector3::operator<(const Vector3 &p_v) const { - if (Math::is_equal_approx(x, p_v.x)) { - if (Math::is_equal_approx(y, p_v.y)) + if (Math::is_equal_approx(y, p_v.y)) { return z < p_v.z; - else + } else { return y < p_v.y; + } } else { return x < p_v.x; } } bool Vector3::operator>(const Vector3 &p_v) const { - if (Math::is_equal_approx(x, p_v.x)) { - if (Math::is_equal_approx(y, p_v.y)) + if (Math::is_equal_approx(y, p_v.y)) { return z > p_v.z; - else + } else { return y > p_v.y; + } } else { return x > p_v.x; } } bool Vector3::operator<=(const Vector3 &p_v) const { - if (Math::is_equal_approx(x, p_v.x)) { - if (Math::is_equal_approx(y, p_v.y)) + if (Math::is_equal_approx(y, p_v.y)) { return z <= p_v.z; - else + } else { return y < p_v.y; + } } else { return x < p_v.x; } } bool Vector3::operator>=(const Vector3 &p_v) const { - if (Math::is_equal_approx(x, p_v.x)) { - if (Math::is_equal_approx(y, p_v.y)) + if (Math::is_equal_approx(y, p_v.y)) { return z >= p_v.z; - else + } else { return y > p_v.y; + } } else { return x > p_v.x; } } _FORCE_INLINE_ Vector3 vec3_cross(const Vector3 &p_a, const Vector3 &p_b) { - return p_a.cross(p_b); } _FORCE_INLINE_ real_t vec3_dot(const Vector3 &p_a, const Vector3 &p_b) { - return p_a.dot(p_b); } real_t Vector3::length() const { - real_t x2 = x * x; real_t y2 = y * y; real_t z2 = z * z; @@ -418,7 +386,6 @@ real_t Vector3::length() const { } real_t Vector3::length_squared() const { - real_t x2 = x * x; real_t y2 = y * y; real_t z2 = z * z; @@ -427,7 +394,6 @@ real_t Vector3::length_squared() const { } void Vector3::normalize() { - real_t lengthsq = length_squared(); if (lengthsq == 0) { x = y = z = 0; @@ -440,7 +406,6 @@ void Vector3::normalize() { } Vector3 Vector3::normalized() const { - Vector3 v = *this; v.normalize(); return v; @@ -452,12 +417,10 @@ bool Vector3::is_normalized() const { } Vector3 Vector3::inverse() const { - return Vector3(1.0 / x, 1.0 / y, 1.0 / z); } void Vector3::zero() { - x = y = z = 0; } diff --git a/core/math/vector3i.cpp b/core/math/vector3i.cpp index 8a4ddf03b9..718a1553a0 100644 --- a/core/math/vector3i.cpp +++ b/core/math/vector3i.cpp @@ -34,22 +34,20 @@ void Vector3i::set_axis(int p_axis, int32_t p_value) { ERR_FAIL_INDEX(p_axis, 3); coord[p_axis] = p_value; } -int32_t Vector3i::get_axis(int p_axis) const { +int32_t Vector3i::get_axis(int p_axis) const { ERR_FAIL_INDEX_V(p_axis, 3, 0); return operator[](p_axis); } int Vector3i::min_axis() const { - return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); } -int Vector3i::max_axis() const { +int Vector3i::max_axis() const { return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); } Vector3i::operator String() const { - return (itos(x) + ", " + itos(y) + ", " + itos(z)); } diff --git a/core/math/vector3i.h b/core/math/vector3i.h index 6f9754d3b9..08729ad056 100644 --- a/core/math/vector3i.h +++ b/core/math/vector3i.h @@ -35,7 +35,6 @@ #include "core/ustring.h" struct Vector3i { - enum Axis { AXIS_X, AXIS_Y, @@ -49,16 +48,14 @@ struct Vector3i { int32_t z; }; - int32_t coord[3]; + int32_t coord[3] = { 0 }; }; _FORCE_INLINE_ const int32_t &operator[](int p_axis) const { - return coord[p_axis]; } _FORCE_INLINE_ int32_t &operator[](int p_axis) { - return coord[p_axis]; } @@ -100,28 +97,25 @@ struct Vector3i { operator String() const; + _FORCE_INLINE_ Vector3i() {} _FORCE_INLINE_ Vector3i(int32_t p_x, int32_t p_y, int32_t p_z) { x = p_x; y = p_y; z = p_z; } - _FORCE_INLINE_ Vector3i() { x = y = z = 0; } }; Vector3i Vector3i::abs() const { - return Vector3i(ABS(x), ABS(y), ABS(z)); } Vector3i Vector3i::sign() const { - return Vector3i(SGN(x), SGN(y), SGN(z)); } /* Operators */ Vector3i &Vector3i::operator+=(const Vector3i &p_v) { - x += p_v.x; y += p_v.y; z += p_v.z; @@ -129,36 +123,32 @@ Vector3i &Vector3i::operator+=(const Vector3i &p_v) { } Vector3i Vector3i::operator+(const Vector3i &p_v) const { - return Vector3i(x + p_v.x, y + p_v.y, z + p_v.z); } Vector3i &Vector3i::operator-=(const Vector3i &p_v) { - x -= p_v.x; y -= p_v.y; z -= p_v.z; return *this; } -Vector3i Vector3i::operator-(const Vector3i &p_v) const { +Vector3i Vector3i::operator-(const Vector3i &p_v) const { return Vector3i(x - p_v.x, y - p_v.y, z - p_v.z); } Vector3i &Vector3i::operator*=(const Vector3i &p_v) { - x *= p_v.x; y *= p_v.y; z *= p_v.z; return *this; } -Vector3i Vector3i::operator*(const Vector3i &p_v) const { +Vector3i Vector3i::operator*(const Vector3i &p_v) const { return Vector3i(x * p_v.x, y * p_v.y, z * p_v.z); } Vector3i &Vector3i::operator/=(const Vector3i &p_v) { - x /= p_v.x; y /= p_v.y; z /= p_v.z; @@ -166,12 +156,10 @@ Vector3i &Vector3i::operator/=(const Vector3i &p_v) { } Vector3i Vector3i::operator/(const Vector3i &p_v) const { - return Vector3i(x / p_v.x, y / p_v.y, z / p_v.z); } Vector3i &Vector3i::operator*=(int32_t p_scalar) { - x *= p_scalar; y *= p_scalar; z *= p_scalar; @@ -179,17 +167,14 @@ Vector3i &Vector3i::operator*=(int32_t p_scalar) { } _FORCE_INLINE_ Vector3i operator*(int32_t p_scalar, const Vector3i &p_vec) { - return p_vec * p_scalar; } Vector3i Vector3i::operator*(int32_t p_scalar) const { - return Vector3i(x * p_scalar, y * p_scalar, z * p_scalar); } Vector3i &Vector3i::operator/=(int32_t p_scalar) { - x /= p_scalar; y /= p_scalar; z /= p_scalar; @@ -197,75 +182,70 @@ Vector3i &Vector3i::operator/=(int32_t p_scalar) { } Vector3i Vector3i::operator/(int32_t p_scalar) const { - return Vector3i(x / p_scalar, y / p_scalar, z / p_scalar); } Vector3i Vector3i::operator-() const { - return Vector3i(-x, -y, -z); } bool Vector3i::operator==(const Vector3i &p_v) const { - return (x == p_v.x && y == p_v.y && z == p_v.z); } bool Vector3i::operator!=(const Vector3i &p_v) const { - return (x != p_v.x || y != p_v.y || z != p_v.z); } bool Vector3i::operator<(const Vector3i &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (y == p_v.y) { return z < p_v.z; - else + } else { return y < p_v.y; + } } else { return x < p_v.x; } } bool Vector3i::operator>(const Vector3i &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (y == p_v.y) { return z > p_v.z; - else + } else { return y > p_v.y; + } } else { return x > p_v.x; } } bool Vector3i::operator<=(const Vector3i &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (y == p_v.y) { return z <= p_v.z; - else + } else { return y < p_v.y; + } } else { return x < p_v.x; } } bool Vector3i::operator>=(const Vector3i &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (y == p_v.y) { return z >= p_v.z; - else + } else { return y > p_v.y; + } } else { return x > p_v.x; } } void Vector3i::zero() { - x = y = z = 0; } |