diff options
Diffstat (limited to 'core/math')
79 files changed, 13769 insertions, 5814 deletions
diff --git a/core/math/SCsub b/core/math/SCsub index be438fcfbe..c8fdac207e 100644 --- a/core/math/SCsub +++ b/core/math/SCsub @@ -1,6 +1,6 @@ #!/usr/bin/env python -Import('env') +Import("env") env_math = env.Clone() diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 73f190a330..88e11a630c 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,13 +30,12 @@ #include "a_star.h" -#include "core/math/geometry.h" -#include "core/script_language.h" +#include "core/math/geometry_3d.h" +#include "core/object/script_language.h" #include "scene/scene_string_names.h" int AStar::get_available_point_id() const { - - if (points.empty()) { + if (points.is_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); @@ -66,7 +64,7 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { pt->id = p_id; pt->pos = p_pos; pt->weight_scale = p_weight_scale; - pt->prev_point = NULL; + pt->prev_point = nullptr; pt->open_pass = 0; pt->closed_pass = 0; pt->enabled = true; @@ -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,10 +154,12 @@ 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 != NULL) { + if (element != nullptr) { s.direction |= element->get().direction; if (s.direction == Segment::BIDIRECTIONAL) { // Both are neighbours of each other now @@ -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); @@ -194,7 +185,7 @@ void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : s.direction; Set<Segment>::Element *element = segments.find(s); - if (element != NULL) { + if (element != nullptr) { // s is the new segment // Erase the directions to be removed s.direction = (element->get().direction & ~remove_direction); @@ -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)) { @@ -235,13 +226,12 @@ Array AStar::get_points() { return point_list; } -PoolVector<int> AStar::get_point_connections(int p_id) { - +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, PoolVector<int>()); + ERR_FAIL_COND_V(!p_exists, Vector<int>()); - PoolVector<int> point_list; + Vector<int> point_list; for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { point_list.push_back((*it.key)); @@ -251,16 +241,14 @@ PoolVector<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); - return element != NULL && + return element != nullptr && (bidirectional || (element->get().direction & s.direction) == s.direction); } 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; @@ -351,8 +341,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); open_list.push_back(begin_point); - while (!open_list.empty()) { - + while (!open_list.is_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) { @@ -399,10 +387,10 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { return found_route; } -float AStar::_estimate_cost(int p_from_id, int p_to_id) { - - if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost)) +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)) { 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); @@ -415,10 +403,10 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -float AStar::_compute_cost(int p_from_id, int p_to_id) { - - if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost)) +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)) { 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); @@ -431,18 +419,17 @@ float AStar::_compute_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -PoolVector<Vector3> AStar::get_point_path(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, PoolVector<Vector3>()); + ERR_FAIL_COND_V(!from_exists, Vector<Vector3>()); Point *b; bool to_exists = points.lookup(p_to_id, b); - ERR_FAIL_COND_V(!to_exists, PoolVector<Vector3>()); + ERR_FAIL_COND_V(!to_exists, Vector<Vector3>()); if (a == b) { - PoolVector<Vector3> ret; + Vector<Vector3> ret; ret.push_back(a->pos); return ret; } @@ -451,7 +438,9 @@ PoolVector<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 PoolVector<Vector3>(); + if (!found_route) { + return Vector<Vector3>(); + } Point *p = end_point; int pc = 1; // Begin point @@ -460,11 +449,11 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { p = p->prev_point; } - PoolVector<Vector3> path; + Vector<Vector3> path; path.resize(pc); { - PoolVector<Vector3>::Write w = path.write(); + Vector3 *w = path.ptrw(); Point *p2 = end_point; int idx = pc - 1; @@ -479,18 +468,17 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { return path; } -PoolVector<int> AStar::get_id_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, PoolVector<int>()); + ERR_FAIL_COND_V(!from_exists, Vector<int>()); Point *b; bool to_exists = points.lookup(p_to_id, b); - ERR_FAIL_COND_V(!to_exists, PoolVector<int>()); + ERR_FAIL_COND_V(!to_exists, Vector<int>()); if (a == b) { - PoolVector<int> ret; + Vector<int> ret; ret.push_back(a->id); return ret; } @@ -499,7 +487,9 @@ PoolVector<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 PoolVector<int>(); + if (!found_route) { + return Vector<int>(); + } Point *p = end_point; int pc = 1; // Begin point @@ -508,11 +498,11 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { p = p->prev_point; } - PoolVector<int> path; + Vector<int> path; path.resize(pc); { - PoolVector<int>::Write w = path.write(); + int *w = path.ptrw(); p = end_point; int idx = pc - 1; @@ -528,7 +518,6 @@ PoolVector<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); @@ -576,13 +563,8 @@ void AStar::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar::get_point_path); ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar::get_id_path); - BIND_VMETHOD(MethodInfo(Variant::REAL, "_estimate_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id"))); - BIND_VMETHOD(MethodInfo(Variant::REAL, "_compute_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id"))); -} - -AStar::AStar() { - last_free_id = 0; - pass = 1; + 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"))); } AStar::~AStar() { @@ -624,7 +606,7 @@ bool AStar2D::has_point(int p_id) const { return astar.has_point(p_id); } -PoolVector<int> AStar2D::get_point_connections(int p_id) { +Vector<int> AStar2D::get_point_connections(int p_id) { return astar.get_point_connections(p_id); } @@ -677,29 +659,199 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { return Vector2(p.x, p.y); } -PoolVector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { +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)) { + 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); + ERR_FAIL_COND_V(!from_exists, 0); + + AStar::Point *to_point; + bool to_exists = astar.points.lookup(p_to_id, to_point); + ERR_FAIL_COND_V(!to_exists, 0); + + return from_point->pos.distance_to(to_point->pos); +} + +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)) { + 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); + ERR_FAIL_COND_V(!from_exists, 0); + + AStar::Point *to_point; + bool to_exists = astar.points.lookup(p_to_id, to_point); + ERR_FAIL_COND_V(!to_exists, 0); + + return from_point->pos.distance_to(to_point->pos); +} + +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>()); + + AStar::Point *b; + bool to_exists = astar.points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, Vector<Vector2>()); + + if (a == b) { + Vector<Vector2> ret; + ret.push_back(Vector2(a->pos.x, a->pos.y)); + return ret; + } + + AStar::Point *begin_point = a; + AStar::Point *end_point = b; + + bool found_route = _solve(begin_point, end_point); + if (!found_route) { + return Vector<Vector2>(); + } + + AStar::Point *p = end_point; + int pc = 1; // Begin point + while (p != begin_point) { + pc++; + p = p->prev_point; + } + + Vector<Vector2> path; + path.resize(pc); + + { + Vector2 *w = path.ptrw(); + + AStar::Point *p2 = end_point; + int idx = pc - 1; + while (p2 != begin_point) { + w[idx--] = Vector2(p2->pos.x, p2->pos.y); + p2 = p2->prev_point; + } + + w[0] = Vector2(p2->pos.x, p2->pos.y); // Assign first + } + + return path; +} + +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>()); + + AStar::Point *b; + bool to_exists = astar.points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, Vector<int>()); + + if (a == b) { + Vector<int> ret; + ret.push_back(a->id); + return ret; + } + + AStar::Point *begin_point = a; + AStar::Point *end_point = b; + + bool found_route = _solve(begin_point, end_point); + if (!found_route) { + return Vector<int>(); + } + + AStar::Point *p = end_point; + int pc = 1; // Begin point + while (p != begin_point) { + pc++; + p = p->prev_point; + } + + Vector<int> path; + path.resize(pc); - PoolVector3Array pv = astar.get_point_path(p_from_id, p_to_id); - int size = pv.size(); - PoolVector2Array path; - path.resize(size); { - PoolVector<Vector3>::Read r = pv.read(); - PoolVector<Vector2>::Write w = path.write(); - for (int i = 0; i < size; i++) { - Vector3 p = r[i]; - w[i] = Vector2(p.x, p.y); + int *w = path.ptrw(); + + p = end_point; + int idx = pc - 1; + while (p != begin_point) { + w[idx--] = p->id; + p = p->prev_point; } + + w[0] = p->id; // Assign first } + return path; } -PoolVector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { - return astar.get_id_path(p_from_id, p_to_id); +bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { + astar.pass++; + + if (!end_point->enabled) { + return false; + } + + bool found_route = false; + + Vector<AStar::Point *> open_list; + SortArray<AStar::Point *, AStar::SortPoints> sorter; + + begin_point->g_score = 0; + begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); + open_list.push_back(begin_point); + + while (!open_list.is_empty()) { + AStar::Point *p = open_list[0]; // The currently processed point + + if (p == end_point) { + found_route = true; + break; + } + + sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list + open_list.remove(open_list.size() - 1); + 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) { + continue; + } + + real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale; + + bool new_point = false; + + if (e->open_pass != astar.pass) { // The point wasn't inside the open list. + e->open_pass = astar.pass; + open_list.push_back(e); + new_point = true; + } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous. + continue; + } + + e->prev_point = p; + e->g_score = tentative_g_score; + e->f_score = e->g_score + _estimate_cost(e->id, end_point->id); + + if (new_point) { // The position of the new points is already known. + sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw()); + } else { + sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw()); + } + } + } + + return found_route; } 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); @@ -728,10 +880,7 @@ void AStar2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path); ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::get_id_path); -} - -AStar2D::AStar2D() { -} -AStar2D::~AStar2D() { + 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"))); } diff --git a/core/math/a_star.h b/core/math/a_star.h index 0b10976932..44758cb046 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -28,11 +28,11 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef ASTAR_H -#define ASTAR_H +#ifndef A_STAR_H +#define A_STAR_H -#include "core/oa_hash_map.h" -#include "core/reference.h" +#include "core/object/ref_counted.h" +#include "core/templates/oa_hash_map.h" /** A* pathfinding algorithm @@ -40,30 +40,27 @@ @author Juan Linietsky <reduzio@gmail.com> */ -class AStar : public Reference { - - GDCLASS(AStar, Reference); +class AStar : public RefCounted { + GDCLASS(AStar, RefCounted); + friend class AStar2D; struct Point { + Point() {} - Point() : - neighbours(4u), - unlinked_neighbours(4u) {} - - int id; + int id = 0; Vector3 pos; - real_t weight_scale; - bool enabled; + real_t weight_scale = 0; + bool enabled = false; - 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; - real_t g_score; - real_t f_score; - uint64_t open_pass; - uint64_t closed_pass; + Point *prev_point = nullptr; + real_t g_score = 0; + real_t f_score = 0; + uint64_t open_pass = 0; + uint64_t closed_pass = 0; }; struct SortPoints { @@ -84,7 +81,7 @@ class AStar : public Reference { int32_t u; int32_t v; }; - uint64_t key; + uint64_t key = 0; }; enum { @@ -93,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; @@ -113,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; @@ -124,8 +119,8 @@ class AStar : public Reference { protected: static void _bind_methods(); - virtual float _estimate_cost(int p_from_id, int p_to_id); - virtual float _compute_cost(int p_from_id, int p_to_id); + virtual real_t _estimate_cost(int p_from_id, int p_to_id); + virtual real_t _compute_cost(int p_from_id, int p_to_id); public: int get_available_point_id() const; @@ -137,7 +132,7 @@ public: void set_point_weight_scale(int p_id, real_t p_weight_scale); void remove_point(int p_id); bool has_point(int p_id) const; - PoolVector<int> get_point_connections(int p_id); + Vector<int> get_point_connections(int p_id); Array get_points(); void set_point_disabled(int p_id, bool p_disabled = true); @@ -155,20 +150,25 @@ public: int get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const; Vector3 get_closest_position_in_segment(const Vector3 &p_point) const; - PoolVector<Vector3> get_point_path(int p_from_id, int p_to_id); - PoolVector<int> get_id_path(int p_from_id, int p_to_id); + 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(); }; -class AStar2D : public Reference { - GDCLASS(AStar2D, Reference); +class AStar2D : public RefCounted { + GDCLASS(AStar2D, RefCounted); AStar astar; + bool _solve(AStar::Point *begin_point, AStar::Point *end_point); + protected: static void _bind_methods(); + virtual real_t _estimate_cost(int p_from_id, int p_to_id); + virtual real_t _compute_cost(int p_from_id, int p_to_id); + public: int get_available_point_id() const; @@ -179,7 +179,7 @@ public: void set_point_weight_scale(int p_id, real_t p_weight_scale); void remove_point(int p_id); bool has_point(int p_id) const; - PoolVector<int> get_point_connections(int p_id); + Vector<int> get_point_connections(int p_id); Array get_points(); void set_point_disabled(int p_id, bool p_disabled = true); @@ -197,11 +197,11 @@ public: int get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const; Vector2 get_closest_position_in_segment(const Vector2 &p_point) const; - PoolVector<Vector2> get_point_path(int p_from_id, int p_to_id); - PoolVector<int> get_id_path(int p_from_id, int p_to_id); + 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 // ASTAR_H +#endif // A_STAR_H diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 19d60fea72..33aa65f15d 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,24 +30,22 @@ #include "aabb.h" -#include "core/print_string.h" +#include "core/string/print_string.h" +#include "core/variant/variant.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 +68,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 +79,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 +104,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 +135,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 +147,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 +160,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 +183,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 +199,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 +222,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 +247,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 +265,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 +279,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 +297,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 +331,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 +349,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); @@ -396,7 +376,21 @@ void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { } } -AABB::operator String() const { +Variant AABB::intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const { + Vector3 inters; + if (intersects_segment(p_from, p_to, &inters)) { + return inters; + } + return Variant(); +} +Variant AABB::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const { + Vector3 inters; + if (intersects_ray(p_from, p_dir, &inters)) { + return inters; + } + return Variant(); +} - return String() + position + " - " + size; +AABB::operator String() const { + return "[P: " + position.operator String() + ", S: " + size + "]"; } diff --git a/core/math/aabb.h b/core/math/aabb.h index d9d50c7139..e16246902a 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -39,6 +39,7 @@ * AABB / AABB (Axis Aligned Bounding Box) * This is implemented by a point (position) and the box size */ +class Variant; class AABB { public: @@ -47,12 +48,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); } @@ -72,11 +71,11 @@ public: AABB merge(const AABB &p_with) const; void merge_with(const AABB &p_aabb); ///merge with another AABB AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs - bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip = NULL, Vector3 *r_normal = NULL) const; - bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = NULL, Vector3 *r_normal = NULL) const; + bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const; + bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const; _FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const; - _FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count) const; + _FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const; _FORCE_INLINE_ bool inside_convex_shape(const Plane *p_planes, int p_plane_count) const; bool intersects_plane(const Plane &p_plane) const; @@ -101,6 +100,24 @@ public: _FORCE_INLINE_ void project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const; _FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */ + _FORCE_INLINE_ AABB abs() const { + return AABB(Vector3(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0), position.z + MIN(size.z, 0)), size.abs()); + } + + Variant intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const; + Variant intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const; + + _FORCE_INLINE_ void quantize(real_t p_unit); + _FORCE_INLINE_ AABB quantized(real_t p_unit) const; + + _FORCE_INLINE_ void set_end(const Vector3 &p_end) { + size = p_end - position; + } + + _FORCE_INLINE_ Vector3 get_end() const { + return position + size; + } + operator String() const; _FORCE_INLINE_ AABB() {} @@ -111,43 +128,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,35 +189,40 @@ 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; return Vector3( - (p_normal.x > 0) ? -half_extents.x : half_extents.x, - (p_normal.y > 0) ? -half_extents.y : half_extents.y, - (p_normal.z > 0) ? -half_extents.z : half_extents.z) + + (p_normal.x > 0) ? half_extents.x : -half_extents.x, + (p_normal.y > 0) ? half_extents.y : -half_extents.y, + (p_normal.z > 0) ? half_extents.z : -half_extents.z) + ofs; } 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 { - +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,15 +233,38 @@ 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 + // each axis. + int bad_point_counts_positive[3] = { 0 }; + 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]++; + } + if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) { + bad_point_counts_negative[k]++; + } + } + + if (bad_point_counts_negative[k] == p_point_count) { return false; + } + if (bad_point_counts_positive[k] == p_point_count) { + return false; + } } return true; } 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; @@ -221,56 +275,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); @@ -281,7 +345,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) { @@ -296,7 +359,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) { @@ -311,7 +373,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; @@ -332,12 +393,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; @@ -345,17 +409,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; @@ -364,4 +430,28 @@ void AABB::grow_by(real_t p_amount) { size.z += 2.0 * p_amount; } +void AABB::quantize(real_t p_unit) { + size += position; + + position.x -= Math::fposmodp(position.x, p_unit); + position.y -= Math::fposmodp(position.y, p_unit); + position.z -= Math::fposmodp(position.z, p_unit); + + size.x -= Math::fposmodp(size.x, p_unit); + size.y -= Math::fposmodp(size.y, p_unit); + size.z -= Math::fposmodp(size.z, p_unit); + + size.x += p_unit; + size.y += p_unit; + size.z += p_unit; + + size -= position; +} + +AABB AABB::quantized(real_t p_unit) const { + AABB ret = *this; + ret.quantize(p_unit); + return ret; +} + #endif // AABB_H diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index 6477d029d5..a5616b8d79 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -28,8 +28,8 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef AUDIOFRAME_H -#define AUDIOFRAME_H +#ifndef AUDIO_FRAME_H +#define AUDIO_FRAME_H #include "core/math/vector2.h" #include "core/typedefs.h" @@ -47,8 +47,10 @@ static inline float undenormalise(volatile float f) { return (v.i & 0x7f800000) < 0x08000000 ? 0.0f : f; } -struct AudioFrame { +static const float AUDIO_PEAK_OFFSET = 0.0000000001f; +static const float AUDIO_MIN_PEAK_DB = -200.0f; // linear2db(AUDIO_PEAK_OFFSET) +struct AudioFrame { //left and right samples float l, r; @@ -104,8 +106,7 @@ struct AudioFrame { r = ::undenormalise(r); } - _FORCE_INLINE_ AudioFrame linear_interpolate(const AudioFrame &p_b, float p_t) const { - + _FORCE_INLINE_ AudioFrame lerp(const AudioFrame &p_b, float p_t) const { AudioFrame res = *this; res.l += (p_t * (p_b.l - l)); @@ -123,7 +124,7 @@ struct AudioFrame { r = p_frame.r; } - _ALWAYS_INLINE_ AudioFrame operator=(const AudioFrame &p_frame) { + _ALWAYS_INLINE_ AudioFrame &operator=(const AudioFrame &p_frame) { l = p_frame.l; r = p_frame.r; return *this; @@ -140,4 +141,4 @@ struct AudioFrame { _ALWAYS_INLINE_ AudioFrame() {} }; -#endif +#endif // AUDIO_FRAME_H diff --git a/core/math/basis.cpp b/core/math/basis.cpp index ddf5f13d55..aa3831d4cf 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,23 +31,19 @@ #include "basis.h" #include "core/math/math_funcs.h" -#include "core/os/copymem.h" -#include "core/print_string.h" +#include "core/string/print_string.h" #define cofac(row1, col1, row2, col2) \ (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 +54,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,11 +71,6 @@ void Basis::invert() { } void Basis::orthonormalize() { - -#ifdef MATH_CHECKS - ERR_FAIL_COND(determinant() == 0); -#endif - // Gram-Schmidt Process Vector3 x = get_axis(0); @@ -99,7 +89,6 @@ void Basis::orthonormalize() { } Basis Basis::orthonormalized() const { - Basis c = *this; c.orthonormalize(); return c; @@ -120,25 +109,29 @@ bool Basis::is_diagonal() const { } bool Basis::is_rotation() const { - return Math::is_equal_approx(determinant(), 1, UNIT_EPSILON) && is_orthogonal(); + return Math::is_equal_approx(determinant(), 1, (real_t)UNIT_EPSILON) && is_orthogonal(); } +#ifdef MATH_CHECKS +// This method is only used once, in diagonalize. If it's desired elsewhere, feel free to remove the #ifdef. bool Basis::is_symmetric() const { - - if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) + if (!Math::is_equal_approx(elements[0][1], elements[1][0])) { return false; - if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) + } + if (!Math::is_equal_approx(elements[0][2], elements[2][0])) { return false; - if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) + } + if (!Math::is_equal_approx(elements[1][2], elements[2][1])) { return false; + } return true; } +#endif Basis Basis::diagonalize() { - //NOTE: only implemented for symmetric matrices -//with the Jacobi iterative method method +//with the Jacobi iterative method #ifdef MATH_CHECKS ERR_FAIL_COND_V(!is_symmetric(), Basis()); #endif @@ -197,21 +190,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; @@ -220,7 +210,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; @@ -244,6 +233,18 @@ void Basis::scale_local(const Vector3 &p_scale) { *this = scaled_local(p_scale); } +float Basis::get_uniform_scale() const { + return (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0; +} + +void Basis::make_scale_uniform() { + float l = (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0; + for (int i = 0; i < 3; i++) { + elements[i].normalize(); + elements[i] *= l; + } +} + Basis Basis::scaled_local(const Vector3 &p_scale) const { Basis b; b.set_diagonal(p_scale); @@ -252,7 +253,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(), @@ -316,7 +316,7 @@ Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const { // Multiplies the matrix from left by the rotation matrix: M -> R.M // Note that this does *not* rotate the matrix itself. // -// The main use of Basis is as Transform.basis, which is used a the transformation matrix +// The main use of Basis is as Transform.basis, which is used by the transformation matrix // of 3D object. Rotate here refers to rotation of the object (which is R * (*this)), // not the matrix itself (which is R * (*this) * R.transposed()). Basis Basis::rotated(const Vector3 &p_axis, real_t p_phi) const { @@ -332,8 +332,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); } @@ -345,12 +345,12 @@ void Basis::rotate(const Vector3 &p_euler) { *this = rotated(p_euler); } -Basis Basis::rotated(const Quat &p_quat) const { - return Basis(p_quat) * (*this); +Basis Basis::rotated(const Quaternion &p_quaternion) const { + return Basis(p_quaternion) * (*this); } -void Basis::rotate(const Quat &p_quat) { - *this = rotated(p_quat); +void Basis::rotate(const Quaternion &p_quaternion) { + *this = rotated(p_quaternion); } Vector3 Basis::get_rotation_euler() const { @@ -367,7 +367,7 @@ Vector3 Basis::get_rotation_euler() const { return m.get_euler(); } -Quat Basis::get_rotation_quat() const { +Quaternion Basis::get_rotation_quaternion() const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). // See the comment in get_scale() for further information. @@ -378,7 +378,7 @@ Quat Basis::get_rotation_quat() const { m.scale(Vector3(-1, -1, -1)); } - return m.get_quat(); + return m.get_quaternion(); } void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { @@ -422,7 +422,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 // @@ -431,12 +430,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) @@ -449,12 +445,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; } @@ -466,7 +462,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); @@ -485,16 +480,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 // @@ -506,8 +591,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) @@ -521,12 +606,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; } @@ -538,7 +623,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); @@ -557,29 +641,110 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) { *this = ymat * xmat * zmat; } -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]); +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; } -bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon) const { +void Basis::set_euler_zxy(const Vector3 &p_euler) { + real_t c, s; - 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)) - return false; + 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; +} - return true; +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::operator==(const Basis &p_matrix) const { +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::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; + } } } @@ -587,31 +752,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) - mtx += ", "; - - mtx += rtos(elements[i][j]); - } - } - - return mtx; + return "[X: " + get_axis(0).operator String() + + ", Y: " + get_axis(1).operator String() + + ", Z: " + get_axis(2).operator String() + "]"; } -Quat Basis::get_quat() const { - +Quaternion Basis::get_quaternion() 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."); + ERR_FAIL_COND_V_MSG(!is_rotation(), Quaternion(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quaternion() or call orthonormalized() instead."); #endif /* Allow getting a quaternion from an unnormalized transform */ Basis m = *this; @@ -628,8 +780,8 @@ Quat Basis::get_quat() const { temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); } else { int i = m.elements[0][0] < m.elements[1][1] ? - (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : - (m.elements[0][0] < m.elements[2][2] ? 2 : 0); + (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : + (m.elements[0][0] < m.elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; @@ -642,7 +794,7 @@ Quat Basis::get_quat() const { temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; } - return Quat(temp[0], temp[1], temp[2], temp[3]); + return Quaternion(temp[0], temp[1], temp[2], temp[3]); } static const Basis _ortho_bases[24] = { @@ -673,35 +825,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); @@ -721,7 +871,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { if ((Math::abs(elements[1][0] - elements[0][1]) < epsilon) && (Math::abs(elements[2][0] - elements[0][2]) < epsilon) && (Math::abs(elements[2][1] - elements[1][2]) < epsilon)) { // singularity found // first check for identity matrix which must have +1 for all terms - // in leading diagonaland zero in other terms + // in leading diagonal and zero in other terms if ((Math::abs(elements[1][0] + elements[0][1]) < epsilon2) && (Math::abs(elements[2][0] + elements[0][2]) < epsilon2) && (Math::abs(elements[2][1] + elements[1][2]) < epsilon2) && (Math::abs(elements[0][0] + elements[1][1] + elements[2][2] - 3) < epsilon2)) { // this singularity is identity matrix so angle = 0 r_axis = Vector3(0, 1, 0); @@ -775,7 +925,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; @@ -784,14 +936,13 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { r_angle = angle; } -void Basis::set_quat(const Quat &p_quat) { - - real_t d = p_quat.length_squared(); +void Basis::set_quaternion(const Quaternion &p_quaternion) { + real_t d = p_quaternion.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; - real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs; - real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs; - real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs; + real_t xs = p_quaternion.x * s, ys = p_quaternion.y * s, zs = p_quaternion.z * s; + real_t wx = p_quaternion.w * xs, wy = p_quaternion.w * ys, wz = p_quaternion.w * zs; + real_t xx = p_quaternion.x * xs, xy = p_quaternion.x * ys, xz = p_quaternion.x * zs; + real_t yy = p_quaternion.y * ys, yz = p_quaternion.y * zs, zz = p_quaternion.z * zs; set(1.0 - (yy + zz), xy - wz, xz + wy, xy + wz, 1.0 - (xx + zz), yz - wx, xz - wy, yz + wx, 1.0 - (xx + yy)); @@ -837,9 +988,9 @@ void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) { rotate(p_euler); } -void Basis::set_quat_scale(const Quat &p_quat, const Vector3 &p_scale) { +void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_diagonal(p_scale); - rotate(p_quat); + rotate(p_quaternion); } void Basis::set_diagonal(const Vector3 &p_diag) { @@ -856,16 +1007,125 @@ void Basis::set_diagonal(const Vector3 &p_diag) { elements[2][2] = p_diag.z; } -Basis Basis::slerp(const Basis &target, const real_t &t) const { - +Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { //consider scale - Quat from(*this); - Quat to(target); + Quaternion from(*this); + Quaternion to(p_to); - Basis b(from.slerp(to, t)); - b.elements[0] *= Math::lerp(elements[0].length(), target.elements[0].length(), t); - b.elements[1] *= Math::lerp(elements[1].length(), target.elements[1].length(), t); - b.elements[2] *= Math::lerp(elements[2].length(), target.elements[2].length(), t); + Basis b(from.slerp(to, p_weight)); + b.elements[0] *= Math::lerp(elements[0].length(), p_to.elements[0].length(), p_weight); + b.elements[1] *= Math::lerp(elements[1].length(), p_to.elements[1].length(), p_weight); + b.elements[2] *= Math::lerp(elements[2].length(), p_to.elements[2].length(), p_weight); 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 6c3a939d70..2889a4aa5e 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,19 +31,21 @@ #ifndef BASIS_H #define BASIS_H -#include "core/math/quat.h" +#include "core/math/quaternion.h" #include "core/math/vector3.h" class Basis { public: - Vector3 elements[3]; + Vector3 elements[3] = { + Vector3(1, 0, 0), + Vector3(0, 1, 0), + Vector3(0, 0, 1) + }; _FORCE_INLINE_ const Vector3 &operator[](int axis) const { - return elements[axis]; } _FORCE_INLINE_ Vector3 &operator[](int axis) { - return elements[axis]; } @@ -77,24 +79,37 @@ public: void rotate(const Vector3 &p_euler); Basis rotated(const Vector3 &p_euler) const; - void rotate(const Quat &p_quat); - Basis rotated(const Quat &p_quat) const; + void rotate(const Quaternion &p_quaternion); + Basis rotated(const Quaternion &p_quaternion) const; Vector3 get_rotation_euler() const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; - Quat get_rotation_quat() const; + Quaternion get_rotation_quaternion() const; Vector3 get_rotation() const { return get_rotation_euler(); }; Vector3 rotref_posscale_decomposition(Basis &rotref) const; 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); - Quat get_quat() const; - void set_quat(const Quat &p_quat); + 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); + + Quaternion get_quaternion() const; + void set_quaternion(const Quaternion &p_quaternion); Vector3 get_euler() const { return get_euler_yxz(); } void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); } @@ -108,13 +123,16 @@ public: void scale_local(const Vector3 &p_scale); Basis scaled_local(const Vector3 &p_scale) const; + void make_scale_uniform(); + float get_uniform_scale() const; + Vector3 get_scale() const; Vector3 get_scale_abs() const; Vector3 get_scale_local() const; void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale); void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale); - void set_quat_scale(const Quat &p_quat, const Vector3 &p_scale); + void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale); // transposed dot products _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const { @@ -128,9 +146,6 @@ public: } bool is_equal_approx(const Basis &p_basis) const; - // TODO: Break compatibility in 4.0 by getting rid of this so that it's only an instance method. See also TODO in variant_call.cpp - bool is_equal_approx(const Basis &a, const Basis &b) const { return a.is_equal_approx(b); } - bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon = UNIT_EPSILON) const; bool operator==(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const; @@ -143,8 +158,8 @@ public: _FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const; _FORCE_INLINE_ void operator-=(const Basis &p_matrix); _FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const; - _FORCE_INLINE_ void operator*=(real_t p_val); - _FORCE_INLINE_ Basis operator*(real_t p_val) const; + _FORCE_INLINE_ void operator*=(const real_t p_val); + _FORCE_INLINE_ Basis operator*(const real_t p_val) const; int get_orthogonal_index() const; void set_orthogonal_index(int p_index); @@ -155,14 +170,14 @@ public: bool is_diagonal() const; bool is_rotation() const; - Basis slerp(const Basis &target, const real_t &t) const; + Basis slerp(const Basis &p_to, const real_t &p_weight) 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; @@ -174,18 +189,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 { @@ -217,20 +229,21 @@ 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); } void orthonormalize(); Basis orthonormalized() const; +#ifdef MATH_CHECKS bool is_symmetric() const; +#endif Basis diagonalize(); - operator Quat() const { return get_quat(); } + operator Quaternion() const { return get_quaternion(); } - Basis(const Quat &p_quat) { set_quat(p_quat); }; - Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } + Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); }; + Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, p_scale); } Basis(const Vector3 &p_euler) { set_euler(p_euler); } Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); } @@ -244,22 +257,10 @@ public: elements[2] = row2; } - _FORCE_INLINE_ Basis() { - - elements[0][0] = 1; - elements[0][1] = 0; - elements[0][2] = 0; - elements[1][0] = 0; - elements[1][1] = 1; - elements[1][2] = 0; - elements[2][0] = 0; - elements[2][1] = 0; - elements[2][2] = 1; - } + _FORCE_INLINE_ Basis() {} }; _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]), @@ -267,7 +268,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]), @@ -275,49 +275,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) { - +_FORCE_INLINE_ void Basis::operator*=(const 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 { - +_FORCE_INLINE_ Basis Basis::operator*(const 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), @@ -325,7 +318,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), @@ -333,7 +325,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/bsp_tree.cpp b/core/math/bsp_tree.cpp deleted file mode 100644 index f155d626d7..0000000000 --- a/core/math/bsp_tree.cpp +++ /dev/null @@ -1,581 +0,0 @@ -/*************************************************************************/ -/* bsp_tree.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 "bsp_tree.h" - -#include "core/error_macros.h" -#include "core/print_string.h" - -void BSP_Tree::from_aabb(const AABB &p_aabb) { - - planes.clear(); - - for (int i = 0; i < 3; i++) { - - Vector3 n; - n[i] = 1; - planes.push_back(Plane(n, p_aabb.position[i] + p_aabb.size[i])); - planes.push_back(Plane(-n, -p_aabb.position[i])); - } - - nodes.clear(); - - for (int i = 0; i < 6; i++) { - - Node n; - n.plane = i; - n.under = (i == 0) ? UNDER_LEAF : i - 1; - n.over = OVER_LEAF; - nodes.push_back(n); - } - - aabb = p_aabb; - error_radius = 0; -} - -Vector<BSP_Tree::Node> BSP_Tree::get_nodes() const { - - return nodes; -} -Vector<Plane> BSP_Tree::get_planes() const { - - return planes; -} - -AABB BSP_Tree::get_aabb() const { - - return aabb; -} - -int BSP_Tree::_get_points_inside(int p_node, const Vector3 *p_points, int *p_indices, const Vector3 &p_center, const Vector3 &p_half_extents, int p_indices_count) const { - - const Node *node = &nodes[p_node]; - const Plane &p = planes[node->plane]; - - Vector3 min( - (p.normal.x > 0) ? -p_half_extents.x : p_half_extents.x, - (p.normal.y > 0) ? -p_half_extents.y : p_half_extents.y, - (p.normal.z > 0) ? -p_half_extents.z : p_half_extents.z); - Vector3 max = -min; - max += p_center; - min += p_center; - - real_t dist_min = p.distance_to(min); - real_t dist_max = p.distance_to(max); - - if ((dist_min * dist_max) < CMP_EPSILON) { //intersection, test point by point - - int under_count = 0; - - //sort points, so the are under first, over last - for (int i = 0; i < p_indices_count; i++) { - - int index = p_indices[i]; - - if (p.is_point_over(p_points[index])) { - - // kind of slow (but cache friendly), should try something else, - // but this is a corner case most of the time - - for (int j = index; j < p_indices_count - 1; j++) - p_indices[j] = p_indices[j + 1]; - - p_indices[p_indices_count - 1] = index; - - } else { - under_count++; - } - } - - int total = 0; - - if (under_count > 0) { - if (node->under == UNDER_LEAF) { - total += under_count; - } else { - total += _get_points_inside(node->under, p_points, p_indices, p_center, p_half_extents, under_count); - } - } - - if (under_count != p_indices_count) { - if (node->over == OVER_LEAF) { - //total+=0 //if they are over an OVER_LEAF, they are outside the model - } else { - total += _get_points_inside(node->over, p_points, &p_indices[under_count], p_center, p_half_extents, p_indices_count - under_count); - } - } - - return total; - - } else if (dist_min > 0) { //all points over plane - - if (node->over == OVER_LEAF) { - - return 0; // all these points are not visible - } - - return _get_points_inside(node->over, p_points, p_indices, p_center, p_half_extents, p_indices_count); - } else { //all points behind plane - - if (node->under == UNDER_LEAF) { - - return p_indices_count; // all these points are visible - } - return _get_points_inside(node->under, p_points, p_indices, p_center, p_half_extents, p_indices_count); - } -} - -int BSP_Tree::get_points_inside(const Vector3 *p_points, int p_point_count) const { - - if (nodes.size() == 0) - return 0; - -#if 1 - //this version is easier to debug, and and MUCH faster in real world cases - - int pass_count = 0; - const Node *nodesptr = &nodes[0]; - const Plane *planesptr = &planes[0]; - int node_count = nodes.size(); - - if (node_count == 0) // no nodes! - return 0; - - for (int i = 0; i < p_point_count; i++) { - - const Vector3 &point = p_points[i]; - if (!aabb.has_point(point)) { - continue; - } - - int idx = node_count - 1; - - bool pass = false; - - while (true) { - - if (idx == OVER_LEAF) { - pass = false; - break; - } else if (idx == UNDER_LEAF) { - pass = true; - break; - } - -#ifdef DEBUG_ENABLED - int plane_count = planes.size(); - uint16_t plane = nodesptr[idx].plane; - ERR_FAIL_UNSIGNED_INDEX_V(plane, plane_count, 0); -#endif - - idx = planesptr[nodesptr[idx].plane].is_point_over(point) ? nodes[idx].over : nodes[idx].under; - -#ifdef DEBUG_ENABLED - - ERR_FAIL_COND_V(idx < MAX_NODES && idx >= node_count, 0); -#endif - } - - if (pass) - pass_count++; - } - - return pass_count; - -#else - //this version scales better but it's slower for real world cases - - int *indices = (int *)alloca(p_point_count * sizeof(int)); - AABB bounds; - - for (int i = 0; i < p_point_count; i++) { - - indices[i] = i; - if (i == 0) - bounds.pos = p_points[i]; - else - bounds.expand_to(p_points[i]); - } - - Vector3 half_extents = bounds.size / 2.0; - return _get_points_inside(nodes.size() + 1, p_points, indices, bounds.pos + half_extents, half_extents, p_point_count); -#endif -} - -bool BSP_Tree::point_is_inside(const Vector3 &p_point) const { - - if (!aabb.has_point(p_point)) { - return false; - } - - int node_count = nodes.size(); - - if (node_count == 0) // no nodes! - return false; - - const Node *nodesptr = &nodes[0]; - const Plane *planesptr = &planes[0]; - - int idx = node_count - 1; - - while (true) { - - if (idx == OVER_LEAF) { - return false; - } - if (idx == UNDER_LEAF) { - - return true; - } - -#ifdef DEBUG_ENABLED - int plane_count = planes.size(); - uint16_t plane = nodesptr[idx].plane; - ERR_FAIL_UNSIGNED_INDEX_V(plane, plane_count, false); -#endif - - bool over = planesptr[nodesptr[idx].plane].is_point_over(p_point); - - idx = over ? nodes[idx].over : nodes[idx].under; - -#ifdef DEBUG_ENABLED - ERR_FAIL_COND_V(idx < MAX_NODES && idx >= node_count, false); -#endif - } -} - -static int _bsp_find_best_half_plane(const Face3 *p_faces, const Vector<int> &p_indices, real_t p_tolerance) { - - int ic = p_indices.size(); - const int *indices = p_indices.ptr(); - - int best_plane = -1; - real_t best_plane_cost = 1e20; - - // Loop to find the polygon that best divides the set. - - for (int i = 0; i < ic; i++) { - - const Face3 &f = p_faces[indices[i]]; - Plane p = f.get_plane(); - - int num_over = 0, num_under = 0, num_spanning = 0; - - for (int j = 0; j < ic; j++) { - - if (i == j) - continue; - - const Face3 &g = p_faces[indices[j]]; - int over = 0, under = 0; - - for (int k = 0; k < 3; k++) { - - real_t d = p.distance_to(g.vertex[j]); - - if (Math::abs(d) > p_tolerance) { - - if (d > 0) - over++; - else - under++; - } - } - - if (over && under) - num_spanning++; - else if (over) - num_over++; - else - num_under++; - } - - //real_t split_cost = num_spanning / (real_t) face_count; - real_t relation = Math::abs(num_over - num_under) / (real_t)ic; - - // being honest, i never found a way to add split cost to the mix in a meaninguful way - // in this engine, also, will likely be ignored anyway - - real_t plane_cost = /*split_cost +*/ relation; - - //printf("plane %i, %i over, %i under, %i spanning, cost is %g\n",i,num_over,num_under,num_spanning,plane_cost); - if (plane_cost < best_plane_cost) { - - best_plane = i; - best_plane_cost = plane_cost; - } - } - - return best_plane; -} - -static int _bsp_create_node(const Face3 *p_faces, const Vector<int> &p_indices, Vector<Plane> &p_planes, Vector<BSP_Tree::Node> &p_nodes, real_t p_tolerance) { - - ERR_FAIL_COND_V(p_nodes.size() == BSP_Tree::MAX_NODES, -1); - - // should not reach here - ERR_FAIL_COND_V(p_indices.size() == 0, -1) - - int ic = p_indices.size(); - const int *indices = p_indices.ptr(); - - int divisor_idx = _bsp_find_best_half_plane(p_faces, p_indices, p_tolerance); - - // returned error - ERR_FAIL_COND_V(divisor_idx < 0, -1); - - Vector<int> faces_over; - Vector<int> faces_under; - - Plane divisor_plane = p_faces[indices[divisor_idx]].get_plane(); - - for (int i = 0; i < ic; i++) { - - if (i == divisor_idx) - continue; - - const Face3 &f = p_faces[indices[i]]; - - /* - if (f.get_plane().is_equal_approx(divisor_plane)) - continue; - */ - - int over_count = 0; - int under_count = 0; - - for (int j = 0; j < 3; j++) { - - real_t d = divisor_plane.distance_to(f.vertex[j]); - if (Math::abs(d) > p_tolerance) { - - if (d > 0) - over_count++; - else - under_count++; - } - } - - if (over_count) - faces_over.push_back(indices[i]); - if (under_count) - faces_under.push_back(indices[i]); - } - - uint16_t over_idx = BSP_Tree::OVER_LEAF, under_idx = BSP_Tree::UNDER_LEAF; - - if (faces_over.size() > 0) { //have facess above? - - int idx = _bsp_create_node(p_faces, faces_over, p_planes, p_nodes, p_tolerance); - if (idx >= 0) - over_idx = idx; - } - - if (faces_under.size() > 0) { //have facess above? - - int idx = _bsp_create_node(p_faces, faces_under, p_planes, p_nodes, p_tolerance); - if (idx >= 0) - under_idx = idx; - } - - /* Create the node */ - - // find existing divisor plane - int divisor_plane_idx = -1; - - for (int i = 0; i < p_planes.size(); i++) { - - if (p_planes[i].is_equal_approx(divisor_plane)) { - divisor_plane_idx = i; - break; - } - } - - if (divisor_plane_idx == -1) { - - ERR_FAIL_COND_V(p_planes.size() == BSP_Tree::MAX_PLANES, -1); - divisor_plane_idx = p_planes.size(); - p_planes.push_back(divisor_plane); - } - - BSP_Tree::Node node; - node.plane = divisor_plane_idx; - node.under = under_idx; - node.over = over_idx; - - p_nodes.push_back(node); - - return p_nodes.size() - 1; -} - -BSP_Tree::operator Variant() const { - - Dictionary d; - d["error_radius"] = error_radius; - - Vector<real_t> plane_values; - plane_values.resize(planes.size() * 4); - - for (int i = 0; i < planes.size(); i++) { - - plane_values.write[i * 4 + 0] = planes[i].normal.x; - plane_values.write[i * 4 + 1] = planes[i].normal.y; - plane_values.write[i * 4 + 2] = planes[i].normal.z; - plane_values.write[i * 4 + 3] = planes[i].d; - } - - d["planes"] = plane_values; - - PoolVector<int> dst_nodes; - dst_nodes.resize(nodes.size() * 3); - - for (int i = 0; i < nodes.size(); i++) { - - dst_nodes.set(i * 3 + 0, nodes[i].over); - dst_nodes.set(i * 3 + 1, nodes[i].under); - dst_nodes.set(i * 3 + 2, nodes[i].plane); - } - - d["nodes"] = dst_nodes; - d["aabb"] = aabb; - - return Variant(d); -} - -BSP_Tree::BSP_Tree() { -} - -BSP_Tree::BSP_Tree(const Variant &p_variant) { - - Dictionary d = p_variant; - ERR_FAIL_COND(!d.has("nodes")); - ERR_FAIL_COND(!d.has("planes")); - ERR_FAIL_COND(!d.has("aabb")); - ERR_FAIL_COND(!d.has("error_radius")); - - PoolVector<int> src_nodes = d["nodes"]; - ERR_FAIL_COND(src_nodes.size() % 3); - - if (d["planes"].get_type() == Variant::POOL_REAL_ARRAY) { - - PoolVector<real_t> src_planes = d["planes"]; - int plane_count = src_planes.size(); - ERR_FAIL_COND(plane_count % 4); - planes.resize(plane_count / 4); - - if (plane_count) { - PoolVector<real_t>::Read r = src_planes.read(); - for (int i = 0; i < plane_count / 4; i++) { - - planes.write[i].normal.x = r[i * 4 + 0]; - planes.write[i].normal.y = r[i * 4 + 1]; - planes.write[i].normal.z = r[i * 4 + 2]; - planes.write[i].d = r[i * 4 + 3]; - } - } - - } else { - - planes = d["planes"]; - } - - error_radius = d["error"]; - aabb = d["aabb"]; - - //int node_count = src_nodes.size(); - nodes.resize(src_nodes.size() / 3); - - PoolVector<int>::Read r = src_nodes.read(); - - for (int i = 0; i < nodes.size(); i++) { - - nodes.write[i].over = r[i * 3 + 0]; - nodes.write[i].under = r[i * 3 + 1]; - nodes.write[i].plane = r[i * 3 + 2]; - } -} - -BSP_Tree::BSP_Tree(const PoolVector<Face3> &p_faces, real_t p_error_radius) { - - // compute aabb - - int face_count = p_faces.size(); - PoolVector<Face3>::Read faces_r = p_faces.read(); - const Face3 *facesptr = faces_r.ptr(); - - bool first = true; - - Vector<int> indices; - - for (int i = 0; i < face_count; i++) { - - const Face3 &f = facesptr[i]; - - if (f.is_degenerate()) - continue; - - for (int j = 0; j < 3; j++) { - - if (first) { - - aabb.position = f.vertex[0]; - first = false; - } else { - - aabb.expand_to(f.vertex[j]); - } - } - - indices.push_back(i); - } - - ERR_FAIL_COND(aabb.has_no_area()); - - int top = _bsp_create_node(faces_r.ptr(), indices, planes, nodes, aabb.get_longest_axis_size() * 0.0001); - - if (top < 0) { - - nodes.clear(); - planes.clear(); - ERR_FAIL_COND(top < 0); - } - - error_radius = p_error_radius; -} - -BSP_Tree::BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB &p_aabb, real_t p_error_radius) : - nodes(p_nodes), - planes(p_planes), - aabb(p_aabb), - error_radius(p_error_radius) { -} - -BSP_Tree::~BSP_Tree() { -} diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h deleted file mode 100644 index 1c8ea380ff..0000000000 --- a/core/math/bsp_tree.h +++ /dev/null @@ -1,159 +0,0 @@ -/*************************************************************************/ -/* bsp_tree.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 BSP_TREE_H -#define BSP_TREE_H - -#include "core/math/aabb.h" -#include "core/math/face3.h" -#include "core/math/plane.h" -#include "core/method_ptrcall.h" -#include "core/pool_vector.h" -#include "core/variant.h" -#include "core/vector.h" - -class BSP_Tree { -public: - enum { - - UNDER_LEAF = 0xFFFF, - OVER_LEAF = 0xFFFE, - MAX_NODES = 0xFFFE, - MAX_PLANES = (1 << 16) - }; - - struct Node { - - uint16_t plane; - uint16_t under; - uint16_t over; - }; - -private: - // thanks to the properties of Vector, - // this class can be assigned and passed around between threads - // with no cost. - - Vector<Node> nodes; - Vector<Plane> planes; - AABB aabb; - real_t error_radius; - - int _get_points_inside(int p_node, const Vector3 *p_points, int *p_indices, const Vector3 &p_center, const Vector3 &p_half_extents, int p_indices_count) const; - - template <class T> - bool _test_convex(const Node *p_nodes, const Plane *p_planes, int p_current, const T &p_convex) const; - -public: - bool is_empty() const { return nodes.size() == 0; } - Vector<Node> get_nodes() const; - Vector<Plane> get_planes() const; - AABB get_aabb() const; - - bool point_is_inside(const Vector3 &p_point) const; - int get_points_inside(const Vector3 *p_points, int p_point_count) const; - template <class T> - bool convex_is_inside(const T &p_convex) const; - - operator Variant() const; - - void from_aabb(const AABB &p_aabb); - - BSP_Tree(); - BSP_Tree(const Variant &p_variant); - BSP_Tree(const PoolVector<Face3> &p_faces, real_t p_error_radius = 0); - BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB &p_aabb, real_t p_error_radius = 0); - ~BSP_Tree(); -}; - -template <class T> -bool BSP_Tree::_test_convex(const Node *p_nodes, const Plane *p_planes, int p_current, const T &p_convex) const { - - if (p_current == UNDER_LEAF) - return true; - else if (p_current == OVER_LEAF) - return false; - - bool collided = false; - const Node &n = p_nodes[p_current]; - - const Plane &p = p_planes[n.plane]; - - real_t min, max; - p_convex.project_range(p.normal, min, max); - - bool go_under = min < p.d; - bool go_over = max >= p.d; - - if (go_under && _test_convex(p_nodes, p_planes, n.under, p_convex)) - collided = true; - if (go_over && _test_convex(p_nodes, p_planes, n.over, p_convex)) - collided = true; - - return collided; -} - -template <class T> -bool BSP_Tree::convex_is_inside(const T &p_convex) const { - - int node_count = nodes.size(); - if (node_count == 0) - return false; - const Node *nodes = &this->nodes[0]; - const Plane *planes = &this->planes[0]; - - return _test_convex(nodes, planes, node_count - 1, p_convex); -} - -#ifdef PTRCALL_ENABLED - -template <> -struct PtrToArg<BSP_Tree> { - _FORCE_INLINE_ static BSP_Tree convert(const void *p_ptr) { - BSP_Tree s(Variant(*reinterpret_cast<const Dictionary *>(p_ptr))); - return s; - } - _FORCE_INLINE_ static void encode(BSP_Tree p_val, void *p_ptr) { - Dictionary *d = reinterpret_cast<Dictionary *>(p_ptr); - *d = Variant(p_val); - } -}; - -template <> -struct PtrToArg<const BSP_Tree &> { - _FORCE_INLINE_ static BSP_Tree convert(const void *p_ptr) { - BSP_Tree s(Variant(*reinterpret_cast<const Dictionary *>(p_ptr))); - return s; - } -}; - -#endif - -#endif diff --git a/core/math/bvh.h b/core/math/bvh.h new file mode 100644 index 0000000000..cefbc9b0db --- /dev/null +++ b/core/math/bvh.h @@ -0,0 +1,695 @@ +/*************************************************************************/ +/* bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 BVH_H +#define BVH_H + +// BVH +// This class provides a wrapper around BVH tree, which contains most of the functionality +// for a dynamic BVH with templated leaf size. +// However BVH also adds facilities for pairing, to maintain compatibility with Godot 3.2. +// Pairing is a collision pairing system, on top of the basic BVH. + +// Some notes on the use of BVH / Octree from Godot 3.2. +// This is not well explained elsewhere. +// The rendering tree mask and types that are sent to the BVH are NOT layer masks. +// They are INSTANCE_TYPES (defined in visual_server.h), e.g. MESH, MULTIMESH, PARTICLES etc. +// Thus the lights do no cull by layer mask in the BVH. + +// Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be +// implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights. + +#include "bvh_tree.h" + +#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, Bounds, Point> + +template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class Bounds = AABB, class Point = Vector3> +class BVH_Manager { +public: + // note we are using uint32_t instead of BVHHandle, losing type safety, but this + // is for compatibility with octree + typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int); + typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *); + + // these 2 are crucial for fine tuning, and can be applied manually + // see the variable declarations for more info. + void params_set_node_expansion(real_t p_value) { + if (p_value >= 0.0) { + tree._node_expansion = p_value; + tree._auto_node_expansion = false; + } else { + tree._auto_node_expansion = true; + } + } + + void params_set_pairing_expansion(real_t p_value) { + if (p_value >= 0.0) { + tree._pairing_expansion = p_value; + tree._auto_pairing_expansion = false; + } else { + tree._auto_pairing_expansion = true; + } + } + + void set_pair_callback(PairCallback p_callback, void *p_userdata) { + pair_callback = p_callback; + pair_callback_userdata = p_userdata; + } + void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) { + unpair_callback = p_callback; + unpair_callback_userdata = p_userdata; + } + + BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) { + // not sure if absolutely necessary to flush collisions here. It will cost performance to, instead + // of waiting for update, so only uncomment this if there are bugs. + if (USE_PAIRS) { + //_check_for_collisions(); + } + +#ifdef TOOLS_ENABLED + if (!USE_PAIRS) { + if (p_pairable) { + WARN_PRINT_ONCE("creating pairable item in BVH with USE_PAIRS set to false"); + } + } +#endif + + BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_pairable, p_pairable_type, p_pairable_mask); + + if (USE_PAIRS) { + // for safety initialize the expanded AABB + Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb; + expanded_aabb = p_aabb; + expanded_aabb.grow_by(tree._pairing_expansion); + + // force a collision check no matter the AABB + if (p_active) { + _add_changed_item(h, p_aabb, false); + _check_for_collisions(true); + } + } + + return h; + } + + //////////////////////////////////////////////////// + // wrapper versions that use uint32_t instead of handle + // for backward compatibility. Less type safe + void move(uint32_t p_handle, const Bounds &p_aabb) { + BVHHandle h; + h.set(p_handle); + move(h, p_aabb); + } + + void erase(uint32_t p_handle) { + BVHHandle h; + h.set(p_handle); + erase(h); + } + + void force_collision_check(uint32_t p_handle) { + BVHHandle h; + h.set(p_handle); + force_collision_check(h); + } + + bool activate(uint32_t p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) { + BVHHandle h; + h.set(p_handle); + return activate(h, p_aabb, p_delay_collision_check); + } + + bool deactivate(uint32_t p_handle) { + BVHHandle h; + h.set(p_handle); + return deactivate(h); + } + + void set_pairable(uint32_t p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) { + BVHHandle h; + h.set(p_handle); + set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask, p_force_collision_check); + } + + bool is_pairable(uint32_t p_handle) const { + BVHHandle h; + h.set(p_handle); + return item_is_pairable(h); + } + int get_subindex(uint32_t p_handle) const { + BVHHandle h; + h.set(p_handle); + return item_get_subindex(h); + } + + T *get(uint32_t p_handle) const { + BVHHandle h; + h.set(p_handle); + return item_get_userdata(h); + } + + //////////////////////////////////////////////////// + + void move(BVHHandle p_handle, const Bounds &p_aabb) { + if (tree.item_move(p_handle, p_aabb)) { + if (USE_PAIRS) { + _add_changed_item(p_handle, p_aabb); + } + } + } + + void erase(BVHHandle p_handle) { + // call unpair and remove all references to the item + // before deleting from the tree + if (USE_PAIRS) { + _remove_changed_item(p_handle); + } + + tree.item_remove(p_handle); + + _check_for_collisions(true); + } + + // use in conjunction with activate if you have deferred the collision check, and + // set pairable has never been called. + // (deferred collision checks are a workaround for visual server for historical reasons) + void force_collision_check(BVHHandle p_handle) { + if (USE_PAIRS) { + // the aabb should already be up to date in the BVH + Bounds aabb; + item_get_AABB(p_handle, aabb); + + // add it as changed even if aabb not different + _add_changed_item(p_handle, aabb, false); + + // force an immediate full collision check, much like calls to set_pairable + _check_for_collisions(true); + } + } + + // these should be read as set_visible for render trees, + // but generically this makes items add or remove from the + // tree internally, to speed things up by ignoring inactive items + bool activate(BVHHandle p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) { + // sending the aabb here prevents the need for the BVH to maintain + // a redundant copy of the aabb. + // returns success + if (tree.item_activate(p_handle, p_aabb)) { + if (USE_PAIRS) { + // in the special case of the render tree, when setting visibility we are using the combination of + // activate then set_pairable. This would case 2 sets of collision checks. For efficiency here we allow + // deferring to have a single collision check at the set_pairable call. + // Watch for bugs! This may cause bugs if set_pairable is not called. + if (!p_delay_collision_check) { + _add_changed_item(p_handle, p_aabb, false); + + // force an immediate collision check, much like calls to set_pairable + _check_for_collisions(true); + } + } + return true; + } + + return false; + } + + bool deactivate(BVHHandle p_handle) { + // returns success + if (tree.item_deactivate(p_handle)) { + // call unpair and remove all references to the item + // before deleting from the tree + if (USE_PAIRS) { + _remove_changed_item(p_handle); + + // force check for collisions, much like an erase was called + _check_for_collisions(true); + } + return true; + } + + return false; + } + + bool get_active(BVHHandle p_handle) const { + return tree.item_get_active(p_handle); + } + + // call e.g. once per frame (this does a trickle optimize) + void update() { + tree.update(); + _check_for_collisions(); +#ifdef BVH_INTEGRITY_CHECKS + tree.integrity_check_all(); +#endif + } + + // this can be called more frequently than per frame if necessary + void update_collisions() { + _check_for_collisions(); + } + + // prefer calling this directly as type safe + void set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) { + // Returns true if the pairing state has changed. + bool state_changed = tree.item_set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask); + + if (USE_PAIRS) { + // not sure if absolutely necessary to flush collisions here. It will cost performance to, instead + // of waiting for update, so only uncomment this if there are bugs. + //_check_for_collisions(); + + if ((p_force_collision_check || state_changed) && get_active(p_handle)) { + // when the pairable state changes, we need to force a collision check because newly pairable + // items may be in collision, and unpairable items might move out of collision. + // We cannot depend on waiting for the next update, because that may come much later. + Bounds aabb; + item_get_AABB(p_handle, aabb); + + // passing false disables the optimization which prevents collision checks if + // the aabb hasn't changed + _add_changed_item(p_handle, aabb, false); + + // force an immediate collision check (probably just for this one item) + // but it must be a FULL collision check, also checking pairable state and masks. + // This is because AABB intersecting objects may have changed pairable state / mask + // such that they should no longer be paired. E.g. lights. + _check_for_collisions(true); + } // only if active + } + } + + // cull tests + int cull_aabb(const Bounds &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { + typename BVHTREE_CLASS::CullParams params; + + params.result_count_overall = 0; + params.result_max = p_result_max; + params.result_array = p_result_array; + params.subindex_array = p_subindex_array; + params.mask = p_mask; + params.pairable_type = 0; + params.test_pairable_only = false; + params.abb.from(p_aabb); + + tree.cull_aabb(params); + + return params.result_count_overall; + } + + int cull_segment(const Point &p_from, const Point &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { + typename BVHTREE_CLASS::CullParams params; + + params.result_count_overall = 0; + params.result_max = p_result_max; + params.result_array = p_result_array; + params.subindex_array = p_subindex_array; + params.mask = p_mask; + params.pairable_type = 0; + + params.segment.from = p_from; + params.segment.to = p_to; + + tree.cull_segment(params); + + return params.result_count_overall; + } + + int cull_point(const Point &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { + typename BVHTREE_CLASS::CullParams params; + + params.result_count_overall = 0; + params.result_max = p_result_max; + params.result_array = p_result_array; + params.subindex_array = p_subindex_array; + params.mask = p_mask; + params.pairable_type = 0; + + params.point = p_point; + + tree.cull_point(params); + return params.result_count_overall; + } + + int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF) { + if (!p_convex.size()) { + return 0; + } + + Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + if (convex_points.size() == 0) { + return 0; + } + + typename BVHTREE_CLASS::CullParams params; + params.result_count_overall = 0; + params.result_max = p_result_max; + params.result_array = p_result_array; + params.subindex_array = nullptr; + params.mask = p_mask; + params.pairable_type = 0; + + params.hull.planes = &p_convex[0]; + params.hull.num_planes = p_convex.size(); + params.hull.points = &convex_points[0]; + params.hull.num_points = convex_points.size(); + + tree.cull_convex(params); + + return params.result_count_overall; + } + +private: + // do this after moving etc. + void _check_for_collisions(bool p_full_check = false) { + if (!changed_items.size()) { + // noop + return; + } + + Bounds bb; + + typename BVHTREE_CLASS::CullParams params; + + params.result_count_overall = 0; + params.result_max = INT_MAX; + params.result_array = nullptr; + params.subindex_array = nullptr; + params.mask = 0xFFFFFFFF; + params.pairable_type = 0; + + for (unsigned int n = 0; n < changed_items.size(); n++) { + const BVHHandle &h = changed_items[n]; + + // use the expanded aabb for pairing + const Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb; + BVHABB_CLASS abb; + abb.from(expanded_aabb); + + // find all the existing paired aabbs that are no longer + // paired, and send callbacks + _find_leavers(h, abb, p_full_check); + + uint32_t changed_item_ref_id = h.id(); + + // set up the test from this item. + // this includes whether to test the non pairable tree, + // and the item mask. + tree.item_fill_cullparams(h, params); + + params.abb = abb; + + params.result_count_overall = 0; // might not be needed + tree.cull_aabb(params, false); + + for (unsigned int i = 0; i < tree._cull_hits.size(); i++) { + uint32_t ref_id = tree._cull_hits[i]; + + // don't collide against ourself + if (ref_id == changed_item_ref_id) { + continue; + } + +#ifdef BVH_CHECKS + // if neither are pairable, they should ignore each other + // THIS SHOULD NEVER HAPPEN .. now we only test the pairable tree + // if the changed item is not pairable + CRASH_COND(params.test_pairable_only && !tree._extra[ref_id].pairable); +#endif + + // checkmasks is already done in the cull routine. + BVHHandle h_collidee; + h_collidee.set_id(ref_id); + + // find NEW enterers, and send callbacks for them only + _collide(h, h_collidee); + } + } + _reset(); + } + +public: + void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) { + BVHABB_CLASS abb; + tree.item_get_ABB(p_handle, abb); + abb.to(r_aabb); + } + +private: + // supplemental funcs + bool item_is_pairable(BVHHandle p_handle) const { return _get_extra(p_handle).pairable; } + T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; } + int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; } + + void _unpair(BVHHandle p_from, BVHHandle p_to) { + tree._handle_sort(p_from, p_to); + + typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()]; + typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()]; + + // if the userdata is the same, no collisions should occur + if ((exa.userdata == exb.userdata) && exa.userdata) { + return; + } + + typename BVHTREE_CLASS::ItemPairs &pairs_from = tree._pairs[p_from.id()]; + typename BVHTREE_CLASS::ItemPairs &pairs_to = tree._pairs[p_to.id()]; + + void *ud_from = pairs_from.remove_pair_to(p_to); + pairs_to.remove_pair_to(p_from); + + // callback + if (unpair_callback) { + unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from); + } + } + + // returns true if unpair + bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) { + BVHABB_CLASS abb_to; + tree.item_get_ABB(p_to, abb_to); + + // do they overlap? + if (p_abb_from.intersects(abb_to)) { + // the full check for pairable / non pairable and mask changes is extra expense + // this need not be done in most cases (for speed) except in the case where set_pairable is called + // where the masks etc of the objects in question may have changed + if (!p_full_check) { + return false; + } + const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from); + const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to); + + // one of the two must be pairable to still pair + // if neither are pairable, we always unpair + if (exa.pairable || exb.pairable) { + // the masks must still be compatible to pair + // i.e. if there is a hit between the two, then they should stay paired + if (tree._cull_pairing_mask_test_hit(exa.pairable_mask, exa.pairable_type, exb.pairable_mask, exb.pairable_type)) { + return false; + } + } + } + + _unpair(p_from, p_to); + return true; + } + + // find all the existing paired aabbs that are no longer + // paired, and send callbacks + void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) { + typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()]; + + BVHABB_CLASS abb_from = expanded_abb_from; + + // remove from pairing list for every partner + for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) { + BVHHandle h_to = p_from.extended_pairs[n].handle; + if (_find_leavers_process_pair(p_from, abb_from, p_handle, h_to, p_full_check)) { + // we need to keep the counter n up to date if we deleted a pair + // as the number of items in p_from.extended_pairs will have decreased by 1 + // and we don't want to miss an item + n--; + } + } + } + + // find NEW enterers, and send callbacks for them only + // handle a and b + void _collide(BVHHandle p_ha, BVHHandle p_hb) { + // only have to do this oneway, lower ID then higher ID + tree._handle_sort(p_ha, p_hb); + + const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha); + const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb); + + // if the userdata is the same, no collisions should occur + if ((exa.userdata == exb.userdata) && exa.userdata) { + return; + } + + typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_ha.id()]; + typename BVHTREE_CLASS::ItemPairs &p_to = tree._pairs[p_hb.id()]; + + // does this pair exist already? + // or only check the one with lower number of pairs for greater speed + if (p_from.num_pairs <= p_to.num_pairs) { + if (p_from.contains_pair_to(p_hb)) { + return; + } + } else { + if (p_to.contains_pair_to(p_ha)) { + return; + } + } + + // callback + void *callback_userdata = nullptr; + + if (pair_callback) { + callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex); + } + + // new pair! .. only really need to store the userdata on the lower handle, but both have storage so... + p_from.add_pair_to(p_hb, callback_userdata); + p_to.add_pair_to(p_ha, callback_userdata); + } + + // if we remove an item, we need to immediately remove the pairs, to prevent reading the pair after deletion + void _remove_pairs_containing(BVHHandle p_handle) { + typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()]; + + // remove from pairing list for every partner. + // can't easily use a for loop here, because removing changes the size of the list + while (p_from.extended_pairs.size()) { + BVHHandle h_to = p_from.extended_pairs[0].handle; + _unpair(p_handle, h_to); + } + } + +private: + const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const { + return tree._extra[p_handle.id()]; + } + const typename BVHTREE_CLASS::ItemRef &_get_ref(BVHHandle p_handle) const { + return tree._refs[p_handle.id()]; + } + + void _reset() { + changed_items.clear(); + _tick++; + } + + void _add_changed_item(BVHHandle p_handle, const Bounds &aabb, bool p_check_aabb = true) { + // Note that non pairable items can pair with pairable, + // so all types must be added to the list + + // aabb check with expanded aabb. This greatly decreases processing + // at the cost of slightly less accurate pairing checks + // Note this pairing AABB is separate from the AABB in the actual tree + Bounds &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb; + + // passing p_check_aabb false disables the optimization which prevents collision checks if + // the aabb hasn't changed. This is needed where set_pairable has been called, but the position + // has not changed. + if (p_check_aabb && expanded_aabb.encloses(aabb)) { + return; + } + + // ALWAYS update the new expanded aabb, even if already changed once + // this tick, because it is vital that the AABB is kept up to date + expanded_aabb = aabb; + expanded_aabb.grow_by(tree._pairing_expansion); + + // this code is to ensure that changed items only appear once on the updated list + // collision checking them multiple times is not needed, and repeats the same thing + uint32_t &last_updated_tick = tree._extra[p_handle.id()].last_updated_tick; + + if (last_updated_tick == _tick) { + return; // already on changed list + } + + // mark as on list + last_updated_tick = _tick; + + // add to the list + changed_items.push_back(p_handle); + } + + void _remove_changed_item(BVHHandle p_handle) { + // Care has to be taken here for items that are deleted. The ref ID + // could be reused on the same tick for new items. This is probably + // rare but should be taken into consideration + + // callbacks + _remove_pairs_containing(p_handle); + + // remove from changed items (not very efficient yet) + for (int n = 0; n < (int)changed_items.size(); n++) { + if (changed_items[n] == p_handle) { + changed_items.remove_unordered(n); + + // because we are using an unordered remove, + // the last changed item will now be at spot 'n', + // and we need to redo it, so we prevent moving on to + // the next n at the next for iteration. + n--; + } + } + + // reset the last updated tick (may not be necessary but just in case) + tree._extra[p_handle.id()].last_updated_tick = 0; + } + + PairCallback pair_callback; + UnpairCallback unpair_callback; + void *pair_callback_userdata; + void *unpair_callback_userdata; + + BVHTREE_CLASS tree; + + // for collision pairing, + // maintain a list of all items moved etc on each frame / tick + LocalVector<BVHHandle, uint32_t, true> changed_items; + uint32_t _tick; + +public: + BVH_Manager() { + _tick = 1; // start from 1 so items with 0 indicate never updated + pair_callback = nullptr; + unpair_callback = nullptr; + pair_callback_userdata = nullptr; + unpair_callback_userdata = nullptr; + } +}; + +#undef BVHTREE_CLASS + +#endif // BVH_H diff --git a/core/math/bvh_abb.h b/core/math/bvh_abb.h new file mode 100644 index 0000000000..bd9a01a87e --- /dev/null +++ b/core/math/bvh_abb.h @@ -0,0 +1,276 @@ +/*************************************************************************/ +/* bvh_abb.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 BVH_ABB_H +#define BVH_ABB_H + +// special optimized version of axis aligned bounding box +template <class Bounds = AABB, class Point = Vector3> +struct BVH_ABB { + struct ConvexHull { + // convex hulls (optional) + const Plane *planes; + int num_planes; + const Vector3 *points; + int num_points; + }; + + struct Segment { + Point from; + Point to; + }; + + enum IntersectResult { + IR_MISS = 0, + IR_PARTIAL, + IR_FULL, + }; + + // we store mins with a negative value in order to test them with SIMD + Point min; + Point neg_max; + + bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); } + bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; } + + void set(const Point &_min, const Point &_max) { + min = _min; + neg_max = -_max; + } + + // to and from standard AABB + void from(const Bounds &p_aabb) { + min = p_aabb.position; + neg_max = -(p_aabb.position + p_aabb.size); + } + + void to(Bounds &r_aabb) const { + r_aabb.position = min; + r_aabb.size = calculate_size(); + } + + void merge(const BVH_ABB &p_o) { + for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]); + min[axis] = MIN(min[axis], p_o.min[axis]); + } + } + + Point calculate_size() const { + return -neg_max - min; + } + + Point calculate_centre() const { + return Point((calculate_size() * 0.5) + min); + } + + real_t get_proximity_to(const BVH_ABB &p_b) const { + const Point d = (min - neg_max) - (p_b.min - p_b.neg_max); + real_t proximity = 0.0; + for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + proximity += Math::abs(d[axis]); + } + return proximity; + } + + int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const { + return (get_proximity_to(p_a) < get_proximity_to(p_b) ? 0 : 1); + } + + uint32_t find_cutting_planes(const BVH_ABB::ConvexHull &p_hull, uint32_t *p_plane_ids) const { + uint32_t count = 0; + + for (int n = 0; n < p_hull.num_planes; n++) { + const Plane &p = p_hull.planes[n]; + if (intersects_plane(p)) { + p_plane_ids[count++] = n; + } + } + + return count; + } + + bool intersects_plane(const Plane &p_p) const { + Vector3 size = calculate_size(); + Vector3 half_extents = size * 0.5; + Vector3 ofs = min + half_extents; + + // forward side of plane? + Vector3 point_offset( + (p_p.normal.x < 0) ? -half_extents.x : half_extents.x, + (p_p.normal.y < 0) ? -half_extents.y : half_extents.y, + (p_p.normal.z < 0) ? -half_extents.z : half_extents.z); + Vector3 point = point_offset + ofs; + + if (!p_p.is_point_over(point)) { + return false; + } + + point = -point_offset + ofs; + if (p_p.is_point_over(point)) { + return false; + } + + return true; + } + + bool intersects_convex_optimized(const ConvexHull &p_hull, const uint32_t *p_plane_ids, uint32_t p_num_planes) const { + Vector3 size = calculate_size(); + Vector3 half_extents = size * 0.5; + Vector3 ofs = min + half_extents; + + for (unsigned int i = 0; i < p_num_planes; i++) { + const Plane &p = p_hull.planes[p_plane_ids[i]]; + Vector3 point( + (p.normal.x > 0) ? -half_extents.x : half_extents.x, + (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)) { + return false; + } + } + + return true; + } + + bool intersects_convex_partial(const ConvexHull &p_hull) const { + Bounds bb; + to(bb); + return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points); + } + + IntersectResult intersects_convex(const ConvexHull &p_hull) const { + if (intersects_convex_partial(p_hull)) { + // fully within? very important for tree checks + if (is_within_convex(p_hull)) { + return IR_FULL; + } + + return IR_PARTIAL; + } + + return IR_MISS; + } + + bool is_within_convex(const ConvexHull &p_hull) const { + // use half extents routine + Bounds bb; + to(bb); + return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes); + } + + bool is_point_within_hull(const ConvexHull &p_hull, const Vector3 &p_pt) const { + for (int n = 0; n < p_hull.num_planes; n++) { + if (p_hull.planes[n].distance_to(p_pt) > 0.0f) { + return false; + } + } + return true; + } + + bool intersects_segment(const Segment &p_s) const { + Bounds bb; + to(bb); + return bb.intersects_segment(p_s.from, p_s.to); + } + + bool intersects_point(const Point &p_pt) const { + if (_any_lessthan(-p_pt, neg_max)) { + return false; + } + if (_any_lessthan(p_pt, min)) { + return false; + } + return true; + } + + bool intersects(const BVH_ABB &p_o) const { + if (_any_morethan(p_o.min, -neg_max)) { + return false; + } + if (_any_morethan(min, -p_o.neg_max)) { + return false; + } + return true; + } + + bool is_other_within(const BVH_ABB &p_o) const { + if (_any_lessthan(p_o.neg_max, neg_max)) { + return false; + } + if (_any_lessthan(p_o.min, min)) { + return false; + } + return true; + } + + void grow(const Point &p_change) { + neg_max -= p_change; + min -= p_change; + } + + void expand(real_t p_change) { + Point change; + change.set_all(p_change); + grow(change); + } + + // Actually surface area metric. + float get_area() const { + Point d = calculate_size(); + return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x); + } + + void set_to_max_opposite_extents() { + neg_max.set_all(FLT_MAX); + min = neg_max; + } + + bool _any_morethan(const Point &p_a, const Point &p_b) const { + for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + if (p_a[axis] > p_b[axis]) { + return true; + } + } + return false; + } + + bool _any_lessthan(const Point &p_a, const Point &p_b) const { + for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + if (p_a[axis] < p_b[axis]) { + return true; + } + } + return false; + } +}; + +#endif // BVH_ABB_H diff --git a/core/math/bvh_cull.inc b/core/math/bvh_cull.inc new file mode 100644 index 0000000000..cba8ea6cb3 --- /dev/null +++ b/core/math/bvh_cull.inc @@ -0,0 +1,534 @@ +public: +// cull parameters is a convenient way of passing a bunch +// of arguments through the culling functions without +// writing loads of code. Not all members are used for some cull checks +struct CullParams { + int result_count_overall; // both trees + int result_count; // this tree only + int result_max; + T **result_array; + int *subindex_array; + + // nobody truly understands how masks are intended to work. + uint32_t mask; + uint32_t pairable_type; + + // optional components for different tests + Vector3 point; + BVHABB_CLASS abb; + typename BVHABB_CLASS::ConvexHull hull; + typename BVHABB_CLASS::Segment segment; + + // when collision testing, non pairable moving items + // only need to be tested against the pairable tree. + // collisions with other non pairable items are irrelevant. + bool test_pairable_only; +}; + +private: +void _cull_translate_hits(CullParams &p) { + int num_hits = _cull_hits.size(); + int left = p.result_max - p.result_count_overall; + + if (num_hits > left) { + num_hits = left; + } + + int out_n = p.result_count_overall; + + for (int n = 0; n < num_hits; n++) { + uint32_t ref_id = _cull_hits[n]; + + const ItemExtra &ex = _extra[ref_id]; + p.result_array[out_n] = ex.userdata; + + if (p.subindex_array) { + p.subindex_array[out_n] = ex.subindex; + } + + out_n++; + } + + p.result_count = num_hits; + p.result_count_overall += num_hits; +} + +public: +int cull_convex(CullParams &r_params, bool p_translate_hits = true) { + _cull_hits.clear(); + r_params.result_count = 0; + + for (int n = 0; n < NUM_TREES; n++) { + if (_root_node_id[n] == BVHCommon::INVALID) { + continue; + } + + _cull_convex_iterative(_root_node_id[n], r_params); + } + + if (p_translate_hits) { + _cull_translate_hits(r_params); + } + + return r_params.result_count; +} + +int cull_segment(CullParams &r_params, bool p_translate_hits = true) { + _cull_hits.clear(); + r_params.result_count = 0; + + for (int n = 0; n < NUM_TREES; n++) { + if (_root_node_id[n] == BVHCommon::INVALID) { + continue; + } + + _cull_segment_iterative(_root_node_id[n], r_params); + } + + if (p_translate_hits) { + _cull_translate_hits(r_params); + } + + return r_params.result_count; +} + +int cull_point(CullParams &r_params, bool p_translate_hits = true) { + _cull_hits.clear(); + r_params.result_count = 0; + + for (int n = 0; n < NUM_TREES; n++) { + if (_root_node_id[n] == BVHCommon::INVALID) { + continue; + } + + _cull_point_iterative(_root_node_id[n], r_params); + } + + if (p_translate_hits) { + _cull_translate_hits(r_params); + } + + return r_params.result_count; +} + +int cull_aabb(CullParams &r_params, bool p_translate_hits = true) { + _cull_hits.clear(); + r_params.result_count = 0; + + for (int n = 0; n < NUM_TREES; n++) { + if (_root_node_id[n] == BVHCommon::INVALID) { + continue; + } + + if ((n == 0) && r_params.test_pairable_only) { + continue; + } + + _cull_aabb_iterative(_root_node_id[n], r_params); + } + + if (p_translate_hits) { + _cull_translate_hits(r_params); + } + + return r_params.result_count; +} + +bool _cull_hits_full(const CullParams &p) { + // instead of checking every hit, we can do a lazy check for this condition. + // it isn't a problem if we write too much _cull_hits because they only the + // result_max amount will be translated and outputted. But we might as + // well stop our cull checks after the maximum has been reached. + return (int)_cull_hits.size() >= p.result_max; +} + +// write this logic once for use in all routines +// double check this as a possible source of bugs in future. +bool _cull_pairing_mask_test_hit(uint32_t p_maskA, uint32_t p_typeA, uint32_t p_maskB, uint32_t p_typeB) const { + // double check this as a possible source of bugs in future. + bool A_match_B = p_maskA & p_typeB; + + if (!A_match_B) { + bool B_match_A = p_maskB & p_typeA; + if (!B_match_A) { + return false; + } + } + + return true; +} + +void _cull_hit(uint32_t p_ref_id, CullParams &p) { + // take into account masks etc + // this would be more efficient to do before plane checks, + // but done here for ease to get started + if (USE_PAIRS) { + const ItemExtra &ex = _extra[p_ref_id]; + + if (!_cull_pairing_mask_test_hit(p.mask, p.pairable_type, ex.pairable_mask, ex.pairable_type)) { + return; + } + } + + _cull_hits.push_back(p_ref_id); +} + +bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) { + // our function parameters to keep on a stack + struct CullSegParams { + uint32_t node_id; + }; + + // most of the iterative functionality is contained in this helper class + BVH_IterativeInfo<CullSegParams> ii; + + // alloca must allocate the stack from this function, it cannot be allocated in the + // helper class + ii.stack = (CullSegParams *)alloca(ii.get_alloca_stacksize()); + + // seed the stack + ii.get_first()->node_id = p_node_id; + + CullSegParams csp; + + // while there are still more nodes on the stack + while (ii.pop(csp)) { + TNode &tnode = _nodes[csp.node_id]; + + if (tnode.is_leaf()) { + // lazy check for hits full up condition + if (_cull_hits_full(r_params)) { + return false; + } + + TLeaf &leaf = _node_get_leaf(tnode); + + // test children individually + for (int n = 0; n < leaf.num_items; n++) { + const BVHABB_CLASS &aabb = leaf.get_aabb(n); + + if (aabb.intersects_segment(r_params.segment)) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // register hit + _cull_hit(child_id, r_params); + } + } + } else { + // test children individually + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + const BVHABB_CLASS &child_abb = _nodes[child_id].aabb; + + if (child_abb.intersects_segment(r_params.segment)) { + // add to the stack + CullSegParams *child = ii.request(); + child->node_id = child_id; + } + } + } + + } // while more nodes to pop + + // true indicates results are not full + return true; +} + +bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) { + // our function parameters to keep on a stack + struct CullPointParams { + uint32_t node_id; + }; + + // most of the iterative functionality is contained in this helper class + BVH_IterativeInfo<CullPointParams> ii; + + // alloca must allocate the stack from this function, it cannot be allocated in the + // helper class + ii.stack = (CullPointParams *)alloca(ii.get_alloca_stacksize()); + + // seed the stack + ii.get_first()->node_id = p_node_id; + + CullPointParams cpp; + + // while there are still more nodes on the stack + while (ii.pop(cpp)) { + TNode &tnode = _nodes[cpp.node_id]; + // no hit with this node? + if (!tnode.aabb.intersects_point(r_params.point)) { + continue; + } + + if (tnode.is_leaf()) { + // lazy check for hits full up condition + if (_cull_hits_full(r_params)) { + return false; + } + + TLeaf &leaf = _node_get_leaf(tnode); + + // test children individually + for (int n = 0; n < leaf.num_items; n++) { + if (leaf.get_aabb(n).intersects_point(r_params.point)) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // register hit + _cull_hit(child_id, r_params); + } + } + } else { + // test children individually + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + + // add to the stack + CullPointParams *child = ii.request(); + child->node_id = child_id; + } + } + + } // while more nodes to pop + + // true indicates results are not full + return true; +} + +bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) { + // our function parameters to keep on a stack + struct CullAABBParams { + uint32_t node_id; + bool fully_within; + }; + + // most of the iterative functionality is contained in this helper class + BVH_IterativeInfo<CullAABBParams> ii; + + // alloca must allocate the stack from this function, it cannot be allocated in the + // helper class + ii.stack = (CullAABBParams *)alloca(ii.get_alloca_stacksize()); + + // seed the stack + ii.get_first()->node_id = p_node_id; + ii.get_first()->fully_within = p_fully_within; + + CullAABBParams cap; + + // while there are still more nodes on the stack + while (ii.pop(cap)) { + TNode &tnode = _nodes[cap.node_id]; + + if (tnode.is_leaf()) { + // lazy check for hits full up condition + if (_cull_hits_full(r_params)) { + return false; + } + + TLeaf &leaf = _node_get_leaf(tnode); + + // if fully within we can just add all items + // as long as they pass mask checks + if (cap.fully_within) { + for (int n = 0; n < leaf.num_items; n++) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // register hit + _cull_hit(child_id, r_params); + } + } else { + for (int n = 0; n < leaf.num_items; n++) { + const BVHABB_CLASS &aabb = leaf.get_aabb(n); + + if (aabb.intersects(r_params.abb)) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // register hit + _cull_hit(child_id, r_params); + } + } + } // not fully within + } else { + if (!cap.fully_within) { + // test children individually + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + const BVHABB_CLASS &child_abb = _nodes[child_id].aabb; + + if (child_abb.intersects(r_params.abb)) { + // is the node totally within the aabb? + bool fully_within = r_params.abb.is_other_within(child_abb); + + // add to the stack + CullAABBParams *child = ii.request(); + + // should always return valid child + child->node_id = child_id; + child->fully_within = fully_within; + } + } + } else { + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + + // add to the stack + CullAABBParams *child = ii.request(); + + // should always return valid child + child->node_id = child_id; + child->fully_within = true; + } + } + } + + } // while more nodes to pop + + // true indicates results are not full + return true; +} + +// returns full up with results +bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) { + // our function parameters to keep on a stack + struct CullConvexParams { + uint32_t node_id; + bool fully_within; + }; + + // most of the iterative functionality is contained in this helper class + BVH_IterativeInfo<CullConvexParams> ii; + + // alloca must allocate the stack from this function, it cannot be allocated in the + // helper class + ii.stack = (CullConvexParams *)alloca(ii.get_alloca_stacksize()); + + // seed the stack + ii.get_first()->node_id = p_node_id; + ii.get_first()->fully_within = p_fully_within; + + // preallocate these as a once off to be reused + uint32_t max_planes = r_params.hull.num_planes; + uint32_t *plane_ids = (uint32_t *)alloca(sizeof(uint32_t) * max_planes); + + CullConvexParams ccp; + + // while there are still more nodes on the stack + while (ii.pop(ccp)) { + const TNode &tnode = _nodes[ccp.node_id]; + + if (!ccp.fully_within) { + typename BVHABB_CLASS::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull); + + switch (res) { + default: { + continue; // miss, just move on to the next node in the stack + } break; + case BVHABB_CLASS::IR_PARTIAL: { + } break; + case BVHABB_CLASS::IR_FULL: { + ccp.fully_within = true; + } break; + } + + } // if not fully within already + + if (tnode.is_leaf()) { + // lazy check for hits full up condition + if (_cull_hits_full(r_params)) { + return false; + } + + const TLeaf &leaf = _node_get_leaf(tnode); + + // if fully within, simply add all items to the result + // (taking into account masks) + if (ccp.fully_within) { + for (int n = 0; n < leaf.num_items; n++) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // register hit + _cull_hit(child_id, r_params); + } + + } else { + // we can either use a naive check of all the planes against the AABB, + // or an optimized check, which finds in advance which of the planes can possibly + // cut the AABB, and only tests those. This can be much faster. +#define BVH_CONVEX_CULL_OPTIMIZED +#ifdef BVH_CONVEX_CULL_OPTIMIZED + // first find which planes cut the aabb + uint32_t num_planes = tnode.aabb.find_cutting_planes(r_params.hull, plane_ids); + BVH_ASSERT(num_planes <= max_planes); + +//#define BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK +#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK + // rigorous check + uint32_t results[MAX_ITEMS]; + uint32_t num_results = 0; +#endif + + // test children individually + for (int n = 0; n < leaf.num_items; n++) { + //const Item &item = leaf.get_item(n); + const BVHABB_CLASS &aabb = leaf.get_aabb(n); + + if (aabb.intersects_convex_optimized(r_params.hull, plane_ids, num_planes)) { + uint32_t child_id = leaf.get_item_ref_id(n); + +#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK + results[num_results++] = child_id; +#endif + + // register hit + _cull_hit(child_id, r_params); + } + } + +#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK + uint32_t test_count = 0; + + for (int n = 0; n < leaf.num_items; n++) { + const BVHABB_CLASS &aabb = leaf.get_aabb(n); + + if (aabb.intersects_convex_partial(r_params.hull)) { + uint32_t child_id = leaf.get_item_ref_id(n); + + CRASH_COND(child_id != results[test_count++]); + CRASH_COND(test_count > num_results); + } + } +#endif + +#else + // not BVH_CONVEX_CULL_OPTIMIZED + // test children individually + for (int n = 0; n < leaf.num_items; n++) { + const BVHABB_CLASS &aabb = leaf.get_aabb(n); + + if (aabb.intersects_convex_partial(r_params.hull)) { + uint32_t child_id = leaf.get_item_ref_id(n); + + // full up with results? exit early, no point in further testing + if (!_cull_hit(child_id, r_params)) + return false; + } + } +#endif // BVH_CONVEX_CULL_OPTIMIZED + } // if not fully within + } else { + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + + // add to the stack + CullConvexParams *child = ii.request(); + + // should always return valid child + child->node_id = child_id; + child->fully_within = ccp.fully_within; + } + } + + } // while more nodes to pop + + // true indicates results are not full + return true; +} diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc new file mode 100644 index 0000000000..a97304334c --- /dev/null +++ b/core/math/bvh_debug.inc @@ -0,0 +1,68 @@ +public: +#ifdef BVH_VERBOSE +void _debug_recursive_print_tree(int p_tree_id) const { + if (_root_node_id[p_tree_id] != BVHCommon::INVALID) + _debug_recursive_print_tree_node(_root_node_id[p_tree_id]); +} + +String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const { + String sz = "("; + sz += itos(aabb.min.x); + sz += " ~ "; + sz += itos(-aabb.neg_max.x); + sz += ") ("; + + sz += itos(aabb.min.y); + sz += " ~ "; + sz += itos(-aabb.neg_max.y); + sz += ") ("; + + sz += itos(aabb.min.z); + sz += " ~ "; + sz += itos(-aabb.neg_max.z); + sz += ") "; + + Vector3 size = aabb.calculate_size(); + float vol = size.x * size.y * size.z; + sz += "vol " + itos(vol); + + return sz; +} + +void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const { + const TNode &tnode = _nodes[p_node_id]; + + String sz = ""; + for (int n = 0; n < depth; n++) { + sz += "\t"; + } + sz += itos(p_node_id); + + if (tnode.is_leaf()) { + sz += " L"; + sz += itos(tnode.height) + " "; + const TLeaf &leaf = _node_get_leaf(tnode); + + sz += "["; + for (int n = 0; n < leaf.num_items; n++) { + if (n) + sz += ", "; + sz += "r"; + sz += itos(leaf.get_item_ref_id(n)); + } + sz += "] "; + } else { + sz += " N"; + sz += itos(tnode.height) + " "; + } + + sz += _debug_aabb_to_string(tnode.aabb); + print_line(sz); + + if (!tnode.is_leaf()) { + for (int n = 0; n < tnode.num_children; n++) { + _debug_recursive_print_tree_node(tnode.children[n], depth + 1); + } + } +} +#endif diff --git a/core/math/bvh_integrity.inc b/core/math/bvh_integrity.inc new file mode 100644 index 0000000000..02e9d30097 --- /dev/null +++ b/core/math/bvh_integrity.inc @@ -0,0 +1,42 @@ +void _integrity_check_all() { +#ifdef BVH_INTEGRITY_CHECKS + for (int n = 0; n < NUM_TREES; n++) { + uint32_t root = _root_node_id[n]; + if (root != BVHCommon::INVALID) { + _integrity_check_down(root); + } + } +#endif +} + +void _integrity_check_up(uint32_t p_node_id) { + TNode &node = _nodes[p_node_id]; + + BVHABB_CLASS abb = node.aabb; + node_update_aabb(node); + + BVHABB_CLASS abb2 = node.aabb; + abb2.expand(-_node_expansion); + + CRASH_COND(!abb.is_other_within(abb2)); +} + +void _integrity_check_down(uint32_t p_node_id) { + const TNode &node = _nodes[p_node_id]; + + if (node.is_leaf()) { + _integrity_check_up(p_node_id); + } else { + CRASH_COND(node.num_children != 2); + + for (int n = 0; n < node.num_children; n++) { + uint32_t child_id = node.children[n]; + + // check the children parent pointers are correct + TNode &child = _nodes[child_id]; + CRASH_COND(child.parent_id != p_node_id); + + _integrity_check_down(child_id); + } + } +} diff --git a/core/math/bvh_logic.inc b/core/math/bvh_logic.inc new file mode 100644 index 0000000000..afab08f151 --- /dev/null +++ b/core/math/bvh_logic.inc @@ -0,0 +1,230 @@ + +// for slow incremental optimization, we will periodically remove each +// item from the tree and reinsert, to give it a chance to find a better position +void _logic_item_remove_and_reinsert(uint32_t p_ref_id) { + // get the reference + ItemRef &ref = _refs[p_ref_id]; + + // no need to optimize inactive items + if (!ref.is_active()) { + return; + } + + // special case of debug draw + if (ref.item_id == BVHCommon::INVALID) { + return; + } + + BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID); + + // some overlay elaborate way to find out which tree the node is in! + BVHHandle temp_handle; + temp_handle.set_id(p_ref_id); + uint32_t tree_id = _handle_get_tree_id(temp_handle); + + // remove and reinsert + BVHABB_CLASS abb; + node_remove_item(p_ref_id, tree_id, &abb); + + // we must choose where to add to tree + ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb); + _node_add_item(ref.tnode_id, p_ref_id, abb); + + refit_upward_and_balance(ref.tnode_id, tree_id); +} + +// from randy gaul balance function +BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) { + BVHABB_CLASS c = a; + c.merge(b); + return c; +} + +//-------------------------------------------------------------------------------------------------- +/** +@file q3DynamicAABBTree.h +@author Randy Gaul +@date 10/10/2014 + Copyright (c) 2014 Randy Gaul http://www.randygaul.net + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not + be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ +//-------------------------------------------------------------------------------------------------- + +// This function is based on the 'Balance' function from Randy Gaul's qu3e +// https://github.com/RandyGaul/qu3e +// It is MODIFIED from qu3e version. +// This is the only function used (and _logic_abb_merge helper function). +int32_t _logic_balance(int32_t iA, uint32_t p_tree_id) { + // return iA; // uncomment this to bypass balance + + TNode *A = &_nodes[iA]; + + if (A->is_leaf() || A->height == 1) { + return iA; + } + + /* A + / \ + B C + / \ / \ + D E F G + */ + + CRASH_COND(A->num_children != 2); + int32_t iB = A->children[0]; + int32_t iC = A->children[1]; + TNode *B = &_nodes[iB]; + TNode *C = &_nodes[iC]; + + int32_t balance = C->height - B->height; + + // C is higher, promote C + if (balance > 1) { + int32_t iF = C->children[0]; + int32_t iG = C->children[1]; + TNode *F = &_nodes[iF]; + TNode *G = &_nodes[iG]; + + // grandParent point to C + if (A->parent_id != BVHCommon::INVALID) { + if (_nodes[A->parent_id].children[0] == iA) { + _nodes[A->parent_id].children[0] = iC; + + } else { + _nodes[A->parent_id].children[1] = iC; + } + } else { + // check this .. seems dodgy + change_root_node(iC, p_tree_id); + } + + // Swap A and C + C->children[0] = iA; + C->parent_id = A->parent_id; + A->parent_id = iC; + + // Finish rotation + if (F->height > G->height) { + C->children[1] = iF; + A->children[1] = iG; + G->parent_id = iA; + A->aabb = _logic_abb_merge(B->aabb, G->aabb); + C->aabb = _logic_abb_merge(A->aabb, F->aabb); + + A->height = 1 + MAX(B->height, G->height); + C->height = 1 + MAX(A->height, F->height); + } + + else { + C->children[1] = iG; + A->children[1] = iF; + F->parent_id = iA; + A->aabb = _logic_abb_merge(B->aabb, F->aabb); + C->aabb = _logic_abb_merge(A->aabb, G->aabb); + + A->height = 1 + MAX(B->height, F->height); + C->height = 1 + MAX(A->height, G->height); + } + + return iC; + } + + // B is higher, promote B + else if (balance < -1) { + int32_t iD = B->children[0]; + int32_t iE = B->children[1]; + TNode *D = &_nodes[iD]; + TNode *E = &_nodes[iE]; + + // grandParent point to B + if (A->parent_id != BVHCommon::INVALID) { + if (_nodes[A->parent_id].children[0] == iA) { + _nodes[A->parent_id].children[0] = iB; + } else { + _nodes[A->parent_id].children[1] = iB; + } + } + + else { + // check this .. seems dodgy + change_root_node(iB, p_tree_id); + } + + // Swap A and B + B->children[1] = iA; + B->parent_id = A->parent_id; + A->parent_id = iB; + + // Finish rotation + if (D->height > E->height) { + B->children[0] = iD; + A->children[0] = iE; + E->parent_id = iA; + A->aabb = _logic_abb_merge(C->aabb, E->aabb); + B->aabb = _logic_abb_merge(A->aabb, D->aabb); + + A->height = 1 + MAX(C->height, E->height); + B->height = 1 + MAX(A->height, D->height); + } + + else { + B->children[0] = iE; + A->children[0] = iD; + D->parent_id = iA; + A->aabb = _logic_abb_merge(C->aabb, D->aabb); + B->aabb = _logic_abb_merge(A->aabb, E->aabb); + + A->height = 1 + MAX(C->height, D->height); + B->height = 1 + MAX(A->height, E->height); + } + + return iB; + } + + return iA; +} + +// either choose an existing node to add item to, or create a new node and return this +uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) { + while (true) { + BVH_ASSERT(p_node_id != BVHCommon::INVALID); + TNode &tnode = _nodes[p_node_id]; + + if (tnode.is_leaf()) { + // if a leaf, and non full, use this to add to + if (!node_is_leaf_full(tnode)) { + return p_node_id; + } + + // else split the leaf, and use one of the children to add to + return split_leaf(p_node_id, p_aabb); + } + + // this should not happen??? + // is still happening, need to debug and find circumstances. Is not that serious + // but would be nice to prevent. I think it only happens with the root node. + if (tnode.num_children == 1) { + WARN_PRINT_ONCE("BVH::recursive_choose_item_add_node, node with 1 child, recovering"); + p_node_id = tnode.children[0]; + } else { + BVH_ASSERT(tnode.num_children == 2); + TNode &childA = _nodes[tnode.children[0]]; + TNode &childB = _nodes[tnode.children[1]]; + int which = p_aabb.select_by_proximity(childA.aabb, childB.aabb); + + p_node_id = tnode.children[which]; + } + } +} diff --git a/core/math/bvh_misc.inc b/core/math/bvh_misc.inc new file mode 100644 index 0000000000..71aa0e4fe0 --- /dev/null +++ b/core/math/bvh_misc.inc @@ -0,0 +1,55 @@ + +int _handle_get_tree_id(BVHHandle p_handle) const { + if (USE_PAIRS) { + int tree = 0; + if (_extra[p_handle.id()].pairable) { + tree = 1; + } + return tree; + } + return 0; +} + +public: +void _handle_sort(BVHHandle &p_ha, BVHHandle &p_hb) const { + if (p_ha.id() > p_hb.id()) { + BVHHandle temp = p_hb; + p_hb = p_ha; + p_ha = temp; + } +} + +private: +void create_root_node(int p_tree) { + // if there is no root node, create one + if (_root_node_id[p_tree] == BVHCommon::INVALID) { + uint32_t root_node_id; + TNode *node = _nodes.request(root_node_id); + node->clear(); + _root_node_id[p_tree] = root_node_id; + + // make the root node a leaf + uint32_t leaf_id; + TLeaf *leaf = _leaves.request(leaf_id); + leaf->clear(); + node->neg_leaf_id = -(int)leaf_id; + } +} + +bool node_is_leaf_full(TNode &tnode) const { + const TLeaf &leaf = _node_get_leaf(tnode); + return leaf.is_full(); +} + +public: +TLeaf &_node_get_leaf(TNode &tnode) { + BVH_ASSERT(tnode.is_leaf()); + return _leaves[tnode.get_leaf_id()]; +} + +const TLeaf &_node_get_leaf(const TNode &tnode) const { + BVH_ASSERT(tnode.is_leaf()); + return _leaves[tnode.get_leaf_id()]; +} + +private: diff --git a/core/math/bvh_pair.inc b/core/math/bvh_pair.inc new file mode 100644 index 0000000000..839db59a3a --- /dev/null +++ b/core/math/bvh_pair.inc @@ -0,0 +1,62 @@ +public: +// note .. maybe this can be attached to another node structure? +// depends which works best for cache. +struct ItemPairs { + struct Link { + void set(BVHHandle h, void *ud) { + handle = h; + userdata = ud; + } + BVHHandle handle; + void *userdata; + }; + + void clear() { + num_pairs = 0; + extended_pairs.reset(); + expanded_aabb = Bounds(); + } + + Bounds expanded_aabb; + + // maybe we can just use the number in the vector TODO + int32_t num_pairs; + LocalVector<Link> extended_pairs; + + void add_pair_to(BVHHandle h, void *p_userdata) { + Link temp; + temp.set(h, p_userdata); + + extended_pairs.push_back(temp); + num_pairs++; + } + + uint32_t find_pair_to(BVHHandle h) const { + for (int n = 0; n < num_pairs; n++) { + if (extended_pairs[n].handle == h) { + return n; + } + } + return -1; + } + + bool contains_pair_to(BVHHandle h) const { + return find_pair_to(h) != BVHCommon::INVALID; + } + + // return success + void *remove_pair_to(BVHHandle h) { + void *userdata = nullptr; + + for (int n = 0; n < num_pairs; n++) { + if (extended_pairs[n].handle == h) { + userdata = extended_pairs[n].userdata; + extended_pairs.remove_unordered(n); + num_pairs--; + break; + } + } + + return userdata; + } +}; diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc new file mode 100644 index 0000000000..2c1e406712 --- /dev/null +++ b/core/math/bvh_public.inc @@ -0,0 +1,423 @@ +public: +BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) { +#ifdef BVH_VERBOSE_TREE + VERBOSE_PRINT("\nitem_add BEFORE"); + _debug_recursive_print_tree(0); + VERBOSE_PRINT("\n"); +#endif + + BVHABB_CLASS abb; + abb.from(p_aabb); + + // handle to be filled with the new item ref + BVHHandle handle; + + // ref id easier to pass around than handle + uint32_t ref_id; + + // this should never fail + ItemRef *ref = _refs.request(ref_id); + + // the extra data should be parallel list to the references + uint32_t extra_id; + ItemExtra *extra = _extra.request(extra_id); + BVH_ASSERT(extra_id == ref_id); + + // pairs info + if (USE_PAIRS) { + uint32_t pairs_id; + ItemPairs *pairs = _pairs.request(pairs_id); + pairs->clear(); + BVH_ASSERT(pairs_id == ref_id); + } + + extra->subindex = p_subindex; + extra->userdata = p_userdata; + extra->last_updated_tick = 0; + + // add an active reference to the list for slow incremental optimize + // this list must be kept in sync with the references as they are added or removed. + extra->active_ref_id = _active_refs.size(); + _active_refs.push_back(ref_id); + + if (USE_PAIRS) { + extra->pairable_mask = p_pairable_mask; + extra->pairable_type = p_pairable_type; + extra->pairable = p_pairable; + } else { + // just for safety, in case this gets queried etc + extra->pairable = 0; + p_pairable = false; + } + + // assign to handle to return + handle.set_id(ref_id); + + uint32_t tree_id = 0; + if (p_pairable) { + tree_id = 1; + } + + create_root_node(tree_id); + + // we must choose where to add to tree + if (p_active) { + ref->tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb); + + bool refit = _node_add_item(ref->tnode_id, ref_id, abb); + + if (refit) { + // only need to refit from the parent + const TNode &add_node = _nodes[ref->tnode_id]; + if (add_node.parent_id != BVHCommon::INVALID) { + refit_upward_and_balance(add_node.parent_id, tree_id); + } + } + } else { + ref->set_inactive(); + } + +#ifdef BVH_VERBOSE + // memory use + int mem = _refs.estimate_memory_use(); + mem += _nodes.estimate_memory_use(); + + String sz = _debug_aabb_to_string(abb); + VERBOSE_PRINT("\titem_add [" + itos(ref_id) + "] " + itos(_refs.size()) + " refs,\t" + itos(_nodes.size()) + " nodes " + sz); + VERBOSE_PRINT("mem use : " + itos(mem) + ", num nodes : " + itos(_nodes.size())); + +#endif + + return handle; +} + +void _debug_print_refs() { +#ifdef BVH_VERBOSE_TREE + print_line("refs....."); + for (int n = 0; n < _refs.size(); n++) { + const ItemRef &ref = _refs[n]; + print_line("tnode_id " + itos(ref.tnode_id) + ", item_id " + itos(ref.item_id)); + } + +#endif +} + +// returns false if noop +bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { + uint32_t ref_id = p_handle.id(); + + // get the reference + ItemRef &ref = _refs[ref_id]; + if (!ref.is_active()) { + return false; + } + + BVHABB_CLASS abb; + abb.from(p_aabb); + + BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID); + TNode &tnode = _nodes[ref.tnode_id]; + + // does it fit within the current aabb? + if (tnode.aabb.is_other_within(abb)) { + // do nothing .. fast path .. not moved enough to need refit + + // however we WILL update the exact aabb in the leaf, as this will be needed + // for accurate collision detection + TLeaf &leaf = _node_get_leaf(tnode); + + BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id); + + // no change? + if (leaf_abb == abb) { + return false; + } + + leaf_abb = abb; + _integrity_check_all(); + + return true; + } + + uint32_t tree_id = _handle_get_tree_id(p_handle); + + // remove and reinsert + node_remove_item(ref_id, tree_id); + + // we must choose where to add to tree + ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb); + + // add to the tree + bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb); + + // only need to refit from the PARENT + if (needs_refit) { + // only need to refit from the parent + const TNode &add_node = _nodes[ref.tnode_id]; + if (add_node.parent_id != BVHCommon::INVALID) { + // not sure we need to rebalance all the time, this can be done less often + refit_upward(add_node.parent_id); + } + //refit_upward_and_balance(add_node.parent_id); + } + + return true; +} + +void item_remove(BVHHandle p_handle) { + uint32_t ref_id = p_handle.id(); + + uint32_t tree_id = _handle_get_tree_id(p_handle); + + VERBOSE_PRINT("item_remove [" + itos(ref_id) + "] "); + + //////////////////////////////////////// + // remove the active reference from the list for slow incremental optimize + // this list must be kept in sync with the references as they are added or removed. + uint32_t active_ref_id = _extra[ref_id].active_ref_id; + uint32_t ref_id_moved_back = _active_refs[_active_refs.size() - 1]; + + // swap back and decrement for fast unordered remove + _active_refs[active_ref_id] = ref_id_moved_back; + _active_refs.resize(_active_refs.size() - 1); + + // keep the moved active reference up to date + _extra[ref_id_moved_back].active_ref_id = active_ref_id; + //////////////////////////////////////// + + // remove the item from the node (only if active) + if (_refs[ref_id].is_active()) { + node_remove_item(ref_id, tree_id); + } + + // remove the item reference + _refs.free(ref_id); + _extra.free(ref_id); + if (USE_PAIRS) { + _pairs.free(ref_id); + } + + // don't think refit_all is necessary? + //refit_all(_tree_id); + +#ifdef BVH_VERBOSE_TREE + _debug_recursive_print_tree(tree_id); +#endif +} + +// returns success +bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) { + uint32_t ref_id = p_handle.id(); + ItemRef &ref = _refs[ref_id]; + if (ref.is_active()) { + // noop + return false; + } + + // add to tree + BVHABB_CLASS abb; + abb.from(p_aabb); + + uint32_t tree_id = _handle_get_tree_id(p_handle); + + // we must choose where to add to tree + ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb); + _node_add_item(ref.tnode_id, ref_id, abb); + + refit_upward_and_balance(ref.tnode_id, tree_id); + + return true; +} + +// returns success +bool item_deactivate(BVHHandle p_handle) { + uint32_t ref_id = p_handle.id(); + ItemRef &ref = _refs[ref_id]; + if (!ref.is_active()) { + // noop + return false; + } + + uint32_t tree_id = _handle_get_tree_id(p_handle); + + // remove from tree + BVHABB_CLASS abb; + node_remove_item(ref_id, tree_id, &abb); + + // mark as inactive + ref.set_inactive(); + return true; +} + +bool item_get_active(BVHHandle p_handle) const { + uint32_t ref_id = p_handle.id(); + const ItemRef &ref = _refs[ref_id]; + return ref.is_active(); +} + +// during collision testing, we want to set the mask and whether pairable for the item testing from +void item_fill_cullparams(BVHHandle p_handle, CullParams &r_params) const { + uint32_t ref_id = p_handle.id(); + const ItemExtra &extra = _extra[ref_id]; + + // testing from a non pairable item, we only want to test pairable items + r_params.test_pairable_only = extra.pairable == 0; + + // we take into account the mask of the item testing from + r_params.mask = extra.pairable_mask; + r_params.pairable_type = extra.pairable_type; +} + +bool item_is_pairable(const BVHHandle &p_handle) { + uint32_t ref_id = p_handle.id(); + const ItemExtra &extra = _extra[ref_id]; + return extra.pairable != 0; +} + +void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) { + // change tree? + uint32_t ref_id = p_handle.id(); + const ItemRef &ref = _refs[ref_id]; + + TNode &tnode = _nodes[ref.tnode_id]; + TLeaf &leaf = _node_get_leaf(tnode); + + r_abb = leaf.get_aabb(ref.item_id); +} + +bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { + // change tree? + uint32_t ref_id = p_handle.id(); + + ItemExtra &ex = _extra[ref_id]; + ItemRef &ref = _refs[ref_id]; + + bool active = ref.is_active(); + bool pairable_changed = (ex.pairable != 0) != p_pairable; + bool state_changed = pairable_changed || (ex.pairable_type != p_pairable_type) || (ex.pairable_mask != p_pairable_mask); + + ex.pairable_type = p_pairable_type; + ex.pairable_mask = p_pairable_mask; + + if (active && pairable_changed) { + // record abb + TNode &tnode = _nodes[ref.tnode_id]; + TLeaf &leaf = _node_get_leaf(tnode); + BVHABB_CLASS abb = leaf.get_aabb(ref.item_id); + + // make sure current tree is correct prior to changing + uint32_t tree_id = _handle_get_tree_id(p_handle); + + // remove from old tree + node_remove_item(ref_id, tree_id); + + // we must set the pairable AFTER getting the current tree + // because the pairable status determines which tree + ex.pairable = p_pairable; + + // add to new tree + tree_id = _handle_get_tree_id(p_handle); + create_root_node(tree_id); + + // we must choose where to add to tree + ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb); + bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb); + + // only need to refit from the PARENT + if (needs_refit) { + // only need to refit from the parent + const TNode &add_node = _nodes[ref.tnode_id]; + if (add_node.parent_id != BVHCommon::INVALID) { + refit_upward_and_balance(add_node.parent_id, tree_id); + } + } + } else { + // always keep this up to date + ex.pairable = p_pairable; + } + + return state_changed; +} + +void incremental_optimize() { + // first update all aabbs as one off step.. + // this is cheaper than doing it on each move as each leaf may get touched multiple times + // in a frame. + for (int n = 0; n < NUM_TREES; n++) { + if (_root_node_id[n] != BVHCommon::INVALID) { + refit_branch(_root_node_id[n]); + } + } + + // now do small section reinserting to get things moving + // gradually, and keep items in the right leaf + if (_current_active_ref >= _active_refs.size()) { + _current_active_ref = 0; + } + + // special case + if (!_active_refs.size()) { + return; + } + + uint32_t ref_id = _active_refs[_current_active_ref++]; + + _logic_item_remove_and_reinsert(ref_id); + +#ifdef BVH_VERBOSE + /* + // memory use + int mem_refs = _refs.estimate_memory_use(); + int mem_nodes = _nodes.estimate_memory_use(); + int mem_leaves = _leaves.estimate_memory_use(); + + String sz; + sz += "mem_refs : " + itos(mem_refs) + " "; + sz += "mem_nodes : " + itos(mem_nodes) + " "; + sz += "mem_leaves : " + itos(mem_leaves) + " "; + sz += ", num nodes : " + itos(_nodes.size()); + print_line(sz); + */ +#endif +} + +void update() { + incremental_optimize(); + + // keep the expansion values up to date with the world bound +//#define BVH_ALLOW_AUTO_EXPANSION +#ifdef BVH_ALLOW_AUTO_EXPANSION + if (_auto_node_expansion || _auto_pairing_expansion) { + BVHABB_CLASS world_bound; + world_bound.set_to_max_opposite_extents(); + + bool bound_valid = false; + + for (int n = 0; n < NUM_TREES; n++) { + uint32_t node_id = _root_node_id[n]; + if (node_id != BVHCommon::INVALID) { + world_bound.merge(_nodes[node_id].aabb); + bound_valid = true; + } + } + + // if there are no nodes, do nothing, but if there are... + if (bound_valid) { + Bounds bb; + world_bound.to(bb); + real_t size = bb.get_longest_axis_size(); + + // automatic AI decision for best parameters. + // These can be overridden in project settings. + + // these magic numbers are determined by experiment + if (_auto_node_expansion) { + _node_expansion = size * 0.025; + } + if (_auto_pairing_expansion) { + _pairing_expansion = size * 0.009; + } + } + } +#endif +} diff --git a/core/math/bvh_refit.inc b/core/math/bvh_refit.inc new file mode 100644 index 0000000000..717a3438c7 --- /dev/null +++ b/core/math/bvh_refit.inc @@ -0,0 +1,141 @@ +void _debug_node_verify_bound(uint32_t p_node_id) { + TNode &node = _nodes[p_node_id]; + BVHABB_CLASS abb_before = node.aabb; + + node_update_aabb(node); + + BVHABB_CLASS abb_after = node.aabb; + CRASH_COND(abb_before != abb_after); +} + +void node_update_aabb(TNode &tnode) { + tnode.aabb.set_to_max_opposite_extents(); + tnode.height = 0; + + if (!tnode.is_leaf()) { + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_node_id = tnode.children[n]; + + // merge with child aabb + const TNode &tchild = _nodes[child_node_id]; + tnode.aabb.merge(tchild.aabb); + + // do heights at the same time + if (tchild.height > tnode.height) { + tnode.height = tchild.height; + } + } + + // the height of a non leaf is always 1 bigger than the biggest child + tnode.height++; + +#ifdef BVH_CHECKS + if (!tnode.num_children) { + // the 'blank' aabb will screw up parent aabbs + WARN_PRINT("BVH_Tree::TNode no children, AABB is undefined"); + } +#endif + } else { + // leaf + const TLeaf &leaf = _node_get_leaf(tnode); + + for (int n = 0; n < leaf.num_items; n++) { + tnode.aabb.merge(leaf.get_aabb(n)); + } + + // now the leaf items are unexpanded, we expand only in the node AABB + tnode.aabb.expand(_node_expansion); +#ifdef BVH_CHECKS + if (!leaf.num_items) { + // the 'blank' aabb will screw up parent aabbs + WARN_PRINT("BVH_Tree::TLeaf no items, AABB is undefined"); + } +#endif + } +} + +void refit_all(int p_tree_id) { + refit_downward(_root_node_id[p_tree_id]); +} + +void refit_upward(uint32_t p_node_id) { + while (p_node_id != BVHCommon::INVALID) { + TNode &tnode = _nodes[p_node_id]; + node_update_aabb(tnode); + p_node_id = tnode.parent_id; + } +} + +void refit_upward_and_balance(uint32_t p_node_id, uint32_t p_tree_id) { + while (p_node_id != BVHCommon::INVALID) { + uint32_t before = p_node_id; + p_node_id = _logic_balance(p_node_id, p_tree_id); + + if (before != p_node_id) { + VERBOSE_PRINT("REBALANCED!"); + } + + TNode &tnode = _nodes[p_node_id]; + + // update overall aabb from the children + node_update_aabb(tnode); + + p_node_id = tnode.parent_id; + } +} + +void refit_downward(uint32_t p_node_id) { + TNode &tnode = _nodes[p_node_id]; + + // do children first + if (!tnode.is_leaf()) { + for (int n = 0; n < tnode.num_children; n++) { + refit_downward(tnode.children[n]); + } + } + + node_update_aabb(tnode); +} + +// go down to the leaves, then refit upward +void refit_branch(uint32_t p_node_id) { + // our function parameters to keep on a stack + struct RefitParams { + uint32_t node_id; + }; + + // most of the iterative functionality is contained in this helper class + BVH_IterativeInfo<RefitParams> ii; + + // alloca must allocate the stack from this function, it cannot be allocated in the + // helper class + ii.stack = (RefitParams *)alloca(ii.get_alloca_stacksize()); + + // seed the stack + ii.get_first()->node_id = p_node_id; + + RefitParams rp; + + // while there are still more nodes on the stack + while (ii.pop(rp)) { + TNode &tnode = _nodes[rp.node_id]; + + // do children first + if (!tnode.is_leaf()) { + for (int n = 0; n < tnode.num_children; n++) { + uint32_t child_id = tnode.children[n]; + + // add to the stack + RefitParams *child = ii.request(); + child->node_id = child_id; + } + } else { + // leaf .. only refit upward if dirty + TLeaf &leaf = _node_get_leaf(tnode); + if (leaf.is_dirty()) { + leaf.set_dirty(false); + refit_upward(p_node_id); + } + } + } // while more nodes to pop +} diff --git a/core/math/bvh_split.inc b/core/math/bvh_split.inc new file mode 100644 index 0000000000..3fcc4c7b10 --- /dev/null +++ b/core/math/bvh_split.inc @@ -0,0 +1,294 @@ +void _split_inform_references(uint32_t p_node_id) { + TNode &node = _nodes[p_node_id]; + TLeaf &leaf = _node_get_leaf(node); + + for (int n = 0; n < leaf.num_items; n++) { + uint32_t ref_id = leaf.get_item_ref_id(n); + + ItemRef &ref = _refs[ref_id]; + ref.tnode_id = p_node_id; + ref.item_id = n; + } +} + +void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) { + // special case for low leaf sizes .. should static compile out + if (MAX_ITEMS < 4) { + uint32_t ind = group_a[0]; + + // add to b + group_b[num_b++] = ind; + + // remove from a + group_a[0] = group_a[num_a - 1]; + num_a--; + return; + } + + Point centre = full_bound.calculate_centre(); + Point size = full_bound.calculate_size(); + + int order[3]; + + order[0] = size.min_axis(); + order[2] = size.max_axis(); + order[1] = 3 - (order[0] + order[2]); + + // simplest case, split on the longest axis + int split_axis = order[0]; + for (int a = 0; a < num_a; a++) { + uint32_t ind = group_a[a]; + + if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) { + // add to b + group_b[num_b++] = ind; + + // remove from a + group_a[a] = group_a[num_a - 1]; + num_a--; + + // do this one again, as it has been replaced + a--; + } + } + + // detect when split on longest axis failed + int min_threshold = MAX_ITEMS / 4; + int min_group_size[3]; + min_group_size[0] = MIN(num_a, num_b); + if (min_group_size[0] < min_threshold) { + // slow but sure .. first move everything back into a + for (int b = 0; b < num_b; b++) { + group_a[num_a++] = group_b[b]; + } + num_b = 0; + + // now calculate the best split + for (int axis = 1; axis < 3; axis++) { + split_axis = order[axis]; + int count = 0; + + for (int a = 0; a < num_a; a++) { + uint32_t ind = group_a[a]; + + if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) { + count++; + } + } + + min_group_size[axis] = MIN(count, num_a - count); + } // for axis + + // best axis + int best_axis = 0; + int best_min = min_group_size[0]; + for (int axis = 1; axis < 3; axis++) { + if (min_group_size[axis] > best_min) { + best_min = min_group_size[axis]; + best_axis = axis; + } + } + + // now finally do the split + if (best_min > 0) { + split_axis = order[best_axis]; + + for (int a = 0; a < num_a; a++) { + uint32_t ind = group_a[a]; + + if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) { + // add to b + group_b[num_b++] = ind; + + // remove from a + group_a[a] = group_a[num_a - 1]; + num_a--; + + // do this one again, as it has been replaced + a--; + } + } + } // if there was a split! + } // if the longest axis wasn't a good split + + // special case, none crossed threshold + if (!num_b) { + uint32_t ind = group_a[0]; + + // add to b + group_b[num_b++] = ind; + + // remove from a + group_a[0] = group_a[num_a - 1]; + num_a--; + } + // opposite problem! :) + if (!num_a) { + uint32_t ind = group_b[0]; + + // add to a + group_a[num_a++] = ind; + + // remove from b + group_b[0] = group_b[num_b - 1]; + num_b--; + } +} + +void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds) { + BVHABB_CLASS groupb_aabb; + groupb_aabb.set_to_max_opposite_extents(); + for (int n = 0; n < num_b; n++) { + int which = group_b[n]; + groupb_aabb.merge(temp_bounds[which]); + } + BVHABB_CLASS groupb_aabb_new; + + BVHABB_CLASS rest_aabb; + + float best_size = FLT_MAX; + int best_candidate = -1; + + // find most likely from a to move into b + for (int check = 0; check < num_a; check++) { + rest_aabb.set_to_max_opposite_extents(); + groupb_aabb_new = groupb_aabb; + + // find aabb of all the rest + for (int rest = 0; rest < num_a; rest++) { + if (rest == check) { + continue; + } + + int which = group_a[rest]; + rest_aabb.merge(temp_bounds[which]); + } + + groupb_aabb_new.merge(temp_bounds[group_a[check]]); + + // now compare the sizes + float size = groupb_aabb_new.get_area() + rest_aabb.get_area(); + if (size < best_size) { + best_size = size; + best_candidate = check; + } + } + + // we should now have the best, move it from group a to group b + group_b[num_b++] = group_a[best_candidate]; + + // remove best candidate from group a + num_a--; + group_a[best_candidate] = group_a[num_a]; +} + +uint32_t split_leaf(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) { + return split_leaf_complex(p_node_id, p_added_item_aabb); +} + +// aabb is the new inserted node +uint32_t split_leaf_complex(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) { + VERBOSE_PRINT("split_leaf"); + + // note the tnode before and AFTER splitting may be a different address + // in memory because the vector could get relocated. So we need to reget + // the tnode after the split + BVH_ASSERT(_nodes[p_node_id].is_leaf()); + + // first create child leaf nodes + uint32_t *child_ids = (uint32_t *)alloca(sizeof(uint32_t) * MAX_CHILDREN); + + for (int n = 0; n < MAX_CHILDREN; n++) { + // create node children + TNode *child_node = _nodes.request(child_ids[n]); + + child_node->clear(); + + // back link to parent + child_node->parent_id = p_node_id; + + // make each child a leaf node + node_make_leaf(child_ids[n]); + } + + // don't get any leaves or nodes till AFTER the split + TNode &tnode = _nodes[p_node_id]; + uint32_t orig_leaf_id = tnode.get_leaf_id(); + const TLeaf &orig_leaf = _node_get_leaf(tnode); + + // store the final child ids + for (int n = 0; n < MAX_CHILDREN; n++) { + tnode.children[n] = child_ids[n]; + } + + // mark as no longer a leaf node + tnode.num_children = MAX_CHILDREN; + + // 2 groups, A and B, and assign children to each to split equally + int max_children = orig_leaf.num_items + 1; // plus 1 for the wildcard .. the item being added + //CRASH_COND(max_children > MAX_CHILDREN); + + uint16_t *group_a = (uint16_t *)alloca(sizeof(uint16_t) * max_children); + uint16_t *group_b = (uint16_t *)alloca(sizeof(uint16_t) * max_children); + + // we are copying the ABBs. This is ugly, but we need one extra for the inserted item... + BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children); + + int num_a = max_children; + int num_b = 0; + + // setup - start with all in group a + for (int n = 0; n < orig_leaf.num_items; n++) { + group_a[n] = n; + temp_bounds[n] = orig_leaf.get_aabb(n); + } + // wildcard + int wildcard = orig_leaf.num_items; + + group_a[wildcard] = wildcard; + temp_bounds[wildcard] = p_added_item_aabb; + + // we can choose here either an equal split, or just 1 in the new leaf + _split_leaf_sort_groups_simple(num_a, num_b, group_a, group_b, temp_bounds, tnode.aabb); + + uint32_t wildcard_node = BVHCommon::INVALID; + + // now there should be equal numbers in both groups + for (int n = 0; n < num_a; n++) { + int which = group_a[n]; + + if (which != wildcard) { + const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which); + uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which); + //const Item &source_item = orig_leaf.get_item(which); + _node_add_item(tnode.children[0], source_item_ref_id, source_item_aabb); + } else { + wildcard_node = tnode.children[0]; + } + } + for (int n = 0; n < num_b; n++) { + int which = group_b[n]; + + if (which != wildcard) { + const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which); + uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which); + //const Item &source_item = orig_leaf.get_item(which); + _node_add_item(tnode.children[1], source_item_ref_id, source_item_aabb); + } else { + wildcard_node = tnode.children[1]; + } + } + + // now remove all items from the parent and replace with the child nodes + _leaves.free(orig_leaf_id); + + // we should keep the references up to date! + for (int n = 0; n < MAX_CHILDREN; n++) { + _split_inform_references(tnode.children[n]); + } + + refit_upward(p_node_id); + + BVH_ASSERT(wildcard_node != BVHCommon::INVALID); + return wildcard_node; +} diff --git a/core/math/bvh_structs.inc b/core/math/bvh_structs.inc new file mode 100644 index 0000000000..1d1e0e6468 --- /dev/null +++ b/core/math/bvh_structs.inc @@ -0,0 +1,180 @@ + +public: +struct ItemRef { + uint32_t tnode_id; // -1 is invalid + uint32_t item_id; // in the leaf + + bool is_active() const { return tnode_id != BVHCommon::INACTIVE; } + void set_inactive() { + tnode_id = BVHCommon::INACTIVE; + item_id = BVHCommon::INACTIVE; + } +}; + +// extra info kept in separate parallel list to the references, +// as this is less used as keeps cache better +struct ItemExtra { + uint32_t last_updated_tick; + uint32_t pairable; + uint32_t pairable_mask; + uint32_t pairable_type; + + int32_t subindex; + + // the active reference is a separate list of which references + // are active so that we can slowly iterate through it over many frames for + // slow optimize. + uint32_t active_ref_id; + + T *userdata; +}; + +// this is an item OR a child node depending on whether a leaf node +struct Item { + BVHABB_CLASS aabb; + uint32_t item_ref_id; +}; + +// tree leaf +struct TLeaf { + uint16_t num_items; + +private: + uint16_t dirty; + // separate data orientated lists for faster SIMD traversal + uint32_t item_ref_ids[MAX_ITEMS]; + BVHABB_CLASS aabbs[MAX_ITEMS]; + +public: + // accessors + BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; } + const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; } + + uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; } + const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; } + + bool is_dirty() const { return dirty; } + void set_dirty(bool p) { dirty = p; } + + void clear() { + num_items = 0; + set_dirty(true); + } + bool is_full() const { return num_items >= MAX_ITEMS; } + + void remove_item_unordered(uint32_t p_id) { + BVH_ASSERT(p_id < num_items); + num_items--; + aabbs[p_id] = aabbs[num_items]; + item_ref_ids[p_id] = item_ref_ids[num_items]; + } + + uint32_t request_item() { + if (num_items < MAX_ITEMS) { + uint32_t id = num_items; + num_items++; + return id; + } + return -1; + } +}; + +// tree node +struct TNode { + BVHABB_CLASS aabb; + // either number of children if positive + // or leaf id if negative (leaf id 0 is disallowed) + union { + int32_t num_children; + int32_t neg_leaf_id; + }; + uint32_t parent_id; // or -1 + uint16_t children[MAX_CHILDREN]; + + // height in the tree, where leaves are 0, and all above are 1+ + // (or the highest where there is a tie off) + int32_t height; + + bool is_leaf() const { return num_children < 0; } + void set_leaf_id(int id) { neg_leaf_id = -id; } + int get_leaf_id() const { return -neg_leaf_id; } + + void clear() { + num_children = 0; + parent_id = BVHCommon::INVALID; + height = 0; // or -1 for testing + + // for safety set to improbable value + aabb.set_to_max_opposite_extents(); + + // other members are not blanked for speed .. they may be uninitialized + } + + bool is_full_of_children() const { return num_children >= MAX_CHILDREN; } + + void remove_child_internal(uint32_t child_num) { + children[child_num] = children[num_children - 1]; + num_children--; + } + + int find_child(uint32_t p_child_node_id) { + BVH_ASSERT(!is_leaf()); + + for (int n = 0; n < num_children; n++) { + if (children[n] == p_child_node_id) { + return n; + } + } + + // not found + return -1; + } +}; + +// instead of using linked list we maintain +// item references (for quick lookup) +PooledList<ItemRef, true> _refs; +PooledList<ItemExtra, true> _extra; +PooledList<ItemPairs> _pairs; + +// these 2 are not in sync .. nodes != leaves! +PooledList<TNode, true> _nodes; +PooledList<TLeaf, true> _leaves; + +// we can maintain an un-ordered list of which references are active, +// in order to do a slow incremental optimize of the tree over each frame. +// This will work best if dynamic objects and static objects are in a different tree. +LocalVector<uint32_t, uint32_t, true> _active_refs; +uint32_t _current_active_ref = 0; + +// instead of translating directly to the userdata output, +// we keep an intermediate list of hits as reference IDs, which can be used +// for pairing collision detection +LocalVector<uint32_t, uint32_t, true> _cull_hits; + +// we now have multiple root nodes, allowing us to store +// more than 1 tree. This can be more efficient, while sharing the same +// common lists +enum { NUM_TREES = 2, +}; + +// Tree 0 - Non pairable +// Tree 1 - Pairable +// This is more efficient because in physics we only need check non pairable against the pairable tree. +uint32_t _root_node_id[NUM_TREES]; + +// these values may need tweaking according to the project +// the bound of the world, and the average velocities of the objects + +// node expansion is important in the rendering tree +// larger values give less re-insertion as items move... +// but on the other hand over estimates the bounding box of nodes. +// we can either use auto mode, where the expansion is based on the root node size, or specify manually +real_t _node_expansion = 0.5; +bool _auto_node_expansion = true; + +// pairing expansion important for physics pairing +// larger values gives more 'sticky' pairing, and is less likely to exhibit tunneling +// we can either use auto mode, where the expansion is based on the root node size, or specify manually +real_t _pairing_expansion = 0.1; +bool _auto_pairing_expansion = true; diff --git a/core/math/bvh_tree.h b/core/math/bvh_tree.h new file mode 100644 index 0000000000..3169d31ec7 --- /dev/null +++ b/core/math/bvh_tree.h @@ -0,0 +1,421 @@ +/*************************************************************************/ +/* bvh_tree.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 BVH_TREE_H +#define BVH_TREE_H + +// BVH Tree +// This is an implementation of a dynamic BVH with templated leaf size. +// This differs from most dynamic BVH in that it can handle more than 1 object +// in leaf nodes. This can make it far more efficient in certain circumstances. +// It also means that the splitting logic etc have to be completely different +// to a simpler tree. +// Note that MAX_CHILDREN should be fixed at 2 for now. + +#include "core/math/aabb.h" +#include "core/math/bvh_abb.h" +#include "core/math/geometry_3d.h" +#include "core/math/vector3.h" +#include "core/string/print_string.h" +#include "core/templates/local_vector.h" +#include "core/templates/pooled_list.h" +#include <limits.h> + +#define BVHABB_CLASS BVH_ABB<Bounds, Point> + +// never do these checks in release +#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED) +//#define BVH_VERBOSE +//#define BVH_VERBOSE_TREE + +//#define BVH_VERBOSE_FRAME +//#define BVH_CHECKS +//#define BVH_INTEGRITY_CHECKS +#endif + +// debug only assert +#ifdef BVH_CHECKS +#define BVH_ASSERT(a) CRASH_COND((a) == false) +#else +#define BVH_ASSERT(a) +#endif + +#ifdef BVH_VERBOSE +#define VERBOSE_PRINT print_line +#else +#define VERBOSE_PRINT(a) +#endif + +// really just a namespace +struct BVHCommon { + // these could possibly also be the same constant, + // although this may be useful for debugging. + // or use zero for invalid and +1 based indices. + static const uint32_t INVALID = (0xffffffff); + static const uint32_t INACTIVE = (0xfffffffe); +}; + +// really a handle, can be anything +// note that zero is a valid reference for the BVH .. this may involve using +// a plus one based ID for clients that expect 0 to be invalid. +struct BVHHandle { + // conversion operator + operator uint32_t() const { return _data; } + void set(uint32_t p_value) { _data = p_value; } + + uint32_t _data; + + void set_invalid() { _data = BVHCommon::INVALID; } + bool is_invalid() const { return _data == BVHCommon::INVALID; } + uint32_t id() const { return _data; } + void set_id(uint32_t p_id) { _data = p_id; } + + bool operator==(const BVHHandle &p_h) const { return _data == p_h._data; } + bool operator!=(const BVHHandle &p_h) const { return (*this == p_h) == false; } +}; + +// helper class to make iterative versions of recursive functions +template <class T> +class BVH_IterativeInfo { +public: + enum { + ALLOCA_STACK_SIZE = 128 + }; + + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + T *stack; + //only used in rare occasions when you run out of alloca memory + // because tree is too unbalanced. + LocalVector<T> aux_stack; + int32_t get_alloca_stacksize() const { return ALLOCA_STACK_SIZE * sizeof(T); } + + T *get_first() const { + return &stack[0]; + } + + // pop the last member of the stack, or return false + bool pop(T &r_value) { + if (!depth) { + return false; + } + + depth--; + r_value = stack[depth]; + return true; + } + + // request new addition to stack + T *request() { + if (depth > threshold) { + if (aux_stack.is_empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + memcpy(aux_stack.ptr(), stack, get_alloca_stacksize()); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + return &stack[depth++]; + } +}; + +template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class Bounds = AABB, class Point = Vector3> +class BVH_Tree { + friend class BVH; + +#include "bvh_pair.inc" +#include "bvh_structs.inc" + +public: + BVH_Tree() { + for (int n = 0; n < NUM_TREES; n++) { + _root_node_id[n] = BVHCommon::INVALID; + } + + // disallow zero leaf ids + // (as these ids are stored as negative numbers in the node) + uint32_t dummy_leaf_id; + _leaves.request(dummy_leaf_id); + } + +private: + bool node_add_child(uint32_t p_node_id, uint32_t p_child_node_id) { + TNode &tnode = _nodes[p_node_id]; + if (tnode.is_full_of_children()) { + return false; + } + + tnode.children[tnode.num_children] = p_child_node_id; + tnode.num_children += 1; + + // back link in the child to the parent + TNode &tnode_child = _nodes[p_child_node_id]; + tnode_child.parent_id = p_node_id; + + return true; + } + + void node_replace_child(uint32_t p_parent_id, uint32_t p_old_child_id, uint32_t p_new_child_id) { + TNode &parent = _nodes[p_parent_id]; + BVH_ASSERT(!parent.is_leaf()); + + int child_num = parent.find_child(p_old_child_id); + BVH_ASSERT(child_num != BVHCommon::INVALID); + parent.children[child_num] = p_new_child_id; + + TNode &new_child = _nodes[p_new_child_id]; + new_child.parent_id = p_parent_id; + } + + void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, uint32_t p_tree_id, bool p_prevent_sibling = false) { + TNode &parent = _nodes[p_parent_id]; + BVH_ASSERT(!parent.is_leaf()); + + int child_num = parent.find_child(p_child_id); + BVH_ASSERT(child_num != BVHCommon::INVALID); + + parent.remove_child_internal(child_num); + + // no need to keep back references for children at the moment + + uint32_t sibling_id; // always a node id, as tnode is never a leaf + bool sibling_present = false; + + // if there are more children, or this is the root node, don't try and delete + if (parent.num_children > 1) { + return; + } + + // if there is 1 sibling, it can be moved to be a child of the + if (parent.num_children == 1) { + // else there is now a redundant node with one child, which can be removed + sibling_id = parent.children[0]; + sibling_present = true; + } + + // now there may be no children in this node .. in which case it can be deleted + // remove node if empty + // remove link from parent + uint32_t grandparent_id = parent.parent_id; + + // special case for root node + if (grandparent_id == BVHCommon::INVALID) { + if (sibling_present) { + // change the root node + change_root_node(sibling_id, p_tree_id); + + // delete the old root node as no longer needed + _nodes.free(p_parent_id); + } + + return; + } + + if (sibling_present) { + node_replace_child(grandparent_id, p_parent_id, sibling_id); + } else { + node_remove_child(grandparent_id, p_parent_id, p_tree_id, true); + } + + // put the node on the free list to recycle + _nodes.free(p_parent_id); + } + + void change_root_node(uint32_t p_new_root_id, uint32_t p_tree_id) { + _root_node_id[p_tree_id] = p_new_root_id; + TNode &root = _nodes[p_new_root_id]; + + // mark no parent + root.parent_id = BVHCommon::INVALID; + } + + void node_make_leaf(uint32_t p_node_id) { + uint32_t child_leaf_id; + TLeaf *child_leaf = _leaves.request(child_leaf_id); + child_leaf->clear(); + + // zero is reserved at startup, to prevent this id being used + // (as they are stored as negative values in the node, and zero is already taken) + BVH_ASSERT(child_leaf_id != 0); + + TNode &node = _nodes[p_node_id]; + node.neg_leaf_id = -(int)child_leaf_id; + } + + void node_remove_item(uint32_t p_ref_id, uint32_t p_tree_id, BVHABB_CLASS *r_old_aabb = nullptr) { + // get the reference + ItemRef &ref = _refs[p_ref_id]; + uint32_t owner_node_id = ref.tnode_id; + + // debug draw special + // This may not be needed + if (owner_node_id == BVHCommon::INVALID) { + return; + } + + TNode &tnode = _nodes[owner_node_id]; + CRASH_COND(!tnode.is_leaf()); + + TLeaf &leaf = _node_get_leaf(tnode); + + // if the aabb is not determining the corner size, then there is no need to refit! + // (optimization, as merging AABBs takes a lot of time) + const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id); + + // shrink a little to prevent using corner aabbs + // in order to miss the corners first we shrink by node_expansion + // (which is added to the overall bound of the leaf), then we also + // shrink by an epsilon, in order to miss out the very corner aabbs + // which are important in determining the bound. Any other aabb + // within this can be removed and not affect the overall bound. + BVHABB_CLASS node_bound = tnode.aabb; + node_bound.expand(-_node_expansion - 0.001f); + bool refit = true; + + if (node_bound.is_other_within(old_aabb)) { + refit = false; + } + + // record the old aabb if required (for incremental remove_and_reinsert) + if (r_old_aabb) { + *r_old_aabb = old_aabb; + } + + leaf.remove_item_unordered(ref.item_id); + + if (leaf.num_items) { + // the swapped item has to have its reference changed to, to point to the new item id + uint32_t swapped_ref_id = leaf.get_item_ref_id(ref.item_id); + + ItemRef &swapped_ref = _refs[swapped_ref_id]; + + swapped_ref.item_id = ref.item_id; + + // only have to refit if it is an edge item + // This is a VERY EXPENSIVE STEP + // we defer the refit updates until the update function is called once per frame + if (refit) { + leaf.set_dirty(true); + } + } else { + // remove node if empty + // remove link from parent + if (tnode.parent_id != BVHCommon::INVALID) { + // DANGER .. this can potentially end up with root node with 1 child ... + // we don't want this and must check for it + + uint32_t parent_id = tnode.parent_id; + + node_remove_child(parent_id, owner_node_id, p_tree_id); + refit_upward(parent_id); + + // put the node on the free list to recycle + _nodes.free(owner_node_id); + } + + // else if no parent, it is the root node. Do not delete + } + + ref.tnode_id = BVHCommon::INVALID; + ref.item_id = BVHCommon::INVALID; // unset + } + + // returns true if needs refit of PARENT tree only, the node itself AABB is calculated + // within this routine + bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) { + ItemRef &ref = _refs[p_ref_id]; + ref.tnode_id = p_node_id; + + TNode &node = _nodes[p_node_id]; + BVH_ASSERT(node.is_leaf()); + TLeaf &leaf = _node_get_leaf(node); + + // optimization - we only need to do a refit + // if the added item is changing the AABB of the node. + // in most cases it won't. + bool needs_refit = true; + + // expand bound now + BVHABB_CLASS expanded = p_aabb; + expanded.expand(_node_expansion); + + // the bound will only be valid if there is an item in there already + if (leaf.num_items) { + if (node.aabb.is_other_within(expanded)) { + // no change to node AABBs + needs_refit = false; + } else { + node.aabb.merge(expanded); + } + } else { + // bound of the node = the new aabb + node.aabb = expanded; + } + + ref.item_id = leaf.request_item(); + BVH_ASSERT(ref.item_id != BVHCommon::INVALID); + + // set the aabb of the new item + leaf.get_aabb(ref.item_id) = p_aabb; + + // back reference on the item back to the item reference + leaf.get_item_ref_id(ref.item_id) = p_ref_id; + + return needs_refit; + } + + uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) { + uint32_t child_node_id; + TNode *child_node = _nodes.request(child_node_id); + child_node->clear(); + + // may not be necessary + child_node->aabb = p_aabb; + + node_add_child(p_node_id, child_node_id); + + return child_node_id; + } + +#include "bvh_cull.inc" +#include "bvh_debug.inc" +#include "bvh_integrity.inc" +#include "bvh_logic.inc" +#include "bvh_misc.inc" +#include "bvh_public.inc" +#include "bvh_refit.inc" +#include "bvh_split.inc" +}; + +#undef VERBOSE_PRINT + +#endif // BVH_TREE_H diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index 380bae871a..66c18f7b3c 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,32 +31,40 @@ #include "camera_matrix.h" #include "core/math/math_funcs.h" -#include "core/print_string.h" +#include "core/string/print_string.h" + +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; @@ -66,14 +74,22 @@ Plane CameraMatrix::xform4(const Plane &p_vec4) const { return ret; } -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) { +void CameraMatrix::adjust_perspective_znear(real_t p_new_znear) { + real_t zfar = get_z_far(); + real_t znear = p_new_znear; + real_t deltaZ = zfar - znear; + matrix[2][2] = -(zfar + znear) / deltaZ; + matrix[3][2] = -2 * znear * zfar / deltaZ; +} + +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); } real_t sine, cotangent, deltaZ; - real_t radians = p_fovy_degrees / 2.0 * Math_PI / 180.0; + real_t radians = Math::deg2rad(p_fovy_degrees / 2.0); deltaZ = p_z_far - p_z_near; sine = Math::sin(radians); @@ -100,7 +116,7 @@ void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_ real_t left, right, modeltranslation, ymax, xmax, frustumshift; - ymax = p_z_near * tan(p_fovy_degrees * Math_PI / 360.0f); + ymax = p_z_near * tan(Math::deg2rad(p_fovy_degrees / 2.0)); xmax = ymax * p_aspect; frustumshift = (p_intraocular_dist / 2.0) * p_z_near / p_convergence_dist; @@ -109,18 +125,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); @@ -145,22 +161,21 @@ void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_ f3 *= p_oversample; // always apply KEEP_WIDTH aspect ratio - f3 *= p_aspect; + f3 /= p_aspect; 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 +188,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 +196,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 +236,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 +247,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 +260,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], @@ -276,9 +287,36 @@ Vector2 CameraMatrix::get_viewport_half_extents() const { return Vector2(res.x, res.y); } -bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8points) const { +Vector2 CameraMatrix::get_far_plane_half_extents() const { + const real_t *matrix = (const real_t *)this->matrix; + ///////--- Far Plane ---/////// + Plane far_plane = Plane(matrix[3] - matrix[2], + matrix[7] - matrix[6], + matrix[11] - matrix[10], + -matrix[15] + matrix[14]); + far_plane.normalize(); + + ///////--- Right Plane ---/////// + Plane right_plane = Plane(matrix[3] - matrix[0], + matrix[7] - matrix[4], + matrix[11] - matrix[8], + -matrix[15] + matrix[12]); + right_plane.normalize(); + + Plane top_plane = Plane(matrix[3] - matrix[1], + matrix[7] - matrix[5], + matrix[11] - matrix[9], + -matrix[15] + matrix[13]); + top_plane.normalize(); + + Vector3 res; + far_plane.intersect_3(right_plane, top_plane, &res); + + return Vector2(res.x, res.y); +} - Vector<Plane> planes = get_projection_planes(Transform()); +bool CameraMatrix::get_endpoints(const Transform3D &p_transform, Vector3 *p_8points) const { + Vector<Plane> planes = get_projection_planes(Transform3D()); const Planes intersections[8][3] = { { PLANE_FAR, PLANE_LEFT, PLANE_TOP }, { PLANE_FAR, PLANE_LEFT, PLANE_BOTTOM }, @@ -291,7 +329,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); @@ -301,8 +338,7 @@ bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8point return true; } -Vector<Plane> CameraMatrix::get_projection_planes(const Transform &p_transform) const { - +Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &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 @@ -385,14 +421,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 */ @@ -443,20 +477,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). **/ @@ -476,29 +516,35 @@ 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; } + } } } -CameraMatrix::CameraMatrix() { +void CameraMatrix::flip_y() { + for (int i = 0; i < 4; i++) { + matrix[1][i] = -matrix[1][i]; + } +} +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; } } @@ -506,8 +552,28 @@ CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { return new_matrix; } -void CameraMatrix::set_light_bias() { +void CameraMatrix::set_depth_correction(bool p_flip_y) { + real_t *m = &matrix[0][0]; + m[0] = 1; + m[1] = 0.0; + m[2] = 0.0; + m[3] = 0.0; + m[4] = 0.0; + m[5] = p_flip_y ? -1 : 1; + m[6] = 0.0; + m[7] = 0.0; + m[8] = 0.0; + m[9] = 0.0; + m[10] = 0.5; + m[11] = 0.0; + m[12] = 0.0; + m[13] = 0.0; + m[14] = 0.5; + m[15] = 1.0; +} + +void CameraMatrix::set_light_bias() { real_t *m = &matrix[0][0]; m[0] = 0.5; @@ -529,7 +595,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; @@ -551,30 +616,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; } @@ -601,8 +664,18 @@ real_t CameraMatrix::get_fov() const { } } -void CameraMatrix::make_scale(const Vector3 &p_scale) { +float CameraMatrix::get_lod_multiplier() const { + if (is_orthogonal()) { + return get_viewport_half_extents().x; + } else { + float zn = get_z_near(); + float width = get_viewport_half_extents().x * 2.0; + return 1.0 / (zn / width); + } + //usage is lod_size / (lod_distance * multiplier) < threshold +} +void CameraMatrix::make_scale(const Vector3 &p_scale) { set_identity(); matrix[0][0] = p_scale.x; matrix[1][1] = p_scale.y; @@ -610,7 +683,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; @@ -635,9 +707,8 @@ void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { matrix[3][3] = 1; } -CameraMatrix::operator Transform() const { - - Transform tr; +CameraMatrix::operator Transform3D() const { + Transform3D tr; const real_t *m = &matrix[0][0]; tr.basis.elements[0][0] = m[0]; @@ -659,9 +730,8 @@ CameraMatrix::operator Transform() const { return tr; } -CameraMatrix::CameraMatrix(const Transform &p_transform) { - - const Transform &tr = p_transform; +CameraMatrix::CameraMatrix(const Transform3D &p_transform) { + const Transform3D &tr = p_transform; real_t *m = &matrix[0][0]; m[0] = tr.basis.elements[0][0]; diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index 2eed6d25d6..786d46055a 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,10 +32,9 @@ #define CAMERA_MATRIX_H #include "core/math/rect2.h" -#include "core/math/transform.h" +#include "core/math/transform_3d.h" struct CameraMatrix { - enum Planes { PLANE_NEAR, PLANE_FAR, @@ -47,9 +46,11 @@ struct CameraMatrix { real_t matrix[4][4]; + float determinant() const; void set_identity(); void set_zero(); void set_light_bias(); + void set_depth_correction(bool p_flip_y = true); void set_light_atlas_rect(const Rect2 &p_rect); void 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 = false); void 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, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist); @@ -58,9 +59,9 @@ struct CameraMatrix { void set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false); void 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); 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); + void adjust_perspective_znear(real_t p_new_znear); 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); } @@ -70,10 +71,11 @@ struct CameraMatrix { real_t get_fov() const; bool is_orthogonal() const; - Vector<Plane> get_projection_planes(const Transform &p_transform) const; + Vector<Plane> get_projection_planes(const Transform3D &p_transform) const; - bool get_endpoints(const Transform &p_transform, Vector3 *p_8points) const; + bool get_endpoints(const Transform3D &p_transform, Vector3 *p_8points) const; Vector2 get_viewport_half_extents() const; + Vector2 get_far_plane_half_extents() const; void invert(); CameraMatrix inverse() const; @@ -88,15 +90,33 @@ struct CameraMatrix { void scale_translate_to_fit(const AABB &p_aabb); void make_scale(const Vector3 &p_scale); int get_pixels_per_meter(int p_for_pixel_width) const; - operator Transform() const; + operator Transform3D() const; + + void flip_y(); + + bool operator==(const CameraMatrix &p_cam) const { + for (uint32_t i = 0; i < 4; i++) { + for (uint32_t j = 0; j < 4; j++) { + if (matrix[i][j] != p_cam.matrix[i][j]) { + return false; + } + } + } + return true; + } + + bool operator!=(const CameraMatrix &p_cam) const { + return !(*this == p_cam); + } + + float get_lod_multiplier() const; CameraMatrix(); - CameraMatrix(const Transform &p_transform); + CameraMatrix(const Transform3D &p_transform); ~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]; @@ -105,4 +125,4 @@ Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const { return ret / w; } -#endif +#endif // CAMERA_MATRIX_H diff --git a/core/math/color.cpp b/core/math/color.cpp new file mode 100644 index 0000000000..dc86cacf8f --- /dev/null +++ b/core/math/color.cpp @@ -0,0 +1,570 @@ +/*************************************************************************/ +/* color.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "color.h" + +#include "color_names.inc" +#include "core/math/math_funcs.h" +#include "core/string/print_string.h" +#include "core/templates/map.h" + +uint32_t Color::to_argb32() const { + uint32_t c = (uint8_t)Math::round(a * 255); + c <<= 8; + c |= (uint8_t)Math::round(r * 255); + c <<= 8; + c |= (uint8_t)Math::round(g * 255); + c <<= 8; + c |= (uint8_t)Math::round(b * 255); + + return c; +} + +uint32_t Color::to_abgr32() const { + uint32_t c = (uint8_t)Math::round(a * 255); + c <<= 8; + c |= (uint8_t)Math::round(b * 255); + c <<= 8; + c |= (uint8_t)Math::round(g * 255); + c <<= 8; + c |= (uint8_t)Math::round(r * 255); + + return c; +} + +uint32_t Color::to_rgba32() const { + uint32_t c = (uint8_t)Math::round(r * 255); + c <<= 8; + c |= (uint8_t)Math::round(g * 255); + c <<= 8; + c |= (uint8_t)Math::round(b * 255); + c <<= 8; + c |= (uint8_t)Math::round(a * 255); + + return c; +} + +uint64_t Color::to_abgr64() const { + uint64_t c = (uint16_t)Math::round(a * 65535); + c <<= 16; + c |= (uint16_t)Math::round(b * 65535); + c <<= 16; + c |= (uint16_t)Math::round(g * 65535); + c <<= 16; + c |= (uint16_t)Math::round(r * 65535); + + return c; +} + +uint64_t Color::to_argb64() const { + uint64_t c = (uint16_t)Math::round(a * 65535); + c <<= 16; + c |= (uint16_t)Math::round(r * 65535); + c <<= 16; + c |= (uint16_t)Math::round(g * 65535); + c <<= 16; + c |= (uint16_t)Math::round(b * 65535); + + return c; +} + +uint64_t Color::to_rgba64() const { + uint64_t c = (uint16_t)Math::round(r * 65535); + c <<= 16; + c |= (uint16_t)Math::round(g * 65535); + c <<= 16; + c |= (uint16_t)Math::round(b * 65535); + c <<= 16; + c |= (uint16_t)Math::round(a * 65535); + + return c; +} + +float Color::get_h() const { + float min = MIN(r, g); + min = MIN(min, b); + float max = MAX(r, g); + max = MAX(max, b); + + float delta = max - min; + + if (delta == 0) { + return 0; + } + + float h; + if (r == max) { + h = (g - b) / delta; // between yellow & magenta + } else if (g == max) { + h = 2 + (b - r) / delta; // between cyan & yellow + } else { + h = 4 + (r - g) / delta; // between magenta & cyan + } + + h /= 6.0; + if (h < 0) { + h += 1.0; + } + + return h; +} + +float Color::get_s() const { + float min = MIN(r, g); + min = MIN(min, b); + float max = MAX(r, g); + max = MAX(max, b); + + float delta = max - min; + + return (max != 0) ? (delta / max) : 0; +} + +float Color::get_v() const { + float max = MAX(r, g); + max = MAX(max, b); + return max; +} + +void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) { + int i; + float f, p, q, t; + a = p_alpha; + + if (p_s == 0) { + // Achromatic (grey) + r = g = b = p_v; + return; + } + + p_h *= 6.0; + p_h = Math::fmod(p_h, 6); + i = Math::floor(p_h); + + f = p_h - i; + p = p_v * (1 - p_s); + q = p_v * (1 - p_s * f); + t = p_v * (1 - p_s * (1 - f)); + + switch (i) { + case 0: // Red is the dominant color + r = p_v; + g = t; + b = p; + break; + case 1: // Green is the dominant color + r = q; + g = p_v; + b = p; + break; + case 2: + r = p; + g = p_v; + b = t; + break; + case 3: // Blue is the dominant color + r = p; + g = q; + b = p_v; + break; + case 4: + r = t; + g = p; + b = p_v; + break; + default: // (5) Red is the dominant color + r = p_v; + g = p; + b = q; + break; + } +} + +bool Color::is_equal_approx(const Color &p_color) const { + return Math::is_equal_approx(r, p_color.r) && Math::is_equal_approx(g, p_color.g) && Math::is_equal_approx(b, p_color.b) && Math::is_equal_approx(a, p_color.a); +} + +Color Color::clamp(const Color &p_min, const Color &p_max) const { + return Color( + CLAMP(r, p_min.r, p_max.r), + CLAMP(g, p_min.g, p_max.g), + CLAMP(b, p_min.b, p_max.b), + CLAMP(a, p_min.a, p_max.a)); +} + +void Color::invert() { + r = 1.0 - r; + g = 1.0 - g; + b = 1.0 - b; +} + +Color Color::hex(uint32_t p_hex) { + float a = (p_hex & 0xFF) / 255.0; + p_hex >>= 8; + float b = (p_hex & 0xFF) / 255.0; + p_hex >>= 8; + float g = (p_hex & 0xFF) / 255.0; + p_hex >>= 8; + float r = (p_hex & 0xFF) / 255.0; + + return Color(r, g, b, a); +} + +Color Color::hex64(uint64_t p_hex) { + float a = (p_hex & 0xFFFF) / 65535.0; + p_hex >>= 16; + float b = (p_hex & 0xFFFF) / 65535.0; + p_hex >>= 16; + float g = (p_hex & 0xFFFF) / 65535.0; + p_hex >>= 16; + float r = (p_hex & 0xFFFF) / 65535.0; + + return Color(r, g, b, a); +} + +Color Color::from_rgbe9995(uint32_t p_rgbe) { + float r = p_rgbe & 0x1ff; + float g = (p_rgbe >> 9) & 0x1ff; + float b = (p_rgbe >> 18) & 0x1ff; + float e = (p_rgbe >> 27); + float m = Math::pow(2, e - 15.0 - 9.0); + + float rd = r * m; + float gd = g * m; + float bd = b * m; + + return Color(rd, gd, bd, 1.0f); +} + +static int _parse_col4(const String &p_str, int p_ofs) { + char character = p_str[p_ofs]; + + if (character >= '0' && character <= '9') { + return character - '0'; + } else if (character >= 'a' && character <= 'f') { + return character + (10 - 'a'); + } else if (character >= 'A' && character <= 'F') { + return character + (10 - 'A'); + } + return -1; +} + +static int _parse_col8(const String &p_str, int p_ofs) { + return _parse_col4(p_str, p_ofs) * 16 + _parse_col4(p_str, p_ofs + 1); +} + +Color Color::inverted() const { + Color c = *this; + c.invert(); + return c; +} + +Color Color::html(const String &p_rgba) { + String color = p_rgba; + if (color.length() == 0) { + return Color(); + } + if (color[0] == '#') { + color = color.substr(1); + } + + // If enabled, use 1 hex digit per channel instead of 2. + // Other sizes aren't in the HTML/CSS spec but we could add them if desired. + bool is_shorthand = color.length() < 5; + bool alpha = false; + + if (color.length() == 8) { + alpha = true; + } else if (color.length() == 6) { + alpha = false; + } else if (color.length() == 4) { + alpha = true; + } else if (color.length() == 3) { + alpha = false; + } else { + ERR_FAIL_V_MSG(Color(), "Invalid color code: " + p_rgba + "."); + } + + float r, g, b, a = 1.0; + if (is_shorthand) { + r = _parse_col4(color, 0) / 15.0; + g = _parse_col4(color, 1) / 15.0; + b = _parse_col4(color, 2) / 15.0; + if (alpha) { + a = _parse_col4(color, 3) / 15.0; + } + } else { + r = _parse_col8(color, 0) / 255.0; + g = _parse_col8(color, 2) / 255.0; + b = _parse_col8(color, 4) / 255.0; + if (alpha) { + a = _parse_col8(color, 6) / 255.0; + } + } + ERR_FAIL_COND_V_MSG(r < 0, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(g < 0, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(b < 0, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(a < 0, Color(), "Invalid color code: " + p_rgba + "."); + + return Color(r, g, b, a); +} + +bool Color::html_is_valid(const String &p_color) { + String color = p_color; + + if (color.length() == 0) { + return false; + } + if (color[0] == '#') { + color = color.substr(1); + } + + // Check if the amount of hex digits is valid. + int len = color.length(); + if (!(len == 3 || len == 4 || len == 6 || len == 8)) { + return false; + } + + // Check if each hex digit is valid. + for (int i = 0; i < len; i++) { + if (_parse_col4(color, i) == -1) { + return false; + } + } + + return true; +} + +Color Color::named(const String &p_name) { + int idx = find_named_color(p_name); + if (idx == -1) { + ERR_FAIL_V_MSG(Color(), "Invalid color name: " + p_name + "."); + return Color(); + } + return named_colors[idx].color; +} + +Color Color::named(const String &p_name, const Color &p_default) { + int idx = find_named_color(p_name); + if (idx == -1) { + return p_default; + } + return named_colors[idx].color; +} + +int Color::find_named_color(const String &p_name) { + String name = p_name; + // Normalize name + name = name.replace(" ", ""); + name = name.replace("-", ""); + name = name.replace("_", ""); + name = name.replace("'", ""); + name = name.replace(".", ""); + name = name.to_upper(); + + int idx = 0; + while (named_colors[idx].name != nullptr) { + if (name == String(named_colors[idx].name).replace("_", "")) { + return idx; + } + idx++; + } + + return -1; +} + +int Color::get_named_color_count() { + int idx = 0; + while (named_colors[idx].name != nullptr) { + idx++; + } + return idx; +} + +String Color::get_named_color_name(int p_idx) { + ERR_FAIL_INDEX_V(p_idx, get_named_color_count(), ""); + return named_colors[p_idx].name; +} + +Color Color::get_named_color(int p_idx) { + ERR_FAIL_INDEX_V(p_idx, get_named_color_count(), Color()); + return named_colors[p_idx].color; +} + +// For a version that errors on invalid values instead of returning +// a default color, use the Color(String) constructor instead. +Color Color::from_string(const String &p_string, const Color &p_default) { + if (html_is_valid(p_string)) { + return html(p_string); + } else { + return named(p_string, p_default); + } +} + +String _to_hex(float p_val) { + int v = Math::round(p_val * 255); + v = CLAMP(v, 0, 255); + String ret; + + for (int i = 0; i < 2; i++) { + char32_t c[2] = { 0, 0 }; + int lv = v & 0xF; + if (lv < 10) { + c[0] = '0' + lv; + } else { + c[0] = 'a' + lv - 10; + } + + v >>= 4; + String cs = (const char32_t *)c; + ret = cs + ret; + } + + return ret; +} + +String Color::to_html(bool p_alpha) const { + String txt; + txt += _to_hex(r); + txt += _to_hex(g); + txt += _to_hex(b); + if (p_alpha) { + txt += _to_hex(a); + } + return txt; +} + +Color Color::from_hsv(float p_h, float p_s, float p_v, float p_a) const { + Color c; + c.set_hsv(p_h, p_s, p_v, p_a); + return c; +} + +Color::operator String() const { + return "(" + String::num(r, 4) + ", " + String::num(g, 4) + ", " + String::num(b, 4) + ", " + String::num(a, 4) + ")"; +} + +Color Color::operator+(const Color &p_color) const { + return Color( + r + p_color.r, + g + p_color.g, + b + p_color.b, + a + p_color.a); +} + +void Color::operator+=(const Color &p_color) { + r = r + p_color.r; + g = g + p_color.g; + b = b + p_color.b; + a = a + p_color.a; +} + +Color Color::operator-(const Color &p_color) const { + return Color( + r - p_color.r, + g - p_color.g, + b - p_color.b, + a - p_color.a); +} + +void Color::operator-=(const Color &p_color) { + r = r - p_color.r; + g = g - p_color.g; + b = b - p_color.b; + a = a - p_color.a; +} + +Color Color::operator*(const Color &p_color) const { + return Color( + r * p_color.r, + g * p_color.g, + b * p_color.b, + a * p_color.a); +} + +Color Color::operator*(float p_scalar) const { + return Color( + r * p_scalar, + g * p_scalar, + b * p_scalar, + a * p_scalar); +} + +void Color::operator*=(const Color &p_color) { + r = r * p_color.r; + g = g * p_color.g; + b = b * p_color.b; + a = a * p_color.a; +} + +void Color::operator*=(float p_scalar) { + r = r * p_scalar; + g = g * p_scalar; + b = b * p_scalar; + a = a * p_scalar; +} + +Color Color::operator/(const Color &p_color) const { + return Color( + r / p_color.r, + g / p_color.g, + b / p_color.b, + a / p_color.a); +} + +Color Color::operator/(float p_scalar) const { + return Color( + r / p_scalar, + g / p_scalar, + b / p_scalar, + a / p_scalar); +} + +void Color::operator/=(const Color &p_color) { + r = r / p_color.r; + g = g / p_color.g; + b = b / p_color.b; + a = a / p_color.a; +} + +void Color::operator/=(float p_scalar) { + r = r / p_scalar; + g = g / p_scalar; + b = b / p_scalar; + a = a / p_scalar; +} + +Color Color::operator-() const { + return Color( + 1.0 - r, + 1.0 - g, + 1.0 - b, + 1.0 - a); +} diff --git a/core/math/color.h b/core/math/color.h new file mode 100644 index 0000000000..a95dbf4f60 --- /dev/null +++ b/core/math/color.h @@ -0,0 +1,280 @@ +/*************************************************************************/ +/* color.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 COLOR_H +#define COLOR_H + +#include "core/math/math_funcs.h" +#include "core/string/ustring.h" + +struct Color { + union { + struct { + float r; + float g; + float b; + float a; + }; + float components[4] = { 0, 0, 0, 1.0 }; + }; + + uint32_t to_rgba32() const; + uint32_t to_argb32() const; + uint32_t to_abgr32() const; + uint64_t to_rgba64() const; + uint64_t to_argb64() const; + uint64_t to_abgr64() const; + float get_h() const; + float get_s() const; + float get_v() const; + void set_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0); + + _FORCE_INLINE_ float &operator[](int p_idx) { + return components[p_idx]; + } + _FORCE_INLINE_ const float &operator[](int p_idx) const { + return components[p_idx]; + } + + bool operator==(const Color &p_color) const { + return (r == p_color.r && g == p_color.g && b == p_color.b && a == p_color.a); + } + bool operator!=(const Color &p_color) const { + return (r != p_color.r || g != p_color.g || b != p_color.b || a != p_color.a); + } + + Color operator+(const Color &p_color) const; + void operator+=(const Color &p_color); + + Color operator-() const; + Color operator-(const Color &p_color) const; + void operator-=(const Color &p_color); + + Color operator*(const Color &p_color) const; + Color operator*(float p_scalar) const; + void operator*=(const Color &p_color); + void operator*=(float p_scalar); + + Color operator/(const Color &p_color) const; + Color operator/(float p_scalar) const; + void operator/=(const Color &p_color); + void operator/=(float p_scalar); + + bool is_equal_approx(const Color &p_color) const; + + Color clamp(const Color &p_min = Color(0, 0, 0, 0), const Color &p_max = Color(1, 1, 1, 1)) const; + void invert(); + Color inverted() const; + + _FORCE_INLINE_ Color lerp(const Color &p_to, float p_weight) const { + Color res = *this; + + res.r += (p_weight * (p_to.r - r)); + res.g += (p_weight * (p_to.g - g)); + res.b += (p_weight * (p_to.b - b)); + res.a += (p_weight * (p_to.a - a)); + + return res; + } + + _FORCE_INLINE_ Color darkened(float p_amount) const { + Color res = *this; + res.r = res.r * (1.0f - p_amount); + res.g = res.g * (1.0f - p_amount); + res.b = res.b * (1.0f - p_amount); + return res; + } + + _FORCE_INLINE_ Color lightened(float p_amount) const { + Color res = *this; + res.r = res.r + (1.0f - res.r) * p_amount; + res.g = res.g + (1.0f - res.g) * p_amount; + res.b = res.b + (1.0f - res.b) * p_amount; + return res; + } + + _FORCE_INLINE_ uint32_t to_rgbe9995() const { + const float pow2to9 = 512.0f; + const float B = 15.0f; + const float N = 9.0f; + + float sharedexp = 65408.000f; // Result of: ((pow2to9 - 1.0f) / pow2to9) * powf(2.0f, 31.0f - 15.0f) + + float cRed = MAX(0.0f, MIN(sharedexp, r)); + float cGreen = MAX(0.0f, MIN(sharedexp, g)); + float cBlue = MAX(0.0f, MIN(sharedexp, b)); + + float cMax = MAX(cRed, MAX(cGreen, cBlue)); + + float expp = MAX(-B - 1.0f, floor(Math::log(cMax) / Math_LN2)) + 1.0f + B; + + float sMax = (float)floor((cMax / Math::pow(2.0f, expp - B - N)) + 0.5f); + + float exps = expp + 1.0f; + + if (0.0 <= sMax && sMax < pow2to9) { + exps = expp; + } + + float sRed = Math::floor((cRed / pow(2.0f, exps - B - N)) + 0.5f); + float sGreen = Math::floor((cGreen / pow(2.0f, exps - B - N)) + 0.5f); + float sBlue = Math::floor((cBlue / pow(2.0f, exps - B - N)) + 0.5f); + + return (uint32_t(Math::fast_ftoi(sRed)) & 0x1FF) | ((uint32_t(Math::fast_ftoi(sGreen)) & 0x1FF) << 9) | ((uint32_t(Math::fast_ftoi(sBlue)) & 0x1FF) << 18) | ((uint32_t(Math::fast_ftoi(exps)) & 0x1F) << 27); + } + + _FORCE_INLINE_ Color blend(const Color &p_over) const { + Color res; + float sa = 1.0 - p_over.a; + res.a = a * sa + p_over.a; + if (res.a == 0) { + return Color(0, 0, 0, 0); + } else { + res.r = (r * a * sa + p_over.r * p_over.a) / res.a; + res.g = (g * a * sa + p_over.g * p_over.a) / res.a; + res.b = (b * a * sa + p_over.b * p_over.a) / res.a; + } + return res; + } + + _FORCE_INLINE_ Color to_linear() const { + return Color( + r < 0.04045 ? r * (1.0 / 12.92) : Math::pow((r + 0.055) * (1.0 / (1 + 0.055)), 2.4), + g < 0.04045 ? g * (1.0 / 12.92) : Math::pow((g + 0.055) * (1.0 / (1 + 0.055)), 2.4), + b < 0.04045 ? b * (1.0 / 12.92) : Math::pow((b + 0.055) * (1.0 / (1 + 0.055)), 2.4), + a); + } + _FORCE_INLINE_ Color to_srgb() const { + return Color( + r < 0.0031308 ? 12.92 * r : (1.0 + 0.055) * Math::pow(r, 1.0f / 2.4f) - 0.055, + g < 0.0031308 ? 12.92 * g : (1.0 + 0.055) * Math::pow(g, 1.0f / 2.4f) - 0.055, + b < 0.0031308 ? 12.92 * b : (1.0 + 0.055) * Math::pow(b, 1.0f / 2.4f) - 0.055, a); + } + + static Color hex(uint32_t p_hex); + static Color hex64(uint64_t p_hex); + static Color html(const String &p_rgba); + static bool html_is_valid(const String &p_color); + static Color named(const String &p_name); + static Color named(const String &p_name, const Color &p_default); + static int find_named_color(const String &p_name); + static int get_named_color_count(); + static String get_named_color_name(int p_idx); + static Color get_named_color(int p_idx); + static Color from_string(const String &p_string, const Color &p_default); + String to_html(bool p_alpha = true) const; + Color from_hsv(float p_h, float p_s, float p_v, float p_a) const; + static Color from_rgbe9995(uint32_t p_rgbe); + + _FORCE_INLINE_ bool operator<(const Color &p_color) const; //used in set keys + operator String() const; + + // For the binder. + _FORCE_INLINE_ void set_r8(int32_t r8) { r = (CLAMP(r8, 0, 255) / 255.0); } + _FORCE_INLINE_ int32_t get_r8() const { return int32_t(CLAMP(Math::round(r * 255.0f), 0.0f, 255.0f)); } + _FORCE_INLINE_ void set_g8(int32_t g8) { g = (CLAMP(g8, 0, 255) / 255.0); } + _FORCE_INLINE_ int32_t get_g8() const { return int32_t(CLAMP(Math::round(g * 255.0f), 0.0f, 255.0f)); } + _FORCE_INLINE_ void set_b8(int32_t b8) { b = (CLAMP(b8, 0, 255) / 255.0); } + _FORCE_INLINE_ int32_t get_b8() const { return int32_t(CLAMP(Math::round(b * 255.0f), 0.0f, 255.0f)); } + _FORCE_INLINE_ void set_a8(int32_t a8) { a = (CLAMP(a8, 0, 255) / 255.0); } + _FORCE_INLINE_ int32_t get_a8() const { return int32_t(CLAMP(Math::round(a * 255.0f), 0.0f, 255.0f)); } + + _FORCE_INLINE_ void set_h(float p_h) { set_hsv(p_h, get_s(), get_v()); } + _FORCE_INLINE_ void set_s(float p_s) { set_hsv(get_h(), p_s, get_v()); } + _FORCE_INLINE_ void set_v(float p_v) { set_hsv(get_h(), get_s(), p_v); } + + _FORCE_INLINE_ Color() {} + + /** + * RGBA construct parameters. + * Alpha is not optional as otherwise we can't bind the RGB version for scripting. + */ + _FORCE_INLINE_ Color(float p_r, float p_g, float p_b, float p_a) { + r = p_r; + g = p_g; + b = p_b; + a = p_a; + } + + /** + * RGB construct parameters. + */ + _FORCE_INLINE_ Color(float p_r, float p_g, float p_b) { + r = p_r; + g = p_g; + b = p_b; + a = 1.0; + } + + /** + * Construct a Color from another Color, but with the specified alpha value. + */ + _FORCE_INLINE_ Color(const Color &p_c, float p_a) { + r = p_c.r; + g = p_c.g; + b = p_c.b; + a = p_a; + } + + Color(const String &p_code) { + if (html_is_valid(p_code)) { + *this = html(p_code); + } else { + *this = named(p_code); + } + } + + Color(const String &p_code, float p_a) { + *this = Color(p_code); + a = p_a; + } +}; + +bool Color::operator<(const Color &p_color) const { + if (r == p_color.r) { + if (g == p_color.g) { + if (b == p_color.b) { + return (a < p_color.a); + } else { + return (b < p_color.b); + } + } else { + return g < p_color.g; + } + } else { + return r < p_color.r; + } +} + +_FORCE_INLINE_ Color operator*(float p_scalar, const Color &p_color) { + return p_color * p_scalar; +} + +#endif // COLOR_H diff --git a/core/math/color_names.inc b/core/math/color_names.inc new file mode 100644 index 0000000000..2020bdbfca --- /dev/null +++ b/core/math/color_names.inc @@ -0,0 +1,163 @@ +// Names from https://en.wikipedia.org/wiki/X11_color_names + +// So this in a way that does not require memory allocation +// the old way leaked memory +// this is not used as often as for more performance to make sense + +struct NamedColor { + const char *name; + Color color; +}; + +// NOTE: This data is duplicated in the file: +// modules/mono/glue/GodotSharp/GodotSharp/Core/Colors.cs + +static NamedColor named_colors[] = { + { "ALICE_BLUE", Color(0.94, 0.97, 1.00) }, + { "ANTIQUE_WHITE", Color(0.98, 0.92, 0.84) }, + { "AQUA", Color(0.00, 1.00, 1.00) }, + { "AQUAMARINE", Color(0.50, 1.00, 0.83) }, + { "AZURE", Color(0.94, 1.00, 1.00) }, + { "BEIGE", Color(0.96, 0.96, 0.86) }, + { "BISQUE", Color(1.00, 0.89, 0.77) }, + { "BLACK", Color(0.00, 0.00, 0.00) }, + { "BLANCHED_ALMOND", Color(1.00, 0.92, 0.80) }, + { "BLUE", Color(0.00, 0.00, 1.00) }, + { "BLUE_VIOLET", Color(0.54, 0.17, 0.89) }, + { "BROWN", Color(0.65, 0.16, 0.16) }, + { "BURLYWOOD", Color(0.87, 0.72, 0.53) }, + { "CADET_BLUE", Color(0.37, 0.62, 0.63) }, + { "CHARTREUSE", Color(0.50, 1.00, 0.00) }, + { "CHOCOLATE", Color(0.82, 0.41, 0.12) }, + { "CORAL", Color(1.00, 0.50, 0.31) }, + { "CORNFLOWER_BLUE", Color(0.39, 0.58, 0.93) }, + { "CORNSILK", Color(1.00, 0.97, 0.86) }, + { "CRIMSON", Color(0.86, 0.08, 0.24) }, + { "CYAN", Color(0.00, 1.00, 1.00) }, + { "DARK_BLUE", Color(0.00, 0.00, 0.55) }, + { "DARK_CYAN", Color(0.00, 0.55, 0.55) }, + { "DARK_GOLDENROD", Color(0.72, 0.53, 0.04) }, + { "DARK_GRAY", Color(0.66, 0.66, 0.66) }, + { "DARK_GREEN", Color(0.00, 0.39, 0.00) }, + { "DARK_KHAKI", Color(0.74, 0.72, 0.42) }, + { "DARK_MAGENTA", Color(0.55, 0.00, 0.55) }, + { "DARK_OLIVE_GREEN", Color(0.33, 0.42, 0.18) }, + { "DARK_ORANGE", Color(1.00, 0.55, 0.00) }, + { "DARK_ORCHID", Color(0.60, 0.20, 0.80) }, + { "DARK_RED", Color(0.55, 0.00, 0.00) }, + { "DARK_SALMON", Color(0.91, 0.59, 0.48) }, + { "DARK_SEA_GREEN", Color(0.56, 0.74, 0.56) }, + { "DARK_SLATE_BLUE", Color(0.28, 0.24, 0.55) }, + { "DARK_SLATE_GRAY", Color(0.18, 0.31, 0.31) }, + { "DARK_TURQUOISE", Color(0.00, 0.81, 0.82) }, + { "DARK_VIOLET", Color(0.58, 0.00, 0.83) }, + { "DEEP_PINK", Color(1.00, 0.08, 0.58) }, + { "DEEP_SKY_BLUE", Color(0.00, 0.75, 1.00) }, + { "DIM_GRAY", Color(0.41, 0.41, 0.41) }, + { "DODGER_BLUE", Color(0.12, 0.56, 1.00) }, + { "FIREBRICK", Color(0.70, 0.13, 0.13) }, + { "FLORAL_WHITE", Color(1.00, 0.98, 0.94) }, + { "FOREST_GREEN", Color(0.13, 0.55, 0.13) }, + { "FUCHSIA", Color(1.00, 0.00, 1.00) }, + { "GAINSBORO", Color(0.86, 0.86, 0.86) }, + { "GHOST_WHITE", Color(0.97, 0.97, 1.00) }, + { "GOLD", Color(1.00, 0.84, 0.00) }, + { "GOLDENROD", Color(0.85, 0.65, 0.13) }, + { "GRAY", Color(0.75, 0.75, 0.75) }, + { "GREEN", Color(0.00, 1.00, 0.00) }, + { "GREEN_YELLOW", Color(0.68, 1.00, 0.18) }, + { "HONEYDEW", Color(0.94, 1.00, 0.94) }, + { "HOT_PINK", Color(1.00, 0.41, 0.71) }, + { "INDIAN_RED", Color(0.80, 0.36, 0.36) }, + { "INDIGO", Color(0.29, 0.00, 0.51) }, + { "IVORY", Color(1.00, 1.00, 0.94) }, + { "KHAKI", Color(0.94, 0.90, 0.55) }, + { "LAVENDER", Color(0.90, 0.90, 0.98) }, + { "LAVENDER_BLUSH", Color(1.00, 0.94, 0.96) }, + { "LAWN_GREEN", Color(0.49, 0.99, 0.00) }, + { "LEMON_CHIFFON", Color(1.00, 0.98, 0.80) }, + { "LIGHT_BLUE", Color(0.68, 0.85, 0.90) }, + { "LIGHT_CORAL", Color(0.94, 0.50, 0.50) }, + { "LIGHT_CYAN", Color(0.88, 1.00, 1.00) }, + { "LIGHT_GOLDENROD", Color(0.98, 0.98, 0.82) }, + { "LIGHT_GRAY", Color(0.83, 0.83, 0.83) }, + { "LIGHT_GREEN", Color(0.56, 0.93, 0.56) }, + { "LIGHT_PINK", Color(1.00, 0.71, 0.76) }, + { "LIGHT_SALMON", Color(1.00, 0.63, 0.48) }, + { "LIGHT_SEA_GREEN", Color(0.13, 0.70, 0.67) }, + { "LIGHT_SKY_BLUE", Color(0.53, 0.81, 0.98) }, + { "LIGHT_SLATE_GRAY", Color(0.47, 0.53, 0.60) }, + { "LIGHT_STEEL_BLUE", Color(0.69, 0.77, 0.87) }, + { "LIGHT_YELLOW", Color(1.00, 1.00, 0.88) }, + { "LIME", Color(0.00, 1.00, 0.00) }, + { "LIME_GREEN", Color(0.20, 0.80, 0.20) }, + { "LINEN", Color(0.98, 0.94, 0.90) }, + { "MAGENTA", Color(1.00, 0.00, 1.00) }, + { "MAROON", Color(0.69, 0.19, 0.38) }, + { "MEDIUM_AQUAMARINE", Color(0.40, 0.80, 0.67) }, + { "MEDIUM_BLUE", Color(0.00, 0.00, 0.80) }, + { "MEDIUM_ORCHID", Color(0.73, 0.33, 0.83) }, + { "MEDIUM_PURPLE", Color(0.58, 0.44, 0.86) }, + { "MEDIUM_SEA_GREEN", Color(0.24, 0.70, 0.44) }, + { "MEDIUM_SLATE_BLUE", Color(0.48, 0.41, 0.93) }, + { "MEDIUM_SPRING_GREEN", Color(0.00, 0.98, 0.60) }, + { "MEDIUM_TURQUOISE", Color(0.28, 0.82, 0.80) }, + { "MEDIUM_VIOLET_RED", Color(0.78, 0.08, 0.52) }, + { "MIDNIGHT_BLUE", Color(0.10, 0.10, 0.44) }, + { "MINT_CREAM", Color(0.96, 1.00, 0.98) }, + { "MISTY_ROSE", Color(1.00, 0.89, 0.88) }, + { "MOCCASIN", Color(1.00, 0.89, 0.71) }, + { "NAVAJO_WHITE", Color(1.00, 0.87, 0.68) }, + { "NAVY_BLUE", Color(0.00, 0.00, 0.50) }, + { "OLD_LACE", Color(0.99, 0.96, 0.90) }, + { "OLIVE", Color(0.50, 0.50, 0.00) }, + { "OLIVE_DRAB", Color(0.42, 0.56, 0.14) }, + { "ORANGE", Color(1.00, 0.65, 0.00) }, + { "ORANGE_RED", Color(1.00, 0.27, 0.00) }, + { "ORCHID", Color(0.85, 0.44, 0.84) }, + { "PALE_GOLDENROD", Color(0.93, 0.91, 0.67) }, + { "PALE_GREEN", Color(0.60, 0.98, 0.60) }, + { "PALE_TURQUOISE", Color(0.69, 0.93, 0.93) }, + { "PALE_VIOLET_RED", Color(0.86, 0.44, 0.58) }, + { "PAPAYA_WHIP", Color(1.00, 0.94, 0.84) }, + { "PEACH_PUFF", Color(1.00, 0.85, 0.73) }, + { "PERU", Color(0.80, 0.52, 0.25) }, + { "PINK", Color(1.00, 0.75, 0.80) }, + { "PLUM", Color(0.87, 0.63, 0.87) }, + { "POWDER_BLUE", Color(0.69, 0.88, 0.90) }, + { "PURPLE", Color(0.63, 0.13, 0.94) }, + { "REBECCA_PURPLE", Color(0.40, 0.20, 0.60) }, + { "RED", Color(1.00, 0.00, 0.00) }, + { "ROSY_BROWN", Color(0.74, 0.56, 0.56) }, + { "ROYAL_BLUE", Color(0.25, 0.41, 0.88) }, + { "SADDLE_BROWN", Color(0.55, 0.27, 0.07) }, + { "SALMON", Color(0.98, 0.50, 0.45) }, + { "SANDY_BROWN", Color(0.96, 0.64, 0.38) }, + { "SEA_GREEN", Color(0.18, 0.55, 0.34) }, + { "SEASHELL", Color(1.00, 0.96, 0.93) }, + { "SIENNA", Color(0.63, 0.32, 0.18) }, + { "SILVER", Color(0.75, 0.75, 0.75) }, + { "SKY_BLUE", Color(0.53, 0.81, 0.92) }, + { "SLATE_BLUE", Color(0.42, 0.35, 0.80) }, + { "SLATE_GRAY", Color(0.44, 0.50, 0.56) }, + { "SNOW", Color(1.00, 0.98, 0.98) }, + { "SPRING_GREEN", Color(0.00, 1.00, 0.50) }, + { "STEEL_BLUE", Color(0.27, 0.51, 0.71) }, + { "TAN", Color(0.82, 0.71, 0.55) }, + { "TEAL", Color(0.00, 0.50, 0.50) }, + { "THISTLE", Color(0.85, 0.75, 0.85) }, + { "TOMATO", Color(1.00, 0.39, 0.28) }, + { "TRANSPARENT", Color(1.00, 1.00, 1.00, 0.00) }, + { "TURQUOISE", Color(0.25, 0.88, 0.82) }, + { "VIOLET", Color(0.93, 0.51, 0.93) }, + { "WEB_GRAY", Color(0.50, 0.50, 0.50) }, + { "WEB_GREEN", Color(0.00, 0.50, 0.00) }, + { "WEB_MAROON", Color(0.50, 0.00, 0.00) }, + { "WEB_PURPLE", Color(0.50, 0.00, 0.50) }, + { "WHEAT", Color(0.96, 0.87, 0.70) }, + { "WHITE", Color(1.00, 1.00, 1.00) }, + { "WHITE_SMOKE", Color(0.96, 0.96, 0.96) }, + { "YELLOW", Color(1.00, 1.00, 0.00) }, + { "YELLOW_GREEN", Color(0.60, 0.80, 0.20) }, + { nullptr, Color() }, +}; diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp new file mode 100644 index 0000000000..682a7ea39e --- /dev/null +++ b/core/math/convex_hull.cpp @@ -0,0 +1,2290 @@ +/*************************************************************************/ +/* convex_hull.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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. */ +/*************************************************************************/ + +/* + * Based on Godot's patched VHACD-version of Bullet's btConvexHullComputer. + * See /thirdparty/vhacd/btConvexHullComputer.cpp at 64403ddcab9f1dca2408f0a412a22d899708bbb1 + * In turn, based on /src/LinearMath/btConvexHullComputer.cpp in <https://github.com/bulletphysics/bullet3> + * at 73b217fb07e7e3ce126caeb28ab3c9ddd0718467 + * + * Changes: + * - int32_t is consistently used instead of int in some cases + * - integrated patch db0d6c92927f5a1358b887f2645c11f3014f0e8a from Bullet (CWE-190 integer overflow in btConvexHullComputer) + * - adapted to Godot's code style + * - replaced Bullet's types (e.g. vectors) with Godot's + * - replaced custom Pool implementation with PagedAllocator + */ + +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "convex_hull.h" + +#include "core/error/error_macros.h" +#include "core/math/aabb.h" +#include "core/math/math_defs.h" +#include "core/os/memory.h" +#include "core/templates/paged_allocator.h" + +#include <string.h> + +//#define DEBUG_CONVEX_HULL +//#define SHOW_ITERATIONS + +// -- GODOT start -- +// Assembly optimizations are not used at the moment. +//#define USE_X86_64_ASM +// -- GODOT end -- + +#ifdef DEBUG_ENABLED +#define CHULL_ASSERT(m_cond) \ + do { \ + if (unlikely(!(m_cond))) { \ + ERR_PRINT("Assertion \"" _STR(m_cond) "\" failed."); \ + } \ + } while (0) +#else +#define CHULL_ASSERT(m_cond) \ + do { \ + } while (0) +#endif + +#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) +#include <stdio.h> +#endif + +// Convex hull implementation based on Preparata and Hong +// Ole Kniemeyer, MAXON Computer GmbH +class ConvexHullInternal { +public: + class Point64 { + public: + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t p_x, int64_t p_y, int64_t p_z) { + x = p_x; + y = p_y; + z = p_z; + } + + bool is_zero() { + return (x == 0) && (y == 0) && (z == 0); + } + + int64_t dot(const Point64 &b) const { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 { + public: + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + int32_t index = -1; + + Point32() { + } + + Point32(int32_t p_x, int32_t p_y, int32_t p_z) { + x = p_x; + y = p_y; + z = p_z; + } + + bool operator==(const Point32 &b) const { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32 &b) const { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool is_zero() { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32 &b) const { + return Point64((int64_t)y * b.z - (int64_t)z * b.y, (int64_t)z * b.x - (int64_t)x * b.z, (int64_t)x * b.y - (int64_t)y * b.x); + } + + Point64 cross(const Point64 &b) const { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + int64_t dot(const Point32 &b) const { + return (int64_t)x * b.x + (int64_t)y * b.y + (int64_t)z * b.z; + } + + int64_t dot(const Point64 &b) const { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32 &b) const { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32 &b) const { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 { + public: + uint64_t low = 0; + uint64_t high = 0; + + Int128() { + } + + Int128(uint64_t p_low, uint64_t p_high) { + low = p_low; + high = p_high; + } + + Int128(uint64_t p_low) { + low = p_low; + high = 0; + } + + Int128(int64_t p_value) { + low = p_value; + if (p_value >= 0) { + high = 0; + } else { + high = (uint64_t)-1LL; + } + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const { + return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128 &b) const { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; +#else + uint64_t lo = low + b.low; + return Int128(lo, high + b.high + (lo < low)); +#endif + } + + Int128 operator-(const Int128 &b) const { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__("subq %[bl], %[rl]\n\t" + "sbbq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; +#else + return *this + -b; +#endif + } + + Int128 &operator+=(const Int128 &b) { +#ifdef USE_X86_64_ASM + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(low), [rh] "=r"(high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); +#else + uint64_t lo = low + b.low; + if (lo < low) { + ++high; + } + low = lo; + high += b.high; +#endif + return *this; + } + + Int128 &operator++() { + if (++low == 0) { + ++high; + } + return *this; + } + + Int128 operator*(int64_t b) const; + + real_t to_scalar() const { + return ((int64_t)high >= 0) ? real_t(high) * (real_t(0x100000000LL) * real_t(0x100000000LL)) + real_t(low) : -(-*this).to_scalar(); + } + + int32_t get_sign() const { + return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : + 0; + } + + bool operator<(const Int128 &b) const { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int32_t ucmp(const Int128 &b) const { + if (high < b.high) { + return -1; + } + if (high > b.high) { + return 1; + } + if (low < b.low) { + return -1; + } + if (low > b.low) { + return 1; + } + return 0; + } + }; + + class Rational64 { + private: + uint64_t numerator; + uint64_t denominator; + int32_t sign; + + public: + Rational64(int64_t p_numerator, int64_t p_denominator) { + if (p_numerator > 0) { + sign = 1; + numerator = (uint64_t)p_numerator; + } else if (p_numerator < 0) { + sign = -1; + numerator = (uint64_t)-p_numerator; + } else { + sign = 0; + numerator = 0; + } + if (p_denominator > 0) { + denominator = (uint64_t)p_denominator; + } else if (p_denominator < 0) { + sign = -sign; + denominator = (uint64_t)-p_denominator; + } else { + denominator = 0; + } + } + + bool is_negative_infinity() const { + return (sign < 0) && (denominator == 0); + } + + bool is_nan() const { + return (sign == 0) && (denominator == 0); + } + + int32_t compare(const Rational64 &b) const; + + real_t to_scalar() const { + return sign * ((denominator == 0) ? FLT_MAX : (real_t)numerator / denominator); + } + }; + + class Rational128 { + private: + Int128 numerator; + Int128 denominator; + int32_t sign; + bool is_int_64; + + public: + Rational128(int64_t p_value) { + if (p_value > 0) { + sign = 1; + this->numerator = p_value; + } else if (p_value < 0) { + sign = -1; + this->numerator = -p_value; + } else { + sign = 0; + this->numerator = (uint64_t)0; + } + this->denominator = (uint64_t)1; + is_int_64 = true; + } + + Rational128(const Int128 &p_numerator, const Int128 &p_denominator) { + sign = p_numerator.get_sign(); + if (sign >= 0) { + this->numerator = p_numerator; + } else { + this->numerator = -p_numerator; + } + int32_t dsign = p_denominator.get_sign(); + if (dsign >= 0) { + this->denominator = p_denominator; + } else { + sign = -sign; + this->denominator = -p_denominator; + } + is_int_64 = false; + } + + int32_t compare(const Rational128 &b) const; + + int32_t compare(int64_t b) const; + + real_t to_scalar() const { + return sign * ((denominator.get_sign() == 0) ? FLT_MAX : numerator.to_scalar() / denominator.to_scalar()); + } + }; + + class PointR128 { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() { + } + + PointR128(Int128 p_x, Int128 p_y, Int128 p_z, Int128 p_denominator) { + x = p_x; + y = p_y; + z = p_z; + denominator = p_denominator; + } + + real_t xvalue() const { + return x.to_scalar() / denominator.to_scalar(); + } + + real_t yvalue() const { + return y.to_scalar() / denominator.to_scalar(); + } + + real_t zvalue() const { + return z.to_scalar() / denominator.to_scalar(); + } + }; + + class Edge; + class Face; + + class Vertex { + public: + Vertex *next = nullptr; + Vertex *prev = nullptr; + Edge *edges = nullptr; + Face *first_nearby_face = nullptr; + Face *last_nearby_face = nullptr; + PointR128 point128; + Point32 point; + int32_t copy = -1; + + Vertex() { + } + +#ifdef DEBUG_CONVEX_HULL + void print() { + printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } + + void print_graph(); +#endif + + Point32 operator-(const Vertex &b) const { + return point - b.point; + } + + Rational128 dot(const Point64 &b) const { + return (point.index >= 0) ? Rational128(point.dot(b)) : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + real_t xvalue() const { + return (point.index >= 0) ? real_t(point.x) : point128.xvalue(); + } + + real_t yvalue() const { + return (point.index >= 0) ? real_t(point.y) : point128.yvalue(); + } + + real_t zvalue() const { + return (point.index >= 0) ? real_t(point.z) : point128.zvalue(); + } + + void receive_nearby_faces(Vertex *p_src) { + if (last_nearby_face) { + last_nearby_face->next_with_same_nearby_vertex = p_src->first_nearby_face; + } else { + first_nearby_face = p_src->first_nearby_face; + } + if (p_src->last_nearby_face) { + last_nearby_face = p_src->last_nearby_face; + } + for (Face *f = p_src->first_nearby_face; f; f = f->next_with_same_nearby_vertex) { + CHULL_ASSERT(f->nearby_vertex == p_src); + f->nearby_vertex = this; + } + p_src->first_nearby_face = nullptr; + p_src->last_nearby_face = nullptr; + } + }; + + class Edge { + public: + Edge *next = nullptr; + Edge *prev = nullptr; + Edge *reverse = nullptr; + Vertex *target = nullptr; + Face *face = nullptr; + int32_t copy = -1; + + void link(Edge *n) { + CHULL_ASSERT(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#ifdef DEBUG_CONVEX_HULL + void print() { + printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev, + reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); + } +#endif + }; + + class Face { + public: + Face *next = nullptr; + Vertex *nearby_vertex = nullptr; + Face *next_with_same_nearby_vertex = nullptr; + Point32 origin; + Point32 dir0; + Point32 dir1; + + Face() { + } + + void init(Vertex *p_a, Vertex *p_b, Vertex *p_c) { + nearby_vertex = p_a; + origin = p_a->point; + dir0 = *p_b - *p_a; + dir1 = *p_c - *p_a; + if (p_a->last_nearby_face) { + p_a->last_nearby_face->next_with_same_nearby_vertex = this; + } else { + p_a->first_nearby_face = this; + } + p_a->last_nearby_face = this; + } + + Point64 get_normal() { + return dir0.cross(dir1); + } + }; + + template <typename UWord, typename UHWord> + class DMul { + private: + static uint32_t high(uint64_t p_value) { + return (uint32_t)(p_value >> 32); + } + + static uint32_t low(uint64_t p_value) { + return (uint32_t)p_value; + } + + static uint64_t mul(uint32_t a, uint32_t b) { + return (uint64_t)a * (uint64_t)b; + } + + static void shl_half(uint64_t &p_value) { + p_value <<= 32; + } + + static uint64_t high(Int128 p_value) { + return p_value.high; + } + + static uint64_t low(Int128 p_value) { + return p_value.low; + } + + static Int128 mul(uint64_t a, uint64_t b) { + return Int128::mul(a, b); + } + + static void shl_half(Int128 &p_value) { + p_value.high = p_value.low; + p_value.low = 0; + } + + public: + static void mul(UWord p_a, UWord p_b, UWord &r_low, UWord &r_high) { + UWord p00 = mul(low(p_a), low(p_b)); + UWord p01 = mul(low(p_a), high(p_b)); + UWord p10 = mul(high(p_a), low(p_b)); + UWord p11 = mul(high(p_a), high(p_b)); + UWord p0110 = UWord(low(p01)) + UWord(low(p10)); + p11 += high(p01); + p11 += high(p10); + p11 += high(p0110); + shl_half(p0110); + p00 += p0110; + if (p00 < p0110) { + ++p11; + } + r_low = p00; + r_high = p11; + } + }; + +private: + class IntermediateHull { + public: + Vertex *min_xy = nullptr; + Vertex *max_xy = nullptr; + Vertex *min_yx = nullptr; + Vertex *max_yx = nullptr; + + IntermediateHull() { + } + + void print(); + }; + + enum Orientation { NONE, + CLOCKWISE, + COUNTER_CLOCKWISE }; + + Vector3 scaling; + Vector3 center; + PagedAllocator<Vertex> vertex_pool; + PagedAllocator<Edge> edge_pool; + PagedAllocator<Face> face_pool; + LocalVector<Vertex *> original_vertices; + int32_t merge_stamp = 0; + int32_t min_axis = 0; + int32_t med_axis = 0; + int32_t max_axis = 0; + int32_t used_edge_pairs = 0; + int32_t max_used_edge_pairs = 0; + + static Orientation get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t); + Edge *find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_ssxrxs, Rational64 &p_min_cot); + void find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, Vertex *p_stop0, Vertex *p_stop1); + + Edge *new_edge_pair(Vertex *p_from, Vertex *p_to); + + void remove_edge_pair(Edge *p_edge) { + Edge *n = p_edge->next; + Edge *r = p_edge->reverse; + + CHULL_ASSERT(p_edge->target && r->target); + + if (n != p_edge) { + n->prev = p_edge->prev; + p_edge->prev->next = n; + r->target->edges = n; + } else { + r->target->edges = nullptr; + } + + n = r->next; + + if (n != r) { + n->prev = r->prev; + r->prev->next = n; + p_edge->target->edges = n; + } else { + p_edge->target->edges = nullptr; + } + + edge_pool.free(p_edge); + edge_pool.free(r); + used_edge_pairs--; + } + + void compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result); + + bool merge_projection(IntermediateHull &p_h0, IntermediateHull &p_h1, Vertex *&r_c0, Vertex *&r_c1); + + void merge(IntermediateHull &p_h0, IntermediateHull &p_h1); + + Vector3 to_gd_vector(const Point32 &p_v); + + Vector3 get_gd_normal(Face *p_face); + + bool shift_face(Face *p_face, real_t p_amount, LocalVector<Vertex *> p_stack); + +public: + ~ConvexHullInternal() { + vertex_pool.reset(true); + edge_pool.reset(true); + face_pool.reset(true); + } + + Vertex *vertex_list; + + void compute(const Vector3 *p_coords, int32_t p_count); + + Vector3 get_coordinates(const Vertex *p_v); + + real_t shrink(real_t amount, real_t p_clamp_amount); +}; + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::operator*(int64_t b) const { + bool negative = (int64_t)high < 0; + Int128 a = negative ? -*this : *this; + if (b < 0) { + negative = !negative; + b = -b; + } + Int128 result = mul(a.low, (uint64_t)b); + result.high += a.high * (uint64_t)b; + return negative ? -result : result; +} + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(int64_t a, int64_t b) { + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__("imulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); + return result; + +#else + bool negative = a < 0; + if (negative) { + a = -a; + } + if (b < 0) { + negative = !negative; + b = -b; + } + DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high); + return negative ? -result : result; +#endif +} + +ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) { + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__("mulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); + +#else + DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high); +#endif + + return result; +} + +int32_t ConvexHullInternal::Rational64::compare(const Rational64 &b) const { + if (sign != b.sign) { + return sign - b.sign; + } else if (sign == 0) { + return 0; + } + + // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; + +#ifdef USE_X86_64_ASM + + int32_t result; + int64_t tmp; + int64_t dummy; + __asm__("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc"); + return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : + 0; + +#else + + return sign * Int128::mul(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator)); + +#endif +} + +int32_t ConvexHullInternal::Rational128::compare(const Rational128 &b) const { + if (sign != b.sign) { + return sign - b.sign; + } else if (sign == 0) { + return 0; + } + if (is_int_64) { + return -b.compare(sign * (int64_t)numerator.low); + } + + Int128 nbd_low, nbd_high, dbn_low, dbn_high; + DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbd_low, nbd_high); + DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbn_low, dbn_high); + + int32_t cmp = nbd_high.ucmp(dbn_high); + if (cmp) { + return cmp * sign; + } + return nbd_low.ucmp(dbn_low) * sign; +} + +int32_t ConvexHullInternal::Rational128::compare(int64_t b) const { + if (is_int_64) { + int64_t a = sign * (int64_t)numerator.low; + return (a > b) ? 1 : (a < b) ? -1 : + 0; + } + if (b > 0) { + if (sign <= 0) { + return -1; + } + } else if (b < 0) { + if (sign >= 0) { + return 1; + } + b = -b; + } else { + return sign; + } + + return numerator.ucmp(denominator * b) * sign; +} + +ConvexHullInternal::Edge *ConvexHullInternal::new_edge_pair(Vertex *p_from, Vertex *p_to) { + CHULL_ASSERT(p_from && p_to); + Edge *e = edge_pool.alloc(); + Edge *r = edge_pool.alloc(); + e->reverse = r; + r->reverse = e; + e->copy = merge_stamp; + r->copy = merge_stamp; + e->target = p_to; + r->target = p_from; + e->face = nullptr; + r->face = nullptr; + used_edge_pairs++; + if (used_edge_pairs > max_used_edge_pairs) { + max_used_edge_pairs = used_edge_pairs; + } + return e; +} + +bool ConvexHullInternal::merge_projection(IntermediateHull &r_h0, IntermediateHull &r_h1, Vertex *&r_c0, Vertex *&r_c1) { + Vertex *v0 = r_h0.max_yx; + Vertex *v1 = r_h1.min_yx; + if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) { + CHULL_ASSERT(v0->point.z < v1->point.z); + Vertex *v1p = v1->prev; + if (v1p == v1) { + r_c0 = v0; + if (v1->edges) { + CHULL_ASSERT(v1->edges->next == v1->edges); + v1 = v1->edges->target; + CHULL_ASSERT(v1->edges->next == v1->edges); + } + r_c1 = v1; + return false; + } + Vertex *v1n = v1->next; + v1p->next = v1n; + v1n->prev = v1p; + if (v1 == r_h1.min_xy) { + if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) { + r_h1.min_xy = v1n; + } else { + r_h1.min_xy = v1p; + } + } + if (v1 == r_h1.max_xy) { + if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) { + r_h1.max_xy = v1n; + } else { + r_h1.max_xy = v1p; + } + } + } + + v0 = r_h0.max_xy; + v1 = r_h1.max_xy; + Vertex *v00 = nullptr; + Vertex *v10 = nullptr; + int32_t sign = 1; + + for (int32_t side = 0; side <= 1; side++) { + int32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex *w0 = side ? v0->next : v0->prev; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex *w1 = side ? v1->next : v1->prev; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + int32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } else if (dx < 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex *w1 = side ? v1->prev : v1->next; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex *w0 = side ? v0->prev : v0->next; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + int32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } else { + int32_t x = v0->point.x; + int32_t y0 = v0->point.y; + Vertex *w0 = v0; + Vertex *t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + int32_t y1 = v1->point.y; + Vertex *w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) { + v00 = v0; + v10 = v1; + + v0 = r_h0.min_xy; + v1 = r_h1.min_xy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (r_h1.min_xy->point.x < r_h0.min_xy->point.x) { + r_h0.min_xy = r_h1.min_xy; + } + if (r_h1.max_xy->point.x >= r_h0.max_xy->point.x) { + r_h0.max_xy = r_h1.max_xy; + } + + r_h0.max_yx = r_h1.max_yx; + + r_c0 = v00; + r_c1 = v10; + + return true; +} + +void ConvexHullInternal::compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result) { + int32_t n = p_end - p_start; + switch (n) { + case 0: + r_result.min_xy = nullptr; + r_result.max_xy = nullptr; + r_result.min_yx = nullptr; + r_result.max_yx = nullptr; + return; + case 2: { + Vertex *v = original_vertices[p_start]; + Vertex *w = original_vertices[p_start + 1]; + if (v->point != w->point) { + int32_t dx = v->point.x - w->point.x; + int32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) { + if (v->point.z > w->point.z) { + Vertex *t = w; + w = v; + v = t; + } + CHULL_ASSERT(v->point.z < w->point.z); + v->next = v; + v->prev = v; + r_result.min_xy = v; + r_result.max_xy = v; + r_result.min_yx = v; + r_result.max_yx = v; + } else { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) { + r_result.min_xy = v; + r_result.max_xy = w; + } else { + r_result.min_xy = w; + r_result.max_xy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) { + r_result.min_yx = v; + r_result.max_yx = w; + } else { + r_result.min_yx = w; + r_result.max_yx = v; + } + } + + Edge *e = new_edge_pair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + [[fallthrough]]; + } + case 1: { + Vertex *v = original_vertices[p_start]; + v->edges = nullptr; + v->next = v; + v->prev = v; + + r_result.min_xy = v; + r_result.max_xy = v; + r_result.min_yx = v; + r_result.max_yx = v; + + return; + } + } + + int32_t split0 = p_start + n / 2; + Point32 p = original_vertices[split0 - 1]->point; + int32_t split1 = split0; + while ((split1 < p_end) && (original_vertices[split1]->point == p)) { + split1++; + } + compute_internal(p_start, split0, r_result); + IntermediateHull hull1; + compute_internal(split1, p_end, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n\nMerge\n"); + r_result.print(); + hull1.print(); +#endif + merge(r_result, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n Result\n"); + r_result.print(); +#endif +} + +#ifdef DEBUG_CONVEX_HULL +void ConvexHullInternal::IntermediateHull::print() { + printf(" Hull\n"); + for (Vertex *v = min_xy; v;) { + printf(" "); + v->print(); + if (v == max_xy) { + printf(" max_xy"); + } + if (v == min_yx) { + printf(" min_yx"); + } + if (v == max_yx) { + printf(" max_yx"); + } + if (v->next->prev != v) { + printf(" Inconsistency"); + } + printf("\n"); + v = v->next; + if (v == min_xy) { + break; + } + } + if (min_xy) { + min_xy->copy = (min_xy->copy == -1) ? -2 : -1; + min_xy->print_graph(); + } +} + +void ConvexHullInternal::Vertex::print_graph() { + print(); + printf("\nEdges\n"); + Edge *e = edges; + if (e) { + do { + e->print(); + printf("\n"); + e = e->next; + } while (e != edges); + do { + Vertex *v = e->target; + if (v->copy != copy) { + v->copy = copy; + v->print_graph(); + } + e = e->next; + } while (e != edges); + } +} +#endif + +ConvexHullInternal::Orientation ConvexHullInternal::get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t) { + CHULL_ASSERT(p_prev->reverse->target == p_next->reverse->target); + if (p_prev->next == p_next) { + if (p_prev->prev == p_next) { + Point64 n = p_t.cross(p_s); + Point64 m = (*p_prev->target - *p_next->reverse->target).cross(*p_next->target - *p_next->reverse->target); + CHULL_ASSERT(!m.is_zero()); + int64_t dot = n.dot(m); + CHULL_ASSERT(dot != 0); + return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; + } + return COUNTER_CLOCKWISE; + } else if (p_prev->prev == p_next) { + return CLOCKWISE; + } else { + return NONE; + } +} + +ConvexHullInternal::Edge *ConvexHullInternal::find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_sxrxs, Rational64 &p_min_cot) { + Edge *min_edge = nullptr; + +#ifdef DEBUG_CONVEX_HULL + printf("find max edge for %d\n", p_start->point.index); +#endif + Edge *e = p_start->edges; + if (e) { + do { + if (e->copy > merge_stamp) { + Point32 t = *e->target - *p_start; + Rational64 cot(t.dot(p_sxrxs), t.dot(p_rxs)); +#ifdef DEBUG_CONVEX_HULL + printf(" Angle is %f (%d) for ", Math::atan(cot.to_scalar()), (int32_t)cot.is_nan()); + e->print(); +#endif + if (cot.is_nan()) { + CHULL_ASSERT(p_ccw ? (t.dot(p_s) < 0) : (t.dot(p_s) > 0)); + } else { + int32_t cmp; + if (min_edge == nullptr) { + p_min_cot = cot; + min_edge = e; + } else if ((cmp = cot.compare(p_min_cot)) < 0) { + p_min_cot = cot; + min_edge = e; + } else if ((cmp == 0) && (p_ccw == (get_orientation(min_edge, e, p_s, t) == COUNTER_CLOCKWISE))) { + min_edge = e; + } + } +#ifdef DEBUG_CONVEX_HULL + printf("\n"); +#endif + } + e = e->next; + } while (e != p_start->edges); + } + return min_edge; +} + +void ConvexHullInternal::find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, Vertex *p_stop0, Vertex *p_stop1) { + Edge *start0 = p_e0; + Edge *start1 = p_e1; + Point32 et0 = start0 ? start0->target->point : p_c0->point; + Point32 et1 = start1 ? start1->target->point : p_c1->point; + Point32 s = p_c1->point - p_c0->point; + Point64 normal = ((start0 ? start0 : start1)->target->point - p_c0->point).cross(s); + int64_t dist = p_c0->point.dot(normal); + CHULL_ASSERT(!start1 || (start1->target->point.dot(normal) == dist)); + Point64 perp = s.cross(normal); + CHULL_ASSERT(!perp.is_zero()); + +#ifdef DEBUG_CONVEX_HULL + printf(" Advancing %d %d (%p %p, %d %d)\n", p_c0->point.index, p_c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1); +#endif + + int64_t max_dot0 = et0.dot(perp); + if (p_e0) { + while (p_e0->target != p_stop0) { + Edge *e = p_e0->reverse->prev; + if (e->target->point.dot(normal) < dist) { + break; + } + CHULL_ASSERT(e->target->point.dot(normal) == dist); + if (e->copy == merge_stamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= max_dot0) { + break; + } + max_dot0 = dot; + p_e0 = e; + et0 = e->target->point; + } + } + + int64_t max_dot1 = et1.dot(perp); + if (p_e1) { + while (p_e1->target != p_stop1) { + Edge *e = p_e1->reverse->next; + if (e->target->point.dot(normal) < dist) { + break; + } + CHULL_ASSERT(e->target->point.dot(normal) == dist); + if (e->copy == merge_stamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= max_dot1) { + break; + } + max_dot1 = dot; + p_e1 = e; + et1 = e->target->point; + } + } + +#ifdef DEBUG_CONVEX_HULL + printf(" Starting at %d %d\n", et0.index, et1.index); +#endif + + int64_t dx = max_dot1 - max_dot0; + if (dx > 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (p_e0 && (p_e0->target != p_stop0)) { + Edge *f0 = p_e0->next->reverse; + if (f0->copy > merge_stamp) { + int64_t dx0 = (f0->target->point - et0).dot(perp); + int64_t dy0 = (f0->target->point - et0).dot(s); + if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) { + et0 = f0->target->point; + dx = (et1 - et0).dot(perp); + p_e0 = (p_e0 == start0) ? nullptr : f0; + continue; + } + } + } + + if (p_e1 && (p_e1->target != p_stop1)) { + Edge *f1 = p_e1->reverse->next; + if (f1->copy > merge_stamp) { + Point32 d1 = f1->target->point - et1; + if (d1.dot(normal) == 0) { + int64_t dx1 = d1.dot(perp); + int64_t dy1 = d1.dot(s); + int64_t dxn = (f1->target->point - et0).dot(perp); + if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) { + p_e1 = f1; + et1 = p_e1->target->point; + dx = dxn; + continue; + } + } else { + CHULL_ASSERT((p_e1 == start1) && (d1.dot(normal) < 0)); + } + } + } + + break; + } + } else if (dx < 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (p_e1 && (p_e1->target != p_stop1)) { + Edge *f1 = p_e1->prev->reverse; + if (f1->copy > merge_stamp) { + int64_t dx1 = (f1->target->point - et1).dot(perp); + int64_t dy1 = (f1->target->point - et1).dot(s); + if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) { + et1 = f1->target->point; + dx = (et1 - et0).dot(perp); + p_e1 = (p_e1 == start1) ? nullptr : f1; + continue; + } + } + } + + if (p_e0 && (p_e0->target != p_stop0)) { + Edge *f0 = p_e0->reverse->prev; + if (f0->copy > merge_stamp) { + Point32 d0 = f0->target->point - et0; + if (d0.dot(normal) == 0) { + int64_t dx0 = d0.dot(perp); + int64_t dy0 = d0.dot(s); + int64_t dxn = (et1 - f0->target->point).dot(perp); + if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) { + p_e0 = f0; + et0 = p_e0->target->point; + dx = dxn; + continue; + } + } else { + CHULL_ASSERT((p_e0 == start0) && (d0.dot(normal) < 0)); + } + } + } + + break; + } + } +#ifdef DEBUG_CONVEX_HULL + printf(" Advanced edges to %d %d\n", et0.index, et1.index); +#endif +} + +void ConvexHullInternal::merge(IntermediateHull &p_h0, IntermediateHull &p_h1) { + if (!p_h1.max_xy) { + return; + } + if (!p_h0.max_xy) { + p_h0 = p_h1; + return; + } + + merge_stamp--; + + Vertex *c0 = nullptr; + Edge *to_prev0 = nullptr; + Edge *first_new0 = nullptr; + Edge *pending_head0 = nullptr; + Edge *pending_tail0 = nullptr; + Vertex *c1 = nullptr; + Edge *to_prev1 = nullptr; + Edge *first_new1 = nullptr; + Edge *pending_head1 = nullptr; + Edge *pending_tail1 = nullptr; + Point32 prev_point; + + if (merge_projection(p_h0, p_h1, c0, c1)) { + Point32 s = *c1 - *c0; + Point64 normal = Point32(0, 0, -1).cross(s); + Point64 t = s.cross(normal); + CHULL_ASSERT(!t.is_zero()); + + Edge *e = c0->edges; + Edge *start0 = nullptr; + if (e) { + do { + int64_t dot = (*e->target - *c0).dot(normal); + CHULL_ASSERT(dot <= 0); + if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) { + if (!start0 || (get_orientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) { + start0 = e; + } + } + e = e->next; + } while (e != c0->edges); + } + + e = c1->edges; + Edge *start1 = nullptr; + if (e) { + do { + int64_t dot = (*e->target - *c1).dot(normal); + CHULL_ASSERT(dot <= 0); + if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) { + if (!start1 || (get_orientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) { + start1 = e; + } + } + e = e->next; + } while (e != c1->edges); + } + + if (start0 || start1) { + find_edge_for_coplanar_faces(c0, c1, start0, start1, nullptr, nullptr); + if (start0) { + c0 = start0->target; + } + if (start1) { + c1 = start1->target; + } + } + + prev_point = c1->point; + prev_point.z++; + } else { + prev_point = c1->point; + prev_point.x++; + } + + Vertex *first0 = c0; + Vertex *first1 = c1; + bool first_run = true; + + while (true) { + Point32 s = *c1 - *c0; + Point32 r = prev_point - c0->point; + Point64 rxs = r.cross(s); + Point64 sxrxs = s.cross(rxs); + +#ifdef DEBUG_CONVEX_HULL + printf("\n Checking %d %d\n", c0->point.index, c1->point.index); +#endif + Rational64 min_cot0(0, 0); + Edge *min0 = find_max_angle(false, c0, s, rxs, sxrxs, min_cot0); + Rational64 min_cot1(0, 0); + Edge *min1 = find_max_angle(true, c1, s, rxs, sxrxs, min_cot1); + if (!min0 && !min1) { + Edge *e = new_edge_pair(c0, c1); + e->link(e); + c0->edges = e; + + e = e->reverse; + e->link(e); + c1->edges = e; + return; + } else { + int32_t cmp = !min0 ? 1 : !min1 ? -1 : + min_cot0.compare(min_cot1); +#ifdef DEBUG_CONVEX_HULL + printf(" -> Result %d\n", cmp); +#endif + if (first_run || ((cmp >= 0) ? !min_cot1.is_negative_infinity() : !min_cot0.is_negative_infinity())) { + Edge *e = new_edge_pair(c0, c1); + if (pending_tail0) { + pending_tail0->prev = e; + } else { + pending_head0 = e; + } + e->next = pending_tail0; + pending_tail0 = e; + + e = e->reverse; + if (pending_tail1) { + pending_tail1->next = e; + } else { + pending_head1 = e; + } + e->prev = pending_tail1; + pending_tail1 = e; + } + + Edge *e0 = min0; + Edge *e1 = min1; + +#ifdef DEBUG_CONVEX_HULL + printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); +#endif + + if (cmp == 0) { + find_edge_for_coplanar_faces(c0, c1, e0, e1, nullptr, nullptr); + } + + if ((cmp >= 0) && e1) { + if (to_prev1) { + for (Edge *e = to_prev1->next, *n = nullptr; e != min1; e = n) { + n = e->next; + remove_edge_pair(e); + } + } + + if (pending_tail1) { + if (to_prev1) { + to_prev1->link(pending_head1); + } else { + min1->prev->link(pending_head1); + first_new1 = pending_head1; + } + pending_tail1->link(min1); + pending_head1 = nullptr; + pending_tail1 = nullptr; + } else if (!to_prev1) { + first_new1 = min1; + } + + prev_point = c1->point; + c1 = e1->target; + to_prev1 = e1->reverse; + } + + if ((cmp <= 0) && e0) { + if (to_prev0) { + for (Edge *e = to_prev0->prev, *n = nullptr; e != min0; e = n) { + n = e->prev; + remove_edge_pair(e); + } + } + + if (pending_tail0) { + if (to_prev0) { + pending_head0->link(to_prev0); + } else { + pending_head0->link(min0->next); + first_new0 = pending_head0; + } + min0->link(pending_tail0); + pending_head0 = nullptr; + pending_tail0 = nullptr; + } else if (!to_prev0) { + first_new0 = min0; + } + + prev_point = c0->point; + c0 = e0->target; + to_prev0 = e0->reverse; + } + } + + if ((c0 == first0) && (c1 == first1)) { + if (to_prev0 == nullptr) { + pending_head0->link(pending_tail0); + c0->edges = pending_tail0; + } else { + for (Edge *e = to_prev0->prev, *n = nullptr; e != first_new0; e = n) { + n = e->prev; + remove_edge_pair(e); + } + if (pending_tail0) { + pending_head0->link(to_prev0); + first_new0->link(pending_tail0); + } + } + + if (to_prev1 == nullptr) { + pending_tail1->link(pending_head1); + c1->edges = pending_tail1; + } else { + for (Edge *e = to_prev1->next, *n = nullptr; e != first_new1; e = n) { + n = e->next; + remove_edge_pair(e); + } + if (pending_tail1) { + to_prev1->link(pending_head1); + pending_tail1->link(first_new1); + } + } + + return; + } + + first_run = false; + } +} + +struct PointComparator { + _FORCE_INLINE_ bool operator()(const ConvexHullInternal::Point32 &p, const ConvexHullInternal::Point32 &q) const { + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); + } +}; + +void ConvexHullInternal::compute(const Vector3 *p_coords, int32_t p_count) { + AABB aabb; + for (int32_t i = 0; i < p_count; i++) { + Vector3 p = p_coords[i]; + if (i == 0) { + aabb.position = p; + } else { + aabb.expand_to(p); + } + } + + Vector3 s = aabb.size; + max_axis = s.max_axis(); + min_axis = s.min_axis(); + if (min_axis == max_axis) { + min_axis = (max_axis + 1) % 3; + } + med_axis = 3 - max_axis - min_axis; + + s /= real_t(10216); + if (((med_axis + 1) % 3) != max_axis) { + s *= -1; + } + scaling = s; + + if (s[0] != 0) { + s[0] = real_t(1) / s[0]; + } + if (s[1] != 0) { + s[1] = real_t(1) / s[1]; + } + if (s[2] != 0) { + s[2] = real_t(1) / s[2]; + } + + center = aabb.position; + + LocalVector<Point32> points; + points.resize(p_count); + for (int32_t i = 0; i < p_count; i++) { + Vector3 p = p_coords[i]; + p = (p - center) * s; + points[i].x = (int32_t)p[med_axis]; + points[i].y = (int32_t)p[max_axis]; + points[i].z = (int32_t)p[min_axis]; + points[i].index = i; + } + + points.sort_custom<PointComparator>(); + + vertex_pool.reset(true); + original_vertices.resize(p_count); + for (int32_t i = 0; i < p_count; i++) { + Vertex *v = vertex_pool.alloc(); + v->edges = nullptr; + v->point = points[i]; + v->copy = -1; + original_vertices[i] = v; + } + + points.clear(); + + edge_pool.reset(true); + + used_edge_pairs = 0; + max_used_edge_pairs = 0; + + merge_stamp = -3; + + IntermediateHull hull; + compute_internal(0, p_count, hull); + vertex_list = hull.min_xy; +#ifdef DEBUG_CONVEX_HULL + printf("max. edges %d (3v = %d)", max_used_edge_pairs, 3 * p_count); +#endif +} + +Vector3 ConvexHullInternal::to_gd_vector(const Point32 &p_v) { + Vector3 p; + p[med_axis] = real_t(p_v.x); + p[max_axis] = real_t(p_v.y); + p[min_axis] = real_t(p_v.z); + return p * scaling; +} + +Vector3 ConvexHullInternal::get_gd_normal(Face *p_face) { + return to_gd_vector(p_face->dir0).cross(to_gd_vector(p_face->dir1)).normalized(); +} + +Vector3 ConvexHullInternal::get_coordinates(const Vertex *p_v) { + Vector3 p; + p[med_axis] = p_v->xvalue(); + p[max_axis] = p_v->yvalue(); + p[min_axis] = p_v->zvalue(); + return p * scaling + center; +} + +real_t ConvexHullInternal::shrink(real_t p_amount, real_t p_clamp_amount) { + if (!vertex_list) { + return 0; + } + int32_t stamp = --merge_stamp; + LocalVector<Vertex *> stack; + vertex_list->copy = stamp; + stack.push_back(vertex_list); + LocalVector<Face *> faces; + + Point32 ref = vertex_list->point; + Int128 hull_center_x(0, 0); + Int128 hull_center_y(0, 0); + Int128 hull_center_z(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) { + Vertex *v = stack[stack.size() - 1]; + stack.remove(stack.size() - 1); + Edge *e = v->edges; + if (e) { + do { + if (e->target->copy != stamp) { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) { + Face *face = face_pool.alloc(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge *f = e; + + Vertex *a = nullptr; + Vertex *b = nullptr; + do { + if (a && b) { + int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + CHULL_ASSERT(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hull_center_x += vol * c.x; + hull_center_y += vol * c.y; + hull_center_z += vol * c.z; + volume += vol; + } + + CHULL_ASSERT(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.get_sign() <= 0) { + return 0; + } + + Vector3 hull_center; + hull_center[med_axis] = hull_center_x.to_scalar(); + hull_center[max_axis] = hull_center_y.to_scalar(); + hull_center[min_axis] = hull_center_z.to_scalar(); + hull_center /= 4 * volume.to_scalar(); + hull_center *= scaling; + + int32_t face_count = faces.size(); + + if (p_clamp_amount > 0) { + real_t min_dist = FLT_MAX; + for (int32_t i = 0; i < face_count; i++) { + Vector3 normal = get_gd_normal(faces[i]); + real_t dist = normal.dot(to_gd_vector(faces[i]->origin) - hull_center); + if (dist < min_dist) { + min_dist = dist; + } + } + + if (min_dist <= 0) { + return 0; + } + + p_amount = MIN(p_amount, min_dist * p_clamp_amount); + } + + uint32_t seed = 243703; + for (int32_t i = 0; i < face_count; i++, seed = 1664525 * seed + 1013904223) { + SWAP(faces[i], faces[seed % face_count]); + } + + for (int32_t i = 0; i < face_count; i++) { + if (!shift_face(faces[i], p_amount, stack)) { + return -p_amount; + } + } + + return p_amount; +} + +bool ConvexHullInternal::shift_face(Face *p_face, real_t p_amount, LocalVector<Vertex *> p_stack) { + Vector3 orig_shift = get_gd_normal(p_face) * -p_amount; + if (scaling[0] != 0) { + orig_shift[0] /= scaling[0]; + } + if (scaling[1] != 0) { + orig_shift[1] /= scaling[1]; + } + if (scaling[2] != 0) { + orig_shift[2] /= scaling[2]; + } + Point32 shift((int32_t)orig_shift[med_axis], (int32_t)orig_shift[max_axis], (int32_t)orig_shift[min_axis]); + if (shift.is_zero()) { + return true; + } + Point64 normal = p_face->get_normal(); +#ifdef DEBUG_CONVEX_HULL + printf("\nShrinking p_face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", + p_face->origin.x, p_face->origin.y, p_face->origin.z, p_face->dir0.x, p_face->dir0.y, p_face->dir0.z, p_face->dir1.x, p_face->dir1.y, p_face->dir1.z, shift.x, shift.y, shift.z); +#endif + int64_t orig_dot = p_face->origin.dot(normal); + Point32 shifted_origin = p_face->origin + shift; + int64_t shifted_dot = shifted_origin.dot(normal); + CHULL_ASSERT(shifted_dot <= orig_dot); + if (shifted_dot >= orig_dot) { + return false; + } + + Edge *intersection = nullptr; + + Edge *start_edge = p_face->nearby_vertex->edges; +#ifdef DEBUG_CONVEX_HULL + printf("Start edge is "); + start_edge->print(); + printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shifted_dot); +#endif + Rational128 opt_dot = p_face->nearby_vertex->dot(normal); + int32_t cmp = opt_dot.compare(shifted_dot); +#ifdef SHOW_ITERATIONS + int32_t n = 0; +#endif + if (cmp >= 0) { + Edge *e = start_edge; + do { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + CHULL_ASSERT(dot.compare(orig_dot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving downwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot); +#endif + if (dot.compare(opt_dot) < 0) { + int32_t c = dot.compare(shifted_dot); + opt_dot = dot; + e = e->reverse; + start_edge = e; + if (c < 0) { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != start_edge); + + if (!intersection) { + return false; + } + } else { + Edge *e = start_edge; + do { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + CHULL_ASSERT(dot.compare(orig_dot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving upwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot); +#endif + if (dot.compare(opt_dot) > 0) { + cmp = dot.compare(shifted_dot); + if (cmp >= 0) { + intersection = e; + break; + } + opt_dot = dot; + e = e->reverse; + start_edge = e; + } + e = e->prev; + } while (e != start_edge); + + if (!intersection) { + return true; + } + } + +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find initial intersection\n", n); +#endif + + if (cmp == 0) { + Edge *e = intersection->reverse->next; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (e->target->dot(normal).compare(shifted_dot) <= 0) { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->next; + if (e == intersection->reverse) { + return true; + } +#ifdef DEBUG_CONVEX_HULL + printf("Checking for outwards edge, current edge is "); + e->print(); + printf("\n"); +#endif + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to check for complete containment\n", n); +#endif + } + + Edge *first_intersection = nullptr; + Edge *face_edge = nullptr; + Edge *first_face_edge = nullptr; + +#ifdef SHOW_ITERATIONS + int32_t m = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + m++; +#endif +#ifdef DEBUG_CONVEX_HULL + printf("Intersecting edge is "); + intersection->print(); + printf("\n"); +#endif + if (cmp == 0) { + Edge *e = intersection->reverse->next; + start_edge = e; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + n++; +#endif + if (e->target->dot(normal).compare(shifted_dot) >= 0) { + break; + } + intersection = e->reverse; + e = e->next; + if (e == start_edge) { + return true; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to advance intersection\n", n); +#endif + } + +#ifdef DEBUG_CONVEX_HULL + printf("Advanced intersecting edge to "); + intersection->print(); + printf(", cmp = %d\n", cmp); +#endif + + if (!first_intersection) { + first_intersection = intersection; + } else if (intersection == first_intersection) { + break; + } + + int32_t prev_cmp = cmp; + Edge *prev_intersection = intersection; + Edge *prev_face_edge = face_edge; + + Edge *e = intersection->reverse; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->reverse->prev; + CHULL_ASSERT(e != intersection->reverse); + cmp = e->target->dot(normal).compare(shifted_dot); +#ifdef DEBUG_CONVEX_HULL + printf("Testing edge "); + e->print(); + printf(" -> cmp = %d\n", cmp); +#endif + if (cmp >= 0) { + intersection = e; + break; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find other intersection of p_face\n", n); +#endif + + if (cmp > 0) { + Vertex *removed = intersection->target; + e = intersection->reverse; + if (e->prev == e) { + removed->edges = nullptr; + } else { + removed->edges = e->prev; + e->prev->link(e->next); + e->link(e); + } +#ifdef DEBUG_CONVEX_HULL + printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + + Point64 n0 = intersection->face->get_normal(); + Point64 n1 = intersection->reverse->face->get_normal(); + int64_t m00 = p_face->dir0.dot(n0); + int64_t m01 = p_face->dir1.dot(n0); + int64_t m10 = p_face->dir0.dot(n1); + int64_t m11 = p_face->dir1.dot(n1); + int64_t r0 = (intersection->face->origin - shifted_origin).dot(n0); + int64_t r1 = (intersection->reverse->face->origin - shifted_origin).dot(n1); + Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); + CHULL_ASSERT(det.get_sign() != 0); + Vertex *v = vertex_pool.alloc(); + v->point.index = -1; + v->copy = -1; + v->point128 = PointR128(Int128::mul(p_face->dir0.x * r0, m11) - Int128::mul(p_face->dir0.x * r1, m01) + Int128::mul(p_face->dir1.x * r1, m00) - Int128::mul(p_face->dir1.x * r0, m10) + det * shifted_origin.x, + Int128::mul(p_face->dir0.y * r0, m11) - Int128::mul(p_face->dir0.y * r1, m01) + Int128::mul(p_face->dir1.y * r1, m00) - Int128::mul(p_face->dir1.y * r0, m10) + det * shifted_origin.y, + Int128::mul(p_face->dir0.z * r0, m11) - Int128::mul(p_face->dir0.z * r1, m01) + Int128::mul(p_face->dir1.z * r1, m00) - Int128::mul(p_face->dir1.z * r0, m10) + det * shifted_origin.z, + det); + v->point.x = (int32_t)v->point128.xvalue(); + v->point.y = (int32_t)v->point128.yvalue(); + v->point.z = (int32_t)v->point128.zvalue(); + intersection->target = v; + v->edges = e; + + p_stack.push_back(v); + p_stack.push_back(removed); + p_stack.push_back(nullptr); + } + + if (cmp || prev_cmp || (prev_intersection->reverse->next->target != intersection->target)) { + face_edge = new_edge_pair(prev_intersection->target, intersection->target); + if (prev_cmp == 0) { + face_edge->link(prev_intersection->reverse->next); + } + if ((prev_cmp == 0) || prev_face_edge) { + prev_intersection->reverse->link(face_edge); + } + if (cmp == 0) { + intersection->reverse->prev->link(face_edge->reverse); + } + face_edge->reverse->link(intersection->reverse); + } else { + face_edge = prev_intersection->reverse->next; + } + + if (prev_face_edge) { + if (prev_cmp > 0) { + face_edge->link(prev_face_edge->reverse); + } else if (face_edge != prev_face_edge->reverse) { + p_stack.push_back(prev_face_edge->target); + while (face_edge->next != prev_face_edge->reverse) { + Vertex *removed = face_edge->next->target; + remove_edge_pair(face_edge->next); + p_stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + p_stack.push_back(nullptr); + } + } + face_edge->face = p_face; + face_edge->reverse->face = intersection->face; + + if (!first_face_edge) { + first_face_edge = face_edge; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to process all intersections\n", m); +#endif + + if (cmp > 0) { + first_face_edge->reverse->target = face_edge->target; + first_intersection->reverse->link(first_face_edge); + first_face_edge->link(face_edge->reverse); + } else if (first_face_edge != face_edge->reverse) { + p_stack.push_back(face_edge->target); + while (first_face_edge->next != face_edge->reverse) { + Vertex *removed = first_face_edge->next->target; + remove_edge_pair(first_face_edge->next); + p_stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + p_stack.push_back(nullptr); + } + + CHULL_ASSERT(p_stack.size() > 0); + vertex_list = p_stack[0]; + +#ifdef DEBUG_CONVEX_HULL + printf("Removing part\n"); +#endif +#ifdef SHOW_ITERATIONS + n = 0; +#endif + uint32_t pos = 0; + while (pos < p_stack.size()) { + uint32_t end = p_stack.size(); + while (pos < end) { + Vertex *kept = p_stack[pos++]; +#ifdef DEBUG_CONVEX_HULL + kept->print(); +#endif + bool deeper = false; + Vertex *removed; + while ((removed = p_stack[pos++]) != nullptr) { +#ifdef SHOW_ITERATIONS + n++; +#endif + kept->receive_nearby_faces(removed); + while (removed->edges) { + if (!deeper) { + deeper = true; + p_stack.push_back(kept); + } + p_stack.push_back(removed->edges->target); + remove_edge_pair(removed->edges); + } + } + if (deeper) { + p_stack.push_back(nullptr); + } + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to remove part\n", n); +#endif + + p_stack.resize(0); + p_face->origin = shifted_origin; + + return true; +} + +static int32_t get_vertex_copy(ConvexHullInternal::Vertex *p_vertex, LocalVector<ConvexHullInternal::Vertex *> &p_vertices) { + int32_t index = p_vertex->copy; + if (index < 0) { + index = p_vertices.size(); + p_vertex->copy = index; + p_vertices.push_back(p_vertex); +#ifdef DEBUG_CONVEX_HULL + printf("Vertex %d gets index *%d\n", p_vertex->point.index, index); +#endif + } + return index; +} + +real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp) { + if (p_count <= 0) { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + ConvexHullInternal hull; + hull.compute(p_coords, p_count); + + real_t shift = 0; + if ((p_shrink > 0) && ((shift = hull.shrink(p_shrink, p_shrink_clamp)) < 0)) { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + LocalVector<ConvexHullInternal::Vertex *> old_vertices; + get_vertex_copy(hull.vertex_list, old_vertices); + int32_t copied = 0; + while (copied < (int32_t)old_vertices.size()) { + ConvexHullInternal::Vertex *v = old_vertices[copied]; + vertices.push_back(hull.get_coordinates(v)); + ConvexHullInternal::Edge *first_edge = v->edges; + if (first_edge) { + int32_t first_copy = -1; + int32_t prev_copy = -1; + ConvexHullInternal::Edge *e = first_edge; + do { + if (e->copy < 0) { + int32_t s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge *c = &edges[s]; + Edge *r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->target_vertex = get_vertex_copy(e->target, old_vertices); + r->target_vertex = copied; +#ifdef DEBUG_CONVEX_HULL + printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->get_target_vertex()); +#endif + } + if (prev_copy >= 0) { + edges[e->copy].next = prev_copy - e->copy; + } else { + first_copy = e->copy; + } + prev_copy = e->copy; + e = e->next; + } while (e != first_edge); + edges[first_copy].next = prev_copy - first_copy; + } + copied++; + } + + for (int32_t i = 0; i < copied; i++) { + ConvexHullInternal::Vertex *v = old_vertices[i]; + ConvexHullInternal::Edge *first_edge = v->edges; + if (first_edge) { + ConvexHullInternal::Edge *e = first_edge; + do { + if (e->copy >= 0) { +#ifdef DEBUG_CONVEX_HULL + printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].get_target_vertex()); +#endif + faces.push_back(e->copy); + ConvexHullInternal::Edge *f = e; + do { +#ifdef DEBUG_CONVEX_HULL + printf(" Face *%d\n", edges[f->copy].get_target_vertex()); +#endif + f->copy = -1; + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != first_edge); + } + } + + return shift; +} + +Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) { + r_mesh = Geometry3D::MeshData(); // clear + + if (p_points.size() == 0) { + return FAILED; // matches QuickHull + } + + ConvexHullComputer ch; + ch.compute(p_points.ptr(), p_points.size(), -1.0, -1.0); + + r_mesh.vertices = ch.vertices; + + r_mesh.edges.resize(ch.edges.size()); + for (uint32_t i = 0; i < ch.edges.size(); i++) { + r_mesh.edges.write[i].a = (&ch.edges[i])->get_source_vertex(); + r_mesh.edges.write[i].b = (&ch.edges[i])->get_target_vertex(); + } + + r_mesh.faces.resize(ch.faces.size()); + for (uint32_t i = 0; i < ch.faces.size(); i++) { + const Edge *e_start = &ch.edges[ch.faces[i]]; + const Edge *e = e_start; + Geometry3D::MeshData::Face &face = r_mesh.faces.write[i]; + + do { + face.indices.push_back(e->get_target_vertex()); + + e = e->get_next_edge_of_face(); + } while (e != e_start); + + // compute normal + if (face.indices.size() >= 3) { + face.plane = Plane(r_mesh.vertices[face.indices[0]], r_mesh.vertices[face.indices[2]], r_mesh.vertices[face.indices[1]]); + } else { + WARN_PRINT("Too few vertices per face."); + } + } + + return OK; +} diff --git a/core/math/convex_hull.h b/core/math/convex_hull.h new file mode 100644 index 0000000000..ba7be9c5e8 --- /dev/null +++ b/core/math/convex_hull.h @@ -0,0 +1,112 @@ +/*************************************************************************/ +/* convex_hull.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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. */ +/*************************************************************************/ + +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_HULL_H +#define CONVEX_HULL_H + +#include "core/math/geometry_3d.h" +#include "core/math/vector3.h" +#include "core/templates/local_vector.h" +#include "core/templates/vector.h" + +/// Convex hull implementation based on Preparata and Hong +/// See http://code.google.com/p/bullet/issues/detail?id=275 +/// Ole Kniemeyer, MAXON Computer GmbH +class ConvexHullComputer { +public: + class Edge { + private: + int32_t next = 0; + int32_t reverse = 0; + int32_t target_vertex = 0; + + friend class ConvexHullComputer; + + public: + int32_t get_source_vertex() const { + return (this + reverse)->target_vertex; + } + + int32_t get_target_vertex() const { + return target_vertex; + } + + const Edge *get_next_edge_of_vertex() const // clockwise list of all edges of a vertex + { + return this + next; + } + + const Edge *get_next_edge_of_face() const // counter-clockwise list of all edges of a face + { + return (this + reverse)->get_next_edge_of_vertex(); + } + + const Edge *get_reverse_edge() const { + return this + reverse; + } + }; + + // Vertices of the output hull + Vector<Vector3> vertices; + + // Edges of the output hull + LocalVector<Edge> edges; + + // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons + LocalVector<int32_t> faces; + + /* + Compute convex hull of "count" vertices stored in "coords". + If "shrink" is positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length units + towards the center along its normal). + If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius" + is the minimum distance of a face to the center of the convex hull. + The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large + that the resulting convex hull is empty. + The output convex hull can be found in the member variables "vertices", "edges", "faces". + */ + real_t compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp); + + static Error convex_hull(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh); +}; + +#endif // CONVEX_HULL_H diff --git a/core/math/delaunay.h b/core/math/delaunay_2d.h index 29f84210d2..95064e5700 100644 --- a/core/math/delaunay.h +++ b/core/math/delaunay_2d.h @@ -1,12 +1,12 @@ /*************************************************************************/ -/* delaunay.h */ +/* delaunay_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). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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..6f7209556e --- /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-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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/io/file_access.h" +#include "core/math/aabb.h" +#include "core/math/camera_matrix.h" +#include "core/math/vector3.h" +#include "core/string/print_string.h" +#include "core/templates/local_vector.h" +#include "core/templates/oa_hash_map.h" +#include "core/templates/vector.h" +#include "core/variant/variant.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 fb89941ce4..b155412f64 100644 --- a/core/math/disjoint_set.h +++ b/core/math/disjoint_set.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,8 +31,8 @@ #ifndef DISJOINT_SET_H #define DISJOINT_SET_H -#include "core/map.h" -#include "core/vector.h" +#include "core/templates/map.h" +#include "core/templates/vector.h" /** @author Marios Staikopoulos <marios@staik.net> @@ -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) { @@ -152,4 +151,4 @@ void DisjointSet<T, C, AL>::get_members(Vector<T> &out_members, T representative } } -#endif +#endif // DISJOINT_SET_H diff --git a/core/math/dynamic_bvh.cpp b/core/math/dynamic_bvh.cpp new file mode 100644 index 0000000000..8e596f0f9d --- /dev/null +++ b/core/math/dynamic_bvh.cpp @@ -0,0 +1,439 @@ +/*************************************************************************/ +/* dynamic_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "dynamic_bvh.h" + +void DynamicBVH::_delete_node(Node *p_node) { + node_allocator.free(p_node); +} + +void DynamicBVH::_recurse_delete_node(Node *p_node) { + if (!p_node->is_leaf()) { + _recurse_delete_node(p_node->childs[0]); + _recurse_delete_node(p_node->childs[1]); + } + if (p_node == bvh_root) { + bvh_root = nullptr; + } + _delete_node(p_node); +} + +DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) { + Node *node = node_allocator.alloc(); + node->parent = p_parent; + node->data = p_data; + return (node); +} + +DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) { + Node *node = _create_node(p_parent, p_data); + node->volume = p_volume; + return node; +} + +void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) { + if (!bvh_root) { + bvh_root = p_leaf; + p_leaf->parent = nullptr; + } else { + if (!p_root->is_leaf()) { + do { + p_root = p_root->childs[p_leaf->volume.select_by_proximity( + p_root->childs[0]->volume, + p_root->childs[1]->volume)]; + } while (!p_root->is_leaf()); + } + Node *prev = p_root->parent; + Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), nullptr); + if (prev) { + prev->childs[p_root->get_index_in_parent()] = node; + node->childs[0] = p_root; + p_root->parent = node; + node->childs[1] = p_leaf; + p_leaf->parent = node; + do { + if (!prev->volume.contains(node->volume)) { + prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume); + } else { + break; + } + node = prev; + } while (nullptr != (prev = node->parent)); + } else { + node->childs[0] = p_root; + p_root->parent = node; + node->childs[1] = p_leaf; + p_leaf->parent = node; + bvh_root = node; + } + } +} + +DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) { + if (leaf == bvh_root) { + bvh_root = nullptr; + return (nullptr); + } else { + Node *parent = leaf->parent; + Node *prev = parent->parent; + Node *sibling = parent->childs[1 - leaf->get_index_in_parent()]; + if (prev) { + prev->childs[parent->get_index_in_parent()] = sibling; + sibling->parent = prev; + _delete_node(parent); + while (prev) { + const Volume pb = prev->volume; + prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume); + if (pb.is_not_equal_to(prev->volume)) { + prev = prev->parent; + } else { + break; + } + } + return (prev ? prev : bvh_root); + } else { + bvh_root = sibling; + sibling->parent = nullptr; + _delete_node(parent); + return (bvh_root); + } + } +} + +void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) { + if (p_root->is_internal() && p_depth) { + _fetch_leaves(p_root->childs[0], r_leaves, p_depth - 1); + _fetch_leaves(p_root->childs[1], r_leaves, p_depth - 1); + _delete_node(p_root); + } else { + r_leaves.push_back(p_root); + } +} + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) { + int begin = 0; + int end = p_count; + for (;;) { + while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) { + ++begin; + } + + if (begin == end) { + break; + } + + while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) { + --end; + } + + if (begin == end) { + break; + } + + // swap out of place nodes + --end; + Node *temp = leaves[begin]; + leaves[begin] = leaves[end]; + leaves[end] = temp; + ++begin; + } + + return begin; +} + +DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) { + Volume volume = leaves[0]->volume; + for (int i = 1, ni = p_count; i < ni; ++i) { + volume = volume.merge(leaves[i]->volume); + } + return (volume); +} + +void DynamicBVH::_bottom_up(Node **leaves, int p_count) { + while (p_count > 1) { + real_t minsize = Math_INF; + int minidx[2] = { -1, -1 }; + for (int i = 0; i < p_count; ++i) { + for (int j = i + 1; j < p_count; ++j) { + const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size(); + if (sz < minsize) { + minsize = sz; + minidx[0] = i; + minidx[1] = j; + } + } + } + Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] }; + Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves[minidx[1]] = leaves[p_count - 1]; + --p_count; + } +} + +DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) { + static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) }; + + ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr); + if (p_count > 1) { + if (p_count > p_bu_threshold) { + const Volume vol = _bounds(leaves, p_count); + const Vector3 org = vol.get_center(); + int partition; + int bestaxis = -1; + int bestmidp = p_count; + int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } }; + int i; + for (i = 0; i < p_count; ++i) { + const Vector3 x = leaves[i]->volume.get_center() - org; + for (int j = 0; j < 3; ++j) { + ++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0]; + } + } + for (i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { + const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1])); + if (midp < bestmidp) { + bestaxis = i; + bestmidp = midp; + } + } + } + if (bestaxis >= 0) { + partition = _split(leaves, p_count, org, axis[bestaxis]); + ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr); + } else { + partition = p_count / 2 + 1; + } + + Node *node = _create_node_with_volume(nullptr, vol, nullptr); + node->childs[0] = _top_down(&leaves[0], partition, p_bu_threshold); + node->childs[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold); + node->childs[0]->parent = node; + node->childs[1]->parent = node; + return (node); + } else { + _bottom_up(leaves, p_count); + return (leaves[0]); + } + } + return (leaves[0]); +} + +DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) { + Node *p = n->parent; + ERR_FAIL_COND_V(!n->is_internal(), nullptr); + if (p > n) { + const int i = n->get_index_in_parent(); + const int j = 1 - i; + Node *s = p->childs[j]; + Node *q = p->parent; + ERR_FAIL_COND_V(n != p->childs[i], nullptr); + if (q) { + q->childs[p->get_index_in_parent()] = n; + } else { + r = n; + } + s->parent = n; + p->parent = n; + n->parent = q; + p->childs[0] = n->childs[0]; + p->childs[1] = n->childs[1]; + n->childs[0]->parent = p; + n->childs[1]->parent = p; + n->childs[i] = p; + n->childs[j] = s; + SWAP(p->volume, n->volume); + return (p); + } + return (n); +} + +void DynamicBVH::clear() { + if (bvh_root) { + _recurse_delete_node(bvh_root); + } + lkhd = -1; + opath = 0; +} + +void DynamicBVH::optimize_bottom_up() { + if (bvh_root) { + LocalVector<Node *> leaves; + _fetch_leaves(bvh_root, leaves); + _bottom_up(&leaves[0], leaves.size()); + bvh_root = leaves[0]; + } +} + +void DynamicBVH::optimize_top_down(int bu_threshold) { + if (bvh_root) { + LocalVector<Node *> leaves; + _fetch_leaves(bvh_root, leaves); + bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold); + } +} + +void DynamicBVH::optimize_incremental(int passes) { + if (passes < 0) { + passes = total_leaves; + } + if (passes > 0) { + do { + if (!bvh_root) { + break; + } + Node *node = bvh_root; + unsigned bit = 0; + while (node->is_internal()) { + node = _node_sort(node, bvh_root)->childs[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned) * 8 - 1); + } + _update(node); + ++opath; + } while (--passes); + } +} + +DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) { + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + + Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata); + _insert_leaf(bvh_root, leaf); + ++total_leaves; + + ID id; + id.node = leaf; + + return id; +} + +void DynamicBVH::_update(Node *leaf, int lookahead) { + Node *root = _remove_leaf(leaf); + if (root) { + if (lookahead >= 0) { + for (int i = 0; (i < lookahead) && root->parent; ++i) { + root = root->parent; + } + } else { + root = bvh_root; + } + } + _insert_leaf(root, leaf); +} + +bool DynamicBVH::update(const ID &p_id, const AABB &p_box) { + ERR_FAIL_COND_V(!p_id.is_valid(), false); + Node *leaf = p_id.node; + + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + + if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) { + // noop + return false; + } + + Node *base = _remove_leaf(leaf); + if (base) { + if (lkhd >= 0) { + for (int i = 0; (i < lkhd) && base->parent; ++i) { + base = base->parent; + } + } else { + base = bvh_root; + } + } + leaf->volume = volume; + _insert_leaf(base, leaf); + return true; +} + +void DynamicBVH::remove(const ID &p_id) { + ERR_FAIL_COND(!p_id.is_valid()); + Node *leaf = p_id.node; + _remove_leaf(leaf); + _delete_node(leaf); + --total_leaves; +} + +void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) { + if (p_node->is_internal()) { + _extract_leaves(p_node->childs[0], r_elements); + _extract_leaves(p_node->childs[1], r_elements); + } else { + ID id; + id.node = p_node; + r_elements->push_back(id); + } +} + +void DynamicBVH::set_index(uint32_t p_index) { + ERR_FAIL_COND(bvh_root != nullptr); + index = p_index; +} + +uint32_t DynamicBVH::get_index() const { + return index; +} + +void DynamicBVH::get_elements(List<ID> *r_elements) { + if (bvh_root) { + _extract_leaves(bvh_root, r_elements); + } +} + +int DynamicBVH::get_leaf_count() const { + return total_leaves; +} +int DynamicBVH::get_max_depth() const { + if (bvh_root) { + int depth = 1; + int max_depth = 0; + bvh_root->get_max_depth(depth, max_depth); + return max_depth; + } else { + return 0; + } +} + +DynamicBVH::~DynamicBVH() { + clear(); +} diff --git a/core/math/dynamic_bvh.h b/core/math/dynamic_bvh.h new file mode 100644 index 0000000000..0b6286cd9d --- /dev/null +++ b/core/math/dynamic_bvh.h @@ -0,0 +1,477 @@ +/*************************************************************************/ +/* dynamic_bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 DYNAMICBVH_H +#define DYNAMICBVH_H + +#include "core/math/aabb.h" +#include "core/templates/list.h" +#include "core/templates/local_vector.h" +#include "core/templates/paged_allocator.h" +#include "core/typedefs.h" + +// Based on bullet Dbvh + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///DynamicBVH implementation by Nathanael Presson +// The DynamicBVH class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). + +class DynamicBVH { + struct Node; + +public: + struct ID { + Node *node = nullptr; + + public: + _FORCE_INLINE_ bool is_valid() const { return node != nullptr; } + }; + +private: + struct Volume { + Vector3 min, max; + + _FORCE_INLINE_ Vector3 get_center() const { return ((min + max) / 2); } + _FORCE_INLINE_ Vector3 get_length() const { return (max - min); } + + _FORCE_INLINE_ bool contains(const Volume &a) const { + return ((min.x <= a.min.x) && + (min.y <= a.min.y) && + (min.z <= a.min.z) && + (max.x >= a.max.x) && + (max.y >= a.max.y) && + (max.z >= a.max.z)); + } + + _FORCE_INLINE_ Volume merge(const Volume &b) const { + Volume r; + for (int i = 0; i < 3; ++i) { + if (min[i] < b.min[i]) { + r.min[i] = min[i]; + } else { + r.min[i] = b.min[i]; + } + if (max[i] > b.max[i]) { + r.max[i] = max[i]; + } else { + r.max[i] = b.max[i]; + } + } + return r; + } + + _FORCE_INLINE_ real_t get_size() const { + const Vector3 edges = get_length(); + return (edges.x * edges.y * edges.z + + edges.x + edges.y + edges.z); + } + + _FORCE_INLINE_ bool is_not_equal_to(const Volume &b) const { + return ((min.x != b.min.x) || + (min.y != b.min.y) || + (min.z != b.min.z) || + (max.x != b.max.x) || + (max.y != b.max.y) || + (max.z != b.max.z)); + } + + _FORCE_INLINE_ real_t get_proximity_to(const Volume &b) const { + const Vector3 d = (min + max) - (b.min + b.max); + return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z)); + } + + _FORCE_INLINE_ int select_by_proximity(const Volume &a, const Volume &b) const { + return (get_proximity_to(a) < get_proximity_to(b) ? 0 : 1); + } + + // + _FORCE_INLINE_ bool intersects(const Volume &b) const { + return ((min.x <= b.max.x) && + (max.x >= b.min.x) && + (min.y <= b.max.y) && + (max.y >= b.min.y) && + (min.z <= b.max.z) && + (max.z >= b.min.z)); + } + + _FORCE_INLINE_ bool intersects_convex(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const { + Vector3 half_extents = (max - min) * 0.5; + Vector3 ofs = min + half_extents; + + for (int i = 0; i < p_plane_count; i++) { + const Plane &p = p_planes[i]; + Vector3 point( + (p.normal.x > 0) ? -half_extents.x : half_extents.x, + (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)) { + return false; + } + } + + // Make sure all points in the shape aren't fully separated from the AABB on + // each axis. + int bad_point_counts_positive[3] = { 0 }; + 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]++; + } + if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) { + bad_point_counts_negative[k]++; + } + } + + if (bad_point_counts_negative[k] == p_point_count) { + return false; + } + if (bad_point_counts_positive[k] == p_point_count) { + return false; + } + } + + return true; + } + }; + + struct Node { + Volume volume; + Node *parent = nullptr; + union { + Node *childs[2]; + void *data; + }; + + _FORCE_INLINE_ bool is_leaf() const { return childs[1] == nullptr; } + _FORCE_INLINE_ bool is_internal() const { return (!is_leaf()); } + + _FORCE_INLINE_ int get_index_in_parent() const { + ERR_FAIL_COND_V(!parent, 0); + return (parent->childs[1] == this) ? 1 : 0; + } + void get_max_depth(int depth, int &maxdepth) { + if (is_internal()) { + childs[0]->get_max_depth(depth + 1, maxdepth); + childs[1]->get_max_depth(depth + 1, maxdepth); + } else { + maxdepth = MAX(maxdepth, depth); + } + } + + // + int count_leaves() const { + if (is_internal()) { + return childs[0]->count_leaves() + childs[1]->count_leaves(); + } else { + return (1); + } + } + + bool is_left_of_axis(const Vector3 &org, const Vector3 &axis) const { + return axis.dot(volume.get_center() - org) <= 0; + } + + Node() { + childs[0] = nullptr; + childs[1] = nullptr; + } + }; + + PagedAllocator<Node> node_allocator; + // Fields + Node *bvh_root = nullptr; + int lkhd = -1; + int total_leaves = 0; + uint32_t opath = 0; + uint32_t index = 0; + + enum { + ALLOCA_STACK_SIZE = 128 + }; + + _FORCE_INLINE_ void _delete_node(Node *p_node); + void _recurse_delete_node(Node *p_node); + _FORCE_INLINE_ Node *_create_node(Node *p_parent, void *p_data); + _FORCE_INLINE_ DynamicBVH::Node *_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data); + _FORCE_INLINE_ void _insert_leaf(Node *p_root, Node *p_leaf); + _FORCE_INLINE_ Node *_remove_leaf(Node *leaf); + void _fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth = -1); + static int _split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis); + static Volume _bounds(Node **leaves, int p_count); + void _bottom_up(Node **leaves, int p_count); + Node *_top_down(Node **leaves, int p_count, int p_bu_threshold); + Node *_node_sort(Node *n, Node *&r); + + _FORCE_INLINE_ void _update(Node *leaf, int lookahead = -1); + + void _extract_leaves(Node *p_node, List<ID> *r_elements); + + _FORCE_INLINE_ bool _ray_aabb(const Vector3 &rayFrom, const Vector3 &rayInvDirection, const unsigned int raySign[3], const Vector3 bounds[2], real_t &tmin, real_t lambda_min, real_t lambda_max) { + real_t tmax, tymin, tymax, tzmin, tzmax; + tmin = (bounds[raySign[0]].x - rayFrom.x) * rayInvDirection.x; + tmax = (bounds[1 - raySign[0]].x - rayFrom.x) * rayInvDirection.x; + tymin = (bounds[raySign[1]].y - rayFrom.y) * rayInvDirection.y; + tymax = (bounds[1 - raySign[1]].y - rayFrom.y) * rayInvDirection.y; + + if ((tmin > tymax) || (tymin > tmax)) { + return false; + } + + if (tymin > tmin) { + tmin = tymin; + } + + if (tymax < tmax) { + tmax = tymax; + } + + tzmin = (bounds[raySign[2]].z - rayFrom.z) * rayInvDirection.z; + tzmax = (bounds[1 - raySign[2]].z - rayFrom.z) * rayInvDirection.z; + + if ((tmin > tzmax) || (tzmin > tmax)) { + return false; + } + if (tzmin > tmin) { + tmin = tzmin; + } + if (tzmax < tmax) { + tmax = tzmax; + } + return ((tmin < lambda_max) && (tmax > lambda_min)); + } + +public: + // Methods + void clear(); + bool is_empty() const { return (nullptr == bvh_root); } + void optimize_bottom_up(); + void optimize_top_down(int bu_threshold = 128); + void optimize_incremental(int passes); + ID insert(const AABB &p_box, void *p_userdata); + bool update(const ID &p_id, const AABB &p_box); + void remove(const ID &p_id); + void get_elements(List<ID> *r_elements); + + int get_leaf_count() const; + int get_max_depth() const; + + /* Discouraged, but works as a reference on how it must be used */ + struct DefaultQueryResult { + virtual bool operator()(void *p_data) = 0; //return true whether you want to continue the query + virtual ~DefaultQueryResult() {} + }; + + template <class QueryResult> + _FORCE_INLINE_ void aabb_query(const AABB &p_aabb, QueryResult &r_result); + template <class QueryResult> + _FORCE_INLINE_ void convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result); + template <class QueryResult> + _FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result); + + void set_index(uint32_t p_index); + uint32_t get_index() const; + + ~DynamicBVH(); +}; + +template <class QueryResult> +void DynamicBVH::aabb_query(const AABB &p_box, QueryResult &r_result) { + if (!bvh_root) { + return; + } + + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + + const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *)); + stack[0] = bvh_root; + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + + LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time. + + do { + depth--; + const Node *n = stack[depth]; + if (n->volume.intersects(volume)) { + if (n->is_internal()) { + if (depth > threshold) { + if (aux_stack.is_empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *)); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + stack[depth++] = n->childs[0]; + stack[depth++] = n->childs[1]; + } else { + if (r_result(n->data)) { + return; + } + } + } + } while (depth > 0); +} + +template <class QueryResult> +void DynamicBVH::convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result) { + if (!bvh_root) { + return; + } + + //generate a volume anyway to improve pre-testing + Volume volume; + for (int i = 0; i < p_point_count; i++) { + if (i == 0) { + volume.min = p_points[0]; + volume.max = p_points[0]; + } else { + volume.min.x = MIN(volume.min.x, p_points[i].x); + volume.min.y = MIN(volume.min.y, p_points[i].y); + volume.min.z = MIN(volume.min.z, p_points[i].z); + + volume.max.x = MAX(volume.max.x, p_points[i].x); + volume.max.y = MAX(volume.max.y, p_points[i].y); + volume.max.z = MAX(volume.max.z, p_points[i].z); + } + } + + const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *)); + stack[0] = bvh_root; + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + + LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time. + + do { + depth--; + const Node *n = stack[depth]; + if (n->volume.intersects(volume) && n->volume.intersects_convex(p_planes, p_plane_count, p_points, p_point_count)) { + if (n->is_internal()) { + if (depth > threshold) { + if (aux_stack.is_empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *)); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + stack[depth++] = n->childs[0]; + stack[depth++] = n->childs[1]; + } else { + if (r_result(n->data)) { + return; + } + } + } + } while (depth > 0); +} +template <class QueryResult> +void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) { + if (!bvh_root) { + return; + } + + Vector3 ray_dir = (p_to - p_from); + ray_dir.normalize(); + + ///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT + Vector3 inv_dir; + inv_dir[0] = ray_dir[0] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[0]; + inv_dir[1] = ray_dir[1] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[1]; + inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2]; + unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 }; + + real_t lambda_max = ray_dir.dot(p_to - p_from); + + Vector3 bounds[2]; + + const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *)); + stack[0] = bvh_root; + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + + LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time. + + do { + depth--; + const Node *node = stack[depth]; + bounds[0] = node->volume.min; + bounds[1] = node->volume.max; + real_t tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = false; + result1 = _ray_aabb(p_from, inv_dir, signs, bounds, tmin, lambda_min, lambda_max); + if (result1) { + if (node->is_internal()) { + if (depth > threshold) { + if (aux_stack.is_empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + memcpy(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *)); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; + } else { + if (r_result(node->data)) { + return; + } + } + } + } while (depth > 0); +} + +#endif // DYNAMICBVH_H diff --git a/core/math/expression.cpp b/core/math/expression.cpp index 655098376c..0146c345f0 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,863 +30,61 @@ #include "expression.h" -#include "core/class_db.h" -#include "core/func_ref.h" #include "core/io/marshalls.h" #include "core/math/math_funcs.h" +#include "core/object/class_db.h" +#include "core/object/ref_counted.h" #include "core/os/os.h" -#include "core/reference.h" -#include "core/variant_parser.h" - -const char *Expression::func_name[Expression::FUNC_MAX] = { - "sin", - "cos", - "tan", - "sinh", - "cosh", - "tanh", - "asin", - "acos", - "atan", - "atan2", - "sqrt", - "fmod", - "fposmod", - "posmod", - "floor", - "ceil", - "round", - "abs", - "sign", - "pow", - "log", - "exp", - "is_nan", - "is_inf", - "ease", - "decimals", - "step_decimals", - "stepify", - "lerp", - "lerp_angle", - "inverse_lerp", - "range_lerp", - "smoothstep", - "move_toward", - "dectime", - "randomize", - "randi", - "randf", - "rand_range", - "seed", - "rand_seed", - "deg2rad", - "rad2deg", - "linear2db", - "db2linear", - "polar2cartesian", - "cartesian2polar", - "wrapi", - "wrapf", - "max", - "min", - "clamp", - "nearest_po2", - "weakref", - "funcref", - "convert", - "typeof", - "type_exists", - "char", - "ord", - "str", - "print", - "printerr", - "printraw", - "var2str", - "str2var", - "var2bytes", - "bytes2var", - "color_named", -}; - -Expression::BuiltinFunc Expression::find_function(const String &p_string) { - - for (int i = 0; i < FUNC_MAX; 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: - return 0; - case MATH_SIN: - case MATH_COS: - case MATH_TAN: - case MATH_SINH: - case MATH_COSH: - case MATH_TANH: - case MATH_ASIN: - case MATH_ACOS: - case MATH_ATAN: - case MATH_SQRT: - case MATH_FLOOR: - case MATH_CEIL: - case MATH_ROUND: - case MATH_ABS: - case MATH_SIGN: - case MATH_LOG: - case MATH_EXP: - case MATH_ISNAN: - case MATH_ISINF: - case MATH_DECIMALS: - case MATH_STEP_DECIMALS: - case MATH_SEED: - case MATH_RANDSEED: - case MATH_DEG2RAD: - case MATH_RAD2DEG: - case MATH_LINEAR2DB: - case MATH_DB2LINEAR: - case LOGIC_NEAREST_PO2: - case OBJ_WEAKREF: - case TYPE_OF: - case TEXT_CHAR: - case TEXT_ORD: - case TEXT_STR: - case TEXT_PRINT: - case TEXT_PRINTERR: - case TEXT_PRINTRAW: - case VAR_TO_STR: - case STR_TO_VAR: - case TYPE_EXISTS: - return 1; - case VAR_TO_BYTES: - case BYTES_TO_VAR: - case MATH_ATAN2: - case MATH_FMOD: - case MATH_FPOSMOD: - case MATH_POSMOD: - case MATH_POW: - case MATH_EASE: - case MATH_STEPIFY: - case MATH_RANDOM: - case MATH_POLAR2CARTESIAN: - case MATH_CARTESIAN2POLAR: - case LOGIC_MAX: - case LOGIC_MIN: - case FUNC_FUNCREF: - case TYPE_CONVERT: - case COLORN: - return 2; - case MATH_LERP: - case MATH_LERP_ANGLE: - case MATH_INVERSE_LERP: - case MATH_SMOOTHSTEP: - case MATH_MOVE_TOWARD: - case MATH_DECTIME: - case MATH_WRAP: - case MATH_WRAPF: - case LOGIC_CLAMP: - return 3; - case MATH_RANGE_LERP: - return 5; - case FUNC_MAX: { - } - } - return 0; -} - -#define VALIDATE_ARG_NUM(m_arg) \ - if (!p_inputs[m_arg]->is_num()) { \ - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; \ - r_error.argument = m_arg; \ - r_error.expected = Variant::REAL; \ - return; \ - } - -void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return, Variant::CallError &r_error, String &r_error_str) { - r_error.error = Variant::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::REAL) { - - real_t r = *p_inputs[0]; - *r_return = Math::abs(r); - } else { - - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::REAL; - } - } 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::REAL) { - - real_t r = *p_inputs[0]; - *r_return = r < 0.0 ? -1.0 : (r > 0.0 ? +1.0 : 0.0); - } else { - - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::REAL; - } - } 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_DECIMALS: { - - VALIDATE_ARG_NUM(0); - *r_return = Math::step_decimals((double)*p_inputs[0]); - } 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); - VALIDATE_ARG_NUM(3); - VALIDATE_ARG_NUM(4); - *r_return = Math::range_lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2], (double)*p_inputs[3], (double)*p_inputs[4]); - } break; - case MATH_SMOOTHSTEP: { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - VALIDATE_ARG_NUM(2); - *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); - *r_return = Math::dectime((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); - } break; - case MATH_RANDOMIZE: { - Math::randomize(); - - } break; - case MATH_RAND: { - *r_return = Math::rand(); - } break; - case MATH_RANDF: { - *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); - Array reta; - reta.push_back(ret); - reta.push_back(seed); - *r_return = reta; - - } 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; - case MATH_POLAR2CARTESIAN: { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - double r = *p_inputs[0]; - double th = *p_inputs[1]; - *r_return = Vector2(r * Math::cos(th), r * Math::sin(th)); - } break; - case MATH_CARTESIAN2POLAR: { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - double x = *p_inputs[0]; - double y = *p_inputs[1]; - *r_return = Vector2(Math::sqrt(x * x + y * y), Math::atan2(y, x)); - } break; - case MATH_WRAP: { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - VALIDATE_ARG_NUM(2); - *r_return = Math::wrapi((int64_t)*p_inputs[0], (int64_t)*p_inputs[1], (int64_t)*p_inputs[2]); - } break; - case MATH_WRAPF: { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - VALIDATE_ARG_NUM(2); - *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); - } else { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - - real_t a = *p_inputs[0]; - real_t b = *p_inputs[1]; - - *r_return = MAX(a, b); - } - - } 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); - } else { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - - real_t a = *p_inputs[0]; - real_t b = *p_inputs[1]; - - *r_return = MIN(a, b); - } - } 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]; - *r_return = CLAMP(a, b, c); - } else { - VALIDATE_ARG_NUM(0); - VALIDATE_ARG_NUM(1); - VALIDATE_ARG_NUM(2); - - real_t a = *p_inputs[0]; - real_t b = *p_inputs[1]; - real_t c = *p_inputs[2]; - - *r_return = CLAMP(a, b, c); - } - } 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 = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::OBJECT; - - return; - } - - if (p_inputs[0]->is_ref()) { - - REF r = *p_inputs[0]; - if (!r.is_valid()) { - - return; - } - - Ref<WeakRef> wref = memnew(WeakRef); - wref->set_ref(r); - *r_return = wref; - } else { - Object *obj = *p_inputs[0]; - if (!obj) { - - return; - } - Ref<WeakRef> wref = memnew(WeakRef); - wref->set_obj(obj); - *r_return = wref; - } - - } break; - case FUNC_FUNCREF: { - - if (p_inputs[0]->get_type() != Variant::OBJECT) { - - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::OBJECT; - - return; - } - if (p_inputs[1]->get_type() != Variant::STRING && p_inputs[1]->get_type() != Variant::NODE_PATH) { - - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 1; - r_error.expected = Variant::STRING; - - return; - } - - Ref<FuncRef> fr = memnew(FuncRef); - - fr->set_instance(*p_inputs[0]); - fr->set_function(*p_inputs[1]); - - *r_return = fr; - - } 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 = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::INT; - 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 = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::STRING; - - return; - } - - String str = *p_inputs[0]; - - if (str.length() != 1) { - - r_error_str = RTR("Expected a string of length 1 (a character)."); - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::STRING; - - return; - } - - *r_return = str.get(0); - - } 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 = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::STRING; - - return; - } - - VariantParser::StreamString ss; - ss.s = *p_inputs[0]; +#include "core/variant/variant_parser.h" - String errs; - int line; - Error err = VariantParser::parse(&ss, *r_return, errs, line); - - if (err != OK) { - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::STRING; - *r_return = "Parse error at line " + itos(line) + ": " + errs; - return; - } - - } break; - case VAR_TO_BYTES: { - - PoolByteArray barr; - bool full_objects = *p_inputs[1]; - int len; - Error err = encode_variant(*p_inputs[0], NULL, len, full_objects); - if (err) { - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::NIL; - r_error_str = "Unexpected error encoding variable to bytes, likely unserializable type found (Object or RID)."; - return; - } - - barr.resize(len); - { - PoolByteArray::Write w = barr.write(); - encode_variant(*p_inputs[0], w.ptr(), len, full_objects); - } - *r_return = barr; - } break; - case BYTES_TO_VAR: { - - if (p_inputs[0]->get_type() != Variant::POOL_BYTE_ARRAY) { - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::POOL_BYTE_ARRAY; - - return; - } - - PoolByteArray varr = *p_inputs[0]; - bool allow_objects = *p_inputs[1]; - Variant ret; - { - PoolByteArray::Read r = varr.read(); - Error err = decode_variant(ret, r.ptr(), varr.size(), NULL, allow_objects); - if (err != OK) { - r_error_str = RTR("Not enough bytes for decoding bytes, or invalid format."); - r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; - r_error.argument = 0; - r_error.expected = Variant::POOL_BYTE_ARRAY; - return; - } - } - - *r_return = ret; - - } break; - case COLORN: { - - VALIDATE_ARG_NUM(1); - - Color color = Color::named(*p_inputs[0]); - color.a = *p_inputs[1]; - - *r_return = String(color); - - } break; - default: { - } - } -} - -//////// - -static bool _is_number(CharType c) { +static bool _is_number(char32_t c) { return (c >= '0' && c <= '9'); } Error Expression::_get_token(Token &r_token) { - while (true) { #define GET_CHAR() (str_ofs >= expression.length() ? 0 : expression[str_ofs++]) - CharType cchar = GET_CHAR(); + char32_t 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 { @@ -903,9 +101,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; @@ -915,9 +112,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++; @@ -925,9 +121,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++; @@ -938,9 +133,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++; @@ -951,29 +145,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++; @@ -981,9 +174,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++; @@ -991,55 +183,61 @@ 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(); + char32_t 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... - CharType next = GET_CHAR(); + char32_t next = GET_CHAR(); if (next == 0) { _set_error("Unterminated String"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; } - CharType res = 0; + char32_t 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': { - //hexnumbarh - oct is deprecated - + // hex number for (int j = 0; j < 4; j++) { - CharType c = GET_CHAR(); + char32_t c = GET_CHAR(); if (c == 0) { _set_error("Unterminated String"); @@ -1047,12 +245,11 @@ 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; } - CharType v; + char32_t v; if (_is_number(c)) { v = c - '0'; } else if (c >= 'a' && c <= 'f') { @@ -1062,7 +259,7 @@ Error Expression::_get_token(Token &r_token) { v = c - 'A'; v += 10; } else { - ERR_PRINT("BUG"); + ERR_PRINT("Bug parsing hex constant."); v = 0; } @@ -1071,13 +268,8 @@ Error Expression::_get_token(Token &r_token) { } } break; - //case '\"': res='\"'; break; - //case '\\': res='\\'; break; - //case '/': res='/'; break; default: { res = next; - //r_err_str="Invalid escape sequence"; - //return ERR_PARSE_ERROR; } break; } @@ -1094,12 +286,11 @@ Error Expression::_get_token(Token &r_token) { } break; default: { - if (cchar <= 32) { break; } - CharType next_char = (str_ofs >= expression.length()) ? 0 : expression[str_ofs]; + char32_t next_char = (str_ofs >= expression.length()) ? 0 : expression[str_ofs]; if (_is_number(cchar) || (cchar == '.' && _is_number(next_char))) { //a number @@ -1111,16 +302,14 @@ Error Expression::_get_token(Token &r_token) { #define READING_DONE 4 int reading = READING_INT; - CharType c = cchar; + char32_t c = cchar; bool exp_sign = false; bool exp_beg = false; bool is_float = false; while (true) { - switch (reading) { case READING_INT: { - if (_is_number(c)) { //pass } else if (c == '.') { @@ -1134,9 +323,7 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_DEC: { - if (_is_number(c)) { - } else if (c == 'e') { reading = READING_EXP; @@ -1146,13 +333,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 { @@ -1161,8 +348,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(); } @@ -1171,19 +359,18 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_CONSTANT; - if (is_float) - r_token.value = num.to_double(); - else + if (is_float) { + r_token.value = num.to_float(); + } 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; @@ -1223,19 +410,9 @@ 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; - r_token.value = i; - return OK; - } - } - - BuiltinFunc bifunc = find_function(id); - if (bifunc != FUNC_MAX) { + if (Variant::has_utility_function(id)) { r_token.type = TK_BUILTIN_FUNC; - r_token.value = bifunc; + r_token.value = id; return OK; } @@ -1306,17 +483,17 @@ const char *Expression::token_name[TK_MAX] = { }; Expression::ENode *Expression::_parse_expression() { - Vector<ExpressionNode> expression; while (true) { //keep appending stuff to expression - ENode *expr = NULL; + ENode *expr = nullptr; Token tk; _get_token(tk); - if (error_set) - return NULL; + if (error_set) { + return nullptr; + } switch (tk.type) { case TK_CURLY_BRACKET_OPEN: { @@ -1324,7 +501,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) { @@ -1333,19 +509,21 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } dn->dict.push_back(subexpr); _get_token(tk); if (tk.type != TK_COLON) { _set_error("Expected ':'"); - return NULL; + return nullptr; } subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } dn->dict.push_back(subexpr); @@ -1368,7 +546,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) { @@ -1377,8 +554,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } an->array.push_back(subexpr); cofs = str_ofs; @@ -1397,19 +575,19 @@ Expression::ENode *Expression::_parse_expression() { case TK_PARENTHESIS_OPEN: { //a suexpression ENode *e = _parse_expression(); - if (error_set) - return NULL; + if (error_set) { + return nullptr; + } _get_token(tk); if (tk.type != TK_PARENTHESIS_CLOSE) { _set_error("Expected ')'"); - return NULL; + return nullptr; } expr = e; } break; case TK_IDENTIFIER: { - String identifier = tk.value; int cofs = str_ofs; @@ -1422,7 +600,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) { @@ -1431,8 +608,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs2; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } func_call->arguments.push_back(subexpr); @@ -1465,7 +643,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; @@ -1475,13 +652,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; @@ -1497,14 +672,13 @@ Expression::ENode *Expression::_parse_expression() { _get_token(tk); if (tk.type != TK_PARENTHESIS_OPEN) { _set_error("Expected '('"); - return NULL; + return nullptr; } ConstructorNode *constructor = alloc_node<ConstructorNode>(); constructor->data_type = bt; while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1513,8 +687,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } constructor->arguments.push_back(subexpr); @@ -1535,17 +710,18 @@ Expression::ENode *Expression::_parse_expression() { case TK_BUILTIN_FUNC: { //builtin function + StringName func = tk.value; + _get_token(tk); if (tk.type != TK_PARENTHESIS_OPEN) { _set_error("Expected '('"); - return NULL; + return nullptr; } BuiltinFuncNode *bifunc = alloc_node<BuiltinFuncNode>(); - bifunc->func = BuiltinFunc(int(tk.value)); + bifunc->func = func; while (true) { - int cofs = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { @@ -1554,8 +730,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } bifunc->arguments.push_back(subexpr); @@ -1570,16 +747,17 @@ Expression::ENode *Expression::_parse_expression() { } } - int expected_args = get_func_argument_count(bifunc->func); - if (bifunc->arguments.size() != expected_args) { - _set_error("Builtin func '" + get_func_name(bifunc->func) + "' expects " + itos(expected_args) + " arguments."); + if (!Variant::is_utility_function_vararg(bifunc->func)) { + int expected_args = Variant::get_utility_function_argument_count(bifunc->func); + if (expected_args != bifunc->arguments.size()) { + _set_error("Builtin func '" + String(bifunc->func) + "' expects " + itos(expected_args) + " arguments."); + } } expr = bifunc; } break; case TK_OP_SUB: { - ExpressionNode e; e.is_op = true; e.op = Variant::OP_NEGATE; @@ -1587,7 +765,6 @@ Expression::ENode *Expression::_parse_expression() { continue; } break; case TK_OP_NOT: { - ExpressionNode e; e.is_op = true; e.op = Variant::OP_NOT; @@ -1597,7 +774,7 @@ Expression::ENode *Expression::_parse_expression() { default: { _set_error("Expected expression."); - return NULL; + return nullptr; } break; } @@ -1606,8 +783,9 @@ Expression::ENode *Expression::_parse_expression() { while (true) { int cofs2 = str_ofs; _get_token(tk); - if (error_set) - return NULL; + if (error_set) { + return nullptr; + } bool done = false; @@ -1619,15 +797,16 @@ Expression::ENode *Expression::_parse_expression() { index->base = expr; ENode *what = _parse_expression(); - if (!what) - return NULL; + if (!what) { + return nullptr; + } index->index = what; _get_token(tk); if (tk.type != TK_BRACKET_CLOSE) { _set_error("Expected ']' at end of index."); - return NULL; + return nullptr; } expr = index; @@ -1637,7 +816,7 @@ Expression::ENode *Expression::_parse_expression() { _get_token(tk); if (tk.type != TK_IDENTIFIER) { _set_error("Expected identifier after '.'"); - return NULL; + return nullptr; } StringName identifier = tk.value; @@ -1651,7 +830,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) { @@ -1660,8 +838,9 @@ Expression::ENode *Expression::_parse_expression() { str_ofs = cofs3; //revert //parse an expression ENode *subexpr = _parse_expression(); - if (!subexpr) - return NULL; + if (!subexpr) { + return nullptr; + } func_call->arguments.push_back(subexpr); @@ -1694,8 +873,9 @@ Expression::ENode *Expression::_parse_expression() { } break; } - if (done) + if (done) { break; + } } //push expression @@ -1710,35 +890,78 @@ Expression::ENode *Expression::_parse_expression() { int cofs = str_ofs; _get_token(tk); - if (error_set) - return NULL; + 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 @@ -1755,18 +978,15 @@ Expression::ENode *Expression::_parse_expression() { } } - /* Reduce the set set of expressions and place them in an operator tree, respecting precedence */ + /* Reduce the 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; } @@ -1775,7 +995,6 @@ Expression::ENode *Expression::_parse_expression() { bool unary = false; switch (expression[i].op) { - case Variant::OP_BIT_NEGATE: priority = 0; unary = true; @@ -1784,41 +1003,52 @@ Expression::ENode *Expression::_parse_expression() { priority = 1; 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_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_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_EQUAL: priority = 8; break; - case Variant::OP_NOT_EQUAL: priority = 8; break; - - case Variant::OP_IN: priority = 10; break; - + case Variant::OP_MULTIPLY: + case Variant::OP_DIVIDE: + case Variant::OP_MODULE: + priority = 2; + break; + case Variant::OP_ADD: + case Variant::OP_SUBTRACT: + priority = 3; + break; + case Variant::OP_SHIFT_LEFT: + 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_LESS: + case Variant::OP_LESS_EQUAL: + case Variant::OP_GREATER: + case Variant::OP_GREATER_EQUAL: + case Variant::OP_EQUAL: + case Variant::OP_NOT_EQUAL: + priority = 8; + 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)); - return NULL; + return nullptr; } } @@ -1833,51 +1063,45 @@ Expression::ENode *Expression::_parse_expression() { } if (next_op == -1) { - _set_error("Yet another parser bug...."); - ERR_FAIL_V(NULL); + 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.. _set_error("Unexpected end of expression..."); - return NULL; + return nullptr; } } - //consecutively do unary opeators + //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; - op->nodes[1] = NULL; + op->nodes[1] = nullptr; expression.write[i].is_op = false; expression.write[i].node = op; expression.remove(i + 1); } } else { - if (next_op < 1 || next_op >= (expression.size() - 1)) { _set_error("Parser bug..."); - ERR_FAIL_V(NULL); + ERR_FAIL_V(nullptr); } OperatorNode *op = alloc_node<OperatorNode>(); op->op = expression[next_op].op; if (expression[next_op - 1].is_op) { - _set_error("Parser bug..."); - ERR_FAIL_V(NULL); + ERR_FAIL_V(nullptr); } if (expression[next_op + 1].is_op) { @@ -1887,7 +1111,7 @@ Expression::ENode *Expression::_parse_expression() { // due to how precedence works, unaries will always disappear first _set_error("Unexpected two consecutive operators."); - return NULL; + return nullptr; } op->nodes[0] = expression[next_op - 1].node; //expression goes as left @@ -1904,14 +1128,14 @@ Expression::ENode *Expression::_parse_expression() { } bool Expression::_compile_expression() { - - if (!expression_dirty) + if (!expression_dirty) { return error_set; + } if (nodes) { memdelete(nodes); - nodes = NULL; - root = NULL; + nodes = nullptr; + root = nullptr; } error_str = String(); @@ -1921,11 +1145,11 @@ bool Expression::_compile_expression() { root = _parse_expression(); if (error_set) { - root = NULL; + root = nullptr; if (nodes) { memdelete(nodes); } - nodes = NULL; + nodes = nullptr; return true; } @@ -1934,10 +1158,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); @@ -1946,13 +1168,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; @@ -1960,20 +1180,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; @@ -1985,19 +1206,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); @@ -2008,16 +1230,16 @@ 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); + r_ret = base.get_named(index->name, valid); if (!valid) { r_error_str = vformat(RTR("Invalid named index '%s' for base type %s"), String(index->name), Variant::get_type_name(base.get_type())); return true; @@ -2030,12 +1252,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; } @@ -2047,17 +1269,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; } @@ -2065,7 +1288,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; @@ -2074,27 +1296,26 @@ 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]; } - Variant::CallError ce; - r_ret = Variant::construct(constructor->data_type, (const Variant **)argp.ptr(), argp.size(), ce); + Callable::CallError ce; + Variant::construct(constructor->data_type, r_ret, (const Variant **)argp.ptr(), argp.size(), ce); - if (ce.error != Variant::CallError::CALL_OK) { + if (ce.error != Callable::CallError::CALL_OK) { r_error_str = vformat(RTR("Invalid arguments to construct '%s'"), Variant::get_type_name(constructor->data_type)); return true; } } break; case Expression::ENode::TYPE_BUILTIN_FUNC: { - const Expression::BuiltinFuncNode *bifunc = static_cast<const Expression::BuiltinFuncNode *>(p_node); Vector<Variant> arr; @@ -2103,33 +1324,33 @@ 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]; } - Variant::CallError ce; - exec_func(bifunc->func, (const Variant **)argp.ptr(), &r_ret, ce, r_error_str); - - if (ce.error != Variant::CallError::CALL_OK) { - r_error_str = "Builtin Call Failed. " + r_error_str; + r_ret = Variant(); //may not return anything + Callable::CallError ce; + Variant::call_utility_function(bifunc->func, &r_ret, (const Variant **)argp.ptr(), argp.size(), ce); + if (ce.error != Callable::CallError::CALL_OK) { + r_error_str = "Builtin Call Failed. " + Variant::get_call_error_text(bifunc->func, (const Variant **)argp.ptr(), argp.size(), ce); return true; } } 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; @@ -2137,20 +1358,20 @@ 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]; } - Variant::CallError ce; - r_ret = base.call(call->method, (const Variant **)argp.ptr(), argp.size(), ce); + Callable::CallError ce; + base.call(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce); - if (ce.error != Variant::CallError::CALL_OK) { + if (ce.error != Callable::CallError::CALL_OK) { r_error_str = vformat(RTR("On call to '%s':"), String(call->method)); return true; } @@ -2161,11 +1382,10 @@ 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 = NULL; - root = NULL; + nodes = nullptr; + root = nullptr; } error_str = String(); @@ -2177,11 +1397,11 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu root = _parse_expression(); if (error_set) { - root = NULL; + root = nullptr; if (nodes) { memdelete(nodes); } - nodes = NULL; + nodes = nullptr; return ERR_INVALID_PARAMETER; } @@ -2189,7 +1409,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; @@ -2214,24 +1433,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(NULL), - nodes(NULL), - execution_error(false) { -} - Expression::~Expression() { - if (nodes) { memdelete(nodes); } diff --git a/core/math/expression.h b/core/math/expression.h index c5b9d79a16..aecf662d0a 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,111 +31,27 @@ #ifndef EXPRESSION_H #define EXPRESSION_H -#include "core/reference.h" +#include "core/object/ref_counted.h" -class Expression : public Reference { - GDCLASS(Expression, Reference); - -public: - enum BuiltinFunc { - MATH_SIN, - MATH_COS, - MATH_TAN, - MATH_SINH, - MATH_COSH, - MATH_TANH, - MATH_ASIN, - MATH_ACOS, - MATH_ATAN, - MATH_ATAN2, - MATH_SQRT, - MATH_FMOD, - MATH_FPOSMOD, - MATH_POSMOD, - MATH_FLOOR, - MATH_CEIL, - MATH_ROUND, - MATH_ABS, - MATH_SIGN, - MATH_POW, - MATH_LOG, - MATH_EXP, - MATH_ISNAN, - MATH_ISINF, - MATH_EASE, - MATH_DECIMALS, - MATH_STEP_DECIMALS, - MATH_STEPIFY, - MATH_LERP, - MATH_LERP_ANGLE, - MATH_INVERSE_LERP, - MATH_RANGE_LERP, - MATH_SMOOTHSTEP, - MATH_MOVE_TOWARD, - MATH_DECTIME, - MATH_RANDOMIZE, - MATH_RAND, - MATH_RANDF, - MATH_RANDOM, - MATH_SEED, - MATH_RANDSEED, - MATH_DEG2RAD, - MATH_RAD2DEG, - MATH_LINEAR2DB, - MATH_DB2LINEAR, - MATH_POLAR2CARTESIAN, - MATH_CARTESIAN2POLAR, - MATH_WRAP, - MATH_WRAPF, - LOGIC_MAX, - LOGIC_MIN, - LOGIC_CLAMP, - LOGIC_NEAREST_PO2, - OBJ_WEAKREF, - FUNC_FUNCREF, - TYPE_CONVERT, - TYPE_OF, - TYPE_EXISTS, - TEXT_CHAR, - TEXT_ORD, - TEXT_STR, - TEXT_PRINT, - TEXT_PRINTERR, - TEXT_PRINTRAW, - VAR_TO_STR, - STR_TO_VAR, - VAR_TO_BYTES, - BYTES_TO_VAR, - COLORN, - FUNC_MAX - }; - - static int get_func_argument_count(BuiltinFunc p_func); - static String get_func_name(BuiltinFunc p_func); - static void exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return, Variant::CallError &r_error, String &r_error_str); - static BuiltinFunc find_function(const String &p_string); +class Expression : public RefCounted { + GDCLASS(Expression, RefCounted); 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(); @@ -183,14 +99,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; } @@ -198,10 +114,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, @@ -216,11 +131,11 @@ private: TYPE_CALL }; - ENode *next; + ENode *next = nullptr; - Type type; + Type type = TYPE_INPUT; - ENode() { next = NULL; } + ENode() {} virtual ~ENode() { if (next) { memdelete(next); @@ -229,8 +144,7 @@ private: }; struct ExpressionNode { - - bool is_op; + bool is_op = false; union { Variant::Operator op; ENode *node; @@ -240,26 +154,23 @@ private: ENode *_parse_expression(); struct InputNode : public ENode { - - int index; + int index = 0; InputNode() { type = TYPE_INPUT; } }; struct ConstantNode : public ENode { - - Variant value; + Variant value = Variant::NIL; ConstantNode() { type = TYPE_CONSTANT; } }; struct OperatorNode : public ENode { + Variant::Operator op = Variant::Operator::OP_ADD; - Variant::Operator op; - - ENode *nodes[2]; + ENode *nodes[2] = { nullptr, nullptr }; OperatorNode() { type = TYPE_OPERATOR; @@ -267,15 +178,14 @@ private: }; struct SelfNode : public ENode { - SelfNode() { type = TYPE_SELF; } }; struct IndexNode : public ENode { - ENode *base; - ENode *index; + ENode *base = nullptr; + ENode *index = nullptr; IndexNode() { type = TYPE_INDEX; @@ -283,7 +193,7 @@ private: }; struct NamedIndexNode : public ENode { - ENode *base; + ENode *base = nullptr; StringName name; NamedIndexNode() { @@ -292,7 +202,7 @@ private: }; struct ConstructorNode : public ENode { - Variant::Type data_type; + Variant::Type data_type = Variant::Type::NIL; Vector<ENode *> arguments; ConstructorNode() { @@ -301,7 +211,7 @@ private: }; struct CallNode : public ENode { - ENode *base; + ENode *base = nullptr; StringName method; Vector<ENode *> arguments; @@ -325,7 +235,7 @@ private: }; struct BuiltinFuncNode : public ENode { - BuiltinFunc func; + StringName func; Vector<ENode *> arguments; BuiltinFuncNode() { type = TYPE_BUILTIN_FUNC; @@ -340,12 +250,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: @@ -353,11 +263,11 @@ protected: public: Error parse(const String &p_expression, const Vector<String> &p_input_names = Vector<String>()); - Variant execute(Array p_inputs, Object *p_base = NULL, bool p_show_error = true); + Variant execute(Array p_inputs = Array(), Object *p_base = nullptr, bool p_show_error = true); 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..9af3f868d2 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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(); + return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length() * 0.5; } 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); + project_range(axis, Transform3D(), 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 { - +void Face3::project_range(const Vector3 &p_normal, const Transform3D &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 { - +void Face3::get_support(const Vector3 &p_normal, const Transform3D &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..5091b338ef 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -33,7 +33,7 @@ #include "core/math/aabb.h" #include "core/math/plane.h" -#include "core/math/transform.h" +#include "core/math/transform_3d.h" #include "core/math/vector3.h" class Face3 { @@ -69,16 +69,15 @@ 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 - void get_support(const Vector3 &p_normal, const Transform &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const; - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + void get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const; + void project_range(const Vector3 &p_normal, const Transform3D &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.h b/core/math/geometry.h deleted file mode 100644 index b9193242bc..0000000000 --- a/core/math/geometry.h +++ /dev/null @@ -1,1022 +0,0 @@ -/*************************************************************************/ -/* geometry.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_H -#define GEOMETRY_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/pool_vector.h" -#include "core/print_string.h" -#include "core/vector.h" - -class Geometry { - Geometry(); - -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)) - - // Calculate the parametric position on the 2 curves, mua and mub. - real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); - 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; - c1 = p1.linear_interpolate(p2, mua); - c2 = q1.linear_interpolate(q2, mub); - } - - static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { - Vector3 u = p_to_a - p_from_a; - Vector3 v = p_to_b - p_from_b; - Vector3 w = p_from_a - p_to_a; - real_t a = u.dot(u); // Always >= 0 - real_t b = u.dot(v); - real_t c = v.dot(v); // Always >= 0 - real_t d = u.dot(w); - real_t e = v.dot(w); - real_t D = a * c - b * b; // Always >= 0 - real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 - real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - - // Compute the line parameters of the two closest points. - if (D < CMP_EPSILON) { // The lines are almost parallel. - sN = 0.0; // Force using point P0 on segment S1 - sD = 1.0; // to prevent possible division by 0.0 later. - tN = e; - tD = c; - } else { // Get the closest points on the infinite lines - sN = (b * e - c * d); - tN = (a * e - b * d); - if (sN < 0.0) { // sc < 0 => the s=0 edge is visible. - sN = 0.0; - tN = e; - tD = c; - } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. - tN = 0.0; - // Recompute sc for this edge. - if (-d < 0.0) - sN = 0.0; - else if (-d > a) - sN = sD; - 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) - sN = 0; - else if ((-d + b) > a) - sN = sD; - else { - sN = (-d + b); - sD = a; - } - } - // Finally do the division to get sc and tc. - sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD); - tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD); - - // Get the difference of the two closest points. - Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - - 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) { - 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. - 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) - return false; - - Vector3 q = s.cross(e1); - - real_t v = f * p_dir.dot(q); - - 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) - *r_res = p_from + p_dir * t; - return true; - } 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) { - - 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. - 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) - return false; - - Vector3 q = s.cross(e1); - - real_t v = f * rel.dot(q); - - 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) - *r_res = p_from + rel * t; - return true; - } 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) { - - 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) - 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) - 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) - inters_d -= Math::sqrt(inters_d2); - - // Check in segment. - if (inters_d < 0 || inters_d > rel_l) - return false; - - Vector3 result = p_from + normal * inters_d; - - if (r_res) - *r_res = result; - 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) { - - Vector3 rel = (p_to - p_from); - real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) - return false; // Both points are the same. - - // First check if they are parallel. - Vector3 normal = (rel / rel_l); - Vector3 crs = normal.cross(Vector3(0, 0, 1)); - real_t crs_l = crs.length(); - - Vector3 z_dir; - - if (crs_l < CMP_EPSILON) { - z_dir = Vector3(1, 0, 0); // Any x/y vector OK. - } else { - z_dir = crs / crs_l; - } - - real_t dist = z_dir.dot(p_from); - - 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) - 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(); - - Vector2 from2D(x_dir.dot(p_from), p_from.z); - Vector2 to2D(x_dir.dot(p_to), p_to.z); - - real_t min = 0, max = 1; - - 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]; - real_t box_end = size[i]; - real_t cmin, cmax; - - if (seg_from < seg_to) { - - 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) - 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; - } - - if (cmin > min) { - min = cmin; - axis = i; - } - if (cmax < max) - max = cmax; - if (max < min) - return false; - } - - // Convert to 3D again. - Vector3 result = p_from + (rel * min); - Vector3 res_normal = result; - - if (axis == 0) { - res_normal.z = 0; - } else { - res_normal.x = 0; - res_normal.y = 0; - } - - res_normal.normalize(); - - if (r_res) - *r_res = result; - 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) - 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) - continue; // Ignore parallel plane. - - real_t dist = -p.distance_to(p_from) / den; - - if (den > 0) { - // Backwards facing plane. - if (dist < max) - max = dist; - } else { - - // Front facing plane. - if (dist > min) { - min = dist; - min_index = i; - } - } - } - - if (max <= min || min < 0 || min > rel_l || min_index == -1) // Exit conditions. - return false; // No intersection. - - if (p_res) - *p_res = p_from + dir * min; - 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) - 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 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) - 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) - return false; - - Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point); - - 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) - 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. - 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])) { - r_triangle_contact = contact; - r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius; - //printf("solved inside triangle\n"); - return true; - } - - /** 3rd TEST INSIDE EDGE CYLINDERS **/ - - 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]; - Vector3 n2 = p_sphere_pos - verts[i + 1]; - - ///@TODO Maybe discard by range here to make the algorithm quicker. - - // Check point within cylinder radius. - Vector3 axis = n1.cross(n2).cross(n1); - axis.normalize(); - - real_t ad = axis.dot(n2); - - if (ABS(ad) > p_sphere_radius) { - // No chance with this edge, too far away. - continue; - } - - // Check point within edge capsule cylinder. - /** 4th TEST INSIDE EDGE POINTS **/ - - 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. - return true; - } - - 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]; - r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - return true; - } - - if (n2.distance_squared_to(n1) < r2) { - Vector3 n = (p_sphere_pos - verts[i]).normalized(); - - r_triangle_contact = verts[i]; - r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - return true; - } - - break; // It's pointless to continue at this point, so save some CPU cycles. - } - - 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) - return polygon; - - int *location_cache = (int *)alloca(sizeof(int) * polygon.size()); - int inside_count = 0; - int outside_count = 0; - - for (int a = 0; a < polygon.size(); a++) { - real_t dist = p_plane.distance_to(polygon[a]); - if (dist < -CMP_POINT_IN_PLANE_EPSILON) { - location_cache[a] = LOC_INSIDE; - inside_count++; - } else { - if (dist > CMP_POINT_IN_PLANE_EPSILON) { - location_cache[a] = LOC_OUTSIDE; - outside_count++; - } else { - location_cache[a] = LOC_BOUNDARY; - } - } - } - - if (outside_count == 0) { - - return polygon; // No changes. - - } else if (inside_count == 0) { - - return Vector<Vector3>(); // Empty. - } - - long previous = polygon.size() - 1; - Vector<Vector3> clipped; - - for (int index = 0; index < polygon.size(); index++) { - int loc = location_cache[index]; - if (loc == LOC_OUTSIDE) { - if (location_cache[previous] == LOC_INSIDE) { - const Vector3 &v1 = polygon[previous]; - const Vector3 &v2 = polygon[index]; - - Vector3 segment = v1 - v2; - real_t den = p_plane.normal.dot(segment); - real_t dist = p_plane.distance_to(v1) / den; - dist = -dist; - clipped.push_back(v1 + segment * dist); - } - } else { - const Vector3 &v1 = polygon[index]; - if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) { - const Vector3 &v2 = polygon[previous]; - Vector3 segment = v1 - v2; - real_t den = p_plane.normal.dot(segment); - real_t dist = p_plane.distance_to(v1) / den; - dist = -dist; - clipped.push_back(v1 + segment * dist); - } - - clipped.push_back(v1); - } - - previous = index; - } - - 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, NULL)) { - intersections++; - } - } - - return (intersections & 1); - } - - static PoolVector<PoolVector<Face3> > separate_objects(PoolVector<Face3> p_array); - - // Create a "wrap" that encloses the given geometry. - static PoolVector<Face3> wrap_geometry(PoolVector<Face3> p_array, real_t *p_error = NULL); - - struct MeshData { - - struct Face { - Plane plane; - Vector<int> indices; - }; - - Vector<Face> faces; - - struct Edge { - - int a, b; - }; - - Vector<Edge> edges; - - Vector<Vector3> vertices; - - void optimize_vertices(); - }; - - _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) { - return 24; - } else if (lat == 4) { - return 25; - } - - int lon = Math::fast_ftoi(Math::floor((Math_PI + Math::atan2(p_vector.x, p_vector.z)) * 8.0 / (Math_PI * 2.0) + 0.5)) % 8; - - return lon + (lat - 1) * 8; - } - - _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) - ret |= (1 << (p_idx + 7)); - else - ret |= (1 << (p_idx - 1)); - if ((p_idx % 8) == 7) - ret |= (1 << (p_idx - 7)); - else - ret |= (1 << (p_idx + 1)); - - int mask = ret | (1 << p_idx); - if (p_idx < 8) - ret |= 24; - else - ret |= mask >> 8; - - if (p_idx >= 16) - ret |= 25; - else - ret |= mask << 8; - - return ret; - } - } - - 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_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]; - } - - // 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 MeshData build_convex_mesh(const PoolVector<Plane> &p_planes); - static PoolVector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); - static PoolVector<Plane> build_box_planes(const Vector3 &p_extents); - static PoolVector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z); - static PoolVector<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 void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_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 diff --git a/core/math/geometry_2d.cpp b/core/math/geometry_2d.cpp new file mode 100644 index 0000000000..7b2630b4ff --- /dev/null +++ b/core/math/geometry_2d.cpp @@ -0,0 +1,388 @@ +/*************************************************************************/ +/* geometry_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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/polypartition.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<TPPLPoly> in_poly, out_poly; + + TPPLPoly inp; + inp.Init(polygon.size()); + for (int i = 0; i < polygon.size(); i++) { + inp.GetPoint(i) = polygon[i]; + } + inp.SetOrientation(TPPL_ORIENTATION_CCW); + in_poly.push_back(inp); + TPPLPartition 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<TPPLPoly>::Element *I = out_poly.front(); I; I = I->next()) { + TPPLPoly &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 squarer a texture, the more the chances + // that 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); + for (int i = 0; i < p_rects.size(); i++) { + ERR_FAIL_COND(p_rects[i].width <= 0); + ERR_FAIL_COND(p_rects[i].height <= 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); + memset(nodes.ptrw(), 0, 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..a2894bc1d3 --- /dev/null +++ b/core/math/geometry_2d.h @@ -0,0 +1,461 @@ +/*************************************************************************/ +/* geometry_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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/object.h" +#include "core/templates/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. + } + +// Disable False Positives in MSVC compiler; we correctly check for 0 here to prevent a division by 0. +// See: https://github.com/godotengine/godot/pull/44274 +#ifdef _MSC_VER +#pragma warning(disable : 4723) +#endif + + 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; + } + +// Re-enable division by 0 warning +#ifdef _MSC_VER +#pragma warning(default : 4723) +#endif + + 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 bool is_segment_intersecting_polygon(const Vector2 &p_from, const Vector2 &p_to, const Vector<Vector2> &p_polygon) { + int c = p_polygon.size(); + const Vector2 *p = p_polygon.ptr(); + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + if (segment_intersects_segment(p_from, p_to, v1, v2, nullptr)) { + return true; + } + } + return false; + } + + 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<Point2i> bresenham_line(const Point2i &p_start, const Point2i &p_end) { + Vector<Point2i> points; + + Vector2i delta = (p_end - p_start).abs() * 2; + Vector2i step = (p_end - p_start).sign(); + Vector2i current = p_start; + + if (delta.x > delta.y) { + int err = delta.x / 2; + + for (; current.x != p_end.x; current.x += step.x) { + points.push_back(current); + + err -= delta.y; + if (err < 0) { + current.y += step.y; + err += delta.x; + } + } + } else { + int err = delta.y / 2; + + for (; current.y != p_end.y; current.y += step.y) { + points.push_back(current); + + err -= delta.x; + if (err < 0) { + current.x += step.x; + err += delta.y; + } + } + } + + points.push_back(current); + + return points; + } + + 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 7eb48290a8..6628b760e0 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry_3d.cpp @@ -1,12 +1,12 @@ /*************************************************************************/ -/* geometry.cpp */ +/* geometry_3d.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). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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; -} -*/ +#include "core/string/print_string.h" -void Geometry::MeshData::optimize_vertices() { +#include "thirdparty/misc/clipper.hpp" +#include "thirdparty/misc/polypartition.h" +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,40 +182,34 @@ static bool _group_face(_FaceClassify *p_faces, int len, int p_index, int p_grou return true; } -PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_array) { - - PoolVector<PoolVector<Face3> > objects; +Vector<Vector<Face3>> Geometry3D::separate_objects(Vector<Face3> p_array) { + Vector<Vector<Face3>> objects; int len = p_array.size(); - PoolVector<Face3>::Read r = p_array.read(); - - const Face3 *arrayptr = r.ptr(); + const Face3 *arrayptr = p_array.ptr(); - PoolVector<_FaceClassify> fc; + Vector<_FaceClassify> fc; fc.resize(len); - PoolVector<_FaceClassify>::Write fcw = fc.write(); - - _FaceClassify *_fcptr = fcw.ptr(); + _FaceClassify *_fcptr = fc.ptrw(); for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; } bool error = _connect_faces(_fcptr, len, -1); - ERR_FAIL_COND_V_MSG(error, PoolVector<PoolVector<Face3> >(), "Invalid geometry."); + ERR_FAIL_COND_V_MSG(error, Vector<Vector<Face3>>(), "Invalid geometry."); // Group connected faces in separate objects. 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++; } @@ -256,21 +218,18 @@ PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_ar // Group connected faces in separate objects. for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; } if (group >= 0) { - objects.resize(group); - PoolVector<PoolVector<Face3> >::Write obw = objects.write(); - PoolVector<Face3> *group_faces = obw.ptr(); + 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); } } @@ -282,7 +241,6 @@ PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_ar /*** GEOMETRY WRAPPER ***/ enum _CellFlags { - _CELL_SOLID = 1, _CELL_EXTERIOR = 2, _CELL_STEP_MASK = 0x1C, @@ -303,20 +261,18 @@ enum _CellFlags { _CELL_PREV_Z_POS = 5 << 5, _CELL_PREV_Z_NEG = 6 << 5, _CELL_PREV_FIRST = 7 << 5, - }; 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; } @@ -345,15 +301,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); @@ -363,14 +316,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) { @@ -424,9 +376,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; @@ -450,18 +400,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; @@ -470,14 +425,14 @@ 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, PoolVector<Face3> &p_faces) { - +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) @@ -492,7 +447,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); @@ -500,21 +454,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( @@ -530,24 +490,19 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i } } -PoolVector<Face3> Geometry::wrap_geometry(PoolVector<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 int face_count = p_array.size(); - PoolVector<Face3>::Read facesr = p_array.read(); - const Face3 *faces = facesr.ptr(); + const Face3 *faces = p_array.ptr(); 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()); } } @@ -557,20 +512,23 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // 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; @@ -581,15 +539,12 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e 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; } } @@ -598,10 +553,8 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // 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); @@ -610,27 +563,21 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // 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); } @@ -638,14 +585,11 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // Build faces for the inside-outside cell divisors. - PoolVector<Face3> wrapped_faces; + 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); } } @@ -654,13 +598,10 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // Transform face vertices to global coords. int wrapped_faces_count = wrapped_faces.size(); - PoolVector<Face3>::Write wrapped_facesw = wrapped_faces.write(); - Face3 *wrapped_faces_ptr = wrapped_facesw.ptr(); + 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; @@ -670,9 +611,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // 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]); } @@ -680,68 +619,34 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } 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 PoolVector<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(); Vector<Vector3> vertices; - Vector3 center = p.get_any_point(); + Vector3 center = p.center(); // make a quad clockwise vertices.push_back(center - up * subplane_size + right * subplane_size); vertices.push_back(center - up * subplane_size - right * subplane_size); @@ -749,21 +654,22 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<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]; @@ -779,13 +685,13 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<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; @@ -796,8 +702,9 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes vertices = new_vertices; } - if (vertices.size() < 3) + if (vertices.size() < 3) { continue; + } // Result is a clockwise face. @@ -805,19 +712,15 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<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]); } @@ -830,13 +733,11 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<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; @@ -847,8 +748,9 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes } } - if (found) + if (found) { continue; + } MeshData::Edge edge; edge.a = a; edge.b = b; @@ -859,9 +761,8 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes return mesh; } -PoolVector<Plane> Geometry::build_box_planes(const Vector3 &p_extents) { - - PoolVector<Plane> planes; +Vector<Plane> Geometry3D::build_box_planes(const Vector3 &p_extents) { + Vector<Plane> planes; planes.push_back(Plane(Vector3(1, 0, 0), p_extents.x)); planes.push_back(Plane(Vector3(-1, 0, 0), p_extents.x)); @@ -873,15 +774,16 @@ PoolVector<Plane> Geometry::build_box_planes(const Vector3 &p_extents) { return planes; } -PoolVector<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) { + ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>()); - PoolVector<Plane> planes; + Vector<Plane> planes; + const double sides_step = Math_TAU / p_sides; 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); + normal[(p_axis + 1) % 3] = Math::cos(i * sides_step); + normal[(p_axis + 2) % 3] = Math::sin(i * sides_step); planes.push_back(Plane(normal, p_radius)); } @@ -895,9 +797,10 @@ PoolVector<Plane> Geometry::build_cylinder_planes(real_t p_radius, real_t p_heig return planes; } -PoolVector<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) { + ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>()); - PoolVector<Plane> planes; + Vector<Plane> planes; Vector3 axis; axis[p_axis] = 1.0; @@ -907,18 +810,17 @@ PoolVector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int axis_neg[(p_axis + 2) % 3] = 1.0; axis_neg[p_axis] = -1.0; + const double lon_step = Math_TAU / p_lons; 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); + normal[(p_axis + 1) % 3] = Math::cos(i * lon_step); + normal[(p_axis + 2) % 3] = Math::sin(i * lon_step); planes.push_back(Plane(normal, p_radius)); for (int j = 1; j <= p_lats; j++) { - // FIXME: This is stupid. - Vector3 angle = normal.linear_interpolate(axis, j / (real_t)p_lats).normalized(); + Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); Vector3 pos = angle * p_radius; planes.push_back(Plane(pos, angle)); planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); @@ -928,9 +830,10 @@ PoolVector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int return planes; } -PoolVector<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) { + ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>()); - PoolVector<Plane> planes; + Vector<Plane> planes; Vector3 axis; axis[p_axis] = 1.0; @@ -940,17 +843,16 @@ PoolVector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_heigh axis_neg[(p_axis + 2) % 3] = 1.0; axis_neg[p_axis] = -1.0; + const double sides_step = Math_TAU / p_sides; 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); + normal[(p_axis + 1) % 3] = Math::cos(i * sides_step); + normal[(p_axis + 2) % 3] = Math::sin(i * sides_step); planes.push_back(Plane(normal, p_radius)); for (int j = 1; j <= p_lats; j++) { - - Vector3 angle = normal.linear_interpolate(axis, j / (real_t)p_lats).normalized(); + 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)); planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); @@ -960,228 +862,161 @@ PoolVector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_heigh 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; - } - - int from_y = 0; - for (int k = 0; k < wrects[j].s.width; k++) { +Vector<Vector3> Geometry3D::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--) { + // 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) { +#undef square - using namespace ClipperLib; +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; - ClipType op = ctUnion; + ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector<uint32_t>()); - 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; + float *work_memory = memnew_arr(float, float_count); + for (uint32_t i = 0; i < float_count; i++) { + work_memory[i] = INF; } - 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; + uint32_t y_mult = p_size.x; + uint32_t z_mult = y_mult * p_size.y; - 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. + //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; + } + } } - // Have to scale points down now. - Vector<Vector<Point2> > polypaths; - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector<Vector2> polypath; + //process in each direction - const Path &scaled_path = paths[i]; + //xy->z - 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)); + 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); } - 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) { - using namespace ClipperLib; + //xz->y - 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.z; j++) { + edt(&work_memory[i + j * z_mult], y_mult, p_size.y); + } } - 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; + //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); + } } - 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); + 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])); + } } - 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; + memdelete_arr(work_memory); - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector<Vector2> polypath; + return ret; +} - const Path &scaled_path = paths[i]; +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); - 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); + 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 polypaths; + return sdf8; } diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h new file mode 100644 index 0000000000..4ef9b4dbe6 --- /dev/null +++ b/core/math/geometry_3d.h @@ -0,0 +1,921 @@ +/*************************************************************************/ +/* geometry_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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_3D_H +#define GEOMETRY_3D_H + +#include "core/math/face3.h" +#include "core/object/object.h" +#include "core/templates/vector.h" + +class Geometry3D { + Geometry3D(); + +public: + 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)) + + // Calculate the parametric position on the 2 curves, mua and mub. + real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); + 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; + } + c1 = p1.lerp(p2, mua); + c2 = q1.lerp(q2, mub); + } + + static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { + Vector3 u = p_to_a - p_from_a; + Vector3 v = p_to_b - p_from_b; + Vector3 w = p_from_a - p_to_a; + real_t a = u.dot(u); // Always >= 0 + real_t b = u.dot(v); + real_t c = v.dot(v); // Always >= 0 + real_t d = u.dot(w); + real_t e = v.dot(w); + real_t D = a * c - b * b; // Always >= 0 + real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // Compute the line parameters of the two closest points. + if (D < CMP_EPSILON) { // The lines are almost parallel. + sN = 0.0; // Force using point P0 on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later. + tN = e; + tD = c; + } else { // Get the closest points on the infinite lines + sN = (b * e - c * d); + tN = (a * e - b * d); + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible. + sN = 0.0; + tN = e; + tD = c; + } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. + tN = 0.0; + // Recompute sc for this edge. + if (-d < 0.0) { + sN = 0.0; + } else if (-d > a) { + sN = sD; + } 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) { + sN = 0; + } else if ((-d + b) > a) { + sN = sD; + } else { + sN = (-d + b); + sD = a; + } + } + // Finally do the division to get sc and tc. + sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD); + tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD); + + // Get the difference of the two closest points. + Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) + + 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 = 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. + 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) { + return false; + } + + Vector3 q = s.cross(e1); + + real_t v = f * p_dir.dot(q); + + 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) { + *r_res = p_from + p_dir * t; + } + return true; + } 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 = 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. + 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) { + return false; + } + + Vector3 q = s.cross(e1); + + real_t v = f * rel.dot(q); + + 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) { + *r_res = p_from + rel * t; + } + return true; + } 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 = 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) { + 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) { + 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) { + inters_d -= Math::sqrt(inters_d2); + } + + // Check in segment. + if (inters_d < 0 || inters_d > rel_l) { + return false; + } + + Vector3 result = p_from + normal * inters_d; + + if (r_res) { + *r_res = result; + } + 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 = nullptr, Vector3 *r_norm = nullptr, int p_cylinder_axis = 2) { + Vector3 rel = (p_to - p_from); + real_t rel_l = rel.length(); + if (rel_l < CMP_EPSILON) { + return false; // Both points are the same. + } + + ERR_FAIL_COND_V(p_cylinder_axis < 0, false); + ERR_FAIL_COND_V(p_cylinder_axis > 2, false); + Vector3 cylinder_axis; + cylinder_axis[p_cylinder_axis] = 1.0; + + // First check if they are parallel. + Vector3 normal = (rel / rel_l); + Vector3 crs = normal.cross(cylinder_axis); + real_t crs_l = crs.length(); + + Vector3 axis_dir; + + if (crs_l < CMP_EPSILON) { + Vector3 side_axis; + side_axis[(p_cylinder_axis + 1) % 3] = 1.0; // Any side axis OK. + axis_dir = side_axis; + } else { + axis_dir = crs / crs_l; + } + + real_t dist = axis_dir.dot(p_from); + + 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) { + return false; // Avoid numerical error. + } + Size2 size(Math::sqrt(w2), p_height * 0.5); + + Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized(); + + Vector2 from2D(side_dir.dot(p_from), p_from[p_cylinder_axis]); + Vector2 to2D(side_dir.dot(p_to), p_to[p_cylinder_axis]); + + real_t min = 0, max = 1; + + 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]; + real_t box_end = size[i]; + real_t cmin, cmax; + + if (seg_from < seg_to) { + 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) { + 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; + } + + if (cmin > min) { + min = cmin; + axis = i; + } + if (cmax < max) { + max = cmax; + } + if (max < min) { + return false; + } + } + + // Convert to 3D again. + Vector3 result = p_from + (rel * min); + Vector3 res_normal = result; + + if (axis == 0) { + res_normal[p_cylinder_axis] = 0; + } else { + int axis_side = (p_cylinder_axis + 1) % 3; + res_normal[axis_side] = 0; + axis_side = (axis_side + 1) % 3; + res_normal[axis_side] = 0; + } + + res_normal.normalize(); + + if (r_res) { + *r_res = result; + } + 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) { + 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) { + continue; // Ignore parallel plane. + } + + real_t dist = -p.distance_to(p_from) / den; + + if (den > 0) { + // Backwards facing plane. + if (dist < max) { + max = dist; + } + } else { + // Front facing plane. + if (dist > min) { + min = dist; + min_index = i; + } + } + } + + if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions. + return false; // No intersection. + } + + if (p_res) { + *p_res = p_from + dir * min; + } + 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) { + 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 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 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) { + return false; + } + + Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point); + + 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) { + 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. + return false; + } + + Vector3 contact = p_sphere_pos - (p_normal * d); + + /** 2nd) TEST INSIDE TRIANGLE **/ + + 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"); + return true; + } + + /** 3rd TEST INSIDE EDGE CYLINDERS **/ + + 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]; + Vector3 n2 = p_sphere_pos - verts[i + 1]; + + ///@TODO Maybe discard by range here to make the algorithm quicker. + + // Check point within cylinder radius. + Vector3 axis = n1.cross(n2).cross(n1); + axis.normalize(); + + real_t ad = axis.dot(n2); + + if (ABS(ad) > p_sphere_radius) { + // No chance with this edge, too far away. + continue; + } + + // Check point within edge capsule cylinder. + /** 4th TEST INSIDE EDGE POINTS **/ + + 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. + return true; + } + + 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]; + r_sphere_contact = p_sphere_pos - n * p_sphere_radius; + return true; + } + + if (n2.distance_squared_to(n1) < r2) { + Vector3 n = (p_sphere_pos - verts[i]).normalized(); + + r_triangle_contact = verts[i]; + r_sphere_contact = p_sphere_pos - n * p_sphere_radius; + return true; + } + + break; // It's pointless to continue at this point, so save some CPU cycles. + } + + return false; + } + + 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) { + return polygon; + } + + int *location_cache = (int *)alloca(sizeof(int) * polygon.size()); + int inside_count = 0; + int outside_count = 0; + + for (int a = 0; a < polygon.size(); a++) { + real_t dist = p_plane.distance_to(polygon[a]); + if (dist < -CMP_POINT_IN_PLANE_EPSILON) { + location_cache[a] = LOC_INSIDE; + inside_count++; + } else { + if (dist > CMP_POINT_IN_PLANE_EPSILON) { + location_cache[a] = LOC_OUTSIDE; + outside_count++; + } else { + location_cache[a] = LOC_BOUNDARY; + } + } + } + + if (outside_count == 0) { + return polygon; // No changes. + } else if (inside_count == 0) { + return Vector<Vector3>(); // Empty. + } + + long previous = polygon.size() - 1; + Vector<Vector3> clipped; + + for (int index = 0; index < polygon.size(); index++) { + int loc = location_cache[index]; + if (loc == LOC_OUTSIDE) { + if (location_cache[previous] == LOC_INSIDE) { + const Vector3 &v1 = polygon[previous]; + const Vector3 &v2 = polygon[index]; + + Vector3 segment = v1 - v2; + real_t den = p_plane.normal.dot(segment); + real_t dist = p_plane.distance_to(v1) / den; + dist = -dist; + clipped.push_back(v1 + segment * dist); + } + } else { + const Vector3 &v1 = polygon[index]; + if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) { + const Vector3 &v2 = polygon[previous]; + Vector3 segment = v1 - v2; + real_t den = p_plane.normal.dot(segment); + real_t dist = p_plane.distance_to(v1) / den; + dist = -dist; + clipped.push_back(v1 + segment * dist); + } + + clipped.push_back(v1); + } + + previous = index; + } + + return clipped; + } + + 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; + }; + + Vector<Face> faces; + + struct Edge { + int a, b; + }; + + Vector<Edge> edges; + + Vector<Vector3> vertices; + + void optimize_vertices(); + }; + + 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 Vector<Vector3> compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); + +#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]; + } + } + if (normal.dot(vmin) + d > 0.0f) { + return false; + } + if (normal.dot(vmax) + d >= 0.0f) { + return true; + } + + return false; + } + +/*======================== 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; + } + + /* test in Y-direction */ + FINDMINMAX(v0.y, v1.y, v2.y, min, max); + if (min > boxhalfsize.y || max < -boxhalfsize.y) { + return false; + } + + /* test in Z-direction */ + FINDMINMAX(v0.z, v1.z, v2.z, min, max); + if (min > boxhalfsize.z || max < -boxhalfsize.z) { + return false; + } + + /* 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_3D_H diff --git a/core/math/math_defs.h b/core/math/math_defs.h index 4928c96abd..df2223fb78 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -66,35 +66,31 @@ enum ClockDirection { }; enum Orientation { - HORIZONTAL, VERTICAL }; enum HAlign { - HALIGN_LEFT, HALIGN_CENTER, - HALIGN_RIGHT + HALIGN_RIGHT, + HALIGN_FILL, }; enum VAlign { - VALIGN_TOP, VALIGN_CENTER, VALIGN_BOTTOM }; -enum Margin { - - MARGIN_LEFT, - MARGIN_TOP, - MARGIN_RIGHT, - MARGIN_BOTTOM +enum Side { + SIDE_LEFT, + SIDE_TOP, + SIDE_RIGHT, + SIDE_BOTTOM }; enum Corner { - CORNER_TOP_LEFT, CORNER_TOP_RIGHT, CORNER_BOTTOM_RIGHT, diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index a47d4ef7ad..570c57e254 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -41,16 +41,13 @@ } 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 */ /* clang-format off */ switch (p_source.get_type()) { - case Variant::VECTOR2: { - SETUP_TYPE(Vector2) /**/ TRY_TRANSFER_FIELD("x", x) @@ -60,7 +57,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::RECT2: { - SETUP_TYPE(Rect2) /**/ TRY_TRANSFER_FIELD("x", position.x) @@ -72,7 +68,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::VECTOR3: { - SETUP_TYPE(Vector3) /**/ TRY_TRANSFER_FIELD("x", x) @@ -83,7 +78,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::PLANE: { - SETUP_TYPE(Plane) /**/ TRY_TRANSFER_FIELD("x", normal.x) @@ -94,9 +88,8 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } - case Variant::QUAT: { - - SETUP_TYPE(Quat) + case Variant::QUATERNION: { + SETUP_TYPE(Quaternion) /**/ TRY_TRANSFER_FIELD("x", x) else TRY_TRANSFER_FIELD("y", y) @@ -107,7 +100,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::AABB: { - SETUP_TYPE(AABB) /**/ TRY_TRANSFER_FIELD("px", position.x) @@ -121,7 +113,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::TRANSFORM2D: { - SETUP_TYPE(Transform2D) /**/ TRY_TRANSFER_FIELD("xx", elements[0][0]) @@ -135,7 +126,6 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const } case Variant::BASIS: { - SETUP_TYPE(Basis) /**/ TRY_TRANSFER_FIELD("xx", elements[0][0]) @@ -151,9 +141,8 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } - case Variant::TRANSFORM: { - - SETUP_TYPE(Transform) + case Variant::TRANSFORM3D: { + SETUP_TYPE(Transform3D) /**/ TRY_TRANSFER_FIELD("xx", basis.elements[0][0]) else TRY_TRANSFER_FIELD("xy", basis.elements[0][1]) diff --git a/core/math/math_fieldwise.h b/core/math/math_fieldwise.h index c1ee9ec8f0..fe44d09900 100644 --- a/core/math/math_fieldwise.h +++ b/core/math/math_fieldwise.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -33,7 +33,7 @@ #ifdef TOOLS_ENABLED -#include "core/variant.h" +#include "core/variant/variant.h" Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const String &p_field); diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index 7417e64ac1..e92bb9f4aa 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,12 +30,10 @@ #include "math_funcs.h" -#include "core/error_macros.h" +#include "core/error/error_macros.h" RandomPCG Math::default_rand(RandomPCG::DEFAULT_SEED, RandomPCG::DEFAULT_INC); -#define PHI 0x9e3779b9 - uint32_t Math::rand_from_seed(uint64_t *seed) { RandomPCG rng = RandomPCG(*seed, RandomPCG::DEFAULT_INC); uint32_t r = rng.rand(); @@ -94,16 +92,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,11 +118,12 @@ 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) { +double Math::snapped(double p_value, double p_step) { if (p_step != 0) { p_value = Math::floor(p_value / p_step + 0.5) * p_step; } @@ -130,7 +131,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 +166,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++; } } @@ -181,3 +181,7 @@ double Math::random(double from, double to) { float Math::random(float from, float to) { return default_rand.random(from, to); } + +int Math::random(int from, int to) { + return default_rand.random(from, to); +} diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 3e1eb14a6a..3389407e72 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -41,13 +41,13 @@ #include <math.h> class Math { - static RandomPCG default_rand; public: Math() {} // useless to instance - static const uint64_t RANDOM_MAX = 0xFFFFFFFF; + // Not using 'RANDOM_MAX' to avoid conflict with system headers on some OSes (at least NetBSD). + static const uint64_t RANDOM_32BIT_MAX = 0xFFFFFFFF; static _ALWAYS_INLINE_ double sin(double p_x) { return ::sin(p_x); } static _ALWAYS_INLINE_ float sin(float p_x) { return ::sinf(p_x); } @@ -103,6 +103,9 @@ public: static _ALWAYS_INLINE_ double log(double p_x) { return ::log(p_x); } static _ALWAYS_INLINE_ float log(float p_x) { return ::logf(p_x); } + static _ALWAYS_INLINE_ double log2(double p_x) { return ::log2(p_x); } + static _ALWAYS_INLINE_ float log2(float p_x) { return ::log2f(p_x); } + static _ALWAYS_INLINE_ double exp(double p_x) { return ::exp(p_x); } static _ALWAYS_INLINE_ float exp(float p_x) { return ::expf(p_x); } @@ -198,19 +201,36 @@ public: value += 0.0; return value; } - static _ALWAYS_INLINE_ int posmod(int p_x, int p_y) { - int value = p_x % p_y; + static _ALWAYS_INLINE_ float fposmodp(float p_x, float p_y) { + float value = Math::fmod(p_x, p_y); + if (value < 0) { + value += p_y; + } + value += 0.0; + return value; + } + static _ALWAYS_INLINE_ double fposmodp(double p_x, double p_y) { + double value = Math::fmod(p_x, p_y); + if (value < 0) { + value += p_y; + } + value += 0.0; + return value; + } + + static _ALWAYS_INLINE_ int64_t posmod(int64_t p_x, int64_t p_y) { + int64_t value = p_x % p_y; if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { value += p_y; } return value; } - static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * Math_PI / 180.0; } - static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * Math_PI / 180.0; } + static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * (Math_PI / 180.0); } + static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * (Math_PI / 180.0); } - static _ALWAYS_INLINE_ double rad2deg(double p_y) { return p_y * 180.0 / Math_PI; } - static _ALWAYS_INLINE_ float rad2deg(float p_y) { return p_y * 180.0 / Math_PI; } + static _ALWAYS_INLINE_ double rad2deg(double p_y) { return p_y * (180.0 / Math_PI); } + static _ALWAYS_INLINE_ float rad2deg(float p_y) { return p_y * (180.0 / Math_PI); } static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; } static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; } @@ -232,15 +252,19 @@ public: static _ALWAYS_INLINE_ double range_lerp(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } 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; - 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_ double smoothstep(double p_from, double p_to, double p_s) { + if (is_equal_approx(p_from, p_to)) { + return p_from; + } + double s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0, 1.0); + return s * s * (3.0 - 2.0 * s); } - 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; - float x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f); - return x * x * (3.0f - 2.0f * x); + static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_s) { + if (is_equal_approx(p_from, p_to)) { + return p_from; + } + float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f); + return s * s * (3.0f - 2.0f * s); } static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } @@ -251,8 +275,8 @@ public: static _ALWAYS_INLINE_ double db2linear(double p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); } static _ALWAYS_INLINE_ float db2linear(float p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); } - static _ALWAYS_INLINE_ double round(double p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } - static _ALWAYS_INLINE_ float round(float p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } + static _ALWAYS_INLINE_ double round(double p_val) { return ::round(p_val); } + static _ALWAYS_INLINE_ float round(float p_val) { return ::roundf(p_val); } static _ALWAYS_INLINE_ int64_t wrapi(int64_t value, int64_t min, int64_t max) { int64_t range = max - min; @@ -271,7 +295,7 @@ public: static double ease(double p_x, double p_c); static int step_decimals(double p_step); static int range_step_decimals(double p_step); - static double stepify(double p_value, double p_step); + static double snapped(double p_value, double p_step); static double dectime(double p_value, double p_amount, double p_step); static uint32_t larger_prime(uint32_t p_val); @@ -280,39 +304,53 @@ public: static void randomize(); static uint32_t rand_from_seed(uint64_t *seed); static uint32_t rand(); - static _ALWAYS_INLINE_ double randd() { return (double)rand() / (double)Math::RANDOM_MAX; } - static _ALWAYS_INLINE_ float randf() { return (float)rand() / (float)Math::RANDOM_MAX; } + static _ALWAYS_INLINE_ double randd() { return (double)rand() / (double)Math::RANDOM_32BIT_MAX; } + static _ALWAYS_INLINE_ float randf() { return (float)rand() / (float)Math::RANDOM_32BIT_MAX; } static double random(double from, double to); static float random(float from, float to); - static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); } + static int random(int from, int to); + + static _ALWAYS_INLINE_ bool is_equal_approx(float a, float b) { + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { + return true; + } + // Then check for approximate equality. + float tolerance = CMP_EPSILON * abs(a); + if (tolerance < CMP_EPSILON) { + tolerance = CMP_EPSILON; + } + return abs(a - b) < tolerance; + } - static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON, real_t min_epsilon = CMP_EPSILON) { - // this is an approximate way to check that numbers are close, as a ratio of their average size - // helps compare approximate numbers that may be very big or very small - real_t diff = abs(a - b); - if (diff == 0.0 || diff < min_epsilon) { + static _ALWAYS_INLINE_ bool is_equal_approx(float a, float b, float tolerance) { + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { return true; } - real_t avg_size = (abs(a) + abs(b)) / 2.0; - diff /= avg_size; - return diff < epsilon; + // Then check for approximate equality. + return abs(a - b) < tolerance; + } + + static _ALWAYS_INLINE_ bool is_zero_approx(float s) { + return abs(s) < CMP_EPSILON; } - static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b) { + static _ALWAYS_INLINE_ bool is_equal_approx(double a, double b) { // Check for exact equality first, required to handle "infinity" values. if (a == b) { return true; } // Then check for approximate equality. - real_t tolerance = CMP_EPSILON * abs(a); + double tolerance = CMP_EPSILON * abs(a); if (tolerance < CMP_EPSILON) { tolerance = CMP_EPSILON; } return abs(a - b) < tolerance; } - static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b, real_t tolerance) { + static _ALWAYS_INLINE_ bool is_equal_approx(double a, double b, double tolerance) { // Check for exact equality first, required to handle "infinity" values. if (a == b) { return true; @@ -321,12 +359,11 @@ public: return abs(a - b) < tolerance; } - static _ALWAYS_INLINE_ bool is_zero_approx(real_t s) { + static _ALWAYS_INLINE_ bool is_zero_approx(double s) { return abs(s) < CMP_EPSILON; } static _ALWAYS_INLINE_ float absf(float g) { - union { float f; uint32_t i; @@ -338,7 +375,6 @@ public: } static _ALWAYS_INLINE_ double absd(double g) { - union { double d; uint64_t i; @@ -348,29 +384,10 @@ public: return u.d; } - //this function should be as fast as possible and rounding mode should not matter + // 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? - b = (int)((a > 0.0) ? (a + 0.5) : (a - 0.5)); - -#elif defined(_MSC_VER) && _MSC_VER < 1800 - __asm fld a __asm fistp b - /*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) - // use AT&T inline assembly style, document that - // we use memory as output (=m) and input (m) - __asm__ __volatile__ ( - "flds %1 \n\t" - "fistpl %0 \n\t" - : "=m" (b) - : "m" (a));*/ - -#else - b = lrintf(a); //assuming everything but msvc 2012 or earlier has lrint -#endif - return b; + // Assuming every supported compiler has `lrint()`. + return lrintf(a); } static _ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t h) { @@ -405,7 +422,6 @@ public: } static _ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *h) { - union { uint32_t u32; float f32; @@ -420,7 +436,6 @@ public: } static _ALWAYS_INLINE_ uint16_t make_half_float(float f) { - union { float fv; uint32_t ui; @@ -451,7 +466,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); @@ -469,17 +483,18 @@ public: } static _ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) { - return p_step != 0 ? Math::stepify(p_target - p_offset, p_step) + p_offset : p_target; + return p_step != 0 ? Math::snapped(p_target - p_offset, p_step) + p_offset : p_target; } static _ALWAYS_INLINE_ float snap_scalar_separation(float p_offset, float p_step, float p_target, float p_separation) { if (p_step != 0) { - float a = Math::stepify(p_target - p_offset, p_step + p_separation) + p_offset; + float a = Math::snapped(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 5478bdaf77..493a63aa2e 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,12 +31,13 @@ #ifndef OCTREE_H #define OCTREE_H -#include "core/list.h" -#include "core/map.h" #include "core/math/aabb.h" +#include "core/math/geometry_3d.h" #include "core/math/vector3.h" -#include "core/print_string.h" -#include "core/variant.h" +#include "core/string/print_string.h" +#include "core/templates/list.h" +#include "core/templates/map.h" +#include "core/variant/variant.h" typedef uint32_t OctreeElementID; @@ -51,7 +52,6 @@ public: private: enum { - NEG = 0, POS = 1, }; @@ -68,7 +68,6 @@ private: }; struct PairKey { - union { struct { OctreeElementID A; @@ -78,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; } @@ -101,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 = NULL; - for (int i = 0; i < 8; i++) - children[i] = NULL; - } - - ~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; @@ -155,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 = NULL; - } + Element() {} }; struct PairData { - int refcount; bool intersect; Element *A, *B; @@ -203,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); } @@ -227,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; @@ -254,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); @@ -295,32 +257,26 @@ 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 = NULL; + 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] = NULL; + root->children[i] = nullptr; break; } } ERR_FAIL_COND(!new_root); - new_root->parent = NULL; + new_root->parent = nullptr; new_root->parent_index = -1; } @@ -332,15 +288,16 @@ private: void _insert_element(Element *p_element, Octant *p_octant); void _ensure_valid_root(const AABB &p_aabb); - bool _remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit = NULL); + bool _remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit = nullptr); void _remove_element(Element *p_element); void _pair_element(Element *p_element, Octant *p_octant); void _unpair_element(Element *p_element, Octant *p_octant); struct _CullConvexData { - const Plane *planes; int plane_count; + const Vector3 *points; + int point_count; T **result_array; int *result_idx; int result_max; @@ -353,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); @@ -377,10 +334,10 @@ public: int get_subindex(OctreeElementID p_id) const; int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF); - int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); - int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); + int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF); + int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF); - int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); + int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF); void set_pair_callback(PairCallback p_callback, void *p_userdata); void set_unpair_callback(UnpairCallback p_callback, void *p_userdata); @@ -396,13 +353,12 @@ public: template <class T, bool use_pairs, class AL> T *Octree<T, use_pairs, AL>::get(OctreeElementID p_id) const { const typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, NULL); + ERR_FAIL_COND_V(!E, nullptr); return E->get().userdata; } 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; @@ -410,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; @@ -420,29 +375,25 @@ 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) { //if (p_octant->aabb.size.x*0.5 < element_size) { - /* at smallest possible size for the element */ typename Element::OctantOwner owner; 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(); } p_element->octant_owners.push_back(owner); - if (p_element->common_parent == NULL) { + if (p_element->common_parent == nullptr) { p_element->common_parent = p_octant; p_element->container_aabb = p_octant->aabb; } else { @@ -450,11 +401,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]); } @@ -463,10 +412,9 @@ void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_oct } else { /* not big enough, send it to subitems */ int splits = 0; - bool candidate = p_element->common_parent == NULL; + 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)) { @@ -479,12 +427,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 */ @@ -506,13 +457,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) { @@ -533,14 +482,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; @@ -552,18 +499,16 @@ void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) { root = memnew_allocator(Octant, AL); - root->parent = NULL; + root->parent = nullptr; root->parent_index = -1; root->aabb = base; 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); @@ -592,15 +537,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; @@ -628,17 +572,16 @@ 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()) { - + if (p_octant->children_count == 0 && p_octant->elements.is_empty() && p_octant->pairable_elements.is_empty()) { // erase octant if (p_octant == root) { // won't have a parent, just erase - root = NULL; + root = nullptr; } else { ERR_FAIL_INDEX_V(p_octant->parent_index, 8, octant_removed); - parent->children[p_octant->parent_index] = NULL; + parent->children[p_octant->parent_index] = nullptr; parent->children_count--; } @@ -648,8 +591,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; } @@ -659,7 +603,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) { @@ -684,25 +627,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; @@ -723,30 +665,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); } @@ -756,30 +698,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); @@ -788,7 +728,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); @@ -819,8 +758,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; @@ -828,7 +768,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); @@ -849,33 +788,34 @@ 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 = NULL; + e.common_parent = nullptr; e.aabb = AABB(); _optimize(); } else { _ensure_valid_root(p_aabb); // inserting - e.common_parent = NULL; + 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; } @@ -884,7 +824,7 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { combined.merge_with(p_aabb); _ensure_valid_root(combined); - ERR_FAIL_COND(e.octant_owners.front() == NULL); + ERR_FAIL_COND(e.octant_owners.front() == nullptr); /* FIND COMMON PARENT */ @@ -895,14 +835,15 @@ 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); //prepare for reinsert e.octant_owners.clear(); - e.common_parent = NULL; + e.common_parent = nullptr; e.aabb = p_aabb; _insert_element(&e, common_parent); // reinsert from this point @@ -910,7 +851,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(); @@ -919,13 +859,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); } @@ -935,15 +875,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]); + } } } @@ -955,14 +894,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); @@ -971,26 +910,25 @@ void Octree<T, use_pairs, AL>::set_pairable(OctreeElementID p_id, bool p_pairabl e.pairable = p_pairable; e.pairable_type = p_pairable_type; e.pairable_mask = p_pairable_mask; - e.common_parent = NULL; + e.common_parent = nullptr; 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); } @@ -1000,57 +938,50 @@ 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()) { - + if (!p_octant->elements.is_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)) { - + 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 } } } } - if (use_pairs && !p_octant->pairable_elements.empty()) { - + if (use_pairs && !p_octant->pairable_elements.is_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)) { - + 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 } } @@ -1058,8 +989,7 @@ 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)) { + 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); } } @@ -1067,61 +997,55 @@ 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()) { - + if (!p_octant->elements.is_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 } } } } - if (use_pairs && !p_octant->pairable_elements.empty()) { - + if (use_pairs && !p_octant->pairable_elements.is_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 } } @@ -1129,7 +1053,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); } @@ -1138,64 +1061,58 @@ 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()) { - + if (!p_octant->elements.is_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 } } } } - if (use_pairs && !p_octant->pairable_elements.empty()) { - + if (use_pairs && !p_octant->pairable_elements.is_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 } } @@ -1203,7 +1120,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); } @@ -1212,64 +1128,58 @@ 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()) { - + if (!p_octant->elements.is_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 } } } } - if (use_pairs && !p_octant->pairable_elements.empty()) { - + if (use_pairs && !p_octant->pairable_elements.is_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 } } @@ -1277,7 +1187,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); @@ -1287,15 +1196,22 @@ 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 || p_convex.size() == 0) { + return 0; + } - if (!root) + 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++; _CullConvexData cdata; cdata.planes = &p_convex[0]; cdata.plane_count = p_convex.size(); + cdata.points = &convex_points[0]; + cdata.point_count = convex_points.size(); cdata.result_array = p_result_array; cdata.result_max = p_result_max; cdata.result_idx = &result_count; @@ -1308,9 +1224,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++; @@ -1321,9 +1237,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++; @@ -1334,9 +1250,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++; @@ -1347,32 +1263,30 @@ 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; - root = NULL; + root = nullptr; octant_count = 0; pair_count = 0; - pair_callback = NULL; - unpair_callback = NULL; - pair_callback_userdata = NULL; - unpair_callback_userdata = NULL; + pair_callback = nullptr; + unpair_callback = nullptr; + pair_callback_userdata = nullptr; + unpair_callback_userdata = nullptr; } -#endif +#endif // OCTREE_H diff --git a/core/math/plane.cpp b/core/math/plane.cpp index a3818698bc..3c78b55b90 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,14 +31,13 @@ #include "plane.h" #include "core/math/math_funcs.h" +#include "core/variant/variant.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 +48,21 @@ 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 +73,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 +80,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 +95,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 +118,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 +130,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; } @@ -151,14 +139,41 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec return true; } +Variant Plane::intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) const { + Vector3 inters; + if (intersect_3(p_plane1, p_plane2, &inters)) { + return inters; + } else { + return Variant(); + } +} +Variant Plane::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const { + Vector3 inters; + if (intersects_ray(p_from, p_dir, &inters)) { + return inters; + } else { + return Variant(); + } +} +Variant Plane::intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const { + Vector3 inters; + if (intersects_segment(p_begin, p_end, &inters)) { + return inters; + } else { + return Variant(); + } +} + /* 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); + return "[N: " + normal.operator String() + ", D: " + String::num_real(d, false) + "]"; } diff --git a/core/math/plane.h b/core/math/plane.h index 771c8fc705..2267b28c53 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -33,10 +33,12 @@ #include "core/math/vector3.h" +class Variant; + 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 @@ -47,7 +49,6 @@ public: /* Plane-Point operations */ _FORCE_INLINE_ Vector3 center() const { return normal * d; } - Vector3 get_any_point() const; Vector3 get_any_perpendicular_normal() const; _FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane @@ -56,12 +57,16 @@ 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 { + // For Variant bindings. + Variant intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) const; + Variant intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const; + Variant intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const; + _FORCE_INLINE_ Vector3 project(const Vector3 &p_point) const { return p_point - normal * distance_to(p_point); } @@ -69,13 +74,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 +91,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 +115,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 deleted file mode 100644 index 61cd41b23d..0000000000 --- a/core/math/quat.cpp +++ /dev/null @@ -1,253 +0,0 @@ -/*************************************************************************/ -/* quat.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 "quat.h" - -#include "core/math/basis.h" -#include "core/print_string.h" - -// set_euler_xyz expects a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses XYZ convention (Z is the first rotation). -void Quat::set_euler_xyz(const Vector3 &p_euler) { - real_t half_a1 = p_euler.x * 0.5; - real_t half_a2 = p_euler.y * 0.5; - real_t half_a3 = p_euler.z * 0.5; - - // R = X(a1).Y(a2).Z(a3) convention for Euler angles. - // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2) - // a3 is the angle of the first rotation, following the notation in this reference. - - real_t cos_a1 = Math::cos(half_a1); - real_t sin_a1 = Math::sin(half_a1); - real_t cos_a2 = Math::cos(half_a2); - real_t sin_a2 = Math::sin(half_a2); - real_t cos_a3 = Math::cos(half_a3); - real_t sin_a3 = Math::sin(half_a3); - - set(sin_a1 * cos_a2 * cos_a3 + sin_a2 * sin_a3 * cos_a1, - -sin_a1 * sin_a3 * cos_a2 + sin_a2 * cos_a1 * cos_a3, - sin_a1 * sin_a2 * cos_a3 + sin_a3 * cos_a1 * cos_a2, - -sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); -} - -// get_euler_xyz returns a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses XYZ convention (Z is the first rotation). -Vector3 Quat::get_euler_xyz() const { - Basis m(*this); - return m.get_euler_xyz(); -} - -// set_euler_yxz expects a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses YXZ convention (Z is the first rotation). -void Quat::set_euler_yxz(const Vector3 &p_euler) { - real_t half_a1 = p_euler.y * 0.5; - real_t half_a2 = p_euler.x * 0.5; - real_t half_a3 = p_euler.z * 0.5; - - // R = Y(a1).X(a2).Z(a3) convention for Euler angles. - // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) - // a3 is the angle of the first rotation, following the notation in this reference. - - real_t cos_a1 = Math::cos(half_a1); - real_t sin_a1 = Math::sin(half_a1); - real_t cos_a2 = Math::cos(half_a2); - real_t sin_a2 = Math::sin(half_a2); - real_t cos_a3 = Math::cos(half_a3); - real_t sin_a3 = Math::sin(half_a3); - - set(sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3, - sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3, - -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3, - sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); -} - -// get_euler_yxz returns a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses YXZ convention (Z is the first rotation). -Vector3 Quat::get_euler_yxz() const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized."); -#endif - Basis m(*this); - return m.get_euler_yxz(); -} - -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, - w * q.w - x * q.x - y * q.y - z * q.z); -} - -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()); -} - -void Quat::normalize() { - *this /= length(); -} - -Quat Quat::normalized() const { - return *this / length(); -} - -bool Quat::is_normalized() const { - return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); //use less epsilon -} - -Quat Quat::inverse() const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized."); -#endif - return Quat(-x, -y, -z, w); -} - -Quat Quat::slerp(const Quat &q, const real_t &t) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!q.is_normalized(), Quat(), "The end quaternion must be normalized."); -#endif - Quat to1; - real_t omega, cosom, sinom, scale0, scale1; - - // calc cosine - cosom = dot(q); - - // adjust signs (if necessary) - if (cosom < 0.0) { - cosom = -cosom; - to1.x = -q.x; - to1.y = -q.y; - to1.z = -q.z; - to1.w = -q.w; - } else { - to1.x = q.x; - to1.y = q.y; - to1.z = q.z; - to1.w = q.w; - } - - // calculate coefficients - - if ((1.0 - cosom) > CMP_EPSILON) { - // standard case (slerp) - omega = Math::acos(cosom); - sinom = Math::sin(omega); - scale0 = Math::sin((1.0 - t) * omega) / sinom; - scale1 = Math::sin(t * omega) / sinom; - } else { - // "from" and "to" quaternions are very close - // ... so we can do a linear interpolation - scale0 = 1.0 - t; - scale1 = t; - } - // calculate final values - return Quat( - scale0 * x + scale1 * to1.x, - scale0 * y + scale1 * to1.y, - scale0 * z + scale1 * to1.z, - scale0 * w + scale1 * to1.w); -} - -Quat Quat::slerpni(const Quat &q, const real_t &t) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!q.is_normalized(), Quat(), "The end quaternion must be normalized."); -#endif - const Quat &from = *this; - - real_t dot = from.dot(q); - - if (Math::absf(dot) > 0.9999) return from; - - real_t theta = Math::acos(dot), - sinT = 1.0 / Math::sin(theta), - newFactor = Math::sin(t * theta) * sinT, - invFactor = Math::sin((1.0 - t) * theta) * sinT; - - return Quat(invFactor * from.x + newFactor * q.x, - invFactor * from.y + newFactor * q.y, - invFactor * from.z + newFactor * q.z, - invFactor * from.w + newFactor * q.w); -} - -Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!q.is_normalized(), Quat(), "The end quaternion must be normalized."); -#endif - //the only way to do slerp :| - real_t t2 = (1.0 - t) * t * 2; - Quat sp = this->slerp(q, t); - Quat sq = prep.slerpni(postq, t); - return sp.slerpni(sq, t2); -} - -Quat::operator String() const { - - return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); -} - -void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { -#ifdef MATH_CHECKS - ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized."); -#endif - real_t d = axis.length(); - if (d == 0) - set(0, 0, 0, 0); - 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; - set(axis.x * s, axis.y * s, axis.z * s, - cos_angle); - } -} diff --git a/core/math/quat.h b/core/math/quat.h deleted file mode 100644 index 11ae03dffb..0000000000 --- a/core/math/quat.h +++ /dev/null @@ -1,234 +0,0 @@ -/*************************************************************************/ -/* quat.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. */ -/*************************************************************************/ - -// Circular dependency between Vector3 and Basis :/ -#include "core/math/vector3.h" - -#ifndef QUAT_H -#define QUAT_H - -#include "core/math/math_defs.h" -#include "core/math/math_funcs.h" -#include "core/ustring.h" - -class Quat { -public: - real_t x, y, z, w; - - _FORCE_INLINE_ real_t length_squared() const; - bool is_equal_approx(const Quat &p_quat) const; - real_t length() const; - void normalize(); - Quat normalized() const; - bool is_normalized() const; - Quat inverse() const; - _FORCE_INLINE_ real_t dot(const Quat &q) const; - - void set_euler_xyz(const Vector3 &p_euler); - Vector3 get_euler_xyz() const; - void set_euler_yxz(const Vector3 &p_euler); - Vector3 get_euler_yxz() const; - - void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }; - Vector3 get_euler() const { return get_euler_yxz(); }; - - Quat slerp(const Quat &q, const real_t &t) const; - Quat slerpni(const Quat &q, const real_t &t) const; - Quat cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const; - - void set_axis_angle(const Vector3 &axis, const real_t &angle); - _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { - r_angle = 2 * Math::acos(w); - real_t r = ((real_t)1) / Math::sqrt(1 - w * w); - r_axis.x = x * r; - r_axis.y = y * r; - r_axis.z = z * r; - } - - void operator*=(const Quat &q); - Quat operator*(const Quat &q) const; - - Quat operator*(const Vector3 &v) const { - return Quat(w * v.x + y * v.z - z * v.y, - w * v.y + z * v.x - x * v.z, - w * v.z + x * v.y - y * v.x, - -x * v.x - y * v.y - z * v.z); - } - - _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); -#endif - Vector3 u(x, y, z); - Vector3 uv = u.cross(v); - return v + ((uv * w) + u.cross(uv)) * ((real_t)2); - } - - _FORCE_INLINE_ void operator+=(const Quat &q); - _FORCE_INLINE_ void operator-=(const Quat &q); - _FORCE_INLINE_ void operator*=(const real_t &s); - _FORCE_INLINE_ void operator/=(const real_t &s); - _FORCE_INLINE_ Quat operator+(const Quat &q2) const; - _FORCE_INLINE_ Quat operator-(const Quat &q2) const; - _FORCE_INLINE_ Quat operator-() const; - _FORCE_INLINE_ Quat operator*(const real_t &s) const; - _FORCE_INLINE_ Quat operator/(const real_t &s) const; - - _FORCE_INLINE_ bool operator==(const Quat &p_quat) const; - _FORCE_INLINE_ bool operator!=(const Quat &p_quat) const; - - operator String() const; - - inline void set(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; - w = p_w; - } - 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), - w(p_w) { - } - Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); } - - Quat(const Vector3 &euler) { set_euler(euler); } - Quat(const Quat &q) : - x(q.x), - y(q.y), - z(q.z), - w(q.w) { - } - - Quat operator=(const Quat &q) { - x = q.x; - y = q.y; - z = q.z; - w = q.w; - return *this; - } - - Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc - { - Vector3 c = v0.cross(v1); - real_t d = v0.dot(v1); - - if (d < -1.0 + CMP_EPSILON) { - x = 0; - y = 1; - z = 0; - w = 0; - } else { - - real_t s = Math::sqrt((1.0 + d) * 2.0); - real_t rs = 1.0 / s; - - x = c.x * rs; - y = c.y * rs; - z = c.z * rs; - w = s * 0.5; - } - } - - inline Quat() : - x(0), - y(0), - z(0), - w(1) { - } -}; - -real_t Quat::dot(const Quat &q) const { - return x * q.x + y * q.y + z * q.z + w * q.w; -} - -real_t Quat::length_squared() const { - return dot(*this); -} - -void Quat::operator+=(const Quat &q) { - x += q.x; - y += q.y; - z += q.z; - w += q.w; -} - -void Quat::operator-=(const Quat &q) { - x -= q.x; - y -= q.y; - z -= q.z; - w -= q.w; -} - -void Quat::operator*=(const real_t &s) { - x *= s; - y *= s; - z *= s; - w *= s; -} - -void Quat::operator/=(const real_t &s) { - - *this *= 1.0 / s; -} - -Quat Quat::operator+(const Quat &q2) const { - const Quat &q1 = *this; - return Quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); -} - -Quat Quat::operator-(const Quat &q2) const { - const Quat &q1 = *this; - return Quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); -} - -Quat Quat::operator-() const { - const Quat &q2 = *this; - return Quat(-q2.x, -q2.y, -q2.z, -q2.w); -} - -Quat Quat::operator*(const real_t &s) const { - return Quat(x * s, y * s, z * s, w * s); -} - -Quat Quat::operator/(const real_t &s) const { - return *this * (1.0 / s); -} - -bool Quat::operator==(const Quat &p_quat) const { - return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w; -} - -bool Quat::operator!=(const Quat &p_quat) const { - return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w; -} - -#endif diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp new file mode 100644 index 0000000000..3f1d2c58e5 --- /dev/null +++ b/core/math/quaternion.cpp @@ -0,0 +1,237 @@ +/*************************************************************************/ +/* quaternion.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "quaternion.h" + +#include "core/math/basis.h" +#include "core/string/print_string.h" + +real_t Quaternion::angle_to(const Quaternion &p_to) const { + real_t d = dot(p_to); + return Math::acos(CLAMP(d * d * 2 - 1, -1, 1)); +} + +// get_euler_xyz returns a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses XYZ convention (Z is the first rotation). +Vector3 Quaternion::get_euler_xyz() const { + Basis m(*this); + return m.get_euler_xyz(); +} + +// get_euler_yxz returns a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses YXZ convention (Z is the first rotation). +Vector3 Quaternion::get_euler_yxz() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized."); +#endif + Basis m(*this); + return m.get_euler_yxz(); +} + +void Quaternion::operator*=(const Quaternion &p_q) { + real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y; + real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z; + real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x; + w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z; + x = xx; + y = yy; + z = zz; +} + +Quaternion Quaternion::operator*(const Quaternion &p_q) const { + Quaternion r = *this; + r *= p_q; + return r; +} + +bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const { + return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w); +} + +real_t Quaternion::length() const { + return Math::sqrt(length_squared()); +} + +void Quaternion::normalize() { + *this /= length(); +} + +Quaternion Quaternion::normalized() const { + return *this / length(); +} + +bool Quaternion::is_normalized() const { + return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); //use less epsilon +} + +Quaternion Quaternion::inverse() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized."); +#endif + return Quaternion(-x, -y, -z, w); +} + +Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + Quaternion to1; + real_t omega, cosom, sinom, scale0, scale1; + + // calc cosine + cosom = dot(p_to); + + // adjust signs (if necessary) + if (cosom < 0.0) { + cosom = -cosom; + to1.x = -p_to.x; + to1.y = -p_to.y; + to1.z = -p_to.z; + to1.w = -p_to.w; + } else { + to1.x = p_to.x; + to1.y = p_to.y; + to1.z = p_to.z; + to1.w = p_to.w; + } + + // calculate coefficients + + if ((1.0 - cosom) > CMP_EPSILON) { + // standard case (slerp) + omega = Math::acos(cosom); + sinom = Math::sin(omega); + scale0 = Math::sin((1.0 - p_weight) * omega) / sinom; + scale1 = Math::sin(p_weight * omega) / sinom; + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = 1.0 - p_weight; + scale1 = p_weight; + } + // calculate final values + return Quaternion( + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w); +} + +Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + const Quaternion &from = *this; + + real_t dot = from.dot(p_to); + + if (Math::absf(dot) > 0.9999) { + return from; + } + + real_t theta = Math::acos(dot), + sinT = 1.0 / Math::sin(theta), + newFactor = Math::sin(p_weight * theta) * sinT, + invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; + + return Quaternion(invFactor * from.x + newFactor * p_to.x, + invFactor * from.y + newFactor * p_to.y, + invFactor * from.z + newFactor * p_to.z, + invFactor * from.w + newFactor * p_to.w); +} + +Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + //the only way to do slerp :| + real_t t2 = (1.0 - p_weight) * p_weight * 2; + Quaternion sp = this->slerp(p_b, p_weight); + Quaternion sq = p_pre_a.slerpni(p_post_b, p_weight); + return sp.slerpni(sq, t2); +} + +Quaternion::operator String() const { + return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")"; +} + +Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized."); +#endif + real_t d = p_axis.length(); + if (d == 0) { + x = 0; + y = 0; + z = 0; + w = 0; + } else { + real_t sin_angle = Math::sin(p_angle * 0.5); + real_t cos_angle = Math::cos(p_angle * 0.5); + real_t s = sin_angle / d; + x = p_axis.x * s; + y = p_axis.y * s; + z = p_axis.z * s; + w = cos_angle; + } +} + +// Euler constructor expects a vector containing the Euler angles in the format +// (ax, ay, az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses YXZ convention (Z is the first rotation). +Quaternion::Quaternion(const Vector3 &p_euler) { + real_t half_a1 = p_euler.y * 0.5; + real_t half_a2 = p_euler.x * 0.5; + real_t half_a3 = p_euler.z * 0.5; + + // R = Y(a1).X(a2).Z(a3) convention for Euler angles. + // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) + // a3 is the angle of the first rotation, following the notation in this reference. + + real_t cos_a1 = Math::cos(half_a1); + real_t sin_a1 = Math::sin(half_a1); + real_t cos_a2 = Math::cos(half_a2); + real_t sin_a2 = Math::sin(half_a2); + real_t cos_a3 = Math::cos(half_a3); + real_t sin_a3 = Math::sin(half_a3); + + x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3; + y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3; + z = -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3; + w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3; +} diff --git a/core/math/quaternion.h b/core/math/quaternion.h new file mode 100644 index 0000000000..35324323b3 --- /dev/null +++ b/core/math/quaternion.h @@ -0,0 +1,239 @@ +/*************************************************************************/ +/* quaternion.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 QUATERNION_H +#define QUATERNION_H + +#include "core/math/math_defs.h" +#include "core/math/math_funcs.h" +#include "core/math/vector3.h" +#include "core/string/ustring.h" + +class Quaternion { +public: + union { + struct { + real_t x; + real_t y; + real_t z; + real_t w; + }; + real_t components[4] = { 0, 0, 0, 1.0 }; + }; + + _FORCE_INLINE_ real_t &operator[](int idx) { + return components[idx]; + } + _FORCE_INLINE_ const real_t &operator[](int idx) const { + return components[idx]; + } + _FORCE_INLINE_ real_t length_squared() const; + bool is_equal_approx(const Quaternion &p_quaternion) const; + real_t length() const; + void normalize(); + Quaternion normalized() const; + bool is_normalized() const; + Quaternion inverse() const; + _FORCE_INLINE_ real_t dot(const Quaternion &p_q) const; + real_t angle_to(const Quaternion &p_to) const; + + Vector3 get_euler_xyz() const; + Vector3 get_euler_yxz() const; + Vector3 get_euler() const { return get_euler_yxz(); }; + + Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const; + Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const; + Quaternion cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const; + + _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { + r_angle = 2 * Math::acos(w); + real_t r = ((real_t)1) / Math::sqrt(1 - w * w); + r_axis.x = x * r; + r_axis.y = y * r; + r_axis.z = z * r; + } + + void operator*=(const Quaternion &p_q); + Quaternion operator*(const Quaternion &p_q) const; + + Quaternion operator*(const Vector3 &v) const { + return Quaternion(w * v.x + y * v.z - z * v.y, + w * v.y + z * v.x - x * v.z, + w * v.z + x * v.y - y * v.x, + -x * v.x - y * v.y - z * v.z); + } + + _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); +#endif + Vector3 u(x, y, z); + Vector3 uv = u.cross(v); + return v + ((uv * w) + u.cross(uv)) * ((real_t)2); + } + + _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const { + return inverse().xform(v); + } + + _FORCE_INLINE_ void operator+=(const Quaternion &p_q); + _FORCE_INLINE_ void operator-=(const Quaternion &p_q); + _FORCE_INLINE_ void operator*=(const real_t &s); + _FORCE_INLINE_ void operator/=(const real_t &s); + _FORCE_INLINE_ Quaternion operator+(const Quaternion &q2) const; + _FORCE_INLINE_ Quaternion operator-(const Quaternion &q2) const; + _FORCE_INLINE_ Quaternion operator-() const; + _FORCE_INLINE_ Quaternion operator*(const real_t &s) const; + _FORCE_INLINE_ Quaternion operator/(const real_t &s) const; + + _FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const; + _FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) const; + + operator String() const; + + _FORCE_INLINE_ Quaternion() {} + + _FORCE_INLINE_ Quaternion(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), + w(p_w) { + } + + Quaternion(const Vector3 &p_axis, real_t p_angle); + + Quaternion(const Vector3 &p_euler); + + Quaternion(const Quaternion &p_q) : + x(p_q.x), + y(p_q.y), + z(p_q.z), + w(p_q.w) { + } + + Quaternion &operator=(const Quaternion &p_q) { + x = p_q.x; + y = p_q.y; + z = p_q.z; + w = p_q.w; + return *this; + } + + Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc + { + Vector3 c = v0.cross(v1); + real_t d = v0.dot(v1); + + if (d < -1.0 + CMP_EPSILON) { + x = 0; + y = 1; + z = 0; + w = 0; + } else { + real_t s = Math::sqrt((1.0 + d) * 2.0); + real_t rs = 1.0 / s; + + x = c.x * rs; + y = c.y * rs; + z = c.z * rs; + w = s * 0.5; + } + } +}; + +real_t Quaternion::dot(const Quaternion &p_q) const { + return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w; +} + +real_t Quaternion::length_squared() const { + return dot(*this); +} + +void Quaternion::operator+=(const Quaternion &p_q) { + x += p_q.x; + y += p_q.y; + z += p_q.z; + w += p_q.w; +} + +void Quaternion::operator-=(const Quaternion &p_q) { + x -= p_q.x; + y -= p_q.y; + z -= p_q.z; + w -= p_q.w; +} + +void Quaternion::operator*=(const real_t &s) { + x *= s; + y *= s; + z *= s; + w *= s; +} + +void Quaternion::operator/=(const real_t &s) { + *this *= 1.0 / s; +} + +Quaternion Quaternion::operator+(const Quaternion &q2) const { + const Quaternion &q1 = *this; + return Quaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); +} + +Quaternion Quaternion::operator-(const Quaternion &q2) const { + const Quaternion &q1 = *this; + return Quaternion(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); +} + +Quaternion Quaternion::operator-() const { + const Quaternion &q2 = *this; + return Quaternion(-q2.x, -q2.y, -q2.z, -q2.w); +} + +Quaternion Quaternion::operator*(const real_t &s) const { + return Quaternion(x * s, y * s, z * s, w * s); +} + +Quaternion Quaternion::operator/(const real_t &s) const { + return *this * (1.0 / s); +} + +bool Quaternion::operator==(const Quaternion &p_quaternion) const { + return x == p_quaternion.x && y == p_quaternion.y && z == p_quaternion.z && w == p_quaternion.w; +} + +bool Quaternion::operator!=(const Quaternion &p_quaternion) const { + return x != p_quaternion.x || y != p_quaternion.y || z != p_quaternion.z || w != p_quaternion.w; +} + +_FORCE_INLINE_ Quaternion operator*(const real_t &p_real, const Quaternion &p_quaternion) { + return p_quaternion * p_real; +} + +#endif // QUATERNION_H diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index 63dd18091f..0d77bfe933 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,17 +30,15 @@ #include "quick_hull.h" -#include "core/map.h" +#include "core/templates/map.h" 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,36 +98,34 @@ 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; } } } - //fourth vertex is the one most further away from the plane + //fourth vertex is the one most further away from the plane { real_t maxd = 0; 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,10 +266,9 @@ 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 + continue; //edge is uninteresting, not on horizon } //create new face! @@ -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,9 +377,9 @@ 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 == NULL); + ERR_CONTINUE(O == nullptr); if (O->get().plane.is_equal_approx(f.plane)) { //merge and delete edge and contiguous face, while repointing edges (uuugh!) @@ -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) - G->get().left = NULL; + if (G->get().left == O) { + G->get().left = nullptr; + } - if (G->get().right == O) - G->get().right = NULL; + 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 aea9ffad8b..48ea139cc9 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,19 +31,17 @@ #ifndef QUICK_HULL_H #define QUICK_HULL_H -#include "core/list.h" #include "core/math/aabb.h" -#include "core/math/geometry.h" -#include "core/set.h" +#include "core/math/geometry_3d.h" +#include "core/templates/list.h" +#include "core/templates/set.h" class QuickHull { - public: struct Edge { - union { uint32_t vertices[2]; - uint64_t id; + uint64_t id = 0; }; bool operator<(const Edge &p_edge) const { @@ -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,30 @@ public: }; struct Face { - Plane plane; - uint32_t vertices[3]; + uint32_t vertices[3] = { 0 }; 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 = NULL; - right = NULL; - } + List<Face>::Element *left = nullptr; + List<Face>::Element *right = nullptr; + FaceConnect() {} }; struct RetFaceConnect { - List<Geometry::MeshData::Face>::Element *left, *right; - RetFaceConnect() { - left = NULL; - right = NULL; - } + List<Geometry3D::MeshData::Face>::Element *left = nullptr; + List<Geometry3D::MeshData::Face>::Element *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..b40d010219 100644 --- a/core/math/random_number_generator.cpp +++ b/core/math/random_number_generator.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,12 +30,12 @@ #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); - ADD_PROPERTY(PropertyInfo(Variant::INT, "seed"), "set_seed", "get_seed"); + + ClassDB::bind_method(D_METHOD("set_state", "state"), &RandomNumberGenerator::set_state); + ClassDB::bind_method(D_METHOD("get_state"), &RandomNumberGenerator::get_state); ClassDB::bind_method(D_METHOD("randi"), &RandomNumberGenerator::randi); ClassDB::bind_method(D_METHOD("randf"), &RandomNumberGenerator::randf); @@ -43,4 +43,10 @@ void RandomNumberGenerator::_bind_methods() { ClassDB::bind_method(D_METHOD("randf_range", "from", "to"), &RandomNumberGenerator::randf_range); ClassDB::bind_method(D_METHOD("randi_range", "from", "to"), &RandomNumberGenerator::randi_range); ClassDB::bind_method(D_METHOD("randomize"), &RandomNumberGenerator::randomize); + + ADD_PROPERTY(PropertyInfo(Variant::INT, "seed"), "set_seed", "get_seed"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "state"), "set_state", "get_state"); + // Default values are non-deterministic, override for doc generation purposes. + ADD_PROPERTY_DEFAULT("seed", 0); + ADD_PROPERTY_DEFAULT("state", 0); } diff --git a/core/math/random_number_generator.h b/core/math/random_number_generator.h index e7f188bb42..06cd3999f3 100644 --- a/core/math/random_number_generator.h +++ b/core/math/random_number_generator.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,40 +32,32 @@ #define RANDOM_NUMBER_GENERATOR_H #include "core/math/random_pcg.h" -#include "core/reference.h" +#include "core/object/ref_counted.h" -class RandomNumberGenerator : public Reference { - GDCLASS(RandomNumberGenerator, Reference); +class RandomNumberGenerator : public RefCounted { + GDCLASS(RandomNumberGenerator, RefCounted); +protected: RandomPCG randbase; -protected: static void _bind_methods(); public: - _FORCE_INLINE_ void set_seed(uint64_t seed) { randbase.seed(seed); } - + _FORCE_INLINE_ void set_seed(uint64_t p_seed) { randbase.seed(p_seed); } _FORCE_INLINE_ uint64_t get_seed() { return randbase.get_seed(); } + _FORCE_INLINE_ void set_state(uint64_t p_state) { randbase.set_state(p_state); } + _FORCE_INLINE_ uint64_t get_state() const { return randbase.get_state(); } + _FORCE_INLINE_ void randomize() { randbase.randomize(); } _FORCE_INLINE_ uint32_t randi() { return randbase.rand(); } - _FORCE_INLINE_ real_t randf() { return randbase.randf(); } + _FORCE_INLINE_ real_t randf_range(real_t p_from, real_t p_to) { return randbase.random(p_from, p_to); } + _FORCE_INLINE_ real_t randfn(real_t p_mean = 0.0, real_t p_deviation = 1.0) { return randbase.randfn(p_mean, p_deviation); } + _FORCE_INLINE_ int randi_range(int p_from, int p_to) { return randbase.random(p_from, p_to); } - _FORCE_INLINE_ real_t randf_range(real_t from, real_t to) { return randbase.random(from, to); } - - _FORCE_INLINE_ real_t randfn(real_t mean = 0.0, real_t deviation = 1.0) { return randbase.randfn(mean, deviation); } - - _FORCE_INLINE_ int randi_range(int from, int to) { - unsigned int ret = randbase.rand(); - if (to < from) - return ret % (from - to + 1) + to; - else - return ret % (to - from + 1) + from; - } - - RandomNumberGenerator(); + RandomNumberGenerator() {} }; #endif // RANDOM_NUMBER_GENERATOR_H diff --git a/core/math/random_pcg.cpp b/core/math/random_pcg.cpp index 02257c38d9..681c2a9717 100644 --- a/core/math/random_pcg.cpp +++ b/core/math/random_pcg.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -39,7 +39,7 @@ RandomPCG::RandomPCG(uint64_t p_seed, uint64_t p_inc) : } void RandomPCG::randomize() { - seed(OS::get_singleton()->get_ticks_usec() * pcg.state + PCG_DEFAULT_INC_64); + seed(((uint64_t)OS::get_singleton()->get_unix_time() + OS::get_singleton()->get_ticks_usec()) * pcg.state + PCG_DEFAULT_INC_64); } double RandomPCG::random(double p_from, double p_to) { @@ -49,3 +49,10 @@ double RandomPCG::random(double p_from, double p_to) { float RandomPCG::random(float p_from, float p_to) { return randf() * (p_to - p_from) + p_from; } + +int RandomPCG::random(int p_from, int p_to) { + if (p_from == p_to) { + return p_from; + } + return rand(abs(p_from - p_to) + 1) + MIN(p_from, p_to); +} diff --git a/core/math/random_pcg.h b/core/math/random_pcg.h index ac65ce3509..5a03b758ce 100644 --- a/core/math/random_pcg.h +++ b/core/math/random_pcg.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -31,16 +31,16 @@ #ifndef RANDOM_PCG_H #define RANDOM_PCG_H -#include <math.h> - #include "core/math/math_defs.h" #include "thirdparty/misc/pcg.h" -#if defined(__GNUC__) || (_llvm_has_builtin(__builtin_clz)) +#include <math.h> + +#if defined(__GNUC__) #define CLZ32(x) __builtin_clz(x) #elif defined(_MSC_VER) -#include "intrin.h" +#include <intrin.h> static int __bsr_clz32(uint32_t x) { unsigned long index; _BitScanReverse(&index, x); @@ -50,24 +50,23 @@ static int __bsr_clz32(uint32_t x) { #else #endif -#if defined(__GNUC__) || (_llvm_has_builtin(__builtin_ldexp) && _llvm_has_builtin(__builtin_ldexpf)) +#if defined(__GNUC__) #define LDEXP(s, e) __builtin_ldexp(s, e) #define LDEXPF(s, e) __builtin_ldexpf(s, e) #else -#include "math.h" +#include <math.h> #define LDEXP(s, e) ldexp(s, e) #define LDEXPF(s, e) ldexp(s, e) #endif class RandomPCG { pcg32_random_t pcg; - uint64_t current_seed; // seed with this to get the same state + uint64_t current_seed; // The seed the current generator state started from. uint64_t current_inc; public: static const uint64_t DEFAULT_SEED = 12047754176567800795U; static const uint64_t DEFAULT_INC = PCG_DEFAULT_INC_64; - static const uint64_t RANDOM_MAX = 0xFFFFFFFF; RandomPCG(uint64_t p_seed = DEFAULT_SEED, uint64_t p_inc = DEFAULT_INC); @@ -77,11 +76,16 @@ public: } _FORCE_INLINE_ uint64_t get_seed() { return current_seed; } + _FORCE_INLINE_ void set_state(uint64_t p_state) { pcg.state = p_state; } + _FORCE_INLINE_ uint64_t get_state() const { return pcg.state; } + void randomize(); _FORCE_INLINE_ uint32_t rand() { - current_seed = pcg.state; return pcg32_random_r(&pcg); } + _FORCE_INLINE_ uint32_t rand(uint32_t bounds) { + return pcg32_boundedrand_r(&pcg, bounds); + } // Obtaining floating point numbers in [0, 1] range with "good enough" uniformity. // These functions sample the output of rand() as the fraction part of an infinite binary number, @@ -130,7 +134,7 @@ public: double random(double p_from, double p_to); float random(float p_from, float p_to); - real_t random(int p_from, int p_to) { return (real_t)random((real_t)p_from, (real_t)p_to); } + int random(int p_from, int p_to); }; #endif // RANDOM_PCG_H diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index 12b9904c88..f64bf560c8 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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,20 @@ 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; } + +Rect2::operator String() const { + return "[P: " + position.operator String() + ", S: " + size + "]"; +} + +Rect2i::operator String() const { + return "[P: " + position.operator String() + ", S: " + size + "]"; +} diff --git a/core/math/rect2.h b/core/math/rect2.h index 9017377770..ab0b489b4a 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -36,7 +36,6 @@ struct Transform2D; struct Rect2 { - Point2 position; Size2 size; @@ -47,21 +46,39 @@ struct Rect2 { real_t get_area() const { return size.width * size.height; } - inline bool intersects(const Rect2 &p_rect) const { - if (position.x >= (p_rect.position.x + p_rect.size.width)) - return false; - if ((position.x + size.width) <= p_rect.position.x) - return false; - if (position.y >= (p_rect.position.y + p_rect.size.height)) - return false; - if ((position.y + size.height) <= p_rect.position.y) - return false; + 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)) { + return false; + } + if ((position.x + size.width) < p_rect.position.x) { + return false; + } + if (position.y > (p_rect.position.y + p_rect.size.height)) { + return false; + } + if ((position.y + size.height) < p_rect.position.y) { + return false; + } + } else { + if (position.x >= (p_rect.position.x + p_rect.size.width)) { + return false; + } + if ((position.x + size.width) <= p_rect.position.x) { + return false; + } + if (position.y >= (p_rect.position.y + p_rect.size.height)) { + return false; + } + 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; @@ -86,33 +103,34 @@ 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; - bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = NULL, Point2 *r_normal = NULL) const; + 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 + // Returns the instersection between two Rect2s or an empty Rect2 if there is no intersection + inline Rect2 intersection(const Rect2 &p_rect) const { 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); @@ -139,17 +157,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; } @@ -158,27 +180,33 @@ struct Rect2 { bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } - inline Rect2 grow(real_t p_by) const { - + inline Rect2 grow(real_t p_amount) const { Rect2 g = *this; - g.position.x -= p_by; - g.position.y -= p_by; - g.size.width += p_by * 2; - g.size.height += p_by * 2; + g.grow_by(p_amount); return g; } - inline Rect2 grow_margin(Margin p_margin, real_t p_amount) const { + inline void grow_by(real_t p_amount) { + position.x -= p_amount; + position.y -= p_amount; + size.width += p_amount * 2; + size.height += p_amount * 2; + } + + inline Rect2 grow_side(Side p_side, real_t p_amount) const { Rect2 g = *this; - g = g.grow_individual((MARGIN_LEFT == p_margin) ? p_amount : 0, - (MARGIN_TOP == p_margin) ? p_amount : 0, - (MARGIN_RIGHT == p_margin) ? p_amount : 0, - (MARGIN_BOTTOM == p_margin) ? p_amount : 0); + g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0, + (SIDE_TOP == p_side) ? p_amount : 0, + (SIDE_RIGHT == p_side) ? p_amount : 0, + (SIDE_BOTTOM == p_side) ? p_amount : 0); return g; } - inline Rect2 grow_individual(real_t p_left, real_t p_top, real_t p_right, real_t p_bottom) const { + inline Rect2 grow_side_bind(uint32_t p_side, real_t p_amount) const { + return grow_side(Side(p_side), p_amount); + } + 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; @@ -189,7 +217,6 @@ struct Rect2 { } _FORCE_INLINE_ Rect2 expand(const Vector2 &p_vector) const { - Rect2 r = *this; r.expand_to(p_vector); return r; @@ -200,26 +227,100 @@ 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()); } - operator String() const { return String(position) + ", " + String(size); } + Vector2 get_support(const Vector2 &p_normal) const { + Vector2 half_extents = size * 0.5; + Vector2 ofs = position + half_extents; + return Vector2( + (p_normal.x > 0) ? -half_extents.x : half_extents.x, + (p_normal.y > 0) ? -half_extents.y : half_extents.y) + + ofs; + } + + _FORCE_INLINE_ bool intersects_filled_polygon(const Vector2 *p_points, int p_point_count) const { + Vector2 center = position + size * 0.5; + int side_plus = 0; + int side_minus = 0; + Vector2 end = position + size; + + int i_f = p_point_count - 1; + for (int i = 0; i < p_point_count; i++) { + const Vector2 &a = p_points[i_f]; + const Vector2 &b = p_points[i]; + i_f = i; + + Vector2 r = (b - a); + float l = r.length(); + if (l == 0.0) { + continue; + } + + //check inside + Vector2 tg = r.orthogonal(); + float s = tg.dot(center) - tg.dot(a); + if (s < 0.0) { + side_plus++; + } else { + side_minus++; + } + + //check ray box + r /= l; + Vector2 ir(1.0 / r.x, 1.0 / r.y); + + // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner + // r.org is origin of ray + Vector2 t13 = (position - a) * ir; + Vector2 t24 = (end - a) * ir; + + float tmin = MAX(MIN(t13.x, t24.x), MIN(t13.y, t24.y)); + float tmax = MIN(MAX(t13.x, t24.x), MAX(t13.y, t24.y)); + + // if tmax < 0, ray (line) is intersecting AABB, but the whole AABB is behind us + if (tmax < 0 || tmin > tmax || tmin >= l) { + continue; + } + + return true; + } + + if (side_plus * side_minus == 0) { + return true; //all inside + } else { + return false; + } + } + + _FORCE_INLINE_ void set_end(const Vector2 &p_end) { + size = p_end - position; + } + + _FORCE_INLINE_ Vector2 get_end() const { + return position + size; + } + + operator String() const; Rect2() {} Rect2(real_t p_x, real_t p_y, real_t p_width, real_t p_height) : @@ -233,7 +334,6 @@ struct Rect2 { }; struct Rect2i { - Point2i position; Size2i size; @@ -245,41 +345,45 @@ 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 + // Returns the instersection between two Rect2is or an empty Rect2i if there is no intersection + inline Rect2i intersection(const Rect2i &p_rect) const { 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); - Point2 p_rect_end = p_rect.position + p_rect.size; - Point2 end = position + size; + Point2i p_rect_end = p_rect.position + p_rect.size; + Point2i end = position + size; new_rect.size.x = (int)(MIN(p_rect_end.x, end.x) - new_rect.position.x); new_rect.size.y = (int)(MIN(p_rect_end.y, end.y) - new_rect.position.y); @@ -300,17 +404,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) + } + bool has_point(const Point2i &p_point) const { + 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; } @@ -318,27 +426,29 @@ struct Rect2i { bool operator==(const Rect2i &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2i &p_rect) const { return position != p_rect.position || size != p_rect.size; } - Rect2i grow(int p_by) const { - + Rect2i grow(int p_amount) const { Rect2i g = *this; - g.position.x -= p_by; - g.position.y -= p_by; - g.size.width += p_by * 2; - g.size.height += p_by * 2; + g.position.x -= p_amount; + g.position.y -= p_amount; + g.size.width += p_amount * 2; + g.size.height += p_amount * 2; return g; } - inline Rect2i grow_margin(Margin p_margin, int p_amount) const { + inline Rect2i grow_side(Side p_side, int p_amount) const { Rect2i g = *this; - g = g.grow_individual((MARGIN_LEFT == p_margin) ? p_amount : 0, - (MARGIN_TOP == p_margin) ? p_amount : 0, - (MARGIN_RIGHT == p_margin) ? p_amount : 0, - (MARGIN_BOTTOM == p_margin) ? p_amount : 0); + g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0, + (SIDE_TOP == p_side) ? p_amount : 0, + (SIDE_RIGHT == p_side) ? p_amount : 0, + (SIDE_BOTTOM == p_side) ? p_amount : 0); return g; } - inline Rect2i grow_individual(int p_left, int p_top, int p_right, int p_bottom) const { + inline Rect2i grow_side_bind(uint32_t p_side, int p_amount) const { + return grow_side(Side(p_side), p_amount); + } + 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; @@ -349,44 +459,59 @@ 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; } - operator String() const { return String(position) + ", " + String(size); } + _FORCE_INLINE_ Rect2i abs() const { + return Rect2i(Point2i(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); + } + + _FORCE_INLINE_ void set_end(const Vector2i &p_end) { + size = p_end - position; + } + + _FORCE_INLINE_ Vector2i get_end() const { + return position + size; + } + + operator String() const; 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)) { + position(Point2i(p_x, p_y)), + size(Size2i(p_width, p_height)) { } - Rect2i(const Point2 &p_pos, const Size2 &p_size) : + Rect2i(const Point2i &p_pos, const Size2i &p_size) : position(p_pos), size(p_size) { } diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index f28b664e46..16934d67df 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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,43 @@ 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 { +Transform2D Transform2D::looking_at(const Vector2 &p_target) const { + Transform2D return_trans = Transform2D(get_rotation(), get_origin()); + Vector2 target_position = affine_inverse().xform(p_target); + return_trans.set_rotation(return_trans.get_rotation() + (target_position * get_scale()).angle()); + return return_trans; +} +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 +202,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(); @@ -262,12 +258,12 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t real_t dot = v1.dot(v2); - dot = (dot < -1.0) ? -1.0 : ((dot > 1.0) ? 1.0 : dot); //clamp dot to [-1,1] + dot = CLAMP(dot, -1.0, 1.0); Vector2 v; if (dot > 0.9995) { - v = Vector2::linear_interpolate(v1, v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues + v = v1.lerp(v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues } else { real_t angle = p_c * Math::acos(dot); Vector2 v3 = (v2 - v1 * dot).normalized(); @@ -275,12 +271,25 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t } //construct matrix - Transform2D res(Math::atan2(v.y, v.x), Vector2::linear_interpolate(p1, p2, p_c)); - res.scale_basis(Vector2::linear_interpolate(s1, s2, p_c)); + Transform2D res(Math::atan2(v.y, v.x), p1.lerp(p2, p_c)); + res.scale_basis(s1.lerp(s2, p_c)); return res; } -Transform2D::operator String() const { +void Transform2D::operator*=(const real_t p_val) { + elements[0] *= p_val; + elements[1] *= p_val; + elements[2] *= p_val; +} + +Transform2D Transform2D::operator*(const real_t p_val) const { + Transform2D ret(*this); + ret *= p_val; + return ret; +} - return String(String() + elements[0] + ", " + elements[1] + ", " + elements[2]); +Transform2D::operator String() const { + return "[X: " + elements[0].operator String() + + ", Y: " + elements[1].operator String() + + ", O: " + elements[2].operator String() + "]"; } diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index 367f697ccf..34cfd0c1a9 100644 --- a/core/math/transform_2d.h +++ b/core/math/transform_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,7 +32,6 @@ #define TRANSFORM_2D_H #include "core/math/rect2.h" // also includes vector2, math_funcs, and ustring -#include "core/pool_vector.h" struct Transform2D { // Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper": @@ -71,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); @@ -98,11 +100,15 @@ struct Transform2D { Transform2D orthonormalized() const; bool is_equal_approx(const Transform2D &p_transform) const; + Transform2D looking_at(const Vector2 &p_target) const; + bool operator==(const Transform2D &p_transform) const; bool operator!=(const Transform2D &p_transform) const; void operator*=(const Transform2D &p_transform); Transform2D operator*(const Transform2D &p_transform) const; + void operator*=(const real_t p_val); + Transform2D operator*(const real_t p_val) const; Transform2D interpolate_with(const Transform2D &p_transform, real_t p_c) const; @@ -112,13 +118,12 @@ struct Transform2D { _FORCE_INLINE_ Vector2 xform_inv(const Vector2 &p_vec) const; _FORCE_INLINE_ Rect2 xform(const Rect2 &p_rect) const; _FORCE_INLINE_ Rect2 xform_inv(const Rect2 &p_rect) const; - _FORCE_INLINE_ PoolVector<Vector2> xform(const PoolVector<Vector2> &p_array) const; - _FORCE_INLINE_ PoolVector<Vector2> xform_inv(const PoolVector<Vector2> &p_array) const; + _FORCE_INLINE_ Vector<Vector2> xform(const Vector<Vector2> &p_array) const; + _FORCE_INLINE_ Vector<Vector2> xform_inv(const Vector<Vector2> &p_array) const; 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; @@ -127,6 +132,12 @@ struct Transform2D { elements[2][1] = oy; } + Transform2D(const Vector2 &p_x, const Vector2 &p_y, const Vector2 &p_origin) { + elements[0] = p_x; + elements[1] = p_y; + elements[2] = p_origin; + } + Transform2D(real_t p_rot, const Vector2 &p_pos); Transform2D() { elements[0][0] = 1.0; @@ -135,36 +146,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); @@ -178,15 +186,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,13 +216,12 @@ Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { return new_rect; } -PoolVector<Vector2> Transform2D::xform(const PoolVector<Vector2> &p_array) const { - - PoolVector<Vector2> array; +Vector<Vector2> Transform2D::xform(const Vector<Vector2> &p_array) const { + Vector<Vector2> array; array.resize(p_array.size()); - PoolVector<Vector2>::Read r = p_array.read(); - PoolVector<Vector2>::Write w = array.write(); + const Vector2 *r = p_array.ptr(); + Vector2 *w = array.ptrw(); for (int i = 0; i < p_array.size(); ++i) { w[i] = xform(r[i]); @@ -217,13 +229,12 @@ PoolVector<Vector2> Transform2D::xform(const PoolVector<Vector2> &p_array) const return array; } -PoolVector<Vector2> Transform2D::xform_inv(const PoolVector<Vector2> &p_array) const { - - PoolVector<Vector2> array; +Vector<Vector2> Transform2D::xform_inv(const Vector<Vector2> &p_array) const { + Vector<Vector2> array; array.resize(p_array.size()); - PoolVector<Vector2>::Read r = p_array.read(); - PoolVector<Vector2>::Write w = array.write(); + const Vector2 *r = p_array.ptr(); + Vector2 *w = array.ptrw(); for (int i = 0; i < p_array.size(); ++i) { w[i] = xform_inv(r[i]); diff --git a/core/math/transform.cpp b/core/math/transform_3d.cpp index 9dad3262d2..51766b39f4 100644 --- a/core/math/transform.cpp +++ b/core/math/transform_3d.cpp @@ -1,12 +1,12 @@ /*************************************************************************/ -/* transform.cpp */ +/* transform_3d.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). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -28,62 +28,54 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "transform.h" +#include "transform_3d.h" #include "core/math/math_funcs.h" -#include "core/os/copymem.h" -#include "core/print_string.h" - -void Transform::affine_invert() { +#include "core/string/print_string.h" +void Transform3D::affine_invert() { basis.invert(); origin = basis.xform(-origin); } -Transform Transform::affine_inverse() const { - - Transform ret = *this; +Transform3D Transform3D::affine_inverse() const { + Transform3D ret = *this; ret.affine_invert(); return ret; } -void Transform::invert() { - +void Transform3D::invert() { basis.transpose(); origin = basis.xform(-origin); } -Transform Transform::inverse() const { +Transform3D Transform3D::inverse() const { // FIXME: this function assumes the basis is a rotation matrix, with no scaling. - // Transform::affine_inverse can handle matrices with scaling, so GDScript should eventually use that. - Transform ret = *this; + // Transform3D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that. + Transform3D ret = *this; ret.invert(); return ret; } -void Transform::rotate(const Vector3 &p_axis, real_t p_phi) { - +void Transform3D::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); +Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_phi) const { + return Transform3D(Basis(p_axis, p_phi), Vector3()) * (*this); } -void Transform::rotate_basis(const Vector3 &p_axis, real_t p_phi) { - +void Transform3D::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; +Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up) const { + Transform3D t = *this; t.set_look_at(origin, p_target, p_up); return t; } -void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { +void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { #ifdef MATH_CHECKS ERR_FAIL_COND(p_eye == p_target); ERR_FAIL_COND(p_up.length() == 0); @@ -116,110 +108,119 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const origin = p_eye; } -Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) const { - +Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { /* not sure if very "efficient" but good enough? */ Vector3 src_scale = basis.get_scale(); - Quat src_rot = basis.get_rotation_quat(); + Quaternion src_rot = basis.get_rotation_quaternion(); Vector3 src_loc = origin; Vector3 dst_scale = p_transform.basis.get_scale(); - Quat dst_rot = p_transform.basis.get_rotation_quat(); + Quaternion dst_rot = p_transform.basis.get_rotation_quaternion(); Vector3 dst_loc = p_transform.origin; - Transform interp; - interp.basis.set_quat_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.linear_interpolate(dst_scale, p_c)); - interp.origin = src_loc.linear_interpolate(dst_loc, p_c); + Transform3D interp; + interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c)); + interp.origin = src_loc.lerp(dst_loc, p_c); return interp; } -void Transform::scale(const Vector3 &p_scale) { - +void Transform3D::scale(const Vector3 &p_scale) { basis.scale(p_scale); origin *= p_scale; } -Transform Transform::scaled(const Vector3 &p_scale) const { - - Transform t = *this; +Transform3D Transform3D::scaled(const Vector3 &p_scale) const { + Transform3D t = *this; t.scale(p_scale); return t; } -void Transform::scale_basis(const Vector3 &p_scale) { - +void Transform3D::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) { +void Transform3D::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 Transform3D::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; +Transform3D Transform3D::translated(const Vector3 &p_translation) const { + Transform3D t = *this; t.translate(p_translation); return t; } -void Transform::orthonormalize() { - +void Transform3D::orthonormalize() { basis.orthonormalize(); } -Transform Transform::orthonormalized() const { - - Transform _copy = *this; +Transform3D Transform3D::orthonormalized() const { + Transform3D _copy = *this; _copy.orthonormalize(); return _copy; } -bool Transform::is_equal_approx(const Transform &p_transform) const { - +bool Transform3D::is_equal_approx(const Transform3D &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 { - +bool Transform3D::operator==(const Transform3D &p_transform) const { return (basis == p_transform.basis && origin == p_transform.origin); } -bool Transform::operator!=(const Transform &p_transform) const { +bool Transform3D::operator!=(const Transform3D &p_transform) const { return (basis != p_transform.basis || origin != p_transform.origin); } -void Transform::operator*=(const Transform &p_transform) { - +void Transform3D::operator*=(const Transform3D &p_transform) { origin = xform(p_transform.origin); basis *= p_transform.basis; } -Transform Transform::operator*(const Transform &p_transform) const { - - Transform t = *this; +Transform3D Transform3D::operator*(const Transform3D &p_transform) const { + Transform3D t = *this; t *= p_transform; return t; } -Transform::operator String() const { +void Transform3D::operator*=(const real_t p_val) { + origin *= p_val; + basis *= p_val; +} + +Transform3D Transform3D::operator*(const real_t p_val) const { + Transform3D ret(*this); + ret *= p_val; + return ret; +} - return basis.operator String() + " - " + origin.operator String(); +Transform3D::operator String() const { + return "[X: " + basis.get_axis(0).operator String() + + ", Y: " + basis.get_axis(1).operator String() + + ", Z: " + basis.get_axis(2).operator String() + + ", O: " + origin.operator String() + "]"; } -Transform::Transform(const Basis &p_basis, const Vector3 &p_origin) : +Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) : basis(p_basis), origin(p_origin) { } -Transform::Transform(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 ox, real_t oy, real_t oz) { +Transform3D::Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) : + origin(p_origin) { + basis.set_axis(0, p_x); + basis.set_axis(1, p_y); + basis.set_axis(2, p_z); +} + +Transform3D::Transform3D(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 ox, real_t oy, real_t oz) { basis = Basis(xx, xy, xz, yx, yy, yz, zx, zy, zz); origin = Vector3(ox, oy, oz); } diff --git a/core/math/transform.h b/core/math/transform_3d.h index ad397d9c09..3d8e70cec7 100644 --- a/core/math/transform.h +++ b/core/math/transform_3d.h @@ -1,12 +1,12 @@ /*************************************************************************/ -/* transform.h */ +/* transform_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). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -34,33 +34,32 @@ #include "core/math/aabb.h" #include "core/math/basis.h" #include "core/math/plane.h" -#include "core/pool_vector.h" -class Transform { +class Transform3D { public: Basis basis; Vector3 origin; void invert(); - Transform inverse() const; + Transform3D inverse() const; void affine_invert(); - Transform affine_inverse() const; + Transform3D affine_inverse() const; - Transform rotated(const Vector3 &p_axis, real_t p_phi) const; + Transform3D rotated(const Vector3 &p_axis, real_t p_phi) const; void rotate(const Vector3 &p_axis, real_t p_phi); void rotate_basis(const Vector3 &p_axis, real_t p_phi); - void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up); - Transform looking_at(const Vector3 &p_target, const Vector3 &p_up) const; + void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)); + Transform3D looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)) const; void scale(const Vector3 &p_scale); - Transform scaled(const Vector3 &p_scale) const; + Transform3D scaled(const Vector3 &p_scale) const; void scale_basis(const Vector3 &p_scale); void translate(real_t p_tx, real_t p_ty, real_t p_tz); void translate(const Vector3 &p_translation); - Transform translated(const Vector3 &p_translation) const; + Transform3D translated(const Vector3 &p_translation) const; const Basis &get_basis() const { return basis; } void set_basis(const Basis &p_basis) { basis = p_basis; } @@ -69,11 +68,11 @@ public: void set_origin(const Vector3 &p_origin) { origin = p_origin; } void orthonormalize(); - Transform orthonormalized() const; - bool is_equal_approx(const Transform &p_transform) const; + Transform3D orthonormalized() const; + bool is_equal_approx(const Transform3D &p_transform) const; - bool operator==(const Transform &p_transform) const; - bool operator!=(const Transform &p_transform) const; + bool operator==(const Transform3D &p_transform) const; + bool operator!=(const Transform3D &p_transform) const; _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const; _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const; @@ -84,23 +83,23 @@ public: _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const; _FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const; - _FORCE_INLINE_ PoolVector<Vector3> xform(const PoolVector<Vector3> &p_array) const; - _FORCE_INLINE_ PoolVector<Vector3> xform_inv(const PoolVector<Vector3> &p_array) const; + _FORCE_INLINE_ Vector<Vector3> xform(const Vector<Vector3> &p_array) const; + _FORCE_INLINE_ Vector<Vector3> xform_inv(const Vector<Vector3> &p_array) const; - void operator*=(const Transform &p_transform); - Transform operator*(const Transform &p_transform) const; + void operator*=(const Transform3D &p_transform); + Transform3D operator*(const Transform3D &p_transform) const; + void operator*=(const real_t p_val); + Transform3D operator*(const real_t p_val) const; - Transform interpolate_with(const Transform &p_transform, real_t p_c) const; - - _FORCE_INLINE_ Transform inverse_xform(const Transform &t) const { + Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const; + _FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const { Vector3 v = t.origin - origin; - return Transform(basis.transpose_xform(t.basis), + return Transform3D(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; @@ -109,20 +108,20 @@ public: operator String() const; - Transform(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 ox, real_t oy, real_t oz); - Transform(const Basis &p_basis, const Vector3 &p_origin = Vector3()); - Transform() {} + Transform3D() {} + Transform3D(const Basis &p_basis, const Vector3 &p_origin = Vector3()); + Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin); + Transform3D(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 ox, real_t oy, real_t oz); }; -_FORCE_INLINE_ Vector3 Transform::xform(const Vector3 &p_vector) const { - +_FORCE_INLINE_ Vector3 Transform3D::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 Transform3D::xform_inv(const Vector3 &p_vector) const { Vector3 v = p_vector - origin; return Vector3( @@ -131,8 +130,7 @@ _FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { (basis.elements[0][2] * v.x) + (basis.elements[1][2] * v.y) + (basis.elements[2][2] * v.z)); } -_FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const { - +_FORCE_INLINE_ Plane Transform3D::xform(const Plane &p_plane) const { Vector3 point = p_plane.normal * p_plane.d; Vector3 point_dir = point + p_plane.normal; point = xform(point); @@ -144,12 +142,12 @@ _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 Transform3D::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); - xform_inv(point_dir); + point = xform_inv(point); + point_dir = xform_inv(point_dir); Vector3 normal = point_dir - point; normal.normalize(); @@ -158,8 +156,7 @@ _FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { return Plane(normal, d); } -_FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { - +_FORCE_INLINE_ AABB Transform3D::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,8 +181,7 @@ _FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { return r_aabb; } -_FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { - +_FORCE_INLINE_ AABB Transform3D::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), @@ -203,20 +199,18 @@ _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])); } return ret; } -PoolVector<Vector3> Transform::xform(const PoolVector<Vector3> &p_array) const { - - PoolVector<Vector3> array; +Vector<Vector3> Transform3D::xform(const Vector<Vector3> &p_array) const { + Vector<Vector3> array; array.resize(p_array.size()); - PoolVector<Vector3>::Read r = p_array.read(); - PoolVector<Vector3>::Write w = array.write(); + const Vector3 *r = p_array.ptr(); + Vector3 *w = array.ptrw(); for (int i = 0; i < p_array.size(); ++i) { w[i] = xform(r[i]); @@ -224,13 +218,12 @@ PoolVector<Vector3> Transform::xform(const PoolVector<Vector3> &p_array) const { return array; } -PoolVector<Vector3> Transform::xform_inv(const PoolVector<Vector3> &p_array) const { - - PoolVector<Vector3> array; +Vector<Vector3> Transform3D::xform_inv(const Vector<Vector3> &p_array) const { + Vector<Vector3> array; array.resize(p_array.size()); - PoolVector<Vector3>::Read r = p_array.read(); - PoolVector<Vector3>::Write w = array.write(); + const Vector3 *r = p_array.ptr(); + Vector3 *w = array.ptrw(); for (int i = 0; i < p_array.size(); ++i) { w[i] = xform_inv(r[i]); diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 53d4ea0a96..903d5951a8 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -30,33 +30,28 @@ #include "triangle_mesh.h" -#include "core/sort_array.h" +#include "core/templates/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]); @@ -89,18 +84,18 @@ int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, in return index; } -void TriangleMesh::get_indices(PoolVector<int> *r_triangles_indices) const { - - if (!valid) +void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const { + if (!valid) { return; + } const int triangles_num = triangles.size(); // Parse vertices indices - PoolVector<Triangle>::Read triangles_read = triangles.read(); + const Triangle *triangles_read = triangles.ptr(); r_triangles_indices->resize(triangles_num * 3); - PoolVector<int>::Write r_indices_write = r_triangles_indices->write(); + int *r_indices_write = r_triangles_indices->ptrw(); for (int i = 0; i < triangles_num; ++i) { r_indices_write[3 * i + 0] = triangles_read[i].indices[0]; @@ -109,8 +104,7 @@ void TriangleMesh::get_indices(PoolVector<int> *r_triangles_indices) const { } } -void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { - +void TriangleMesh::create(const Vector<Vector3> &p_faces) { valid = false; int fc = p_faces.size(); @@ -119,25 +113,22 @@ void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { triangles.resize(fc); bvh.resize(fc * 3); //will never be larger than this (todo make better) - PoolVector<BVH>::Write bw = bvh.write(); + BVH *bw = bvh.ptrw(); { - //create faces and indices and base bvh //except for the Set for repeated triangles, everything //goes in-place. - PoolVector<Vector3>::Read r = p_faces.read(); - PoolVector<Triangle>::Write w = triangles.write(); + const Vector3 *r = p_faces.ptr(); + Triangle *w = triangles.ptrw(); 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 PoolVector<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(); @@ -164,32 +156,29 @@ void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { } vertices.resize(db.size()); - PoolVector<Vector3>::Write vw = vertices.write(); + Vector3 *vw = vertices.ptrw(); for (Map<Vector3, int>::Element *E = db.front(); E; E = E->next()) { vw[E->get()] = E->key(); } } - PoolVector<BVH *> bwptrs; + Vector<BVH *> bwptrs; bwptrs.resize(fc); - PoolVector<BVH *>::Write bwp = bwptrs.write(); + BVH **bwp = bwptrs.ptrw(); for (int i = 0; i < fc; i++) { - bwp[i] = &bw[i]; } max_depth = 0; int max_alloc = fc; - _create_bvh(bw.ptr(), bwp.ptr(), 0, fc, 1, max_depth, max_alloc); + _create_bvh(bw, bwp, 0, fc, 1, max_depth, max_alloc); - bw.release(); //clearup bvh.resize(max_alloc); //resize back valid = true; } Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -208,33 +197,26 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { int level = 0; - PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr = vertices.read(); - PoolVector<BVH>::Read bvhr = bvh.read(); + const Triangle *triangleptr = triangles.ptr(); + // const Vector3 *verticesr = vertices.ptr(); + const BVH *bvhptr = bvh.ptr(); - const Triangle *triangleptr = trianglesr.ptr(); int pos = bvh.size() - 1; - const BVH *bvhptr = bvhr.ptr(); 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++; @@ -242,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 { @@ -304,46 +284,36 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en int level = 0; - PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr = vertices.read(); - PoolVector<BVH>::Read bvhr = bvh.read(); + const Triangle *triangleptr = triangles.ptr(); + const Vector3 *vertexptr = vertices.ptr(); + const BVH *bvhptr = bvh.ptr(); - const Triangle *triangleptr = trianglesr.ptr(); - const Vector3 *vertexptr = verticesr.ptr(); int pos = bvh.size() - 1; - const BVH *bvhptr = bvhr.ptr(); 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(); @@ -354,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 { @@ -419,44 +386,34 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V int level = 0; - PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr = vertices.read(); - PoolVector<BVH>::Read bvhr = bvh.read(); + const Triangle *triangleptr = triangles.ptr(); + const Vector3 *vertexptr = vertices.ptr(); + const BVH *bvhptr = bvh.ptr(); - const Triangle *triangleptr = trianglesr.ptr(); - const Vector3 *vertexptr = verticesr.ptr(); int pos = bvh.size() - 1; - const BVH *bvhptr = bvhr.ptr(); 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(); @@ -467,51 +424,49 @@ 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; } -bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_count) const { +bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); //p_fully_inside = true; @@ -529,34 +484,26 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou int level = 0; - PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr = vertices.read(); - PoolVector<BVH>::Read bvhr = bvh.read(); + const Triangle *triangleptr = triangles.ptr(); + const Vector3 *vertexptr = vertices.ptr(); + const BVH *bvhptr = bvh.ptr(); - const Triangle *triangleptr = trianglesr.ptr(); - const Vector3 *vertexptr = verticesr.ptr(); int pos = bvh.size() - 1; - const BVH *bvhptr = bvhr.ptr(); 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); + 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) { @@ -570,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)) { @@ -585,51 +536,51 @@ 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; } -bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale) const { +bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -645,105 +596,99 @@ bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, int level = 0; - PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr = vertices.read(); - PoolVector<BVH>::Read bvhr = bvh.read(); + const Triangle *triangleptr = triangles.ptr(); + const Vector3 *vertexptr = vertices.ptr(); + const BVH *bvhptr = bvh.ptr(); - Transform scale(Basis().scaled(p_scale)); + Transform3D scale(Basis().scaled(p_scale)); - const Triangle *triangleptr = trianglesr.ptr(); - const Vector3 *vertexptr = verticesr.ptr(); int pos = bvh.size() - 1; - const BVH *bvhptr = bvhr.ptr(); 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); - if (!intersects) return false; + bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); + 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; } -PoolVector<Face3> TriangleMesh::get_faces() const { - - if (!valid) - return PoolVector<Face3>(); +Vector<Face3> TriangleMesh::get_faces() const { + if (!valid) { + return Vector<Face3>(); + } - PoolVector<Face3> faces; + Vector<Face3> faces; int ts = triangles.size(); faces.resize(triangles.size()); - PoolVector<Face3>::Write w = faces.write(); - PoolVector<Triangle>::Read r = triangles.read(); - PoolVector<Vector3>::Read rv = vertices.read(); + Face3 *w = faces.ptrw(); + const Triangle *r = triangles.ptr(); + const Vector3 *rv = vertices.ptr(); for (int i = 0; i < ts; i++) { for (int j = 0; j < 3; j++) { @@ -751,12 +696,10 @@ PoolVector<Face3> TriangleMesh::get_faces() const { } } - w.release(); return faces; } TriangleMesh::TriangleMesh() { - valid = false; max_depth = 0; } diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 575a78b0b5..463b0dd5c8 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,23 +32,20 @@ #define TRIANGLE_MESH_H #include "core/math/face3.h" -#include "core/reference.h" +#include "core/object/ref_counted.h" -class TriangleMesh : public Reference { - - GDCLASS(TriangleMesh, Reference); +class TriangleMesh : public RefCounted { + GDCLASS(TriangleMesh, RefCounted); struct Triangle { - Vector3 normal; int indices[3]; }; - PoolVector<Triangle> triangles; - PoolVector<Vector3> vertices; + Vector<Triangle> triangles; + Vector<Vector3> vertices; struct BVH { - AABB aabb; Vector3 center; //used for sorting int left; @@ -58,31 +55,25 @@ 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; } }; int _create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc); - PoolVector<BVH> bvh; + Vector<BVH> bvh; int max_depth; bool valid; @@ -90,16 +81,16 @@ public: bool is_valid() const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; bool intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const; - bool intersect_convex_shape(const Plane *p_planes, int p_plane_count) const; - bool inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale = Vector3(1, 1, 1)) const; + bool intersect_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const; + bool inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale = Vector3(1, 1, 1)) const; Vector3 get_area_normal(const AABB &p_aabb) const; - PoolVector<Face3> get_faces() const; + Vector<Face3> get_faces() const; - PoolVector<Triangle> get_triangles() const { return triangles; } - PoolVector<Vector3> get_vertices() const { return vertices; } - void get_indices(PoolVector<int> *r_triangles_indices) const; + Vector<Triangle> get_triangles() const { return triangles; } + Vector<Vector3> get_vertices() const { return vertices; } + void get_indices(Vector<int> *r_triangles_indices) const; - void create(const PoolVector<Vector3> &p_faces); + void create(const Vector<Vector3> &p_faces); TriangleMesh(); }; diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index cbcb232745..fa1588dbc5 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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; @@ -98,18 +97,24 @@ bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, in // It can happen that the triangulation ends up with three aligned vertices to deal with. // In this scenario, making the check below strict may reject the possibility of - // forming a last triangle with these aligned vertices, preventing the triangulatiom + // forming a last triangle with these aligned vertices, preventing the triangulation // from completing. // 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/triangulate.h b/core/math/triangulate.h index f9bcb37141..55dc4e8e7d 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -58,4 +58,4 @@ private: static bool snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed); }; -#endif +#endif // TRIANGULATE_H diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index f4259e388b..eb3301f5d0 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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; @@ -65,65 +59,55 @@ Vector2 Vector2::normalized() const { bool Vector2::is_normalized() const { // use length_squared() instead of length() to avoid sqrt(), makes it more stringent. - return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); + return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); } 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)); } Vector2 Vector2::rotated(real_t p_by) const { - - Vector2 v; - v.set_rotation(angle() + p_by); - v *= length(); - return v; + real_t sine = Math::sin(p_by); + real_t cosi = Math::cos(p_by); + return Vector2( + x * cosi - y * sine, + x * sine + y * cosi); } Vector2 Vector2::posmod(const real_t p_mod) const { @@ -134,23 +118,26 @@ Vector2 Vector2::posmodv(const Vector2 &p_modv) const { return Vector2(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y)); } -Vector2 Vector2::project(const Vector2 &p_b) const { - return p_b * (dot(p_b) / p_b.length_squared()); +Vector2 Vector2::project(const Vector2 &p_to) const { + return p_to * (dot(p_to) / p_to.length_squared()); } -Vector2 Vector2::snapped(const Vector2 &p_by) const { - +Vector2 Vector2::clamp(const Vector2 &p_min, const Vector2 &p_max) const { return Vector2( - Math::stepify(x, p_by.x), - Math::stepify(y, p_by.y)); + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y)); } -Vector2 Vector2::clamped(real_t p_len) const { +Vector2 Vector2::snapped(const Vector2 &p_step) const { + return Vector2( + Math::snapped(x, p_step.x), + Math::snapped(y, p_step.y)); +} - real_t l = length(); +Vector2 Vector2::limit_length(const real_t p_len) const { + const real_t l = length(); Vector2 v = *this; if (l > 0 && p_len < l) { - v /= l; v *= p_len; } @@ -158,14 +145,13 @@ Vector2 Vector2::clamped(real_t p_len) const { return v; } -Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const { - +Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const { Vector2 p0 = p_pre_a; Vector2 p1 = *this; Vector2 p2 = p_b; Vector2 p3 = p_post_b; - real_t t = p_t; + real_t t = p_weight; real_t t2 = t * t; real_t t3 = t2 * t; @@ -207,68 +193,87 @@ bool Vector2::is_equal_approx(const Vector2 &p_v) const { return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y); } +Vector2::operator String() const { + return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ")"; +} + /* Vector2i */ -Vector2i Vector2i::operator+(const Vector2i &p_v) const { +Vector2i Vector2i::clamp(const Vector2i &p_min, const Vector2i &p_max) const { + return Vector2i( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y)); +} +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 { +} +Vector2i Vector2i::operator*(const int32_t &rvalue) const { return Vector2i(x * rvalue, y * rvalue); -}; -void Vector2i::operator*=(const int &rvalue) { +} +void Vector2i::operator*=(const int32_t &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 { +} +Vector2i Vector2i::operator/(const int32_t &rvalue) const { return Vector2i(x / rvalue, y / rvalue); -}; - -void Vector2i::operator/=(const int &rvalue) { +} +void Vector2i::operator/=(const int32_t &rvalue) { x /= rvalue; y /= rvalue; -}; +} -Vector2i Vector2i::operator-() const { +Vector2i Vector2i::operator%(const Vector2i &p_v1) const { + return Vector2i(x % p_v1.x, y % p_v1.y); +} + +Vector2i Vector2i::operator%(const int32_t &rvalue) const { + return Vector2i(x % rvalue, y % rvalue); +} + +void Vector2i::operator%=(const int32_t &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; } + +Vector2i::operator String() const { + return "(" + itos(x) + ", " + itos(y) + ")"; +} diff --git a/core/math/vector2.h b/core/math/vector2.h index 1dec830821..78deb473b4 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,11 +32,12 @@ #define VECTOR2_H #include "core/math/math_funcs.h" -#include "core/ustring.h" +#include "core/string/ustring.h" struct Vector2i; struct Vector2 { + static const int AXIS_COUNT = 2; enum Axis { AXIS_X, @@ -44,12 +45,18 @@ struct Vector2 { }; union { - real_t x; - real_t width; - }; - union { - real_t y; - real_t height; + struct { + union { + real_t x; + real_t width; + }; + union { + real_t y; + real_t height; + }; + }; + + real_t coord[2] = { 0 }; }; _FORCE_INLINE_ real_t &operator[](int p_idx) { @@ -59,33 +66,51 @@ struct Vector2 { return p_idx ? y : x; } + _FORCE_INLINE_ void set_all(real_t p_value) { + x = y = p_value; + } + + _FORCE_INLINE_ int min_axis() const { + return x < y ? 0 : 1; + } + + _FORCE_INLINE_ int max_axis() const { + return x < y ? 1 : 0; + } + void normalize(); Vector2 normalized() const; bool is_normalized() const; real_t length() const; real_t length_squared() const; + Vector2 limit_length(const real_t p_len = 1.0) const; + + Vector2 min(const Vector2 &p_vector2) const { + return Vector2(MIN(x, p_vector2.x), MIN(y, p_vector2.y)); + } + + Vector2 max(const Vector2 &p_vector2) const { + return Vector2(MAX(x, p_vector2.x), MAX(y, p_vector2.y)); + } real_t distance_to(const Vector2 &p_vector2) const; real_t distance_squared_to(const Vector2 &p_vector2) const; real_t angle_to(const Vector2 &p_vector2) const; real_t angle_to_point(const Vector2 &p_vector2) const; - _FORCE_INLINE_ Vector2 direction_to(const Vector2 &p_b) const; + _FORCE_INLINE_ Vector2 direction_to(const Vector2 &p_to) const; real_t dot(const Vector2 &p_other) const; real_t cross(const Vector2 &p_other) const; Vector2 posmod(const real_t p_mod) const; Vector2 posmodv(const Vector2 &p_modv) const; - Vector2 project(const Vector2 &p_b) const; + Vector2 project(const Vector2 &p_to) const; Vector2 plane_project(real_t p_d, const Vector2 &p_vec) const; - Vector2 clamped(real_t p_len) const; - - _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t); - _FORCE_INLINE_ Vector2 linear_interpolate(const Vector2 &p_b, real_t p_t) const; - _FORCE_INLINE_ Vector2 slerp(const Vector2 &p_b, real_t p_t) const; - Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const; + _FORCE_INLINE_ Vector2 lerp(const Vector2 &p_to, real_t p_weight) const; + _FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, real_t p_weight) const; + Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const; Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const; Vector2 slide(const Vector2 &p_normal) const; @@ -116,27 +141,19 @@ struct Vector2 { bool operator==(const Vector2 &p_vec2) const; bool operator!=(const Vector2 &p_vec2) const; - bool operator<(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } - bool operator>(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); } - bool operator<=(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y <= p_vec2.y) : (x < p_vec2.x); } - bool operator>=(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y >= p_vec2.y) : (x > p_vec2.x); } + bool operator<(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y < p_vec2.y) : (x < p_vec2.x); } + bool operator>(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y > p_vec2.y) : (x > p_vec2.x); } + bool operator<=(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y <= p_vec2.y) : (x < p_vec2.x); } + bool operator>=(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y >= p_vec2.y) : (x > p_vec2.x); } real_t angle() const; - void set_rotation(real_t p_radians) { - - x = Math::cos(p_radians); - y = Math::sin(p_radians); - } - _FORCE_INLINE_ Vector2 abs() const { - return Vector2(Math::abs(x), Math::abs(y)); } Vector2 rotated(real_t p_by) const; - Vector2 tangent() const { - + Vector2 orthogonal() const { return Vector2(y, -x); } @@ -145,194 +162,215 @@ struct Vector2 { Vector2 ceil() const; Vector2 round() const; Vector2 snapped(const Vector2 &p_by) const; + Vector2 clamp(const Vector2 &p_min, const Vector2 &p_max) const; real_t aspect() const { return width / height; } - operator String() const { return String::num(x) + ", " + String::num(y); } + operator String() const; + _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) { +_FORCE_INLINE_ Vector2 operator*(float p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; +} +_FORCE_INLINE_ Vector2 operator*(double p_scalar, const Vector2 &p_vec) { return p_vec * p_scalar; } -_FORCE_INLINE_ Vector2 Vector2::operator+(const Vector2 &p_v) const { +_FORCE_INLINE_ Vector2 operator*(int32_t p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; +} + +_FORCE_INLINE_ Vector2 operator*(int64_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::linear_interpolate(const Vector2 &p_b, real_t p_t) const { - +Vector2 Vector2::lerp(const Vector2 &p_to, real_t p_weight) const { Vector2 res = *this; - res.x += (p_t * (p_b.x - x)); - res.y += (p_t * (p_b.y - y)); + res.x += (p_weight * (p_to.x - x)); + res.y += (p_weight * (p_to.y - y)); return res; } -Vector2 Vector2::slerp(const Vector2 &p_b, real_t p_t) const { +Vector2 Vector2::slerp(const Vector2 &p_to, real_t p_weight) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_normalized(), Vector2(), "The start Vector2 must be normalized."); #endif - real_t theta = angle_to(p_b); - return rotated(theta * p_t); + real_t theta = angle_to(p_to); + return rotated(theta * p_weight); } -Vector2 Vector2::direction_to(const Vector2 &p_b) const { - Vector2 ret(p_b.x - x, p_b.y - y); +Vector2 Vector2::direction_to(const Vector2 &p_to) const { + Vector2 ret(p_to.x - x, p_to.y - y); ret.normalize(); return ret; } -Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) { - - Vector2 res = p_a; - - res.x += (p_t * (p_b.x - p_a.x)); - res.y += (p_t * (p_b.y - p_a.y)); - - return res; -} - typedef Vector2 Size2; typedef Vector2 Point2; /* INTEGER STUFF */ struct Vector2i { - enum Axis { AXIS_X, AXIS_Y, }; union { - int x; - int width; + int32_t x = 0; + int32_t width; }; union { - int y; - int height; + int32_t y = 0; + int32_t height; }; - _FORCE_INLINE_ int &operator[](int p_idx) { + _FORCE_INLINE_ int32_t &operator[](int p_idx) { return p_idx ? y : x; } - _FORCE_INLINE_ const int &operator[](int p_idx) const { + _FORCE_INLINE_ const int32_t &operator[](int p_idx) const { return p_idx ? y : x; } + Vector2i min(const Vector2i &p_vector2i) const { + return Vector2(MIN(x, p_vector2i.x), MIN(y, p_vector2i.y)); + } + + Vector2i max(const Vector2i &p_vector2i) const { + return Vector2(MAX(x, p_vector2i.x), MAX(y, p_vector2i.y)); + } + Vector2i operator+(const Vector2i &p_v) const; void operator+=(const Vector2i &p_v); Vector2i operator-(const Vector2i &p_v) const; void operator-=(const Vector2i &p_v); Vector2i operator*(const Vector2i &p_v1) const; - Vector2i operator*(const int &rvalue) const; - void operator*=(const int &rvalue); + Vector2i operator*(const int32_t &rvalue) const; + void operator*=(const int32_t &rvalue); Vector2i operator/(const Vector2i &p_v1) const; + Vector2i operator/(const int32_t &rvalue) const; + void operator/=(const int32_t &rvalue); - Vector2i operator/(const int &rvalue) const; - - void operator/=(const int &rvalue); + Vector2i operator%(const Vector2i &p_v1) const; + Vector2i operator%(const int32_t &rvalue) const; + void operator%=(const int32_t &rvalue); Vector2i operator-() const; bool operator<(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } bool operator>(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); } + bool operator<=(const Vector2i &p_vec2) const { return x == p_vec2.x ? (y <= p_vec2.y) : (x < p_vec2.x); } + bool operator>=(const Vector2i &p_vec2) const { return x == p_vec2.x ? (y >= p_vec2.y) : (x > p_vec2.x); } + bool operator==(const Vector2i &p_vec2) const; bool operator!=(const Vector2i &p_vec2) const; - real_t get_aspect() const { return width / (real_t)height; } + real_t aspect() const { return width / (real_t)height; } + Vector2i sign() const { return Vector2i(SGN(x), SGN(y)); } + Vector2i abs() const { return Vector2i(ABS(x), ABS(y)); } + Vector2i clamp(const Vector2i &p_min, const Vector2i &p_max) const; - operator String() const { return String::num(x) + ", " + String::num(y); } + operator String() const; 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; + x = (int32_t)p_vec2.x; + y = (int32_t)p_vec2.y; } - inline Vector2i(int p_x, int p_y) { + inline Vector2i(int32_t p_x, int32_t p_y) { x = p_x; y = p_y; } - inline Vector2i() { - x = 0; - y = 0; - } }; +_FORCE_INLINE_ Vector2i operator*(const int32_t &p_scalar, const Vector2i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector2i operator*(const int64_t &p_scalar, const Vector2i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector2i operator*(const float &p_scalar, const Vector2i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector2i operator*(const double &p_scalar, const Vector2i &p_vector) { + return p_vector * p_scalar; +} + typedef Vector2i Size2i; typedef Vector2i Point2i; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index 71ff79c0fc..3d59064af6 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -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,81 +46,56 @@ 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 { - - return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); +Vector3 Vector3::clamp(const Vector3 &p_min, const Vector3 &p_max) const { + return Vector3( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y), + CLAMP(z, p_min.z, p_max.z)); } -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); +void Vector3::snap(Vector3 p_step) { + x = Math::snapped(x, p_step.x); + y = Math::snapped(y, p_step.y); + z = Math::snapped(z, p_step.z); } -Vector3 Vector3::snapped(Vector3 p_val) const { +Vector3 Vector3::snapped(Vector3 p_step) const { Vector3 v = *this; - v.snap(p_val); + v.snap(p_step); 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; - Vector3 p3 = p_post_b; - - { - //normalize - - real_t ab = p0.distance_to(p1); - real_t bc = p1.distance_to(p2); - real_t cd = p2.distance_to(p3); - - if (ab > 0) - p0 = p1 + (p0 - p1) * (bc / ab); - if (cd > 0) - p3 = p2 + (p3 - p2) * (bc / cd); +Vector3 Vector3::limit_length(const real_t p_len) const { + const real_t l = length(); + Vector3 v = *this; + if (l > 0 && p_len < l) { + v /= l; + v *= p_len; } - real_t t = p_t; - real_t t2 = t * t; - real_t t3 = t2 * t; - - Vector3 out; - out = 0.5 * ((p1 * 2.0) + - (-p0 + p2) * t + - (2.0 * p0 - 5.0 * p1 + 4 * p2 - p3) * t2 + - (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3); - return out; + return v; } -Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const { - +Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const { Vector3 p0 = p_pre_a; Vector3 p1 = *this; Vector3 p2 = p_b; Vector3 p3 = p_post_b; - real_t t = p_t; + real_t t = p_weight; real_t t2 = t * t; real_t t3 = t2 * t; Vector3 out; out = 0.5 * ((p1 * 2.0) + (-p0 + p2) * t + - (2.0 * p0 - 5.0 * p1 + 4 * p2 - p3) * t2 + + (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) * t2 + (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3); return out; } @@ -135,7 +108,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 +122,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)); + return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")"; } diff --git a/core/math/vector3.h b/core/math/vector3.h index 4ad3017109..d8d3cd3cc0 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -32,11 +32,13 @@ #define VECTOR3_H #include "core/math/math_funcs.h" -#include "core/ustring.h" +#include "core/math/vector3i.h" +#include "core/string/ustring.h" class Basis; struct Vector3 { + static const int AXIS_COUNT = 3; enum Axis { AXIS_X, @@ -51,24 +53,31 @@ 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]; } void set_axis(int p_axis, real_t p_value); real_t get_axis(int p_axis) const; - int min_axis() const; - int max_axis() const; + _FORCE_INLINE_ void set_all(real_t p_value) { + x = y = z = p_value; + } + + _FORCE_INLINE_ int min_axis() const { + return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); + } + + _FORCE_INLINE_ int max_axis() const { + return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); + } _FORCE_INLINE_ real_t length() const; _FORCE_INLINE_ real_t length_squared() const; @@ -77,6 +86,7 @@ struct Vector3 { _FORCE_INLINE_ Vector3 normalized() const; _FORCE_INLINE_ bool is_normalized() const; _FORCE_INLINE_ Vector3 inverse() const; + Vector3 limit_length(const real_t p_len = 1.0) const; _FORCE_INLINE_ void zero(); @@ -88,10 +98,9 @@ struct Vector3 { /* Static Methods between 2 vector3s */ - _FORCE_INLINE_ Vector3 linear_interpolate(const Vector3 &p_b, real_t p_t) const; - _FORCE_INLINE_ Vector3 slerp(const Vector3 &p_b, real_t p_t) const; - Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const; - Vector3 cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const; + _FORCE_INLINE_ Vector3 lerp(const Vector3 &p_to, real_t p_weight) const; + _FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, real_t p_weight) const; + Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const; Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; _FORCE_INLINE_ Vector3 cross(const Vector3 &p_b) const; @@ -104,16 +113,18 @@ struct Vector3 { _FORCE_INLINE_ Vector3 sign() const; _FORCE_INLINE_ Vector3 ceil() const; _FORCE_INLINE_ Vector3 round() const; + Vector3 clamp(const Vector3 &p_min, const Vector3 &p_max) const; - _FORCE_INLINE_ real_t distance_to(const Vector3 &p_b) const; - _FORCE_INLINE_ real_t distance_squared_to(const Vector3 &p_b) const; + _FORCE_INLINE_ real_t distance_to(const Vector3 &p_to) const; + _FORCE_INLINE_ real_t distance_squared_to(const Vector3 &p_to) const; _FORCE_INLINE_ Vector3 posmod(const real_t p_mod) const; _FORCE_INLINE_ Vector3 posmodv(const Vector3 &p_modv) const; - _FORCE_INLINE_ Vector3 project(const Vector3 &p_b) const; + _FORCE_INLINE_ Vector3 project(const Vector3 &p_to) const; - _FORCE_INLINE_ real_t angle_to(const Vector3 &p_b) const; - _FORCE_INLINE_ Vector3 direction_to(const Vector3 &p_b) const; + _FORCE_INLINE_ real_t angle_to(const Vector3 &p_to) const; + _FORCE_INLINE_ real_t signed_angle_to(const Vector3 &p_to, const Vector3 &p_axis) const; + _FORCE_INLINE_ Vector3 direction_to(const Vector3 &p_to) const; _FORCE_INLINE_ Vector3 slide(const Vector3 &p_normal) const; _FORCE_INLINE_ Vector3 bounce(const Vector3 &p_normal) const; @@ -147,17 +158,24 @@ struct Vector3 { _FORCE_INLINE_ bool operator>=(const Vector3 &p_v) const; operator String() const; + _FORCE_INLINE_ operator Vector3i() const { + 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), @@ -167,56 +185,47 @@ 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::linear_interpolate(const Vector3 &p_b, real_t p_t) const { - +Vector3 Vector3::lerp(const Vector3 &p_to, real_t p_weight) const { return Vector3( - x + (p_t * (p_b.x - x)), - y + (p_t * (p_b.y - y)), - z + (p_t * (p_b.z - z))); + x + (p_weight * (p_to.x - x)), + y + (p_weight * (p_to.y - y)), + z + (p_weight * (p_to.z - z))); } -Vector3 Vector3::slerp(const Vector3 &p_b, real_t p_t) const { - real_t theta = angle_to(p_b); - return rotated(cross(p_b).normalized(), theta * p_t); +Vector3 Vector3::slerp(const Vector3 &p_to, real_t p_weight) const { + real_t theta = angle_to(p_to); + return rotated(cross(p_to).normalized(), theta * p_weight); } -real_t Vector3::distance_to(const Vector3 &p_b) const { - - return (p_b - *this).length(); +real_t Vector3::distance_to(const Vector3 &p_to) const { + return (p_to - *this).length(); } -real_t Vector3::distance_squared_to(const Vector3 &p_b) const { - - return (p_b - *this).length_squared(); +real_t Vector3::distance_squared_to(const Vector3 &p_to) const { + return (p_to - *this).length_squared(); } Vector3 Vector3::posmod(const real_t p_mod) const { @@ -227,17 +236,23 @@ Vector3 Vector3::posmodv(const Vector3 &p_modv) const { return Vector3(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z)); } -Vector3 Vector3::project(const Vector3 &p_b) const { - return p_b * (dot(p_b) / p_b.length_squared()); +Vector3 Vector3::project(const Vector3 &p_to) const { + return p_to * (dot(p_to) / p_to.length_squared()); } -real_t Vector3::angle_to(const Vector3 &p_b) const { +real_t Vector3::angle_to(const Vector3 &p_to) const { + return Math::atan2(cross(p_to).length(), dot(p_to)); +} - return Math::atan2(cross(p_b).length(), dot(p_b)); +real_t Vector3::signed_angle_to(const Vector3 &p_to, const Vector3 &p_axis) const { + Vector3 cross_to = cross(p_to); + real_t unsigned_angle = Math::atan2(cross_to.length(), dot(p_to)); + real_t sign = cross_to.dot(p_axis); + return (sign < 0) ? -unsigned_angle : unsigned_angle; } -Vector3 Vector3::direction_to(const Vector3 &p_b) const { - Vector3 ret(p_b.x - x, p_b.y - y, p_b.z - z); +Vector3 Vector3::direction_to(const Vector3 &p_to) const { + Vector3 ret(p_to.x - x, p_to.y - y, p_to.z - z); ret.normalize(); return ret; } @@ -245,7 +260,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; @@ -253,36 +267,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; @@ -290,12 +300,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; @@ -303,17 +311,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; @@ -321,85 +326,70 @@ 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 (x == p_v.x) { + if (y == p_v.y) { return z < p_v.z; - else - return y < p_v.y; - } else { - return x < p_v.x; + } + return y < p_v.y; } + 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 (x == p_v.x) { + if (y == p_v.y) { return z > p_v.z; - else - return y > p_v.y; - } else { - return x > p_v.x; + } + return y > p_v.y; } + 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 (x == p_v.x) { + if (y == p_v.y) { return z <= p_v.z; - else - return y < p_v.y; - } else { - return x < p_v.x; + } + return y < p_v.y; } + 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 (x == p_v.x) { + if (y == p_v.y) { return z >= p_v.z; - else - return y > p_v.y; - } else { - return x > p_v.x; + } + return y > p_v.y; } + 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; @@ -408,7 +398,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; @@ -417,7 +406,6 @@ real_t Vector3::length_squared() const { } void Vector3::normalize() { - real_t lengthsq = length_squared(); if (lengthsq == 0) { x = y = z = 0; @@ -430,7 +418,6 @@ void Vector3::normalize() { } Vector3 Vector3::normalized() const { - Vector3 v = *this; v.normalize(); return v; @@ -438,16 +425,14 @@ Vector3 Vector3::normalized() const { bool Vector3::is_normalized() const { // use length_squared() instead of length() to avoid sqrt(), makes it more stringent. - return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); + return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); } 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/audio_frame.cpp b/core/math/vector3i.cpp index c565ea9b13..2de1e4e331 100644 --- a/core/math/audio_frame.cpp +++ b/core/math/vector3i.cpp @@ -1,12 +1,12 @@ /*************************************************************************/ -/* audio_frame.cpp */ +/* vector3i.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). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ @@ -28,4 +28,33 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "audio_frame.h" +#include "vector3i.h" + +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 { + 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 { + return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); +} + +Vector3i Vector3i::clamp(const Vector3i &p_min, const Vector3i &p_max) const { + return Vector3i( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y), + CLAMP(z, p_min.z, p_max.z)); +} + +Vector3i::operator String() const { + return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ")"; +} diff --git a/core/math/vector3i.h b/core/math/vector3i.h new file mode 100644 index 0000000000..37c7c1c368 --- /dev/null +++ b/core/math/vector3i.h @@ -0,0 +1,279 @@ +/*************************************************************************/ +/* vector3i.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 VECTOR3I_H +#define VECTOR3I_H + +#include "core/string/ustring.h" +#include "core/typedefs.h" + +struct Vector3i { + enum Axis { + AXIS_X, + AXIS_Y, + AXIS_Z, + }; + + union { + struct { + int32_t x; + int32_t y; + int32_t z; + }; + + 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]; + } + + void set_axis(int p_axis, int32_t p_value); + int32_t get_axis(int p_axis) const; + + int min_axis() const; + int max_axis() const; + + _FORCE_INLINE_ void zero(); + + _FORCE_INLINE_ Vector3i abs() const; + _FORCE_INLINE_ Vector3i sign() const; + Vector3i clamp(const Vector3i &p_min, const Vector3i &p_max) const; + + /* Operators */ + + _FORCE_INLINE_ Vector3i &operator+=(const Vector3i &p_v); + _FORCE_INLINE_ Vector3i operator+(const Vector3i &p_v) const; + _FORCE_INLINE_ Vector3i &operator-=(const Vector3i &p_v); + _FORCE_INLINE_ Vector3i operator-(const Vector3i &p_v) const; + _FORCE_INLINE_ Vector3i &operator*=(const Vector3i &p_v); + _FORCE_INLINE_ Vector3i operator*(const Vector3i &p_v) const; + _FORCE_INLINE_ Vector3i &operator/=(const Vector3i &p_v); + _FORCE_INLINE_ Vector3i operator/(const Vector3i &p_v) const; + _FORCE_INLINE_ Vector3i &operator%=(const Vector3i &p_v); + _FORCE_INLINE_ Vector3i operator%(const Vector3i &p_v) const; + + _FORCE_INLINE_ Vector3i &operator*=(int32_t p_scalar); + _FORCE_INLINE_ Vector3i operator*(int32_t p_scalar) const; + _FORCE_INLINE_ Vector3i &operator/=(int32_t p_scalar); + _FORCE_INLINE_ Vector3i operator/(int32_t p_scalar) const; + _FORCE_INLINE_ Vector3i &operator%=(int32_t p_scalar); + _FORCE_INLINE_ Vector3i operator%(int32_t p_scalar) const; + + _FORCE_INLINE_ Vector3i operator-() const; + + _FORCE_INLINE_ bool operator==(const Vector3i &p_v) const; + _FORCE_INLINE_ bool operator!=(const Vector3i &p_v) const; + _FORCE_INLINE_ bool operator<(const Vector3i &p_v) const; + _FORCE_INLINE_ bool operator<=(const Vector3i &p_v) const; + _FORCE_INLINE_ bool operator>(const Vector3i &p_v) const; + _FORCE_INLINE_ bool operator>=(const Vector3i &p_v) const; + + 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; + } +}; + +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; + return *this; +} + +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 { + 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 { + 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 { + 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 { + 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; + return *this; +} + +_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; + return *this; +} + +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; + return *this; +} + +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) { + return z < p_v.z; + } 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) { + return z > p_v.z; + } 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) { + return z <= p_v.z; + } 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) { + return z >= p_v.z; + } else { + return y > p_v.y; + } + } else { + return x > p_v.x; + } +} + +void Vector3i::zero() { + x = y = z = 0; +} + +#endif // VECTOR3I_H |