diff options
Diffstat (limited to 'core/math')
80 files changed, 4866 insertions, 3880 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index d59dbf1ba8..b4281820e2 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,22 @@ #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 { +int64_t AStar3D::get_available_point_id() const { if (points.has(last_free_id)) { - int cur_new_id = last_free_id + 1; + int64_t cur_new_id = last_free_id + 1; while (points.has(cur_new_id)) { cur_new_id++; } - const_cast<int &>(last_free_id) = cur_new_id; + const_cast<int64_t &>(last_free_id) = cur_new_id; } return last_free_id; } -void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { +void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scale) { ERR_FAIL_COND_MSG(p_id < 0, vformat("Can't add a point with negative id: %d.", p_id)); - ERR_FAIL_COND_MSG(p_weight_scale < 1, vformat("Can't add a point with weight scale less than one: %f.", p_weight_scale)); + ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't add a point with weight scale less than 0.0: %f.", p_weight_scale)); Point *found_pt; bool p_exists = points.lookup(p_id, found_pt); @@ -69,7 +68,7 @@ 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 { +Vector3 AStar3D::get_point_position(int64_t p_id) const { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V_MSG(!p_exists, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id)); @@ -77,7 +76,7 @@ Vector3 AStar::get_point_position(int p_id) const { return p->pos; } -void AStar::set_point_position(int p_id, const Vector3 &p_pos) { +void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id)); @@ -85,7 +84,7 @@ void AStar::set_point_position(int p_id, const Vector3 &p_pos) { p->pos = p_pos; } -real_t AStar::get_point_weight_scale(int p_id) const { +real_t AStar3D::get_point_weight_scale(int64_t p_id) const { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V_MSG(!p_exists, 0, vformat("Can't get point's weight scale. Point with id: %d doesn't exist.", p_id)); @@ -93,21 +92,21 @@ real_t AStar::get_point_weight_scale(int p_id) const { return p->weight_scale; } -void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) { +void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id)); - ERR_FAIL_COND_MSG(p_weight_scale < 1, vformat("Can't set point's weight scale less than one: %f.", p_weight_scale)); + ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale)); p->weight_scale = p_weight_scale; } -void AStar::remove_point(int p_id) { +void AStar3D::remove_point(int64_t p_id) { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_MSG(!p_exists, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id)); - for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { Segment s(p_id, (*it.key)); segments.erase(s); @@ -115,7 +114,7 @@ void AStar::remove_point(int p_id) { (*it.value)->unlinked_neighbours.remove(p->id); } - for (OAHashMap<int, Point *>::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) { + for (OAHashMap<int64_t, 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); @@ -128,7 +127,7 @@ void AStar::remove_point(int p_id) { last_free_id = p_id; } -void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { +void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) { ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id)); Point *a; @@ -152,21 +151,21 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { s.direction = Segment::BIDIRECTIONAL; } - Set<Segment>::Element *element = segments.find(s); - if (element != nullptr) { - s.direction |= element->get().direction; + HashSet<Segment, Segment>::Iterator element = segments.find(s); + if (element) { + s.direction |= element->direction; if (s.direction == Segment::BIDIRECTIONAL) { // Both are neighbours of each other now a->unlinked_neighbours.remove(b->id); b->unlinked_neighbours.remove(a->id); } - segments.erase(element); + segments.remove(element); } segments.insert(s); } -void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { +void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) { Point *a; bool a_exists = points.lookup(p_id, a); ERR_FAIL_COND_MSG(!a_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id)); @@ -176,18 +175,18 @@ void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { ERR_FAIL_COND_MSG(!b_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id)); Segment s(p_id, p_with_id); - int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : s.direction; + int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : (int)s.direction; - Set<Segment>::Element *element = segments.find(s); - if (element != nullptr) { + HashSet<Segment, Segment>::Iterator element = segments.find(s); + if (element) { // s is the new segment // Erase the directions to be removed - s.direction = (element->get().direction & ~remove_direction); + s.direction = (element->direction & ~remove_direction); a->neighbours.remove(b->id); if (bidirectional) { b->neighbours.remove(a->id); - if (element->get().direction != Segment::BIDIRECTIONAL) { + if (element->direction != Segment::BIDIRECTIONAL) { a->unlinked_neighbours.remove(b->id); b->unlinked_neighbours.remove(a->id); } @@ -199,77 +198,77 @@ void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) { } } - segments.erase(element); + segments.remove(element); if (s.direction != Segment::NONE) { segments.insert(s); } } } -bool AStar::has_point(int p_id) const { +bool AStar3D::has_point(int64_t p_id) const { return points.has(p_id); } -Array AStar::get_points() { - Array point_list; +PackedInt64Array AStar3D::get_point_ids() { + PackedInt64Array point_list; - for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { point_list.push_back(*(it.key)); } return point_list; } -Vector<int> AStar::get_point_connections(int p_id) { +Vector<int64_t> AStar3D::get_point_connections(int64_t p_id) { Point *p; bool p_exists = points.lookup(p_id, p); - ERR_FAIL_COND_V_MSG(!p_exists, Vector<int>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id)); + ERR_FAIL_COND_V_MSG(!p_exists, Vector<int64_t>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id)); - Vector<int> point_list; + Vector<int64_t> point_list; - for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { point_list.push_back((*it.key)); } return point_list; } -bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) const { +bool AStar3D::are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirectional) const { Segment s(p_id, p_with_id); - const Set<Segment>::Element *element = segments.find(s); + const HashSet<Segment, Segment>::Iterator element = segments.find(s); - return element != nullptr && - (bidirectional || (element->get().direction & s.direction) == s.direction); + return element && + (bidirectional || (element->direction & s.direction) == s.direction); } -void AStar::clear() { +void AStar3D::clear() { last_free_id = 0; - for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { memdelete(*(it.value)); } segments.clear(); points.clear(); } -int AStar::get_point_count() const { +int64_t AStar3D::get_point_count() const { return points.get_num_elements(); } -int AStar::get_point_capacity() const { +int64_t AStar3D::get_point_capacity() const { return points.get_capacity(); } -void AStar::reserve_space(int p_num_nodes) { +void AStar3D::reserve_space(int64_t p_num_nodes) { ERR_FAIL_COND_MSG(p_num_nodes <= 0, vformat("New capacity must be greater than 0, new was: %d.", p_num_nodes)); ERR_FAIL_COND_MSG((uint32_t)p_num_nodes < points.get_capacity(), vformat("New capacity must be greater than current capacity: %d, new was: %d.", points.get_capacity(), p_num_nodes)); points.reserve(p_num_nodes); } -int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const { - int closest_id = -1; +int64_t AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const { + int64_t closest_id = -1; real_t closest_dist = 1e20; - for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + for (OAHashMap<int64_t, 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. } @@ -277,7 +276,7 @@ int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) co // 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); - int id = *(it.key); + int64_t id = *(it.key); if (d <= closest_dist) { if (d == closest_dist && id > closest_id) { // Keep lowest ID. continue; @@ -290,14 +289,14 @@ int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) co return closest_id; } -Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { +Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const { real_t closest_dist = 1e20; Vector3 closest_point; - for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) { + for (const Segment &E : segments) { Point *from_point = nullptr, *to_point = nullptr; - points.lookup(E->get().u, from_point); - points.lookup(E->get().v, to_point); + points.lookup(E.key.first, from_point); + points.lookup(E.key.second, to_point); if (!(from_point->enabled && to_point->enabled)) { continue; @@ -319,7 +318,7 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { return closest_point; } -bool AStar::_solve(Point *begin_point, Point *end_point) { +bool AStar3D::_solve(Point *begin_point, Point *end_point) { pass++; if (!end_point->enabled) { @@ -344,10 +343,10 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { } 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); + open_list.remove_at(open_list.size() - 1); 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)) { + for (OAHashMap<int64_t, 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) { @@ -381,7 +380,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { return found_route; } -real_t AStar::_estimate_cost(int p_from_id, int p_to_id) { +real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) { real_t scost; if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) { return scost; @@ -398,7 +397,7 @@ real_t AStar::_estimate_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -real_t AStar::_compute_cost(int p_from_id, int p_to_id) { +real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) { real_t scost; if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) { return scost; @@ -415,7 +414,7 @@ real_t AStar::_compute_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { +Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id) { Point *a; bool from_exists = points.lookup(p_from_id, a); ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id)); @@ -439,7 +438,7 @@ Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { } Point *p = end_point; - int pc = 1; // Begin point + int64_t pc = 1; // Begin point while (p != begin_point) { pc++; p = p->prev_point; @@ -452,7 +451,7 @@ Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { Vector3 *w = path.ptrw(); Point *p2 = end_point; - int idx = pc - 1; + int64_t idx = pc - 1; while (p2 != begin_point) { w[idx--] = p2->pos; p2 = p2->prev_point; @@ -464,17 +463,17 @@ Vector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { return path; } -Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { +Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id) { Point *a; bool from_exists = points.lookup(p_from_id, a); - ERR_FAIL_COND_V_MSG(!from_exists, Vector<int>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id)); + ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id)); Point *b; bool to_exists = points.lookup(p_to_id, b); - ERR_FAIL_COND_V_MSG(!to_exists, Vector<int>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id)); + ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id)); if (a == b) { - Vector<int> ret; + Vector<int64_t> ret; ret.push_back(a->id); return ret; } @@ -484,24 +483,24 @@ Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { bool found_route = _solve(begin_point, end_point); if (!found_route) { - return Vector<int>(); + return Vector<int64_t>(); } Point *p = end_point; - int pc = 1; // Begin point + int64_t pc = 1; // Begin point while (p != begin_point) { pc++; p = p->prev_point; } - Vector<int> path; + Vector<int64_t> path; path.resize(pc); { - int *w = path.ptrw(); + int64_t *w = path.ptrw(); p = end_point; - int idx = pc - 1; + int64_t idx = pc - 1; while (p != begin_point) { w[idx--] = p->id; p = p->prev_point; @@ -513,7 +512,7 @@ Vector<int> AStar::get_id_path(int p_from_id, int p_to_id) { return path; } -void AStar::set_point_disabled(int p_id, bool p_disabled) { +void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id)); @@ -521,7 +520,7 @@ void AStar::set_point_disabled(int p_id, bool p_disabled) { p->enabled = !p_disabled; } -bool AStar::is_point_disabled(int p_id) const { +bool AStar3D::is_point_disabled(int64_t p_id) const { Point *p; bool p_exists = points.lookup(p_id, p); ERR_FAIL_COND_V_MSG(!p_exists, false, vformat("Can't get if point is disabled. Point with id: %d doesn't exist.", p_id)); @@ -529,112 +528,112 @@ bool AStar::is_point_disabled(int p_id) const { return !p->enabled; } -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); - ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar::set_point_position); - ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar::get_point_weight_scale); - ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar::set_point_weight_scale); - ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point); - ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar::has_point); - ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar::get_point_connections); - ClassDB::bind_method(D_METHOD("get_points"), &AStar::get_points); +void AStar3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar3D::get_available_point_id); + ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar3D::add_point, DEFVAL(1.0)); + ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar3D::get_point_position); + ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar3D::set_point_position); + ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar3D::get_point_weight_scale); + ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar3D::set_point_weight_scale); + ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar3D::remove_point); + ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar3D::has_point); + ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar3D::get_point_connections); + ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar3D::get_point_ids); - ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar::set_point_disabled, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar::is_point_disabled); + ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar3D::set_point_disabled, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar3D::is_point_disabled); - ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar::disconnect_points, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar::are_points_connected, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar3D::connect_points, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar3D::disconnect_points, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar3D::are_points_connected, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("get_point_count"), &AStar::get_point_count); - ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar::get_point_capacity); - ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar::reserve_space); - ClassDB::bind_method(D_METHOD("clear"), &AStar::clear); + ClassDB::bind_method(D_METHOD("get_point_count"), &AStar3D::get_point_count); + ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar3D::get_point_capacity); + ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar3D::reserve_space); + ClassDB::bind_method(D_METHOD("clear"), &AStar3D::clear); - ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar::get_closest_point, DEFVAL(false)); - ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar::get_closest_position_in_segment); + ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar3D::get_closest_point, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar3D::get_closest_position_in_segment); - 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); + ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar3D::get_point_path); + ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar3D::get_id_path); GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id") GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id") } -AStar::~AStar() { +AStar3D::~AStar3D() { clear(); } ///////////////////////////////////////////////////////////// -int AStar2D::get_available_point_id() const { +int64_t AStar2D::get_available_point_id() const { return astar.get_available_point_id(); } -void AStar2D::add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale) { +void AStar2D::add_point(int64_t p_id, const Vector2 &p_pos, real_t p_weight_scale) { astar.add_point(p_id, Vector3(p_pos.x, p_pos.y, 0), p_weight_scale); } -Vector2 AStar2D::get_point_position(int p_id) const { +Vector2 AStar2D::get_point_position(int64_t p_id) const { Vector3 p = astar.get_point_position(p_id); return Vector2(p.x, p.y); } -void AStar2D::set_point_position(int p_id, const Vector2 &p_pos) { +void AStar2D::set_point_position(int64_t p_id, const Vector2 &p_pos) { astar.set_point_position(p_id, Vector3(p_pos.x, p_pos.y, 0)); } -real_t AStar2D::get_point_weight_scale(int p_id) const { +real_t AStar2D::get_point_weight_scale(int64_t p_id) const { return astar.get_point_weight_scale(p_id); } -void AStar2D::set_point_weight_scale(int p_id, real_t p_weight_scale) { +void AStar2D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) { astar.set_point_weight_scale(p_id, p_weight_scale); } -void AStar2D::remove_point(int p_id) { +void AStar2D::remove_point(int64_t p_id) { astar.remove_point(p_id); } -bool AStar2D::has_point(int p_id) const { +bool AStar2D::has_point(int64_t p_id) const { return astar.has_point(p_id); } -Vector<int> AStar2D::get_point_connections(int p_id) { +Vector<int64_t> AStar2D::get_point_connections(int64_t p_id) { return astar.get_point_connections(p_id); } -Array AStar2D::get_points() { - return astar.get_points(); +PackedInt64Array AStar2D::get_point_ids() { + return astar.get_point_ids(); } -void AStar2D::set_point_disabled(int p_id, bool p_disabled) { +void AStar2D::set_point_disabled(int64_t p_id, bool p_disabled) { astar.set_point_disabled(p_id, p_disabled); } -bool AStar2D::is_point_disabled(int p_id) const { +bool AStar2D::is_point_disabled(int64_t p_id) const { return astar.is_point_disabled(p_id); } -void AStar2D::connect_points(int p_id, int p_with_id, bool p_bidirectional) { +void AStar2D::connect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) { astar.connect_points(p_id, p_with_id, p_bidirectional); } -void AStar2D::disconnect_points(int p_id, int p_with_id) { - astar.disconnect_points(p_id, p_with_id); +void AStar2D::disconnect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) { + astar.disconnect_points(p_id, p_with_id, p_bidirectional); } -bool AStar2D::are_points_connected(int p_id, int p_with_id) const { - return astar.are_points_connected(p_id, p_with_id); +bool AStar2D::are_points_connected(int64_t p_id, int64_t p_with_id, bool p_bidirectional) const { + return astar.are_points_connected(p_id, p_with_id, p_bidirectional); } -int AStar2D::get_point_count() const { +int64_t AStar2D::get_point_count() const { return astar.get_point_count(); } -int AStar2D::get_point_capacity() const { +int64_t AStar2D::get_point_capacity() const { return astar.get_point_capacity(); } @@ -642,11 +641,11 @@ void AStar2D::clear() { astar.clear(); } -void AStar2D::reserve_space(int p_num_nodes) { +void AStar2D::reserve_space(int64_t p_num_nodes) { astar.reserve_space(p_num_nodes); } -int AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const { +int64_t AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const { return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0), p_include_disabled); } @@ -655,65 +654,64 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { return Vector2(p.x, p.y); } -real_t AStar2D::_estimate_cost(int p_from_id, int p_to_id) { +real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) { real_t scost; if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) { return scost; } - AStar::Point *from_point; + AStar3D::Point *from_point; bool from_exists = astar.points.lookup(p_from_id, from_point); ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id)); - AStar::Point *to_point; + AStar3D::Point *to_point; bool to_exists = astar.points.lookup(p_to_id, to_point); ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id)); return from_point->pos.distance_to(to_point->pos); } -real_t AStar2D::_compute_cost(int p_from_id, int p_to_id) { +real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) { real_t scost; if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) { return scost; } - AStar::Point *from_point; + AStar3D::Point *from_point; bool from_exists = astar.points.lookup(p_from_id, from_point); ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id)); - AStar::Point *to_point; + AStar3D::Point *to_point; bool to_exists = astar.points.lookup(p_to_id, to_point); ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id)); 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; +Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id) { + AStar3D::Point *a; bool from_exists = astar.points.lookup(p_from_id, a); ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id)); - AStar::Point *b; + AStar3D::Point *b; bool to_exists = astar.points.lookup(p_to_id, b); ERR_FAIL_COND_V_MSG(!to_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id)); if (a == b) { - Vector<Vector2> ret; - ret.push_back(Vector2(a->pos.x, a->pos.y)); + Vector<Vector2> ret = { Vector2(a->pos.x, a->pos.y) }; return ret; } - AStar::Point *begin_point = a; - AStar::Point *end_point = b; + AStar3D::Point *begin_point = a; + AStar3D::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 + AStar3D::Point *p = end_point; + int64_t pc = 1; // Begin point while (p != begin_point) { pc++; p = p->prev_point; @@ -725,8 +723,8 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { { Vector2 *w = path.ptrw(); - AStar::Point *p2 = end_point; - int idx = pc - 1; + AStar3D::Point *p2 = end_point; + int64_t idx = pc - 1; while (p2 != begin_point) { w[idx--] = Vector2(p2->pos.x, p2->pos.y); p2 = p2->prev_point; @@ -738,44 +736,44 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { return path; } -Vector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { - AStar::Point *a; +Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id) { + AStar3D::Point *a; bool from_exists = astar.points.lookup(p_from_id, a); - ERR_FAIL_COND_V_MSG(!from_exists, Vector<int>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id)); + ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id)); - AStar::Point *b; + AStar3D::Point *b; bool to_exists = astar.points.lookup(p_to_id, b); - ERR_FAIL_COND_V_MSG(!to_exists, Vector<int>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id)); + ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id)); if (a == b) { - Vector<int> ret; + Vector<int64_t> ret; ret.push_back(a->id); return ret; } - AStar::Point *begin_point = a; - AStar::Point *end_point = b; + AStar3D::Point *begin_point = a; + AStar3D::Point *end_point = b; bool found_route = _solve(begin_point, end_point); if (!found_route) { - return Vector<int>(); + return Vector<int64_t>(); } - AStar::Point *p = end_point; - int pc = 1; // Begin point + AStar3D::Point *p = end_point; + int64_t pc = 1; // Begin point while (p != begin_point) { pc++; p = p->prev_point; } - Vector<int> path; + Vector<int64_t> path; path.resize(pc); { - int *w = path.ptrw(); + int64_t *w = path.ptrw(); p = end_point; - int idx = pc - 1; + int64_t idx = pc - 1; while (p != begin_point) { w[idx--] = p->id; p = p->prev_point; @@ -787,7 +785,7 @@ Vector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { return path; } -bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { +bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) { astar.pass++; if (!end_point->enabled) { @@ -796,15 +794,15 @@ bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { bool found_route = false; - Vector<AStar::Point *> open_list; - SortArray<AStar::Point *, AStar::SortPoints> sorter; + Vector<AStar3D::Point *> open_list; + SortArray<AStar3D::Point *, AStar3D::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 + AStar3D::Point *p = open_list[0]; // The currently processed point if (p == end_point) { found_route = true; @@ -812,11 +810,11 @@ bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) { } 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); + open_list.remove_at(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 + for (OAHashMap<int64_t, AStar3D::Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + AStar3D::Point *e = *(it.value); // The neighbour point if (!e->enabled || e->closed_pass == astar.pass) { continue; @@ -859,14 +857,14 @@ void AStar2D::_bind_methods() { ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar2D::remove_point); ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar2D::has_point); ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar2D::get_point_connections); - ClassDB::bind_method(D_METHOD("get_points"), &AStar2D::get_points); + ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar2D::get_point_ids); ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar2D::set_point_disabled, DEFVAL(true)); ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar2D::is_point_disabled); ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar2D::connect_points, DEFVAL(true)); - ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar2D::disconnect_points); - ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar2D::are_points_connected); + ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar2D::disconnect_points, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar2D::are_points_connected, DEFVAL(true)); ClassDB::bind_method(D_METHOD("get_point_count"), &AStar2D::get_point_count); ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar2D::get_point_capacity); diff --git a/core/math/a_star.h b/core/math/a_star.h index 64fa32a325..a9e2a62bb2 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -37,25 +37,23 @@ #include "core/templates/oa_hash_map.h" /** - A* pathfinding algorithm - - @author Juan Linietsky <reduzio@gmail.com> + A* pathfinding algorithm. */ -class AStar : public RefCounted { - GDCLASS(AStar, RefCounted); +class AStar3D : public RefCounted { + GDCLASS(AStar3D, RefCounted); friend class AStar2D; struct Point { Point() {} - int id = 0; + int64_t id = 0; Vector3 pos; real_t weight_scale = 0; bool enabled = false; - OAHashMap<int, Point *> neighbours = 4u; - OAHashMap<int, Point *> unlinked_neighbours = 4u; + OAHashMap<int64_t, Point *> neighbours = 4u; + OAHashMap<int64_t, Point *> unlinked_neighbours = 4u; // Used for pathfinding. Point *prev_point = nullptr; @@ -78,13 +76,7 @@ class AStar : public RefCounted { }; struct Segment { - union { - struct { - int32_t u; - int32_t v; - }; - uint64_t key = 0; - }; + Pair<int64_t, int64_t> key; enum { NONE = 0, @@ -94,119 +86,122 @@ class AStar : public RefCounted { }; unsigned char direction = NONE; - bool operator<(const Segment &p_s) const { return key < p_s.key; } + static uint32_t hash(const Segment &p_seg) { + return PairHash<int64_t, int64_t>().hash(p_seg.key); + } + bool operator==(const Segment &p_s) const { return key == p_s.key; } Segment() {} - Segment(int p_from, int p_to) { + Segment(int64_t p_from, int64_t p_to) { if (p_from < p_to) { - u = p_from; - v = p_to; + key.first = p_from; + key.second = p_to; direction = FORWARD; } else { - u = p_to; - v = p_from; + key.first = p_to; + key.second = p_from; direction = BACKWARD; } } }; - int last_free_id = 0; + int64_t last_free_id = 0; uint64_t pass = 1; - OAHashMap<int, Point *> points; - Set<Segment> segments; + OAHashMap<int64_t, Point *> points; + HashSet<Segment, Segment> segments; bool _solve(Point *begin_point, 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); + virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_to_id); + virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id); GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t) GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t) public: - int get_available_point_id() const; - - void add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale = 1); - Vector3 get_point_position(int p_id) const; - void set_point_position(int p_id, const Vector3 &p_pos); - real_t get_point_weight_scale(int p_id) const; - 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; - Vector<int> get_point_connections(int p_id); - Array get_points(); - - void set_point_disabled(int p_id, bool p_disabled = true); - bool is_point_disabled(int p_id) const; - - void connect_points(int p_id, int p_with_id, bool bidirectional = true); - void disconnect_points(int p_id, int p_with_id, bool bidirectional = true); - bool are_points_connected(int p_id, int p_with_id, bool bidirectional = true) const; - - int get_point_count() const; - int get_point_capacity() const; - void reserve_space(int p_num_nodes); + int64_t get_available_point_id() const; + + void add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scale = 1); + Vector3 get_point_position(int64_t p_id) const; + void set_point_position(int64_t p_id, const Vector3 &p_pos); + real_t get_point_weight_scale(int64_t p_id) const; + void set_point_weight_scale(int64_t p_id, real_t p_weight_scale); + void remove_point(int64_t p_id); + bool has_point(int64_t p_id) const; + Vector<int64_t> get_point_connections(int64_t p_id); + PackedInt64Array get_point_ids(); + + void set_point_disabled(int64_t p_id, bool p_disabled = true); + bool is_point_disabled(int64_t p_id) const; + + void connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional = true); + void disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional = true); + bool are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirectional = true) const; + + int64_t get_point_count() const; + int64_t get_point_capacity() const; + void reserve_space(int64_t p_num_nodes); void clear(); - int get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const; + int64_t get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const; Vector3 get_closest_position_in_segment(const Vector3 &p_point) const; - 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); + Vector<Vector3> get_point_path(int64_t p_from_id, int64_t p_to_id); + Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id); - AStar() {} - ~AStar(); + AStar3D() {} + ~AStar3D(); }; class AStar2D : public RefCounted { GDCLASS(AStar2D, RefCounted); - AStar astar; + AStar3D astar; - bool _solve(AStar::Point *begin_point, AStar::Point *end_point); + bool _solve(AStar3D::Point *begin_point, AStar3D::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); + virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_to_id); + virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id); GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t) GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t) public: - int get_available_point_id() const; - - void add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale = 1); - Vector2 get_point_position(int p_id) const; - void set_point_position(int p_id, const Vector2 &p_pos); - real_t get_point_weight_scale(int p_id) const; - 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; - Vector<int> get_point_connections(int p_id); - Array get_points(); - - void set_point_disabled(int p_id, bool p_disabled = true); - bool is_point_disabled(int p_id) const; - - void connect_points(int p_id, int p_with_id, bool p_bidirectional = true); - void disconnect_points(int p_id, int p_with_id); - bool are_points_connected(int p_id, int p_with_id) const; - - int get_point_count() const; - int get_point_capacity() const; - void reserve_space(int p_num_nodes); + int64_t get_available_point_id() const; + + void add_point(int64_t p_id, const Vector2 &p_pos, real_t p_weight_scale = 1); + Vector2 get_point_position(int64_t p_id) const; + void set_point_position(int64_t p_id, const Vector2 &p_pos); + real_t get_point_weight_scale(int64_t p_id) const; + void set_point_weight_scale(int64_t p_id, real_t p_weight_scale); + void remove_point(int64_t p_id); + bool has_point(int64_t p_id) const; + Vector<int64_t> get_point_connections(int64_t p_id); + PackedInt64Array get_point_ids(); + + void set_point_disabled(int64_t p_id, bool p_disabled = true); + bool is_point_disabled(int64_t p_id) const; + + void connect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true); + void disconnect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true); + bool are_points_connected(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true) const; + + int64_t get_point_count() const; + int64_t get_point_capacity() const; + void reserve_space(int64_t p_num_nodes); void clear(); - int get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const; + int64_t get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const; Vector2 get_closest_position_in_segment(const Vector2 &p_point) const; - 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); + Vector<Vector2> get_point_path(int64_t p_from_id, int64_t p_to_id); + Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id); AStar2D() {} ~AStar2D() {} diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 51a1309f0e..4c89be7f4d 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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/string/print_string.h" #include "core/variant/variant.h" -real_t AABB::get_area() const { +real_t AABB::get_volume() const { return size.x * size.y * size.z; } @@ -46,6 +46,11 @@ bool AABB::operator!=(const AABB &p_rval) const { } void AABB::merge_with(const AABB &p_aabb) { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif Vector3 beg_1, beg_2; Vector3 end_1, end_2; Vector3 min, max; @@ -72,6 +77,11 @@ bool AABB::is_equal_approx(const AABB &p_aabb) const { } AABB AABB::intersection(const AABB &p_aabb) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif Vector3 src_min = position; Vector3 src_max = position + size; Vector3 dst_min = p_aabb.position; @@ -104,6 +114,11 @@ 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 { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif Vector3 c1, c2; Vector3 end = position + size; real_t near = -1e20; @@ -147,6 +162,11 @@ 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 { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif real_t min = 0, max = 1; int axis = 0; real_t sign = 0; @@ -266,14 +286,14 @@ int AABB::get_longest_axis_index() const { Vector3 AABB::get_shortest_axis() const { Vector3 axis(1, 0, 0); - real_t max_size = size.x; + real_t min_size = size.x; - if (size.y < max_size) { + if (size.y < min_size) { axis = Vector3(0, 1, 0); - max_size = size.y; + min_size = size.y; } - if (size.z < max_size) { + if (size.z < min_size) { axis = Vector3(0, 0, 1); } @@ -282,14 +302,14 @@ Vector3 AABB::get_shortest_axis() const { int AABB::get_shortest_axis_index() const { int axis = 0; - real_t max_size = size.x; + real_t min_size = size.x; - if (size.y < max_size) { + if (size.y < min_size) { axis = 1; - max_size = size.y; + min_size = size.y; } - if (size.z < max_size) { + if (size.z < min_size) { axis = 2; } diff --git a/core/math/aabb.h b/core/math/aabb.h index 97d92fbe37..e88ba33531 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,18 +36,18 @@ #include "core/math/vector3.h" /** - * AABB / AABB (Axis Aligned Bounding Box) - * This is implemented by a point (position) and the box size + * AABB (Axis Aligned Bounding Box) + * This is implemented by a point (position) and the box size. */ + class Variant; -class AABB { -public: +struct _NO_DISCARD_ AABB { Vector3 position; Vector3 size; - real_t get_area() const; /// get area - _FORCE_INLINE_ bool has_no_area() const { + real_t get_volume() const; + _FORCE_INLINE_ bool has_no_volume() const { return (size.x <= 0 || size.y <= 0 || size.z <= 0); } @@ -119,7 +119,7 @@ public: } _FORCE_INLINE_ Vector3 get_center() const { - return position + (size * 0.5); + return position + (size * 0.5f); } operator String() const; @@ -132,6 +132,11 @@ public: }; inline bool AABB::intersects(const AABB &p_aabb) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif if (position.x >= (p_aabb.position.x + p_aabb.size.x)) { return false; } @@ -155,6 +160,11 @@ inline bool AABB::intersects(const AABB &p_aabb) const { } inline bool AABB::intersects_inclusive(const AABB &p_aabb) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif if (position.x > (p_aabb.position.x + p_aabb.size.x)) { return false; } @@ -178,6 +188,11 @@ inline bool AABB::intersects_inclusive(const AABB &p_aabb) const { } inline bool AABB::encloses(const AABB &p_aabb) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif Vector3 src_min = position; Vector3 src_max = position + size; Vector3 dst_min = p_aabb.position; @@ -193,14 +208,14 @@ 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 half_extents = size * 0.5f; 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) + - ofs; + ofs; } Vector3 AABB::get_endpoint(int p_point) const { @@ -227,7 +242,7 @@ Vector3 AABB::get_endpoint(int p_point) 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 half_extents = size * 0.5f; Vector3 ofs = position + half_extents; for (int i = 0; i < p_plane_count; i++) { @@ -269,7 +284,7 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con } bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const { - Vector3 half_extents = size * 0.5; + Vector3 half_extents = size * 0.5f; Vector3 ofs = position + half_extents; for (int i = 0; i < p_plane_count; i++) { @@ -288,6 +303,11 @@ bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const { } bool AABB::has_point(const Vector3 &p_point) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif if (p_point.x < position.x) { return false; } @@ -311,6 +331,11 @@ bool AABB::has_point(const Vector3 &p_point) const { } inline void AABB::expand_to(const Vector3 &p_vector) { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif Vector3 begin = position; Vector3 end = position + size; @@ -339,7 +364,7 @@ inline void AABB::expand_to(const Vector3 &p_vector) { } 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 half_extents(size.x * 0.5f, size.y * 0.5f, size.z * 0.5f); Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z); real_t length = p_plane.normal.abs().dot(half_extents); @@ -377,9 +402,14 @@ 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; +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) { + ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size."); + } +#endif + real_t divx = 1.0f / p_dir.x; + real_t divy = 1.0f / p_dir.y; + real_t divz = 1.0f / p_dir.z; Vector3 upbound = position + size; real_t tmin, tmax, tymin, tymax, tzmin, tzmax; @@ -429,9 +459,9 @@ void AABB::grow_by(real_t p_amount) { position.x -= p_amount; position.y -= p_amount; position.z -= p_amount; - size.x += 2.0 * p_amount; - size.y += 2.0 * p_amount; - size.z += 2.0 * p_amount; + size.x += 2.0f * p_amount; + size.y += 2.0f * p_amount; + size.z += 2.0f * p_amount; } void AABB::quantize(real_t p_unit) { diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index a5616b8d79..b3d63c0094 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -52,7 +52,7 @@ static const float AUDIO_MIN_PEAK_DB = -200.0f; // linear2db(AUDIO_PEAK_OFFSET) struct AudioFrame { //left and right samples - float l, r; + float l = 0.f, r = 0.f; _ALWAYS_INLINE_ const float &operator[](int idx) const { return idx == 0 ? l : r; } _ALWAYS_INLINE_ float &operator[](int idx) { return idx == 0 ? l : r; } @@ -124,10 +124,9 @@ struct AudioFrame { r = p_frame.r; } - _ALWAYS_INLINE_ AudioFrame &operator=(const AudioFrame &p_frame) { + _ALWAYS_INLINE_ void operator=(const AudioFrame &p_frame) { l = p_frame.l; r = p_frame.r; - return *this; } _ALWAYS_INLINE_ operator Vector2() const { @@ -141,4 +140,16 @@ struct AudioFrame { _ALWAYS_INLINE_ AudioFrame() {} }; +_ALWAYS_INLINE_ AudioFrame operator*(float p_scalar, const AudioFrame &p_frame) { + return AudioFrame(p_frame.l * p_scalar, p_frame.r * p_scalar); +} + +_ALWAYS_INLINE_ AudioFrame operator*(int32_t p_scalar, const AudioFrame &p_frame) { + return AudioFrame(p_frame.l * p_scalar, p_frame.r * p_scalar); +} + +_ALWAYS_INLINE_ AudioFrame operator*(int64_t p_scalar, const AudioFrame &p_frame) { + return AudioFrame(p_frame.l * p_scalar, p_frame.r * p_scalar); +} + #endif // AUDIO_FRAME_H diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 0030cb1144..bc50d0e64c 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,36 +34,36 @@ #include "core/string/print_string.h" #define cofac(row1, col1, row2, col2) \ - (elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1]) + (rows[row1][col1] * rows[row2][col2] - rows[row1][col2] * rows[row2][col1]) void Basis::from_z(const Vector3 &p_z) { - if (Math::abs(p_z.z) > Math_SQRT12) { + if (Math::abs(p_z.z) > (real_t)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]); + real_t k = 1.0f / Math::sqrt(a); + rows[0] = Vector3(0, -p_z[2] * k, p_z[1] * k); + rows[1] = Vector3(a * k, -p_z[0] * rows[0][2], p_z[0] * rows[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); - elements[0] = Vector3(-p_z.y * k, p_z.x * k, 0); - elements[1] = Vector3(-p_z.z * elements[0].y, p_z.z * elements[0].x, a * k); + real_t k = 1.0f / Math::sqrt(a); + rows[0] = Vector3(-p_z.y * k, p_z.x * k, 0); + rows[1] = Vector3(-p_z.z * rows[0].y, p_z.z * rows[0].x, a * k); } - elements[2] = p_z; + rows[2] = p_z; } void Basis::invert() { real_t co[3] = { cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) }; - real_t det = elements[0][0] * co[0] + - elements[0][1] * co[1] + - elements[0][2] * co[2]; + real_t det = rows[0][0] * co[0] + + rows[0][1] * co[1] + + rows[0][2] * co[2]; #ifdef MATH_CHECKS ERR_FAIL_COND(det == 0); #endif - real_t s = 1.0 / det; + real_t s = 1.0f / det; set(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, @@ -73,9 +73,9 @@ void Basis::invert() { void Basis::orthonormalize() { // Gram-Schmidt Process - Vector3 x = get_axis(0); - Vector3 y = get_axis(1); - Vector3 z = get_axis(2); + Vector3 x = get_column(0); + Vector3 y = get_column(1); + Vector3 z = get_column(2); x.normalize(); y = (y - x * (x.dot(y))); @@ -83,9 +83,9 @@ void Basis::orthonormalize() { z = (z - x * (x.dot(z)) - y * (y.dot(z))); z.normalize(); - set_axis(0, x); - set_axis(1, y); - set_axis(2, z); + set_column(0, x); + set_column(1, y); + set_column(2, z); } Basis Basis::orthonormalized() const { @@ -94,6 +94,18 @@ Basis Basis::orthonormalized() const { return c; } +void Basis::orthogonalize() { + Vector3 scl = get_scale(); + orthonormalize(); + scale_local(scl); +} + +Basis Basis::orthogonalized() const { + Basis c = *this; + c.orthogonalize(); + return c; +} + bool Basis::is_orthogonal() const { Basis identity; Basis m = (*this) * transposed(); @@ -103,9 +115,9 @@ bool Basis::is_orthogonal() const { bool Basis::is_diagonal() const { return ( - Math::is_zero_approx(elements[0][1]) && Math::is_zero_approx(elements[0][2]) && - Math::is_zero_approx(elements[1][0]) && Math::is_zero_approx(elements[1][2]) && - Math::is_zero_approx(elements[2][0]) && Math::is_zero_approx(elements[2][1])); + Math::is_zero_approx(rows[0][1]) && Math::is_zero_approx(rows[0][2]) && + Math::is_zero_approx(rows[1][0]) && Math::is_zero_approx(rows[1][2]) && + Math::is_zero_approx(rows[2][0]) && Math::is_zero_approx(rows[2][1])); } bool Basis::is_rotation() const { @@ -115,13 +127,13 @@ bool Basis::is_rotation() const { #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(elements[0][1], elements[1][0])) { + if (!Math::is_equal_approx(rows[0][1], rows[1][0])) { return false; } - if (!Math::is_equal_approx(elements[0][2], elements[2][0])) { + if (!Math::is_equal_approx(rows[0][2], rows[2][0])) { return false; } - if (!Math::is_equal_approx(elements[1][2], elements[2][1])) { + if (!Math::is_equal_approx(rows[1][2], rows[2][1])) { return false; } @@ -137,14 +149,14 @@ Basis Basis::diagonalize() { #endif const int ite_max = 1024; - real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; + real_t off_matrix_norm_2 = rows[0][1] * rows[0][1] + rows[0][2] * rows[0][2] + rows[1][2] * rows[1][2]; int ite = 0; Basis acc_rot; - while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max) { - real_t el01_2 = elements[0][1] * elements[0][1]; - real_t el02_2 = elements[0][2] * elements[0][2]; - real_t el12_2 = elements[1][2] * elements[1][2]; + while (off_matrix_norm_2 > (real_t)CMP_EPSILON2 && ite++ < ite_max) { + real_t el01_2 = rows[0][1] * rows[0][1]; + real_t el02_2 = rows[0][2] * rows[0][2]; + real_t el12_2 = rows[1][2] * rows[1][2]; // Find the pivot element int i, j; if (el01_2 > el02_2) { @@ -167,19 +179,19 @@ Basis Basis::diagonalize() { // Compute the rotation angle real_t angle; - if (Math::is_equal_approx(elements[j][j], elements[i][i])) { + if (Math::is_equal_approx(rows[j][j], rows[i][i])) { angle = Math_PI / 4; } else { - angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i])); + angle = 0.5f * Math::atan(2 * rows[i][j] / (rows[j][j] - rows[i][i])); } // Compute the rotation matrix Basis rot; - rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle); - rot.elements[i][j] = -(rot.elements[j][i] = Math::sin(angle)); + rot.rows[i][i] = rot.rows[j][j] = Math::cos(angle); + rot.rows[i][j] = -(rot.rows[j][i] = Math::sin(angle)); // Update the off matrix norm - off_matrix_norm_2 -= elements[i][j] * elements[i][j]; + off_matrix_norm_2 -= rows[i][j] * rows[i][j]; // Apply the rotation *this = rot * *this * rot.transposed(); @@ -196,9 +208,9 @@ Basis Basis::inverse() const { } void Basis::transpose() { - SWAP(elements[0][1], elements[1][0]); - SWAP(elements[0][2], elements[2][0]); - SWAP(elements[1][2], elements[2][1]); + SWAP(rows[0][1], rows[1][0]); + SWAP(rows[0][2], rows[2][0]); + SWAP(rows[1][2], rows[2][1]); } Basis Basis::transposed() const { @@ -214,15 +226,15 @@ Basis Basis::from_scale(const Vector3 &p_scale) { // 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; - elements[1][0] *= p_scale.y; - elements[1][1] *= p_scale.y; - elements[1][2] *= p_scale.y; - elements[2][0] *= p_scale.z; - elements[2][1] *= p_scale.z; - elements[2][2] *= p_scale.z; + rows[0][0] *= p_scale.x; + rows[0][1] *= p_scale.x; + rows[0][2] *= p_scale.x; + rows[1][0] *= p_scale.y; + rows[1][1] *= p_scale.y; + rows[1][2] *= p_scale.y; + rows[2][0] *= p_scale.z; + rows[2][1] *= p_scale.z; + rows[2][2] *= p_scale.z; } Basis Basis::scaled(const Vector3 &p_scale) const { @@ -237,15 +249,33 @@ void Basis::scale_local(const Vector3 &p_scale) { *this = scaled_local(p_scale); } +void Basis::scale_orthogonal(const Vector3 &p_scale) { + *this = scaled_orthogonal(p_scale); +} + +Basis Basis::scaled_orthogonal(const Vector3 &p_scale) const { + Basis m = *this; + Vector3 s = Vector3(-1, -1, -1) + p_scale; + Vector3 dots; + Basis b; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + dots[j] += s[i] * abs(m.get_column(i).normalized().dot(b.get_column(j))); + } + } + m.scale_local(Vector3(1, 1, 1) + dots); + return m; +} + float Basis::get_uniform_scale() const { - return (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0; + return (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0f; } void Basis::make_scale_uniform() { - float l = (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0; + float l = (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0f; for (int i = 0; i < 3; i++) { - elements[i].normalize(); - elements[i] *= l; + rows[i].normalize(); + rows[i] *= l; } } @@ -255,14 +285,14 @@ 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(), - Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); + Vector3(rows[0][0], rows[1][0], rows[2][0]).length(), + Vector3(rows[0][1], rows[1][1], rows[2][1]).length(), + Vector3(rows[0][2], rows[1][2], rows[2][2]).length()); } Vector3 Basis::get_scale_local() const { - real_t det_sign = SGN(determinant()); - return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length()); + real_t det_sign = SIGN(determinant()); + return det_sign * Vector3(rows[0].length(), rows[1].length(), rows[2].length()); } // get_scale works with get_rotation, use get_scale_abs if you need to enforce positive signature. @@ -287,11 +317,8 @@ Vector3 Basis::get_scale() const { // matrix elements. // // The rotation part of this decomposition is returned by get_rotation* functions. - real_t det_sign = SGN(determinant()); - return det_sign * Vector3( - Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), - Vector3(elements[0][1], elements[1][1], elements[2][1]).length(), - Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); + real_t det_sign = SIGN(determinant()); + return det_sign * get_scale_abs(); } // Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S. @@ -320,30 +347,30 @@ Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const { // 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 { - return Basis(p_axis, p_phi) * (*this); +Basis Basis::rotated(const Vector3 &p_axis, real_t p_angle) const { + return Basis(p_axis, p_angle) * (*this); } -void Basis::rotate(const Vector3 &p_axis, real_t p_phi) { - *this = rotated(p_axis, p_phi); +void Basis::rotate(const Vector3 &p_axis, real_t p_angle) { + *this = rotated(p_axis, p_angle); } -void Basis::rotate_local(const Vector3 &p_axis, real_t p_phi) { +void Basis::rotate_local(const Vector3 &p_axis, real_t p_angle) { // performs a rotation in object-local coordinate system: // M -> (M.R.Minv).M = M.R. - *this = rotated_local(p_axis, p_phi); + *this = rotated_local(p_axis, p_angle); } -Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const { - return (*this) * Basis(p_axis, p_phi); +Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_angle) const { + return (*this) * Basis(p_axis, p_angle); } -Basis Basis::rotated(const Vector3 &p_euler) const { - return Basis(p_euler) * (*this); +Basis Basis::rotated(const Vector3 &p_euler, EulerOrder p_order) const { + return Basis::from_euler(p_euler, p_order) * (*this); } -void Basis::rotate(const Vector3 &p_euler) { - *this = rotated(p_euler); +void Basis::rotate(const Vector3 &p_euler, EulerOrder p_order) { + *this = rotated(p_euler, p_order); } Basis Basis::rotated(const Quaternion &p_quaternion) const { @@ -388,7 +415,7 @@ void Basis::rotate_to_align(Vector3 p_start_direction, Vector3 p_end_direction) const Vector3 axis = p_start_direction.cross(p_end_direction).normalized(); if (axis.length_squared() != 0) { real_t dot = p_start_direction.dot(p_end_direction); - dot = CLAMP(dot, -1.0, 1.0); + dot = CLAMP(dot, -1.0f, 1.0f); const real_t angle_rads = Math::acos(dot); set_axis_angle(axis, angle_rads); } @@ -435,29 +462,29 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy Vector3 euler; - real_t sy = elements[0][2]; - if (sy < (1.0 - CMP_EPSILON)) { - if (sy > -(1.0 - CMP_EPSILON)) { + real_t sy = rows[0][2]; + if (sy < (1.0f - (real_t)CMP_EPSILON)) { + if (sy > -(1.0f - (real_t)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) { + if (rows[1][0] == 0 && rows[0][1] == 0 && rows[1][2] == 0 && rows[2][1] == 0 && rows[1][1] == 1) { // return the simplest form (human friendlier in editor and scripts) euler.x = 0; - euler.y = atan2(elements[0][2], elements[0][0]); + euler.y = atan2(rows[0][2], rows[0][0]); euler.z = 0; } else { - euler.x = Math::atan2(-elements[1][2], elements[2][2]); + euler.x = Math::atan2(-rows[1][2], rows[2][2]); euler.y = Math::asin(sy); - euler.z = Math::atan2(-elements[0][1], elements[0][0]); + euler.z = Math::atan2(-rows[0][1], rows[0][0]); } } else { - euler.x = Math::atan2(elements[2][1], elements[1][1]); - euler.y = -Math_PI / 2.0; - euler.z = 0.0; + euler.x = Math::atan2(rows[2][1], rows[1][1]); + euler.y = -Math_PI / 2.0f; + euler.z = 0.0f; } } else { - euler.x = Math::atan2(elements[2][1], elements[1][1]); - euler.y = Math_PI / 2.0; - euler.z = 0.0; + euler.x = Math::atan2(rows[2][1], rows[1][1]); + euler.y = Math_PI / 2.0f; + euler.z = 0.0f; } return euler; } break; @@ -470,23 +497,23 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // 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]); + real_t sz = rows[0][1]; + if (sz < (1.0f - (real_t)CMP_EPSILON)) { + if (sz > -(1.0f - (real_t)CMP_EPSILON)) { + euler.x = Math::atan2(rows[2][1], rows[1][1]); + euler.y = Math::atan2(rows[0][2], rows[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; + euler.x = -Math::atan2(rows[1][2], rows[2][2]); + euler.y = 0.0f; + euler.z = Math_PI / 2.0f; } } else { // It's 1 - euler.x = -Math::atan2(elements[1][2], elements[2][2]); - euler.y = 0.0; - euler.z = -Math_PI / 2.0; + euler.x = -Math::atan2(rows[1][2], rows[2][2]); + euler.y = 0.0f; + euler.z = -Math_PI / 2.0f; } return euler; } break; @@ -500,29 +527,29 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { Vector3 euler; - real_t m12 = elements[1][2]; + real_t m12 = rows[1][2]; - if (m12 < (1 - CMP_EPSILON)) { - if (m12 > -(1 - CMP_EPSILON)) { + if (m12 < (1 - (real_t)CMP_EPSILON)) { + if (m12 > -(1 - (real_t)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) { + if (rows[1][0] == 0 && rows[0][1] == 0 && rows[0][2] == 0 && rows[2][0] == 0 && rows[0][0] == 1) { // return the simplest form (human friendlier in editor and scripts) - euler.x = atan2(-m12, elements[1][1]); + euler.x = atan2(-m12, rows[1][1]); euler.y = 0; euler.z = 0; } else { euler.x = asin(-m12); - euler.y = atan2(elements[0][2], elements[2][2]); - euler.z = atan2(elements[1][0], elements[1][1]); + euler.y = atan2(rows[0][2], rows[2][2]); + euler.z = atan2(rows[1][0], rows[1][1]); } } else { // m12 == -1 - euler.x = Math_PI * 0.5; - euler.y = atan2(elements[0][1], elements[0][0]); + euler.x = Math_PI * 0.5f; + euler.y = atan2(rows[0][1], rows[0][0]); euler.z = 0; } } else { // m12 == 1 - euler.x = -Math_PI * 0.5; - euler.y = -atan2(elements[0][1], elements[0][0]); + euler.x = -Math_PI * 0.5f; + euler.y = -atan2(rows[0][1], rows[0][0]); euler.z = 0; } @@ -537,23 +564,23 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // -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]); + real_t sz = rows[1][0]; + if (sz < (1.0f - (real_t)CMP_EPSILON)) { + if (sz > -(1.0f - (real_t)CMP_EPSILON)) { + euler.x = Math::atan2(-rows[1][2], rows[1][1]); + euler.y = Math::atan2(-rows[2][0], rows[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; + euler.x = Math::atan2(rows[2][1], rows[2][2]); + euler.y = 0.0f; + euler.z = -Math_PI / 2.0f; } } else { // It's 1 - euler.x = Math::atan2(elements[2][1], elements[2][2]); - euler.y = 0.0; - euler.z = Math_PI / 2.0; + euler.x = Math::atan2(rows[2][1], rows[2][2]); + euler.y = 0.0f; + euler.z = Math_PI / 2.0f; } return euler; } break; @@ -565,22 +592,22 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // 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)) { + real_t sx = rows[2][1]; + if (sx < (1.0f - (real_t)CMP_EPSILON)) { + if (sx > -(1.0f - (real_t)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]); + euler.y = Math::atan2(-rows[2][0], rows[2][2]); + euler.z = Math::atan2(-rows[0][1], rows[1][1]); } else { // It's -1 - euler.x = -Math_PI / 2.0; - euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.x = -Math_PI / 2.0f; + euler.y = Math::atan2(rows[0][2], rows[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.x = Math_PI / 2.0f; + euler.y = Math::atan2(rows[0][2], rows[0][0]); euler.z = 0; } return euler; @@ -593,23 +620,23 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // 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]); + real_t sy = rows[2][0]; + if (sy < (1.0f - (real_t)CMP_EPSILON)) { + if (sy > -(1.0f - (real_t)CMP_EPSILON)) { + euler.x = Math::atan2(rows[2][1], rows[2][2]); euler.y = Math::asin(-sy); - euler.z = Math::atan2(elements[1][0], elements[0][0]); + euler.z = Math::atan2(rows[1][0], rows[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]); + euler.y = Math_PI / 2.0f; + euler.z = -Math::atan2(rows[0][1], rows[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]); + euler.y = -Math_PI / 2.0f; + euler.z = -Math::atan2(rows[0][1], rows[1][1]); } return euler; } break; @@ -625,15 +652,15 @@ void Basis::set_euler(const Vector3 &p_euler, EulerOrder p_order) { 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); + Basis xmat(1, 0, 0, 0, c, -s, 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); + Basis ymat(c, 0, s, 0, 1, 0, -s, 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); + Basis zmat(c, -s, 0, s, c, 0, 0, 0, 1); switch (p_order) { case EULER_ORDER_XYZ: { @@ -661,13 +688,13 @@ void Basis::set_euler(const Vector3 &p_euler, EulerOrder p_order) { } 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]); + return rows[0].is_equal_approx(p_basis.rows[0]) && rows[1].is_equal_approx(p_basis.rows[1]) && rows[2].is_equal_approx(p_basis.rows[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 (rows[i][j] != p_matrix.rows[i][j]) { return false; } } @@ -681,9 +708,9 @@ bool Basis::operator!=(const Basis &p_matrix) const { } Basis::operator String() const { - return "[X: " + get_axis(0).operator String() + - ", Y: " + get_axis(1).operator String() + - ", Z: " + get_axis(2).operator String() + "]"; + return "[X: " + get_column(0).operator String() + + ", Y: " + get_column(1).operator String() + + ", Z: " + get_column(2).operator String() + "]"; } Quaternion Basis::get_quaternion() const { @@ -692,97 +719,36 @@ Quaternion Basis::get_quaternion() const { #endif /* Allow getting a quaternion from an unnormalized transform */ Basis m = *this; - real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; + real_t trace = m.rows[0][0] + m.rows[1][1] + m.rows[2][2]; real_t temp[4]; - if (trace > 0.0) { - real_t s = Math::sqrt(trace + 1.0); - temp[3] = (s * 0.5); - s = 0.5 / s; + if (trace > 0.0f) { + real_t s = Math::sqrt(trace + 1.0f); + temp[3] = (s * 0.5f); + s = 0.5f / s; - temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s); - temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); - temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); + temp[0] = ((m.rows[2][1] - m.rows[1][2]) * s); + temp[1] = ((m.rows[0][2] - m.rows[2][0]) * s); + temp[2] = ((m.rows[1][0] - m.rows[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); + int i = m.rows[0][0] < m.rows[1][1] + ? (m.rows[1][1] < m.rows[2][2] ? 2 : 1) + : (m.rows[0][0] < m.rows[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; - real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); - temp[i] = s * 0.5; - s = 0.5 / s; + real_t s = Math::sqrt(m.rows[i][i] - m.rows[j][j] - m.rows[k][k] + 1.0f); + temp[i] = s * 0.5f; + s = 0.5f / s; - temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; - temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; - temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; + temp[3] = (m.rows[k][j] - m.rows[j][k]) * s; + temp[j] = (m.rows[j][i] + m.rows[i][j]) * s; + temp[k] = (m.rows[k][i] + m.rows[i][k]) * s; } return Quaternion(temp[0], temp[1], temp[2], temp[3]); } -static const Basis _ortho_bases[24] = { - Basis(1, 0, 0, 0, 1, 0, 0, 0, 1), - Basis(0, -1, 0, 1, 0, 0, 0, 0, 1), - Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1), - Basis(0, 1, 0, -1, 0, 0, 0, 0, 1), - Basis(1, 0, 0, 0, 0, -1, 0, 1, 0), - Basis(0, 0, 1, 1, 0, 0, 0, 1, 0), - Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0), - Basis(0, 0, -1, -1, 0, 0, 0, 1, 0), - Basis(1, 0, 0, 0, -1, 0, 0, 0, -1), - Basis(0, 1, 0, 1, 0, 0, 0, 0, -1), - Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1), - Basis(0, -1, 0, -1, 0, 0, 0, 0, -1), - Basis(1, 0, 0, 0, 0, 1, 0, -1, 0), - Basis(0, 0, -1, 1, 0, 0, 0, -1, 0), - Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0), - Basis(0, 0, 1, -1, 0, 0, 0, -1, 0), - Basis(0, 0, 1, 0, 1, 0, -1, 0, 0), - Basis(0, -1, 0, 0, 0, 1, -1, 0, 0), - Basis(0, 0, -1, 0, -1, 0, -1, 0, 0), - Basis(0, 1, 0, 0, 0, -1, -1, 0, 0), - Basis(0, 0, 1, 0, -1, 0, 1, 0, 0), - Basis(0, 1, 0, 0, 0, 1, 1, 0, 0), - Basis(0, 0, -1, 0, 1, 0, 1, 0, 0), - Basis(0, -1, 0, 0, 0, -1, 1, 0, 0) -}; - -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) { - v = 1.0; - } else if (v < -0.5) { - v = -1.0; - } else { - v = 0; - } - - orth[i][j] = v; - } - } - - for (int i = 0; i < 24; i++) { - 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); - - *this = _ortho_bases[p_index]; -} - void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { /* checking this is a bad idea, because obtaining from scaled transform is a valid use case #ifdef MATH_CHECKS @@ -790,14 +756,13 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { #endif */ real_t angle, x, y, z; // variables for result - real_t epsilon = 0.01; // margin to allow for rounding errors - real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees + real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees - 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)) { + if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[1][2]) < CMP_EPSILON)) { // singularity found // first check for identity matrix which must have +1 for all 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)) { + if ((Math::abs(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_epsilon)) { // this singularity is identity matrix so angle = 0 r_axis = Vector3(0, 1, 0); r_angle = 0; @@ -805,14 +770,14 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { } // otherwise this singularity is angle = 180 angle = Math_PI; - real_t xx = (elements[0][0] + 1) / 2; - real_t yy = (elements[1][1] + 1) / 2; - real_t zz = (elements[2][2] + 1) / 2; - real_t xy = (elements[1][0] + elements[0][1]) / 4; - real_t xz = (elements[2][0] + elements[0][2]) / 4; - real_t yz = (elements[2][1] + elements[1][2]) / 4; - if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term - if (xx < epsilon) { + real_t xx = (rows[0][0] + 1) / 2; + real_t yy = (rows[1][1] + 1) / 2; + real_t zz = (rows[2][2] + 1) / 2; + real_t xy = (rows[1][0] + rows[0][1]) / 4; + real_t xz = (rows[2][0] + rows[0][2]) / 4; + real_t yz = (rows[2][1] + rows[1][2]) / 4; + if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term + if (xx < CMP_EPSILON) { x = 0; y = Math_SQRT12; z = Math_SQRT12; @@ -821,8 +786,8 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { y = xy / x; z = xz / x; } - } else if (yy > zz) { // elements[1][1] is the largest diagonal term - if (yy < epsilon) { + } else if (yy > zz) { // rows[1][1] is the largest diagonal term + if (yy < CMP_EPSILON) { x = Math_SQRT12; y = 0; z = Math_SQRT12; @@ -831,8 +796,8 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { x = xy / y; z = yz / y; } - } else { // elements[2][2] is the largest diagonal term so base result on this - if (zz < epsilon) { + } else { // rows[2][2] is the largest diagonal term so base result on this + if (zz < CMP_EPSILON) { x = Math_SQRT12; y = Math_SQRT12; z = 0; @@ -847,15 +812,15 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { return; } // as we have reached here there are no singularities so we can handle normally - 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 + real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise - angle = Math::acos((elements[0][0] + elements[1][1] + elements[2][2] - 1) / 2); + angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2); 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; + x = (rows[2][1] - rows[1][2]) / s; + y = (rows[0][2] - rows[2][0]) / s; + z = (rows[1][0] - rows[0][1]) / s; r_axis = Vector3(x, y, z); r_angle = angle; @@ -863,54 +828,54 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { void Basis::set_quaternion(const Quaternion &p_quaternion) { real_t d = p_quaternion.length_squared(); - real_t s = 2.0 / d; + real_t s = 2.0f / d; 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)); + set(1.0f - (yy + zz), xy - wz, xz + wy, + xy + wz, 1.0f - (xx + zz), yz - wx, + xz - wy, yz + wx, 1.0f - (xx + yy)); } -void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { +void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_angle) { // Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle #ifdef MATH_CHECKS ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized."); #endif Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z); - real_t cosine = Math::cos(p_phi); - elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x); - elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y); - elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); + real_t cosine = Math::cos(p_angle); + rows[0][0] = axis_sq.x + cosine * (1.0f - axis_sq.x); + rows[1][1] = axis_sq.y + cosine * (1.0f - axis_sq.y); + rows[2][2] = axis_sq.z + cosine * (1.0f - axis_sq.z); - real_t sine = Math::sin(p_phi); + real_t sine = Math::sin(p_angle); real_t t = 1 - cosine; real_t xyzt = p_axis.x * p_axis.y * t; real_t zyxs = p_axis.z * sine; - elements[0][1] = xyzt - zyxs; - elements[1][0] = xyzt + zyxs; + rows[0][1] = xyzt - zyxs; + rows[1][0] = xyzt + zyxs; xyzt = p_axis.x * p_axis.z * t; zyxs = p_axis.y * sine; - elements[0][2] = xyzt + zyxs; - elements[2][0] = xyzt - zyxs; + rows[0][2] = xyzt + zyxs; + rows[2][0] = xyzt - zyxs; xyzt = p_axis.y * p_axis.z * t; zyxs = p_axis.x * sine; - elements[1][2] = xyzt - zyxs; - elements[2][1] = xyzt + zyxs; + rows[1][2] = xyzt - zyxs; + rows[2][1] = xyzt + zyxs; } -void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { +void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { _set_diagonal(p_scale); - rotate(p_axis, p_phi); + rotate(p_axis, p_angle); } -void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) { +void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order) { _set_diagonal(p_scale); - rotate(p_euler); + rotate(p_euler, p_order); } void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale) { @@ -921,17 +886,26 @@ void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 & // This also sets the non-diagonal elements to 0, which is misleading from the // name, so we want this method to be private. Use `from_scale` externally. void Basis::_set_diagonal(const Vector3 &p_diag) { - elements[0][0] = p_diag.x; - elements[0][1] = 0; - elements[0][2] = 0; + rows[0][0] = p_diag.x; + rows[0][1] = 0; + rows[0][2] = 0; - elements[1][0] = 0; - elements[1][1] = p_diag.y; - elements[1][2] = 0; + rows[1][0] = 0; + rows[1][1] = p_diag.y; + rows[1][2] = 0; - elements[2][0] = 0; - elements[2][1] = 0; - elements[2][2] = p_diag.z; + rows[2][0] = 0; + rows[2][1] = 0; + rows[2][2] = p_diag.z; +} + +Basis Basis::lerp(const Basis &p_to, const real_t &p_weight) const { + Basis b; + b.rows[0] = rows[0].lerp(p_to.rows[0], p_weight); + b.rows[1] = rows[1].lerp(p_to.rows[1], p_weight); + b.rows[2] = rows[2].lerp(p_to.rows[2], p_weight); + + return b; } Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { @@ -940,9 +914,9 @@ Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { Quaternion to(p_to); 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); + b.rows[0] *= Math::lerp(rows[0].length(), p_to.rows[0].length(), p_weight); + b.rows[1] *= Math::lerp(rows[1].length(), p_to.rows[1].length(), p_weight); + b.rows[2] *= Math::lerp(rows[2].length(), p_to.rows[2].length(), p_weight); return b; } @@ -966,17 +940,17 @@ void Basis::rotate_sh(real_t *p_values) { 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] }; + const 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]; + real_t m00 = rows[0][0]; + real_t m01 = rows[0][1]; + real_t m02 = rows[0][2]; + real_t m10 = rows[1][0]; + real_t m11 = rows[1][1]; + real_t m12 = rows[1][2]; + real_t m20 = rows[2][0]; + real_t m21 = rows[2][1]; + real_t m22 = rows[2][2]; p_values[0] = src[0]; p_values[1] = m11 * src[1] - m12 * src[2] + m10 * src[3]; @@ -1071,6 +1045,6 @@ Basis Basis::looking_at(const Vector3 &p_target, const Vector3 &p_up) { Vector3 v_y = v_z.cross(v_x); Basis basis; - basis.set(v_x, v_y, v_z); + basis.set_columns(v_x, v_y, v_z); return basis; } diff --git a/core/math/basis.h b/core/math/basis.h index 617d005f19..2853947ba7 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,22 +34,18 @@ #include "core/math/quaternion.h" #include "core/math/vector3.h" -class Basis { -private: - void _set_diagonal(const Vector3 &p_diag); - -public: - Vector3 elements[3] = { +struct _NO_DISCARD_ Basis { + Vector3 rows[3] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) }; _FORCE_INLINE_ const Vector3 &operator[](int axis) const { - return elements[axis]; + return rows[axis]; } _FORCE_INLINE_ Vector3 &operator[](int axis) { - return elements[axis]; + return rows[axis]; } void invert(); @@ -60,31 +56,6 @@ public: _FORCE_INLINE_ real_t determinant() const; - void from_z(const Vector3 &p_z); - - _FORCE_INLINE_ Vector3 get_axis(int p_axis) const { - // get actual basis axis (elements is transposed for performance) - return Vector3(elements[0][p_axis], elements[1][p_axis], elements[2][p_axis]); - } - _FORCE_INLINE_ void set_axis(int p_axis, const Vector3 &p_value) { - // get actual basis axis (elements is transposed for performance) - elements[0][p_axis] = p_value.x; - elements[1][p_axis] = p_value.y; - elements[2][p_axis] = p_value.z; - } - - void rotate(const Vector3 &p_axis, real_t p_phi); - Basis rotated(const Vector3 &p_axis, real_t p_phi) const; - - void rotate_local(const Vector3 &p_axis, real_t p_phi); - Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const; - - void rotate(const Vector3 &p_euler); - Basis rotated(const Vector3 &p_euler) const; - - void rotate(const Quaternion &p_quaternion); - Basis rotated(const Quaternion &p_quaternion) const; - enum EulerOrder { EULER_ORDER_XYZ, EULER_ORDER_XZY, @@ -94,6 +65,20 @@ public: EULER_ORDER_ZYX }; + void from_z(const Vector3 &p_z); + + void rotate(const Vector3 &p_axis, real_t p_angle); + Basis rotated(const Vector3 &p_axis, real_t p_angle) const; + + void rotate_local(const Vector3 &p_axis, real_t p_angle); + Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const; + + void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ); + Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const; + + void rotate(const Quaternion &p_quaternion); + Basis rotated(const Quaternion &p_quaternion) const; + Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_YXZ) 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; @@ -115,7 +100,7 @@ public: void set_quaternion(const Quaternion &p_quaternion); void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const; - void set_axis_angle(const Vector3 &p_axis, real_t p_phi); + void set_axis_angle(const Vector3 &p_axis, real_t p_angle); void scale(const Vector3 &p_scale); Basis scaled(const Vector3 &p_scale) const; @@ -123,6 +108,9 @@ public: void scale_local(const Vector3 &p_scale); Basis scaled_local(const Vector3 &p_scale) const; + void scale_orthogonal(const Vector3 &p_scale); + Basis scaled_orthogonal(const Vector3 &p_scale) const; + void make_scale_uniform(); float get_uniform_scale() const; @@ -130,19 +118,19 @@ public: 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_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale); + void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EULER_ORDER_YXZ); void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale); // transposed dot products _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const { - return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2]; + return rows[0][0] * v[0] + rows[1][0] * v[1] + rows[2][0] * v[2]; } _FORCE_INLINE_ real_t tdoty(const Vector3 &v) const { - return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2]; + return rows[0][1] * v[0] + rows[1][1] * v[1] + rows[2][1] * v[2]; } _FORCE_INLINE_ real_t tdotz(const Vector3 &v) const { - return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; + return rows[0][2] * v[0] + rows[1][2] * v[1] + rows[2][2] * v[2]; } bool is_equal_approx(const Basis &p_basis) const; @@ -161,13 +149,11 @@ public: _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); - bool is_orthogonal() const; bool is_diagonal() const; bool is_rotation() const; + Basis lerp(const Basis &p_to, const real_t &p_weight) const; Basis slerp(const Basis &p_to, const real_t &p_weight) const; void rotate_sh(real_t *p_values); @@ -176,55 +162,55 @@ public: /* 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; - elements[1][0] = yx; - elements[1][1] = yy; - elements[1][2] = yz; - elements[2][0] = zx; - elements[2][1] = zy; - 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); + rows[0][0] = xx; + rows[0][1] = xy; + rows[0][2] = xz; + rows[1][0] = yx; + rows[1][1] = yy; + rows[1][2] = yz; + rows[2][0] = zx; + rows[2][1] = zy; + rows[2][2] = zz; } - _FORCE_INLINE_ Vector3 get_column(int i) const { - return Vector3(elements[0][i], elements[1][i], elements[2][i]); + _FORCE_INLINE_ void set_columns(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { + set_column(0, p_x); + set_column(1, p_y); + set_column(2, p_z); } - _FORCE_INLINE_ Vector3 get_row(int i) const { - return Vector3(elements[i][0], elements[i][1], elements[i][2]); + _FORCE_INLINE_ Vector3 get_column(int p_index) const { + // Get actual basis axis column (we store transposed as rows for performance). + return Vector3(rows[0][p_index], rows[1][p_index], rows[2][p_index]); } - _FORCE_INLINE_ Vector3 get_main_diagonal() const { - return Vector3(elements[0][0], elements[1][1], elements[2][2]); + + _FORCE_INLINE_ void set_column(int p_index, const Vector3 &p_value) { + // Set actual basis axis column (we store transposed as rows for performance). + rows[0][p_index] = p_value.x; + rows[1][p_index] = p_value.y; + rows[2][p_index] = p_value.z; } - _FORCE_INLINE_ void set_row(int i, const Vector3 &p_row) { - elements[i][0] = p_row.x; - elements[i][1] = p_row.y; - elements[i][2] = p_row.z; + _FORCE_INLINE_ Vector3 get_main_diagonal() const { + return Vector3(rows[0][0], rows[1][1], rows[2][2]); } _FORCE_INLINE_ void set_zero() { - elements[0].zero(); - elements[1].zero(); - elements[2].zero(); + rows[0].zero(); + rows[1].zero(); + rows[2].zero(); } _FORCE_INLINE_ Basis transpose_xform(const Basis &m) const { return Basis( - elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x, - elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y, - elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z, - elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x, - elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y, - elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z, - elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x, - elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y, - elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); + rows[0].x * m[0].x + rows[1].x * m[1].x + rows[2].x * m[2].x, + rows[0].x * m[0].y + rows[1].x * m[1].y + rows[2].x * m[2].y, + rows[0].x * m[0].z + rows[1].x * m[1].z + rows[2].x * m[2].z, + rows[0].y * m[0].x + rows[1].y * m[1].x + rows[2].y * m[2].x, + rows[0].y * m[0].y + rows[1].y * m[1].y + rows[2].y * m[2].y, + rows[0].y * m[0].z + rows[1].y * m[1].z + rows[2].y * m[2].z, + rows[0].z * m[0].x + rows[1].z * m[1].x + rows[2].z * m[2].x, + rows[0].z * m[0].y + rows[1].z * m[1].y + rows[2].z * m[2].y, + rows[0].z * m[0].z + rows[1].z * m[1].z + rows[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); @@ -233,6 +219,9 @@ public: void orthonormalize(); Basis orthonormalized() const; + void orthogonalize(); + Basis orthogonalized() const; + #ifdef MATH_CHECKS bool is_symmetric() const; #endif @@ -245,37 +234,41 @@ public: 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_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); } - Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); } + Basis(const Vector3 &p_axis, real_t p_angle) { set_axis_angle(p_axis, p_angle); } + Basis(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_angle, p_scale); } static Basis from_scale(const Vector3 &p_scale); _FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) { - elements[0] = row0; - elements[1] = row1; - elements[2] = row2; + rows[0] = row0; + rows[1] = row1; + rows[2] = row2; } _FORCE_INLINE_ Basis() {} + +private: + // Helper method. + void _set_diagonal(const Vector3 &p_diag); }; _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]), - p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); + p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]), + p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]), + p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2])); } _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]), - p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); + p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]), + p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]), + p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2])); } _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]; + rows[0] += p_matrix.rows[0]; + rows[1] += p_matrix.rows[1]; + rows[2] += p_matrix.rows[2]; } _FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const { @@ -285,9 +278,9 @@ _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]; + rows[0] -= p_matrix.rows[0]; + rows[1] -= p_matrix.rows[1]; + rows[2] -= p_matrix.rows[2]; } _FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const { @@ -297,9 +290,9 @@ _FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const { } _FORCE_INLINE_ void Basis::operator*=(const real_t p_val) { - elements[0] *= p_val; - elements[1] *= p_val; - elements[2] *= p_val; + rows[0] *= p_val; + rows[1] *= p_val; + rows[2] *= p_val; } _FORCE_INLINE_ Basis Basis::operator*(const real_t p_val) const { @@ -310,21 +303,22 @@ _FORCE_INLINE_ Basis Basis::operator*(const real_t p_val) const { Vector3 Basis::xform(const Vector3 &p_vector) const { return Vector3( - elements[0].dot(p_vector), - elements[1].dot(p_vector), - elements[2].dot(p_vector)); + rows[0].dot(p_vector), + rows[1].dot(p_vector), + rows[2].dot(p_vector)); } 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), - (elements[0][2] * p_vector.x) + (elements[1][2] * p_vector.y) + (elements[2][2] * p_vector.z)); + (rows[0][0] * p_vector.x) + (rows[1][0] * p_vector.y) + (rows[2][0] * p_vector.z), + (rows[0][1] * p_vector.x) + (rows[1][1] * p_vector.y) + (rows[2][1] * p_vector.z), + (rows[0][2] * p_vector.x) + (rows[1][2] * p_vector.y) + (rows[2][2] * p_vector.z)); } 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]); + return rows[0][0] * (rows[1][1] * rows[2][2] - rows[2][1] * rows[1][2]) - + rows[1][0] * (rows[0][1] * rows[2][2] - rows[2][1] * rows[0][2]) + + rows[2][0] * (rows[0][1] * rows[1][2] - rows[1][1] * rows[0][2]); } + #endif // BASIS_H diff --git a/core/math/bvh.h b/core/math/bvh.h index 65b8b102a3..b5f5eda3e6 100644 --- a/core/math/bvh.h +++ b/core/math/bvh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -46,21 +46,35 @@ // 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. +// In the physics, the pairable_type is based on 1 << p_object->get_type() where: +// TYPE_AREA, +// TYPE_BODY +// and pairable_mask is either 0 if static, or set to all if non static + #include "bvh_tree.h" +#include "core/os/mutex.h" -#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, Bounds, Point> +#define BVHTREE_CLASS BVH_Tree<T, NUM_TREES, 2, MAX_ITEMS, USER_PAIR_TEST_FUNCTION, USER_CULL_TEST_FUNCTION, USE_PAIRS, BOUNDS, POINT> +#define BVH_LOCKED_FUNCTION BVHLockedFunction(&_mutex, BVH_THREAD_SAFE &&_thread_safe); -template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class Bounds = AABB, class Point = Vector3> +template <class T, int NUM_TREES = 1, bool USE_PAIRS = false, int MAX_ITEMS = 32, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, class BOUNDS = AABB, class POINT = Vector3, bool BVH_THREAD_SAFE = true> 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 *); + typedef void *(*CheckPairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *); + + // allow locally toggling thread safety if the template has been compiled with BVH_THREAD_SAFE + void params_set_thread_safe(bool p_enable) { + _thread_safe = p_enable; + } // 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) { + BVH_LOCKED_FUNCTION if (p_value >= 0.0) { tree._node_expansion = p_value; tree._auto_node_expansion = false; @@ -70,43 +84,40 @@ public: } 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; - } + BVH_LOCKED_FUNCTION + tree.params_set_pairing_expansion(p_value); } void set_pair_callback(PairCallback p_callback, void *p_userdata) { + BVH_LOCKED_FUNCTION pair_callback = p_callback; pair_callback_userdata = p_userdata; } void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) { + BVH_LOCKED_FUNCTION unpair_callback = p_callback; unpair_callback_userdata = p_userdata; } + void set_check_pair_callback(CheckPairCallback p_callback, void *p_userdata) { + BVH_LOCKED_FUNCTION + check_pair_callback = p_callback; + check_pair_callback_userdata = p_userdata; + } + + BVHHandle create(T *p_userdata, bool p_active = true, uint32_t p_tree_id = 0, uint32_t p_tree_collision_mask = 1, const BOUNDS &p_aabb = BOUNDS(), int p_subindex = 0) { + BVH_LOCKED_FUNCTION - 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); + BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_tree_id, p_tree_collision_mask); if (USE_PAIRS) { // for safety initialize the expanded AABB - Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb; + BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb; expanded_aabb = p_aabb; expanded_aabb.grow_by(tree._pairing_expansion); @@ -123,12 +134,18 @@ public: //////////////////////////////////////////////////// // 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) { + void move(uint32_t p_handle, const BOUNDS &p_aabb) { BVHHandle h; h.set(p_handle); move(h, p_aabb); } + void recheck_pairs(uint32_t p_handle) { + BVHHandle h; + h.set(p_handle); + recheck_pairs(h); + } + void erase(uint32_t p_handle) { BVHHandle h; h.set(p_handle); @@ -141,7 +158,7 @@ public: force_collision_check(h); } - bool activate(uint32_t p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) { + 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); @@ -153,16 +170,16 @@ public: 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) { + void set_tree(uint32_t p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_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); + set_tree(h, p_tree_id, p_tree_collision_mask, p_force_collision_check); } - bool is_pairable(uint32_t p_handle) const { + uint32_t get_tree_id(uint32_t p_handle) const { BVHHandle h; h.set(p_handle); - return item_is_pairable(h); + return item_get_tree_id(h); } int get_subindex(uint32_t p_handle) const { BVHHandle h; @@ -178,7 +195,9 @@ public: //////////////////////////////////////////////////// - void move(BVHHandle p_handle, const Bounds &p_aabb) { + void move(BVHHandle p_handle, const BOUNDS &p_aabb) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION if (tree.item_move(p_handle, p_aabb)) { if (USE_PAIRS) { _add_changed_item(p_handle, p_aabb); @@ -186,7 +205,14 @@ public: } } + void recheck_pairs(BVHHandle p_handle) { + DEV_ASSERT(!p_handle.is_invalid()); + force_collision_check(p_handle); + } + void erase(BVHHandle p_handle) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION // call unpair and remove all references to the item // before deleting from the tree if (USE_PAIRS) { @@ -200,11 +226,13 @@ public: // 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 rendering server for historical reasons) + // (deferred collision checks are a workaround for visual server for historical reasons) void force_collision_check(BVHHandle p_handle) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION if (USE_PAIRS) { // the aabb should already be up to date in the BVH - Bounds aabb; + BOUNDS aabb; item_get_AABB(p_handle, aabb); // add it as changed even if aabb not different @@ -218,7 +246,9 @@ public: // 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) { + bool activate(BVHHandle p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION // sending the aabb here prevents the need for the BVH to maintain // a redundant copy of the aabb. // returns success @@ -242,6 +272,8 @@ public: } bool deactivate(BVHHandle p_handle) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION // returns success if (tree.item_deactivate(p_handle)) { // call unpair and remove all references to the item @@ -258,39 +290,45 @@ public: return false; } - bool get_active(BVHHandle p_handle) const { + bool get_active(BVHHandle p_handle) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION return tree.item_get_active(p_handle); } // call e.g. once per frame (this does a trickle optimize) void update() { + BVH_LOCKED_FUNCTION tree.update(); _check_for_collisions(); #ifdef BVH_INTEGRITY_CHECKS - tree.integrity_check_all(); + tree._integrity_check_all(); #endif } // this can be called more frequently than per frame if necessary void update_collisions() { + BVH_LOCKED_FUNCTION _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) { + void set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) { + DEV_ASSERT(!p_handle.is_invalid()); + BVH_LOCKED_FUNCTION // 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); + bool state_changed = tree.item_set_tree(p_handle, p_tree_id, p_tree_collision_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)) { + if ((p_force_collision_check || state_changed) && tree.item_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; + BOUNDS aabb; item_get_AABB(p_handle, aabb); // passing false disables the optimization which prevents collision checks if @@ -307,32 +345,33 @@ public: } // 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) { + int cull_aabb(const BOUNDS &p_aabb, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) { + BVH_LOCKED_FUNCTION 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.tree_collision_mask = p_tree_collision_mask; params.abb.from(p_aabb); + params.tester = p_tester; 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) { + int cull_segment(const POINT &p_from, const POINT &p_to, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) { + BVH_LOCKED_FUNCTION 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.tester = p_tester; + params.tree_collision_mask = p_tree_collision_mask; params.segment.from = p_from; params.segment.to = p_to; @@ -342,15 +381,16 @@ public: 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) { + int cull_point(const POINT &p_point, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) { + BVH_LOCKED_FUNCTION 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.tester = p_tester; + params.tree_collision_mask = p_tree_collision_mask; params.point = p_point; @@ -358,7 +398,8 @@ public: 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) { + int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF) { + BVH_LOCKED_FUNCTION if (!p_convex.size()) { return 0; } @@ -373,8 +414,8 @@ public: 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.tester = p_tester; + params.tree_collision_mask = p_tree_collision_mask; params.hull.planes = &p_convex[0]; params.hull.num_planes = p_convex.size(); @@ -394,7 +435,7 @@ private: return; } - Bounds bb; + BOUNDS bb; typename BVHTREE_CLASS::CullParams params; @@ -402,28 +443,23 @@ private: 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; + const BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb; BVHABB_CLASS abb; abb.from(expanded_aabb); + tree.item_fill_cullparams(h, params); + // 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 @@ -437,13 +473,6 @@ private: 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); @@ -456,7 +485,8 @@ private: } public: - void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) { + void item_get_AABB(BVHHandle p_handle, BOUNDS &r_aabb) { + DEV_ASSERT(!p_handle.is_invalid()); BVHABB_CLASS abb; tree.item_get_ABB(p_handle, abb); abb.to(r_aabb); @@ -464,7 +494,7 @@ public: private: // supplemental funcs - bool item_is_pairable(BVHHandle p_handle) const { return _get_extra(p_handle).pairable; } + uint32_t item_get_tree_id(BVHHandle p_handle) const { return _get_extra(p_handle).tree_id; } 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; } @@ -485,12 +515,35 @@ private: void *ud_from = pairs_from.remove_pair_to(p_to); pairs_to.remove_pair_to(p_from); +#ifdef BVH_VERBOSE_PAIRING + print_line("_unpair " + itos(p_from.id()) + " from " + itos(p_to.id())); +#endif + // callback if (unpair_callback) { unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from); } } + void *_recheck_pair(BVHHandle p_from, BVHHandle p_to, void *p_pair_data) { + 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 p_pair_data; + } + + // callback + if (check_pair_callback) { + return check_pair_callback(check_pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, p_pair_data); + } + + return p_pair_data; + } + // 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; @@ -498,8 +551,8 @@ private: // 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 + // the full check for pairable / non pairable (i.e. tree_id and tree_masks) and mask changes is extra expense + // this need not be done in most cases (for speed) except in the case where set_tree is called // where the masks etc of the objects in question may have changed if (!p_full_check) { return false; @@ -507,12 +560,13 @@ private: 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) { + // Checking tree_ids and tree_collision_masks + if (exa.are_item_trees_compatible(exb)) { + bool pair_allowed = USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata); + // 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)) { + // i.e. if there is a hit between the two and they intersect, then they should stay paired + if (pair_allowed) { return false; } } @@ -550,6 +604,11 @@ private: const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha); const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb); + // user collision callback + if (!USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata)) { + return; + } + // if the userdata is the same, no collisions should occur if ((exa.userdata == exb.userdata) && exa.userdata) { return; @@ -573,6 +632,10 @@ private: // callback void *callback_userdata = nullptr; +#ifdef BVH_VERBOSE_PAIRING + print_line("_pair " + itos(p_ha.id()) + " to " + itos(p_hb.id())); +#endif + if (pair_callback) { callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex); } @@ -594,6 +657,32 @@ private: } } + // Send pair callbacks again for all existing pairs for the given handle. + void _recheck_pairs(BVHHandle p_handle) { + typename BVHTREE_CLASS::ItemPairs &from = tree._pairs[p_handle.id()]; + + // checking pair for every partner. + for (unsigned int n = 0; n < from.extended_pairs.size(); n++) { + typename BVHTREE_CLASS::ItemPairs::Link &pair = from.extended_pairs[n]; + BVHHandle h_to = pair.handle; + void *new_pair_data = _recheck_pair(p_handle, h_to, pair.userdata); + + if (new_pair_data != pair.userdata) { + pair.userdata = new_pair_data; + + // Update pair data for the second item. + typename BVHTREE_CLASS::ItemPairs &to = tree._pairs[h_to.id()]; + for (unsigned int to_index = 0; to_index < to.extended_pairs.size(); to_index++) { + typename BVHTREE_CLASS::ItemPairs::Link &to_pair = to.extended_pairs[to_index]; + if (to_pair.handle == p_handle) { + to_pair.userdata = new_pair_data; + break; + } + } + } + } + } + private: const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const { return tree._extra[p_handle.id()]; @@ -607,19 +696,24 @@ private: _tick++; } - void _add_changed_item(BVHHandle p_handle, const Bounds &aabb, bool p_check_aabb = true) { + 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 +#ifdef BVH_EXPAND_LEAF_AABBS + // if using expanded AABB in the leaf, the redundancy check will already have been made + BOUNDS &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb; + item_get_AABB(p_handle, expanded_aabb); +#else // 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; + 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)) { + if (p_check_aabb && tree.expanded_aabb_encloses_not_shrink(expanded_aabb, aabb)) { return; } @@ -627,6 +721,7 @@ private: // this tick, because it is vital that the AABB is kept up to date expanded_aabb = aabb; expanded_aabb.grow_by(tree._pairing_expansion); +#endif // 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 @@ -654,7 +749,7 @@ private: // 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); + changed_items.remove_at_unordered(n); // because we are using an unordered remove, // the last changed item will now be at spot 'n', @@ -668,26 +763,54 @@ private: tree._extra[p_handle.id()].last_updated_tick = 0; } - PairCallback pair_callback; - UnpairCallback unpair_callback; - void *pair_callback_userdata; - void *unpair_callback_userdata; + PairCallback pair_callback = nullptr; + UnpairCallback unpair_callback = nullptr; + CheckPairCallback check_pair_callback = nullptr; + void *pair_callback_userdata = nullptr; + void *unpair_callback_userdata = nullptr; + void *check_pair_callback_userdata = nullptr; 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; + uint32_t _tick = 1; // Start from 1 so items with 0 indicate never updated. + + class BVHLockedFunction { + public: + BVHLockedFunction(Mutex *p_mutex, bool p_thread_safe) { + // will be compiled out if not set in template + if (p_thread_safe) { + _mutex = p_mutex; + + if (_mutex->try_lock() != OK) { + WARN_PRINT("Info : multithread BVH access detected (benign)"); + _mutex->lock(); + } + + } else { + _mutex = nullptr; + } + } + ~BVHLockedFunction() { + // will be compiled out if not set in template + if (_mutex) { + _mutex->unlock(); + } + } + + private: + Mutex *_mutex = nullptr; + }; + + Mutex _mutex; + + // local toggle for turning on and off thread safety in project settings + bool _thread_safe = BVH_THREAD_SAFE; 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; - } + BVH_Manager() {} }; #undef BVHTREE_CLASS diff --git a/core/math/bvh_abb.h b/core/math/bvh_abb.h index bd9a01a87e..8a44f1c4da 100644 --- a/core/math/bvh_abb.h +++ b/core/math/bvh_abb.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,7 @@ #define BVH_ABB_H // special optimized version of axis aligned bounding box -template <class Bounds = AABB, class Point = Vector3> +template <class BOUNDS = AABB, class POINT = Vector3> struct BVH_ABB { struct ConvexHull { // convex hulls (optional) @@ -43,8 +43,8 @@ struct BVH_ABB { }; struct Segment { - Point from; - Point to; + POINT from; + POINT to; }; enum IntersectResult { @@ -54,47 +54,47 @@ struct BVH_ABB { }; // we store mins with a negative value in order to test them with SIMD - Point min; - Point neg_max; + 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) { + void set(const POINT &_min, const POINT &_max) { min = _min; neg_max = -_max; } // to and from standard AABB - void from(const Bounds &p_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 { + 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) { + 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 { + POINT calculate_size() const { return -neg_max - min; } - Point calculate_centre() const { - return Point((calculate_size() * 0.5) + 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); + 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) { + for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) { proximity += Math::abs(d[axis]); } return proximity; @@ -104,7 +104,7 @@ struct BVH_ABB { 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 find_cutting_planes(const typename 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++) { @@ -162,7 +162,7 @@ struct BVH_ABB { } bool intersects_convex_partial(const ConvexHull &p_hull) const { - Bounds bb; + BOUNDS bb; to(bb); return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points); } @@ -182,7 +182,7 @@ struct BVH_ABB { bool is_within_convex(const ConvexHull &p_hull) const { // use half extents routine - Bounds bb; + BOUNDS bb; to(bb); return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes); } @@ -197,12 +197,12 @@ struct BVH_ABB { } bool intersects_segment(const Segment &p_s) const { - Bounds bb; + BOUNDS bb; to(bb); return bb.intersects_segment(p_s.from, p_s.to); } - bool intersects_point(const Point &p_pt) const { + bool intersects_point(const POINT &p_pt) const { if (_any_lessthan(-p_pt, neg_max)) { return false; } @@ -212,6 +212,7 @@ struct BVH_ABB { return true; } + // Very hot in profiling, make sure optimized bool intersects(const BVH_ABB &p_o) const { if (_any_morethan(p_o.min, -neg_max)) { return false; @@ -222,6 +223,17 @@ struct BVH_ABB { return true; } + // for pre-swizzled tester (this object) + bool intersects_swizzled(const BVH_ABB &p_o) const { + if (_any_lessthan(min, p_o.min)) { + return false; + } + if (_any_lessthan(neg_max, 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; @@ -232,20 +244,20 @@ struct BVH_ABB { return true; } - void grow(const Point &p_change) { + void grow(const POINT &p_change) { neg_max -= p_change; min -= p_change; } void expand(real_t p_change) { - Point change; + POINT change; change.set_all(p_change); grow(change); } // Actually surface area metric. float get_area() const { - Point d = calculate_size(); + POINT d = calculate_size(); return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x); } @@ -254,8 +266,8 @@ struct BVH_ABB { min = neg_max; } - bool _any_morethan(const Point &p_a, const Point &p_b) const { - for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + 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; } @@ -263,8 +275,8 @@ struct BVH_ABB { return false; } - bool _any_lessthan(const Point &p_a, const Point &p_b) const { - for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) { + 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; } diff --git a/core/math/bvh_cull.inc b/core/math/bvh_cull.inc index d7edc8a884..11f50e41e6 100644 --- a/core/math/bvh_cull.inc +++ b/core/math/bvh_cull.inc @@ -9,20 +9,22 @@ struct CullParams { T **result_array; int *subindex_array; - // nobody truly understands how masks are intended to work. - uint32_t mask; - uint32_t pairable_type; + // We now process masks etc in a user template function, + // and these for simplicity assume even for cull tests there is a + // testing object (which has masks etc) for the user cull checks. + // This means for cull tests on their own, the client will usually + // want to create a dummy object, just in order to specify masks etc. + const T *tester; // optional components for different tests - Point point; + POINT 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; + // When collision testing, we can specify which tree ids + // to collide test against with the tree_collision_mask. + uint32_t tree_collision_mask; }; private: @@ -58,11 +60,22 @@ int cull_convex(CullParams &r_params, bool p_translate_hits = true) { _cull_hits.clear(); r_params.result_count = 0; + uint32_t tree_test_mask = 0; + for (int n = 0; n < NUM_TREES; n++) { + tree_test_mask <<= 1; + if (!tree_test_mask) { + tree_test_mask = 1; + } + if (_root_node_id[n] == BVHCommon::INVALID) { continue; } + if (!(r_params.tree_collision_mask & tree_test_mask)) { + continue; + } + _cull_convex_iterative(_root_node_id[n], r_params); } @@ -77,11 +90,22 @@ int cull_segment(CullParams &r_params, bool p_translate_hits = true) { _cull_hits.clear(); r_params.result_count = 0; + uint32_t tree_test_mask = 0; + for (int n = 0; n < NUM_TREES; n++) { + tree_test_mask <<= 1; + if (!tree_test_mask) { + tree_test_mask = 1; + } + if (_root_node_id[n] == BVHCommon::INVALID) { continue; } + if (!(r_params.tree_collision_mask & tree_test_mask)) { + continue; + } + _cull_segment_iterative(_root_node_id[n], r_params); } @@ -96,11 +120,22 @@ int cull_point(CullParams &r_params, bool p_translate_hits = true) { _cull_hits.clear(); r_params.result_count = 0; + uint32_t tree_test_mask = 0; + for (int n = 0; n < NUM_TREES; n++) { + tree_test_mask <<= 1; + if (!tree_test_mask) { + tree_test_mask = 1; + } + if (_root_node_id[n] == BVHCommon::INVALID) { continue; } + if (!(r_params.tree_collision_mask & tree_test_mask)) { + continue; + } + _cull_point_iterative(_root_node_id[n], r_params); } @@ -115,12 +150,20 @@ int cull_aabb(CullParams &r_params, bool p_translate_hits = true) { _cull_hits.clear(); r_params.result_count = 0; + uint32_t tree_test_mask = 0; + for (int n = 0; n < NUM_TREES; n++) { + tree_test_mask <<= 1; + if (!tree_test_mask) { + tree_test_mask = 1; + } + if (_root_node_id[n] == BVHCommon::INVALID) { continue; } - if ((n == 0) && r_params.test_pairable_only) { + // the tree collision mask determines which trees to collide test against + if (!(r_params.tree_collision_mask & tree_test_mask)) { continue; } @@ -142,22 +185,6 @@ bool _cull_hits_full(const CullParams &p) { 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, @@ -165,7 +192,8 @@ void _cull_hit(uint32_t p_ref_id, CullParams &p) { 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)) { + // user supplied function (for e.g. pairable types and pairable masks in the render tree) + if (!USER_CULL_TEST_FUNCTION::user_cull_check(p.tester, ex.userdata)) { return; } } @@ -294,6 +322,7 @@ bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) { return true; } +// Note: This is a very hot loop profiling wise. Take care when changing this and profile. 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 { @@ -336,16 +365,26 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully _cull_hit(child_id, r_params); } } else { - for (int n = 0; n < leaf.num_items; n++) { + // This section is the hottest area in profiling, so + // is optimized highly + // get this into a local register and preconverted to correct type + int leaf_num_items = leaf.num_items; + + BVHABB_CLASS swizzled_tester; + swizzled_tester.min = -r_params.abb.neg_max; + swizzled_tester.neg_max = -r_params.abb.min; + + for (int n = 0; n < leaf_num_items; n++) { const BVHABB_CLASS &aabb = leaf.get_aabb(n); - if (aabb.intersects(r_params.abb)) { + if (swizzled_tester.intersects_swizzled(aabb)) { 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) { @@ -508,8 +547,9 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful 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)) + if (!_cull_hit(child_id, r_params)) { return false; + } } } #endif // BVH_CONVEX_CULL_OPTIMIZED diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc index 55db794ee3..2e519ceb3d 100644 --- a/core/math/bvh_debug.inc +++ b/core/math/bvh_debug.inc @@ -1,17 +1,18 @@ public: #ifdef BVH_VERBOSE void _debug_recursive_print_tree(int p_tree_id) const { - if (_root_node_id[p_tree_id] != BVHCommon::INVALID) + 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 { - Point size = aabb.calculate_size(); + POINT size = aabb.calculate_size(); String sz; float vol = 0.0; - for (int i = 0; i < Point::AXES_COUNT; ++i) { + for (int i = 0; i < POINT::AXIS_COUNT; ++i) { sz += "("; sz += itos(aabb.min[i]); sz += " ~ "; @@ -42,8 +43,9 @@ void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const { sz += "["; for (int n = 0; n < leaf.num_items; n++) { - if (n) + if (n) { sz += ", "; + } sz += "r"; sz += itos(leaf.get_item_ref_id(n)); } diff --git a/core/math/bvh_logic.inc b/core/math/bvh_logic.inc index afab08f151..dd3b135bb5 100644 --- a/core/math/bvh_logic.inc +++ b/core/math/bvh_logic.inc @@ -42,24 +42,24 @@ BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) { //-------------------------------------------------------------------------------------------------- /** -@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. -*/ + * @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 @@ -67,7 +67,7 @@ BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) { // 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 + //return iA; // uncomment this to bypass balance TNode *A = &_nodes[iA]; @@ -75,12 +75,12 @@ int32_t _logic_balance(int32_t iA, uint32_t p_tree_id) { return iA; } - /* A - / \ - B C - / \ / \ - D E F G - */ + /* A + * / \ + * B C + * / \ / \ + * D E F G + */ CRASH_COND(A->num_children != 2); int32_t iB = A->children[0]; diff --git a/core/math/bvh_misc.inc b/core/math/bvh_misc.inc index 71aa0e4fe0..9b35a1d36d 100644 --- a/core/math/bvh_misc.inc +++ b/core/math/bvh_misc.inc @@ -1,11 +1,7 @@ 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 _extra[p_handle.id()].tree_id; } return 0; } diff --git a/core/math/bvh_pair.inc b/core/math/bvh_pair.inc index 839db59a3a..7b9c7ce6ae 100644 --- a/core/math/bvh_pair.inc +++ b/core/math/bvh_pair.inc @@ -14,10 +14,10 @@ struct ItemPairs { void clear() { num_pairs = 0; extended_pairs.reset(); - expanded_aabb = Bounds(); + expanded_aabb = BOUNDS(); } - Bounds expanded_aabb; + BOUNDS expanded_aabb; // maybe we can just use the number in the vector TODO int32_t num_pairs; @@ -51,7 +51,7 @@ struct ItemPairs { for (int n = 0; n < num_pairs; n++) { if (extended_pairs[n].handle == h) { userdata = extended_pairs[n].userdata; - extended_pairs.remove_unordered(n); + extended_pairs.remove_at_unordered(n); num_pairs--; break; } @@ -59,4 +59,14 @@ struct ItemPairs { return userdata; } + + // experiment : scale the pairing expansion by the number of pairs. + // when the number of pairs is high, the density is high and a lower collision margin is better. + // when there are few local pairs, a larger margin is more optimal. + real_t scale_expansion_margin(real_t p_margin) const { + real_t x = real_t(num_pairs) * (1.0 / 9.0); + x = MIN(x, 1.0); + x = 1.0 - x; + return p_margin * x; + } }; diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc index 2c1e406712..fc1c67a21b 100644 --- a/core/math/bvh_public.inc +++ b/core/math/bvh_public.inc @@ -1,14 +1,21 @@ 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) { +BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p_subindex, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_invisible = false) { #ifdef BVH_VERBOSE_TREE VERBOSE_PRINT("\nitem_add BEFORE"); - _debug_recursive_print_tree(0); + _debug_recursive_print_tree(p_tree_id); VERBOSE_PRINT("\n"); #endif BVHABB_CLASS abb; abb.from(p_aabb); + // NOTE that we do not expand the AABB for the first create even if + // leaf expansion is switched on. This is for two reasons: + // (1) We don't know if this object will move in future, in which case a non-expanded + // bound would be better... + // (2) We don't yet know how many objects will be paired, which is used to modify + // the expansion margin. + // handle to be filled with the new item ref BVHHandle handle; @@ -40,29 +47,17 @@ BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p 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; - } + extra->tree_id = p_tree_id; + extra->tree_collision_mask = p_tree_collision_mask; // 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); + create_root_node(p_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); + ref->tnode_id = _logic_choose_item_add_node(_root_node_id[p_tree_id], abb); bool refit = _node_add_item(ref->tnode_id, ref_id, abb); @@ -70,7 +65,7 @@ BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p // 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); + refit_upward_and_balance(add_node.parent_id, p_tree_id); } } } else { @@ -83,8 +78,8 @@ BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p 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())); + VERBOSE_PRINT("\titem_add [" + itos(ref_id) + "] " + itos(_refs.used_size()) + " refs,\t" + itos(_nodes.used_size()) + " nodes " + sz); + VERBOSE_PRINT("mem use : " + itos(mem) + ", num nodes reserved : " + itos(_nodes.reserved_size())); #endif @@ -103,7 +98,7 @@ void _debug_print_refs() { } // returns false if noop -bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { +bool item_move(BVHHandle p_handle, const BOUNDS &p_aabb) { uint32_t ref_id = p_handle.id(); // get the reference @@ -115,10 +110,19 @@ bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { BVHABB_CLASS abb; abb.from(p_aabb); +#ifdef BVH_EXPAND_LEAF_AABBS + if (USE_PAIRS) { + // scale the pairing expansion by the number of pairs. + abb.expand(_pairs[ref_id].scale_expansion_margin(_pairing_expansion)); + } else { + abb.expand(_pairing_expansion); + } +#endif + BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID); TNode &tnode = _nodes[ref.tnode_id]; - // does it fit within the current aabb? + // does it fit within the current leaf aabb? if (tnode.aabb.is_other_within(abb)) { // do nothing .. fast path .. not moved enough to need refit @@ -129,9 +133,24 @@ bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id); // no change? +#ifdef BVH_EXPAND_LEAF_AABBS + BOUNDS leaf_aabb; + leaf_abb.to(leaf_aabb); + + // This test should pass in a lot of cases, and by returning false we can avoid + // collision pairing checks later, which greatly reduces processing. + if (expanded_aabb_encloses_not_shrink(leaf_aabb, p_aabb)) { + return false; + } +#else if (leaf_abb == abb) { return false; } +#endif + +#ifdef BVH_VERBOSE_MOVES + print_line("item_move " + itos(p_handle.id()) + "(within tnode aabb) : " + _debug_aabb_to_string(abb)); +#endif leaf_abb = abb; _integrity_check_all(); @@ -139,6 +158,10 @@ bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { return true; } +#ifdef BVH_VERBOSE_MOVES + print_line("item_move " + itos(p_handle.id()) + "(outside tnode aabb) : " + _debug_aabb_to_string(abb)); +#endif + uint32_t tree_id = _handle_get_tree_id(p_handle); // remove and reinsert @@ -206,7 +229,7 @@ void item_remove(BVHHandle p_handle) { } // returns success -bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) { +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()) { @@ -260,12 +283,14 @@ 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; + // which trees does this item want to collide detect against? + r_params.tree_collision_mask = extra.tree_collision_mask; - // we take into account the mask of the item testing from - r_params.mask = extra.pairable_mask; - r_params.pairable_type = extra.pairable_type; + // The testing user defined object is passed to the user defined cull check function + // for masks etc. This is usually a dummy object of type T with masks set. + // However, if not using the cull_check callback (i.e. returning true), you can pass + // a nullptr instead of dummy object, as it will not be used. + r_params.tester = extra.userdata; } bool item_is_pairable(const BVHHandle &p_handle) { @@ -285,7 +310,7 @@ void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) { 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) { +bool item_set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask) { // change tree? uint32_t ref_id = p_handle.id(); @@ -293,13 +318,15 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa 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); + bool tree_changed = ex.tree_id != p_tree_id; + bool mask_changed = ex.tree_collision_mask != p_tree_collision_mask; + bool state_changed = tree_changed | mask_changed; - ex.pairable_type = p_pairable_type; - ex.pairable_mask = p_pairable_mask; + // Keep an eye on this for bugs of not noticing changes to objects, + // especially when changing client user masks that will not be detected as a change + // in the BVH. You may need to force a collision check in this case with recheck_pairs(). - if (active && pairable_changed) { + if (active && (tree_changed | mask_changed)) { // record abb TNode &tnode = _nodes[ref.tnode_id]; TLeaf &leaf = _node_get_leaf(tnode); @@ -313,7 +340,8 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa // we must set the pairable AFTER getting the current tree // because the pairable status determines which tree - ex.pairable = p_pairable; + ex.tree_id = p_tree_id; + ex.tree_collision_mask = p_tree_collision_mask; // add to new tree tree_id = _handle_get_tree_id(p_handle); @@ -333,7 +361,8 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa } } else { // always keep this up to date - ex.pairable = p_pairable; + ex.tree_id = p_tree_id; + ex.tree_collision_mask = p_tree_collision_mask; } return state_changed; @@ -403,7 +432,7 @@ void update() { // if there are no nodes, do nothing, but if there are... if (bound_valid) { - Bounds bb; + BOUNDS bb; world_bound.to(bb); real_t size = bb.get_longest_axis_size(); @@ -421,3 +450,50 @@ void update() { } #endif } + +void params_set_pairing_expansion(real_t p_value) { + if (p_value < 0.0) { +#ifdef BVH_ALLOW_AUTO_EXPANSION + _auto_pairing_expansion = true; +#endif + return; + } +#ifdef BVH_ALLOW_AUTO_EXPANSION + _auto_pairing_expansion = false; +#endif + + _pairing_expansion = p_value; + + // calculate shrinking threshold + const real_t fudge_factor = 1.1; + _aabb_shrinkage_threshold = _pairing_expansion * POINT::AXIS_COUNT * 2.0 * fudge_factor; +} + +// This routine is not just an enclose check, it also checks for special case of shrinkage +bool expanded_aabb_encloses_not_shrink(const BOUNDS &p_expanded_aabb, const BOUNDS &p_aabb) const { + if (!p_expanded_aabb.encloses(p_aabb)) { + return false; + } + + // Check for special case of shrinkage. If the aabb has shrunk + // significantly we want to create a new expanded bound, because + // the previous expanded bound will have diverged significantly. + const POINT &exp_size = p_expanded_aabb.size; + const POINT &new_size = p_aabb.size; + + real_t exp_l = 0.0; + real_t new_l = 0.0; + + for (int i = 0; i < POINT::AXIS_COUNT; ++i) { + exp_l += exp_size[i]; + new_l += new_size[i]; + } + + // is difference above some metric + real_t diff = exp_l - new_l; + if (diff < _aabb_shrinkage_threshold) { + return true; + } + + return false; +} diff --git a/core/math/bvh_split.inc b/core/math/bvh_split.inc index 6f54d06ce7..ff07166d4a 100644 --- a/core/math/bvh_split.inc +++ b/core/math/bvh_split.inc @@ -25,16 +25,16 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u return; } - Point centre = full_bound.calculate_centre(); - Point size = full_bound.calculate_size(); + POINT centre = full_bound.calculate_centre(); + POINT size = full_bound.calculate_size(); - int order[Point::AXIS_COUNT]; + int order[POINT::AXIS_COUNT]; - order[0] = size.min_axis(); - order[Point::AXIS_COUNT - 1] = size.max_axis(); + order[0] = size.min_axis_index(); + order[POINT::AXIS_COUNT - 1] = size.max_axis_index(); - static_assert(Point::AXIS_COUNT <= 3); - if (Point::AXIS_COUNT == 3) { + static_assert(POINT::AXIS_COUNT <= 3, "BVH POINT::AXIS_COUNT has unexpected size"); + if (POINT::AXIS_COUNT == 3) { order[1] = 3 - (order[0] + order[2]); } @@ -58,7 +58,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u // detect when split on longest axis failed int min_threshold = MAX_ITEMS / 4; - int min_group_size[Point::AXIS_COUNT]; + int min_group_size[POINT::AXIS_COUNT]; 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 @@ -68,7 +68,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u num_b = 0; // now calculate the best split - for (int axis = 1; axis < Point::AXIS_COUNT; axis++) { + for (int axis = 1; axis < POINT::AXIS_COUNT; axis++) { split_axis = order[axis]; int count = 0; @@ -86,7 +86,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u // best axis int best_axis = 0; int best_min = min_group_size[0]; - for (int axis = 1; axis < Point::AXIS_COUNT; axis++) { + for (int axis = 1; axis < POINT::AXIS_COUNT; axis++) { if (min_group_size[axis] > best_min) { best_min = min_group_size[axis]; best_axis = axis; diff --git a/core/math/bvh_structs.inc b/core/math/bvh_structs.inc index 1d1e0e6468..58c8f0479a 100644 --- a/core/math/bvh_structs.inc +++ b/core/math/bvh_structs.inc @@ -14,25 +14,38 @@ struct ItemRef { // 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; + // Before doing user defined pairing checks (especially in the find_leavers function), + // we may want to check that two items have compatible tree ids and tree masks, + // as if they are incompatible they should not pair / collide. + bool are_item_trees_compatible(const ItemExtra &p_other) const { + uint32_t other_type = 1 << p_other.tree_id; + if (tree_collision_mask & other_type) { + return true; + } + uint32_t our_type = 1 << tree_id; + if (p_other.tree_collision_mask & our_type) { + return true; + } + return false; + } + + // There can be multiple user defined trees + uint32_t tree_id; + + // Defines which trees this item should collision check against. + // 1 << tree_id, and normally items would collide against there own + // tree (but not always). + uint32_t tree_collision_mask; + uint32_t last_updated_tick; int32_t subindex; + T *userdata; + // 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 @@ -47,11 +60,23 @@ private: 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]; } + BVHABB_CLASS &get_aabb(uint32_t p_id) { + BVH_ASSERT(p_id < MAX_ITEMS); + return aabbs[p_id]; + } + const BVHABB_CLASS &get_aabb(uint32_t p_id) const { + BVH_ASSERT(p_id < MAX_ITEMS); + 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]; } + uint32_t &get_item_ref_id(uint32_t p_id) { + BVH_ASSERT(p_id < MAX_ITEMS); + return item_ref_ids[p_id]; + } + const uint32_t &get_item_ref_id(uint32_t p_id) const { + BVH_ASSERT(p_id < MAX_ITEMS); + return item_ref_ids[p_id]; + } bool is_dirty() const { return dirty; } void set_dirty(bool p) { dirty = p; } @@ -133,13 +158,13 @@ struct TNode { // instead of using linked list we maintain // item references (for quick lookup) -PooledList<ItemRef, true> _refs; -PooledList<ItemExtra, true> _extra; +PooledList<ItemRef, uint32_t, true> _refs; +PooledList<ItemExtra, uint32_t, true> _extra; PooledList<ItemPairs> _pairs; // these 2 are not in sync .. nodes != leaves! -PooledList<TNode, true> _nodes; -PooledList<TLeaf, true> _leaves; +PooledList<TNode, uint32_t, true> _nodes; +PooledList<TLeaf, uint32_t, 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. @@ -152,15 +177,11 @@ uint32_t _current_active_ref = 0; // 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. +// We can now have a user definable number of trees. +// This allows using e.g. a non-pairable and pairable tree, +// which can be more efficient for example, if we only need check non pairable against the pairable tree. +// It also may be more efficient in terms of separating static from dynamic objects, by reducing housekeeping. +// However this is a trade off, as there is a cost of traversing two trees. uint32_t _root_node_id[NUM_TREES]; // these values may need tweaking according to the project @@ -177,4 +198,14 @@ bool _auto_node_expansion = true; // 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; + +#ifdef BVH_ALLOW_AUTO_EXPANSION bool _auto_pairing_expansion = true; +#endif + +// when using an expanded bound, we must detect the condition where a new AABB +// is significantly smaller than the expanded bound, as this is a special case where we +// should override the optimization and create a new expanded bound. +// This threshold is derived from the _pairing_expansion, and should be recalculated +// if _pairing_expansion is changed. +real_t _aabb_shrinkage_threshold = 0.0; diff --git a/core/math/bvh_tree.h b/core/math/bvh_tree.h index 3169d31ec7..cdb2bb4413 100644 --- a/core/math/bvh_tree.h +++ b/core/math/bvh_tree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -48,12 +48,17 @@ #include "core/templates/pooled_list.h" #include <limits.h> -#define BVHABB_CLASS BVH_ABB<Bounds, Point> +#define BVHABB_CLASS BVH_ABB<BOUNDS, POINT> + +// not sure if this is better yet so making optional +#define BVH_EXPAND_LEAF_AABBS // never do these checks in release -#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED) +#ifdef DEV_ENABLED //#define BVH_VERBOSE //#define BVH_VERBOSE_TREE +//#define BVH_VERBOSE_PAIRING +//#define BVH_VERBOSE_MOVES //#define BVH_VERBOSE_FRAME //#define BVH_CHECKS @@ -148,7 +153,25 @@ public: } }; -template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class Bounds = AABB, class Point = Vector3> +template <class T> +class BVH_DummyPairTestFunction { +public: + static bool user_collision_check(T *p_a, T *p_b) { + // return false if no collision, decided by masks etc + return true; + } +}; + +template <class T> +class BVH_DummyCullTestFunction { +public: + static bool user_cull_check(T *p_a, T *p_b) { + // return false if no collision + return true; + } +}; + +template <class T, int NUM_TREES, int MAX_CHILDREN, int MAX_ITEMS, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, bool USE_PAIRS = false, class BOUNDS = AABB, class POINT = Vector3> class BVH_Tree { friend class BVH; @@ -165,6 +188,11 @@ public: // (as these ids are stored as negative numbers in the node) uint32_t dummy_leaf_id; _leaves.request(dummy_leaf_id); + + // In many cases you may want to change this default in the client code, + // or expose this value to the user. + // This default may make sense for a typically scaled 3d game, but maybe not for 2d on a pixel scale. + params_set_pairing_expansion(0.1); } private: @@ -189,7 +217,7 @@ private: BVH_ASSERT(!parent.is_leaf()); int child_num = parent.find_child(p_old_child_id); - BVH_ASSERT(child_num != BVHCommon::INVALID); + BVH_ASSERT(child_num != -1); parent.children[child_num] = p_new_child_id; TNode &new_child = _nodes[p_new_child_id]; @@ -201,7 +229,7 @@ private: BVH_ASSERT(!parent.is_leaf()); int child_num = parent.find_child(p_child_id); - BVH_ASSERT(child_num != BVHCommon::INVALID); + BVH_ASSERT(child_num != -1); parent.remove_child_internal(child_num); @@ -234,7 +262,7 @@ private: change_root_node(sibling_id, p_tree_id); // delete the old root node as no longer needed - _nodes.free(p_parent_id); + node_free_node_and_leaf(p_parent_id); } return; @@ -247,7 +275,19 @@ private: } // put the node on the free list to recycle - _nodes.free(p_parent_id); + node_free_node_and_leaf(p_parent_id); + } + + // A node can either be a node, or a node AND a leaf combo. + // Both must be deleted to prevent a leak. + void node_free_node_and_leaf(uint32_t p_node_id) { + TNode &node = _nodes[p_node_id]; + if (node.is_leaf()) { + int leaf_id = node.get_leaf_id(); + _leaves.free(leaf_id); + } + + _nodes.free(p_node_id); } void change_root_node(uint32_t p_new_root_id, uint32_t p_tree_id) { @@ -339,7 +379,7 @@ private: refit_upward(parent_id); // put the node on the free list to recycle - _nodes.free(owner_node_id); + node_free_node_and_leaf(owner_node_id); } // else if no parent, it is the root node. Do not delete diff --git a/core/math/color.cpp b/core/math/color.cpp index dc86cacf8f..4bdeafd2f2 100644 --- a/core/math/color.cpp +++ b/core/math/color.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,9 @@ #include "color_names.inc" #include "core/math/math_funcs.h" #include "core/string/print_string.h" -#include "core/templates/map.h" +#include "core/templates/rb_map.h" + +#include "thirdparty/misc/ok_color.h" uint32_t Color::to_argb32() const { uint32_t c = (uint8_t)Math::round(a * 255); @@ -107,6 +109,39 @@ uint64_t Color::to_rgba64() const { return c; } +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; +} + float Color::get_h() const { float min = MIN(r, g); min = MIN(min, b); @@ -128,9 +163,9 @@ float Color::get_h() const { h = 4 + (r - g) / delta; // between magenta & cyan } - h /= 6.0; + h /= 6.0f; if (h < 0) { - h += 1.0; + h += 1.0f; } return h; @@ -164,7 +199,7 @@ void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) { return; } - p_h *= 6.0; + p_h *= 6.0f; p_h = Math::fmod(p_h, 6); i = Math::floor(p_h); @@ -207,6 +242,20 @@ void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) { } } +void Color::set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha) { + ok_color::HSL hsl; + hsl.h = p_h; + hsl.s = p_s; + hsl.l = p_l; + ok_color new_ok_color; + ok_color::RGB rgb = new_ok_color.okhsl_to_srgb(hsl); + Color c = Color(rgb.r, rgb.g, rgb.b, p_alpha).clamp(); + r = c.r; + g = c.g; + b = c.b; + a = c.a; +} + 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); } @@ -220,49 +269,35 @@ Color Color::clamp(const Color &p_min, const Color &p_max) const { } void Color::invert() { - r = 1.0 - r; - g = 1.0 - g; - b = 1.0 - b; + r = 1.0f - r; + g = 1.0f - g; + b = 1.0f - b; } Color Color::hex(uint32_t p_hex) { - float a = (p_hex & 0xFF) / 255.0; + float a = (p_hex & 0xFF) / 255.0f; p_hex >>= 8; - float b = (p_hex & 0xFF) / 255.0; + float b = (p_hex & 0xFF) / 255.0f; p_hex >>= 8; - float g = (p_hex & 0xFF) / 255.0; + float g = (p_hex & 0xFF) / 255.0f; p_hex >>= 8; - float r = (p_hex & 0xFF) / 255.0; + float r = (p_hex & 0xFF) / 255.0f; return Color(r, g, b, a); } Color Color::hex64(uint64_t p_hex) { - float a = (p_hex & 0xFFFF) / 65535.0; + float a = (p_hex & 0xFFFF) / 65535.0f; p_hex >>= 16; - float b = (p_hex & 0xFFFF) / 65535.0; + float b = (p_hex & 0xFFFF) / 65535.0f; p_hex >>= 16; - float g = (p_hex & 0xFFFF) / 65535.0; + float g = (p_hex & 0xFFFF) / 65535.0f; p_hex >>= 16; - float r = (p_hex & 0xFFFF) / 65535.0; + float r = (p_hex & 0xFFFF) / 65535.0f; 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]; @@ -314,18 +349,18 @@ Color Color::html(const String &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; + r = _parse_col4(color, 0) / 15.0f; + g = _parse_col4(color, 1) / 15.0f; + b = _parse_col4(color, 2) / 15.0f; if (alpha) { - a = _parse_col4(color, 3) / 15.0; + a = _parse_col4(color, 3) / 15.0f; } } else { - r = _parse_col8(color, 0) / 255.0; - g = _parse_col8(color, 2) / 255.0; - b = _parse_col8(color, 4) / 255.0; + r = _parse_col8(color, 0) / 255.0f; + g = _parse_col8(color, 2) / 255.0f; + b = _parse_col8(color, 4) / 255.0f; if (alpha) { - a = _parse_col8(color, 6) / 255.0; + a = _parse_col8(color, 6) / 255.0f; } } ERR_FAIL_COND_V_MSG(r < 0, Color(), "Invalid color code: " + p_rgba + "."); @@ -428,43 +463,24 @@ Color Color::from_string(const String &p_string, const Color &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; +Color Color::from_hsv(float p_h, float p_s, float p_v, float p_alpha) { + Color c; + c.set_hsv(p_h, p_s, p_v, p_alpha); + return c; } -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_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.0f - 9.0f); -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; + float rd = r * m; + float gd = g * m; + float bd = b * m; + + return Color(rd, gd, bd, 1.0f); } Color::operator String() const { @@ -563,8 +579,53 @@ void Color::operator/=(float p_scalar) { Color Color::operator-() const { return Color( - 1.0 - r, - 1.0 - g, - 1.0 - b, - 1.0 - a); + 1.0f - r, + 1.0f - g, + 1.0f - b, + 1.0f - a); +} + +Color Color::from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha) { + Color c; + c.set_ok_hsl(p_h, p_s, p_l, p_alpha); + return c; +} + +float Color::get_ok_hsl_h() const { + ok_color::RGB rgb; + rgb.r = r; + rgb.g = g; + rgb.b = b; + ok_color new_ok_color; + ok_color::HSL ok_hsl = new_ok_color.srgb_to_okhsl(rgb); + if (Math::is_nan(ok_hsl.h)) { + return 0.0f; + } + return CLAMP(ok_hsl.h, 0.0f, 1.0f); +} + +float Color::get_ok_hsl_s() const { + ok_color::RGB rgb; + rgb.r = r; + rgb.g = g; + rgb.b = b; + ok_color new_ok_color; + ok_color::HSL ok_hsl = new_ok_color.srgb_to_okhsl(rgb); + if (Math::is_nan(ok_hsl.s)) { + return 0.0f; + } + return CLAMP(ok_hsl.s, 0.0f, 1.0f); +} + +float Color::get_ok_hsl_l() const { + ok_color::RGB rgb; + rgb.r = r; + rgb.g = g; + rgb.b = b; + ok_color new_ok_color; + ok_color::HSL ok_hsl = new_ok_color.srgb_to_okhsl(rgb); + if (Math::is_nan(ok_hsl.l)) { + return 0.0f; + } + return CLAMP(ok_hsl.l, 0.0f, 1.0f); } diff --git a/core/math/color.h b/core/math/color.h index a95dbf4f60..65036f74cc 100644 --- a/core/math/color.h +++ b/core/math/color.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,7 +34,7 @@ #include "core/math/math_funcs.h" #include "core/string/ustring.h" -struct Color { +struct _NO_DISCARD_ Color { union { struct { float r; @@ -51,10 +51,15 @@ struct Color { uint64_t to_rgba64() const; uint64_t to_argb64() const; uint64_t to_abgr64() const; + String to_html(bool p_alpha = true) 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); + float get_ok_hsl_h() const; + float get_ok_hsl_s() const; + float get_ok_hsl_l() const; + void set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0); _FORCE_INLINE_ float &operator[](int p_idx) { return components[p_idx]; @@ -93,6 +98,10 @@ struct Color { void invert(); Color inverted() const; + _FORCE_INLINE_ float get_luminance() const { + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + _FORCE_INLINE_ Color lerp(const Color &p_to, float p_weight) const { Color res = *this; @@ -133,13 +142,13 @@ struct Color { float cMax = MAX(cRed, MAX(cGreen, cBlue)); - float expp = MAX(-B - 1.0f, floor(Math::log(cMax) / Math_LN2)) + 1.0f + B; + float expp = MAX(-B - 1.0f, floor(Math::log(cMax) / (real_t)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) { + if (0.0f <= sMax && sMax < pow2to9) { exps = expp; } @@ -152,7 +161,7 @@ struct Color { _FORCE_INLINE_ Color blend(const Color &p_over) const { Color res; - float sa = 1.0 - p_over.a; + float sa = 1.0f - p_over.a; res.a = a * sa + p_over.a; if (res.a == 0) { return Color(0, 0, 0, 0); @@ -164,18 +173,18 @@ struct Color { return res; } - _FORCE_INLINE_ Color to_linear() const { + _FORCE_INLINE_ Color srgb_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), + r < 0.04045f ? r * (1.0 / 12.92) : Math::pow((r + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), + g < 0.04045f ? g * (1.0 / 12.92) : Math::pow((g + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), + b < 0.04045f ? b * (1.0 / 12.92) : Math::pow((b + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), a); } - _FORCE_INLINE_ Color to_srgb() const { + _FORCE_INLINE_ Color linear_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); + r < 0.0031308f ? 12.92f * r : (1.0f + 0.055f) * Math::pow(r, 1.0f / 2.4f) - 0.055f, + g < 0.0031308f ? 12.92f * g : (1.0f + 0.055f) * Math::pow(g, 1.0f / 2.4f) - 0.055f, + b < 0.0031308f ? 12.92f * b : (1.0f + 0.055f) * Math::pow(b, 1.0f / 2.4f) - 0.055f, a); } static Color hex(uint32_t p_hex); @@ -189,26 +198,29 @@ struct Color { 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_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0); + static Color from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0); 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_ void set_r8(int32_t r8) { r = (CLAMP(r8, 0, 255) / 255.0f); } _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_ void set_g8(int32_t g8) { g = (CLAMP(g8, 0, 255) / 255.0f); } _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_ void set_b8(int32_t b8) { b = (CLAMP(b8, 0, 255) / 255.0f); } _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_ void set_a8(int32_t a8) { a = (CLAMP(a8, 0, 255) / 255.0f); } _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_ void set_h(float p_h) { set_hsv(p_h, get_s(), get_v(), a); } + _FORCE_INLINE_ void set_s(float p_s) { set_hsv(get_h(), p_s, get_v(), a); } + _FORCE_INLINE_ void set_v(float p_v) { set_hsv(get_h(), get_s(), p_v, a); } + _FORCE_INLINE_ void set_ok_hsl_h(float p_h) { set_ok_hsl(p_h, get_ok_hsl_s(), get_ok_hsl_l(), a); } + _FORCE_INLINE_ void set_ok_hsl_s(float p_s) { set_ok_hsl(get_ok_hsl_h(), p_s, get_ok_hsl_l(), a); } + _FORCE_INLINE_ void set_ok_hsl_l(float p_l) { set_ok_hsl(get_ok_hsl_h(), get_ok_hsl_s(), p_l, a); } _FORCE_INLINE_ Color() {} @@ -230,7 +242,7 @@ struct Color { r = p_r; g = p_g; b = p_b; - a = 1.0; + a = 1.0f; } /** diff --git a/core/math/color_names.inc b/core/math/color_names.inc index 2020bdbfca..654fa83877 100644 --- a/core/math/color_names.inc +++ b/core/math/color_names.inc @@ -13,151 +13,151 @@ struct NamedColor { // 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) }, + { "ALICE_BLUE", Color::hex(0xF0F8FFFF) }, + { "ANTIQUE_WHITE", Color::hex(0xFAEBD7FF) }, + { "AQUA", Color::hex(0x00FFFFFF) }, + { "AQUAMARINE", Color::hex(0x7FFFD4FF) }, + { "AZURE", Color::hex(0xF0FFFFFF) }, + { "BEIGE", Color::hex(0xF5F5DCFF) }, + { "BISQUE", Color::hex(0xFFE4C4FF) }, + { "BLACK", Color::hex(0x000000FF) }, + { "BLANCHED_ALMOND", Color::hex(0xFFEBCDFF) }, + { "BLUE", Color::hex(0x0000FFFF) }, + { "BLUE_VIOLET", Color::hex(0x8A2BE2FF) }, + { "BROWN", Color::hex(0xA52A2AFF) }, + { "BURLYWOOD", Color::hex(0xDEB887FF) }, + { "CADET_BLUE", Color::hex(0x5F9EA0FF) }, + { "CHARTREUSE", Color::hex(0x7FFF00FF) }, + { "CHOCOLATE", Color::hex(0xD2691EFF) }, + { "CORAL", Color::hex(0xFF7F50FF) }, + { "CORNFLOWER_BLUE", Color::hex(0x6495EDFF) }, + { "CORNSILK", Color::hex(0xFFF8DCFF) }, + { "CRIMSON", Color::hex(0xDC143CFF) }, + { "CYAN", Color::hex(0x00FFFFFF) }, + { "DARK_BLUE", Color::hex(0x00008BFF) }, + { "DARK_CYAN", Color::hex(0x008B8BFF) }, + { "DARK_GOLDENROD", Color::hex(0xB8860BFF) }, + { "DARK_GRAY", Color::hex(0xA9A9A9FF) }, + { "DARK_GREEN", Color::hex(0x006400FF) }, + { "DARK_KHAKI", Color::hex(0xBDB76BFF) }, + { "DARK_MAGENTA", Color::hex(0x8B008BFF) }, + { "DARK_OLIVE_GREEN", Color::hex(0x556B2FFF) }, + { "DARK_ORANGE", Color::hex(0xFF8C00FF) }, + { "DARK_ORCHID", Color::hex(0x9932CCFF) }, + { "DARK_RED", Color::hex(0x8B0000FF) }, + { "DARK_SALMON", Color::hex(0xE9967AFF) }, + { "DARK_SEA_GREEN", Color::hex(0x8FBC8FFF) }, + { "DARK_SLATE_BLUE", Color::hex(0x483D8BFF) }, + { "DARK_SLATE_GRAY", Color::hex(0x2F4F4FFF) }, + { "DARK_TURQUOISE", Color::hex(0x00CED1FF) }, + { "DARK_VIOLET", Color::hex(0x9400D3FF) }, + { "DEEP_PINK", Color::hex(0xFF1493FF) }, + { "DEEP_SKY_BLUE", Color::hex(0x00BFFFFF) }, + { "DIM_GRAY", Color::hex(0x696969FF) }, + { "DODGER_BLUE", Color::hex(0x1E90FFFF) }, + { "FIREBRICK", Color::hex(0xB22222FF) }, + { "FLORAL_WHITE", Color::hex(0xFFFAF0FF) }, + { "FOREST_GREEN", Color::hex(0x228B22FF) }, + { "FUCHSIA", Color::hex(0xFF00FFFF) }, + { "GAINSBORO", Color::hex(0xDCDCDCFF) }, + { "GHOST_WHITE", Color::hex(0xF8F8FFFF) }, + { "GOLD", Color::hex(0xFFD700FF) }, + { "GOLDENROD", Color::hex(0xDAA520FF) }, + { "GRAY", Color::hex(0xBEBEBEFF) }, + { "GREEN", Color::hex(0x00FF00FF) }, + { "GREEN_YELLOW", Color::hex(0xADFF2FFF) }, + { "HONEYDEW", Color::hex(0xF0FFF0FF) }, + { "HOT_PINK", Color::hex(0xFF69B4FF) }, + { "INDIAN_RED", Color::hex(0xCD5C5CFF) }, + { "INDIGO", Color::hex(0x4B0082FF) }, + { "IVORY", Color::hex(0xFFFFF0FF) }, + { "KHAKI", Color::hex(0xF0E68CFF) }, + { "LAVENDER", Color::hex(0xE6E6FAFF) }, + { "LAVENDER_BLUSH", Color::hex(0xFFF0F5FF) }, + { "LAWN_GREEN", Color::hex(0x7CFC00FF) }, + { "LEMON_CHIFFON", Color::hex(0xFFFACDFF) }, + { "LIGHT_BLUE", Color::hex(0xADD8E6FF) }, + { "LIGHT_CORAL", Color::hex(0xF08080FF) }, + { "LIGHT_CYAN", Color::hex(0xE0FFFFFF) }, + { "LIGHT_GOLDENROD", Color::hex(0xFAFAD2FF) }, + { "LIGHT_GRAY", Color::hex(0xD3D3D3FF) }, + { "LIGHT_GREEN", Color::hex(0x90EE90FF) }, + { "LIGHT_PINK", Color::hex(0xFFB6C1FF) }, + { "LIGHT_SALMON", Color::hex(0xFFA07AFF) }, + { "LIGHT_SEA_GREEN", Color::hex(0x20B2AAFF) }, + { "LIGHT_SKY_BLUE", Color::hex(0x87CEFAFF) }, + { "LIGHT_SLATE_GRAY", Color::hex(0x778899FF) }, + { "LIGHT_STEEL_BLUE", Color::hex(0xB0C4DEFF) }, + { "LIGHT_YELLOW", Color::hex(0xFFFFE0FF) }, + { "LIME", Color::hex(0x00FF00FF) }, + { "LIME_GREEN", Color::hex(0x32CD32FF) }, + { "LINEN", Color::hex(0xFAF0E6FF) }, + { "MAGENTA", Color::hex(0xFF00FFFF) }, + { "MAROON", Color::hex(0xB03060FF) }, + { "MEDIUM_AQUAMARINE", Color::hex(0x66CDAAFF) }, + { "MEDIUM_BLUE", Color::hex(0x0000CDFF) }, + { "MEDIUM_ORCHID", Color::hex(0xBA55D3FF) }, + { "MEDIUM_PURPLE", Color::hex(0x9370DBFF) }, + { "MEDIUM_SEA_GREEN", Color::hex(0x3CB371FF) }, + { "MEDIUM_SLATE_BLUE", Color::hex(0x7B68EEFF) }, + { "MEDIUM_SPRING_GREEN", Color::hex(0x00FA9AFF) }, + { "MEDIUM_TURQUOISE", Color::hex(0x48D1CCFF) }, + { "MEDIUM_VIOLET_RED", Color::hex(0xC71585FF) }, + { "MIDNIGHT_BLUE", Color::hex(0x191970FF) }, + { "MINT_CREAM", Color::hex(0xF5FFFAFF) }, + { "MISTY_ROSE", Color::hex(0xFFE4E1FF) }, + { "MOCCASIN", Color::hex(0xFFE4B5FF) }, + { "NAVAJO_WHITE", Color::hex(0xFFDEADFF) }, + { "NAVY_BLUE", Color::hex(0x000080FF) }, + { "OLD_LACE", Color::hex(0xFDF5E6FF) }, + { "OLIVE", Color::hex(0x808000FF) }, + { "OLIVE_DRAB", Color::hex(0x6B8E23FF) }, + { "ORANGE", Color::hex(0xFFA500FF) }, + { "ORANGE_RED", Color::hex(0xFF4500FF) }, + { "ORCHID", Color::hex(0xDA70D6FF) }, + { "PALE_GOLDENROD", Color::hex(0xEEE8AAFF) }, + { "PALE_GREEN", Color::hex(0x98FB98FF) }, + { "PALE_TURQUOISE", Color::hex(0xAFEEEEFF) }, + { "PALE_VIOLET_RED", Color::hex(0xDB7093FF) }, + { "PAPAYA_WHIP", Color::hex(0xFFEFD5FF) }, + { "PEACH_PUFF", Color::hex(0xFFDAB9FF) }, + { "PERU", Color::hex(0xCD853FFF) }, + { "PINK", Color::hex(0xFFC0CBFF) }, + { "PLUM", Color::hex(0xDDA0DDFF) }, + { "POWDER_BLUE", Color::hex(0xB0E0E6FF) }, + { "PURPLE", Color::hex(0xA020F0FF) }, + { "REBECCA_PURPLE", Color::hex(0x663399FF) }, + { "RED", Color::hex(0xFF0000FF) }, + { "ROSY_BROWN", Color::hex(0xBC8F8FFF) }, + { "ROYAL_BLUE", Color::hex(0x4169E1FF) }, + { "SADDLE_BROWN", Color::hex(0x8B4513FF) }, + { "SALMON", Color::hex(0xFA8072FF) }, + { "SANDY_BROWN", Color::hex(0xF4A460FF) }, + { "SEA_GREEN", Color::hex(0x2E8B57FF) }, + { "SEASHELL", Color::hex(0xFFF5EEFF) }, + { "SIENNA", Color::hex(0xA0522DFF) }, + { "SILVER", Color::hex(0xC0C0C0FF) }, + { "SKY_BLUE", Color::hex(0x87CEEBFF) }, + { "SLATE_BLUE", Color::hex(0x6A5ACDFF) }, + { "SLATE_GRAY", Color::hex(0x708090FF) }, + { "SNOW", Color::hex(0xFFFAFAFF) }, + { "SPRING_GREEN", Color::hex(0x00FF7FFF) }, + { "STEEL_BLUE", Color::hex(0x4682B4FF) }, + { "TAN", Color::hex(0xD2B48CFF) }, + { "TEAL", Color::hex(0x008080FF) }, + { "THISTLE", Color::hex(0xD8BFD8FF) }, + { "TOMATO", Color::hex(0xFF6347FF) }, + { "TRANSPARENT", Color::hex(0xFFFFFF00) }, + { "TURQUOISE", Color::hex(0x40E0D0FF) }, + { "VIOLET", Color::hex(0xEE82EEFF) }, + { "WEB_GRAY", Color::hex(0x808080FF) }, + { "WEB_GREEN", Color::hex(0x008000FF) }, + { "WEB_MAROON", Color::hex(0x800000FF) }, + { "WEB_PURPLE", Color::hex(0x800080FF) }, + { "WHEAT", Color::hex(0xF5DEB3FF) }, + { "WHITE", Color::hex(0xFFFFFFFF) }, + { "WHITE_SMOKE", Color::hex(0xF5F5F5FF) }, + { "YELLOW", Color::hex(0xFFFF00FF) }, + { "YELLOW_GREEN", Color::hex(0x9ACD32FF) }, { nullptr, Color() }, }; diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp index 684814b1ae..996f4f4d67 100644 --- a/core/math/convex_hull.cpp +++ b/core/math/convex_hull.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -265,8 +265,7 @@ public: } int32_t get_sign() const { - return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : - 0; + return ((int64_t)high < 0) ? -1 : ((high || low) ? 1 : 0); } bool operator<(const Int128 &b) const { @@ -510,7 +509,7 @@ public: Face() { } - void init(Vertex *p_a, Vertex *p_b, Vertex *p_c) { + void init(Vertex *p_a, const Vertex *p_b, const Vertex *p_c) { nearby_vertex = p_a; origin = p_a->point; dir0 = *p_b - *p_a; @@ -607,15 +606,15 @@ private: 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; + Vector3::Axis min_axis = Vector3::Axis::AXIS_X; + Vector3::Axis med_axis = Vector3::Axis::AXIS_X; + Vector3::Axis max_axis = Vector3::Axis::AXIS_X; 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); + void find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, const Vertex *p_stop0, const Vertex *p_stop1); Edge *new_edge_pair(Vertex *p_from, Vertex *p_to); @@ -667,7 +666,7 @@ public: face_pool.reset(true); } - Vertex *vertex_list; + Vertex *vertex_list = nullptr; void compute(const Vector3 *p_coords, int32_t p_count); @@ -735,8 +734,6 @@ int32_t ConvexHullInternal::Rational64::compare(const Rational64 &b) const { 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; @@ -757,10 +754,9 @@ int32_t ConvexHullInternal::Rational64::compare(const Rational64 &b) const { : "=&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; + // 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) + return result ? result ^ sign : 0; #else @@ -793,8 +789,7 @@ int32_t ConvexHullInternal::Rational128::compare(const Rational128 &b) const { 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; + return (a > b) ? 1 : ((a < b) ? -1 : 0); } if (b > 0) { if (sign <= 0) { @@ -1194,7 +1189,7 @@ ConvexHullInternal::Edge *ConvexHullInternal::find_max_angle(bool p_ccw, const V 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) { +void ConvexHullInternal::find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, const Vertex *p_stop0, const Vertex *p_stop1) { Edge *start0 = p_e0; Edge *start1 = p_e1; Point32 et0 = start0 ? start0->target->point : p_c0->point; @@ -1446,8 +1441,7 @@ void ConvexHullInternal::merge(IntermediateHull &p_h0, IntermediateHull &p_h1) { c1->edges = e; return; } else { - int32_t cmp = !min0 ? 1 : !min1 ? -1 : - min_cot0.compare(min_cot1); + int32_t cmp = !min0 ? 1 : (!min1 ? -1 : min_cot0.compare(min_cot1)); #ifdef DEBUG_CONVEX_HULL printf(" -> Result %d\n", cmp); #endif @@ -1591,12 +1585,12 @@ void ConvexHullInternal::compute(const Vector3 *p_coords, int32_t p_count) { } Vector3 s = aabb.size; - max_axis = s.max_axis(); - min_axis = s.min_axis(); + max_axis = s.max_axis_index(); + min_axis = s.min_axis_index(); if (min_axis == max_axis) { - min_axis = (max_axis + 1) % 3; + min_axis = Vector3::Axis((max_axis + 1) % 3); } - med_axis = 3 - max_axis - min_axis; + med_axis = Vector3::Axis(3 - max_axis - min_axis); s /= real_t(10216); if (((med_axis + 1) % 3) != max_axis) { @@ -1694,7 +1688,7 @@ real_t ConvexHullInternal::shrink(real_t p_amount, real_t p_clamp_amount) { while (stack.size() > 0) { Vertex *v = stack[stack.size() - 1]; - stack.remove(stack.size() - 1); + stack.remove_at(stack.size() - 1); Edge *e = v->edges; if (e) { do { @@ -2135,7 +2129,7 @@ bool ConvexHullInternal::shift_face(Face *p_face, real_t p_amount, LocalVector<V printf("Needed %d iterations to remove part\n", n); #endif - p_stack.resize(0); + p_stack.clear(); p_face->origin = shifted_origin; return true; @@ -2173,9 +2167,9 @@ real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, rea return shift; } - vertices.resize(0); - edges.resize(0); - faces.resize(0); + vertices.clear(); + edges.clear(); + faces.clear(); LocalVector<ConvexHullInternal::Vertex *> old_vertices; get_vertex_copy(hull.vertex_list, old_vertices); diff --git a/core/math/convex_hull.h b/core/math/convex_hull.h index 806c6cc3fb..bd86fe0eba 100644 --- a/core/math/convex_hull.h +++ b/core/math/convex_hull.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/delaunay_2d.h b/core/math/delaunay_2d.h index 2f80cb5634..c39997d6a9 100644 --- a/core/math/delaunay_2d.h +++ b/core/math/delaunay_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,6 +32,7 @@ #define DELAUNAY_2D_H #include "core/math/rect2.h" +#include "core/templates/vector.h" class Delaunay2D { public: @@ -123,7 +124,7 @@ public: for (int j = 0; j < triangles.size(); j++) { if (triangles[j].bad) { - triangles.remove(j); + triangles.remove_at(j); j--; } } @@ -154,7 +155,7 @@ public: } } if (invalid) { - triangles.remove(i); + triangles.remove_at(i); i--; } } diff --git a/core/math/delaunay_3d.h b/core/math/delaunay_3d.h index 81adf4d19a..898c3c2d91 100644 --- a/core/math/delaunay_3d.h +++ b/core/math/delaunay_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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/io/file_access.h" #include "core/math/aabb.h" -#include "core/math/camera_matrix.h" +#include "core/math/projection.h" #include "core/math/vector3.h" #include "core/string/print_string.h" #include "core/templates/local_vector.h" @@ -101,7 +101,7 @@ class Delaunay3D { _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); + return hash_fmix32(hash_djb2_one_32(p_triangle.triangle[2], h)); } }; @@ -184,7 +184,7 @@ class Delaunay3D { return true; } - CameraMatrix cm; + Projection cm; cm.matrix[0][0] = p_points[p_simplex.points[0]].x; cm.matrix[0][1] = p_points[p_simplex.points[1]].x; @@ -323,7 +323,6 @@ public: E = N; } - uint32_t good_triangles = 0; for (uint32_t j = 0; j < triangles.size(); j++) { if (triangles[j].bad) { continue; @@ -360,11 +359,8 @@ public: } } } - - 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(); } diff --git a/core/math/disjoint_set.h b/core/math/disjoint_set.h index b155412f64..d07c08e45e 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,15 +31,11 @@ #ifndef DISJOINT_SET_H #define DISJOINT_SET_H -#include "core/templates/map.h" +#include "core/templates/rb_map.h" #include "core/templates/vector.h" -/** - @author Marios Staikopoulos <marios@staik.net> -*/ - /* This DisjointSet class uses Find with path compression and Union by rank */ -template <typename T, class C = Comparator<T>, class AL = DefaultAllocator> +template <typename T, class H = HashMapHasherDefault, class C = HashMapComparatorDefault<T>, class AL = DefaultAllocator> class DisjointSet { struct Element { T object; @@ -47,7 +43,7 @@ class DisjointSet { int rank = 0; }; - typedef Map<T, Element *, C, AL> MapT; + typedef HashMap<T, Element *, H, C> MapT; MapT elements; @@ -69,15 +65,15 @@ public: /* FUNCTIONS */ -template <typename T, class C, class AL> -DisjointSet<T, C, AL>::~DisjointSet() { - for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { - memdelete_allocator<Element, AL>(itr->value()); +template <typename T, class H, class C, class AL> +DisjointSet<T, H, C, AL>::~DisjointSet() { + for (KeyValue<T, Element *> &E : elements) { + memdelete_allocator<Element, AL>(E.value); } } -template <typename T, class C, class AL> -typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::get_parent(Element *element) { +template <typename T, class H, class C, class AL> +typename DisjointSet<T, H, C, AL>::Element *DisjointSet<T, H, C, AL>::get_parent(Element *element) { if (element->parent != element) { element->parent = get_parent(element->parent); } @@ -85,11 +81,11 @@ typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::get_parent(Eleme return element->parent; } -template <typename T, class C, class AL> -typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::insert_or_get(T object) { - typename MapT::Element *itr = elements.find(object); +template <typename T, class H, class C, class AL> +typename DisjointSet<T, H, C, AL>::Element *DisjointSet<T, H, C, AL>::insert_or_get(T object) { + typename MapT::Iterator itr = elements.find(object); if (itr != nullptr) { - return itr->value(); + return itr->value; } Element *new_element = memnew_allocator(Element, AL); @@ -100,8 +96,8 @@ typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::insert_or_get(T return new_element; } -template <typename T, class C, class AL> -void DisjointSet<T, C, AL>::create_union(T a, T b) { +template <typename T, class H, class C, class AL> +void DisjointSet<T, H, C, AL>::create_union(T a, T b) { Element *x = insert_or_get(a); Element *y = insert_or_get(b); @@ -125,28 +121,28 @@ void DisjointSet<T, C, AL>::create_union(T a, T b) { } } -template <typename T, class C, class AL> -void DisjointSet<T, C, AL>::get_representatives(Vector<T> &out_representatives) { - for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { - Element *element = itr->value(); +template <typename T, class H, class C, class AL> +void DisjointSet<T, H, C, AL>::get_representatives(Vector<T> &out_representatives) { + for (KeyValue<T, Element *> &E : elements) { + Element *element = E.value; if (element->parent == element) { out_representatives.push_back(element->object); } } } -template <typename T, class C, class AL> -void DisjointSet<T, C, AL>::get_members(Vector<T> &out_members, T representative) { - typename MapT::Element *rep_itr = elements.find(representative); +template <typename T, class H, class C, class AL> +void DisjointSet<T, H, C, AL>::get_members(Vector<T> &out_members, T representative) { + typename MapT::Iterator rep_itr = elements.find(representative); ERR_FAIL_COND(rep_itr == nullptr); - Element *rep_element = rep_itr->value(); + Element *rep_element = rep_itr->value; ERR_FAIL_COND(rep_element->parent != rep_element); - for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { - Element *parent = get_parent(itr->value()); + for (KeyValue<T, Element *> &E : elements) { + Element *parent = get_parent(E.value); if (parent == rep_element) { - out_members.push_back(itr->key()); + out_members.push_back(E.key); } } } diff --git a/core/math/dynamic_bvh.cpp b/core/math/dynamic_bvh.cpp index f3fb473981..7aeb2aaaac 100644 --- a/core/math/dynamic_bvh.cpp +++ b/core/math/dynamic_bvh.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/dynamic_bvh.h b/core/math/dynamic_bvh.h index 0b6286cd9d..50ec2c2b30 100644 --- a/core/math/dynamic_bvh.h +++ b/core/math/dynamic_bvh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 DYNAMICBVH_H -#define DYNAMICBVH_H +#ifndef DYNAMIC_BVH_H +#define DYNAMIC_BVH_H #include "core/math/aabb.h" #include "core/templates/list.h" @@ -474,4 +474,4 @@ void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResu } while (depth > 0); } -#endif // DYNAMICBVH_H +#endif // DYNAMIC_BVH_H diff --git a/core/math/expression.cpp b/core/math/expression.cpp index 05f2c8dac9..e230b69dc9 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -37,10 +37,6 @@ #include "core/os/os.h" #include "core/variant/variant_parser.h" -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++]) @@ -88,7 +84,7 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_INPUT; int index = 0; do { - if (!_is_number(expression[str_ofs])) { + if (!is_digit(expression[str_ofs])) { _set_error("Expected number after '$'"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; @@ -97,7 +93,7 @@ Error Expression::_get_token(Token &r_token) { index += expression[str_ofs] - '0'; str_ofs++; - } while (_is_number(expression[str_ofs])); + } while (is_digit(expression[str_ofs])); r_token.value = index; return OK; @@ -159,7 +155,12 @@ Error Expression::_get_token(Token &r_token) { return OK; } case '*': { - r_token.type = TK_OP_MUL; + if (expression[str_ofs] == '*') { + r_token.type = TK_OP_POW; + str_ofs++; + } else { + r_token.type = TK_OP_MUL; + } return OK; } case '%': { @@ -197,6 +198,7 @@ Error Expression::_get_token(Token &r_token) { case '\'': case '"': { String str; + char32_t prev = 0; while (true) { char32_t ch = GET_CHAR(); @@ -234,9 +236,11 @@ Error Expression::_get_token(Token &r_token) { case 'r': res = 13; break; + case 'U': case 'u': { - // hex number - for (int j = 0; j < 4; j++) { + // Hexadecimal sequence. + int hex_len = (next == 'U') ? 6 : 4; + for (int j = 0; j < hex_len; j++) { char32_t c = GET_CHAR(); if (c == 0) { @@ -244,13 +248,13 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_ERROR; return ERR_PARSE_ERROR; } - if (!(_is_number(c) || (c >= 'a' && c <= 'f') || (c >= 'A' && c <= 'F'))) { + if (!is_hex_digit(c)) { _set_error("Malformed hex constant in string"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; } char32_t v; - if (_is_number(c)) { + if (is_digit(c)) { v = c - '0'; } else if (c >= 'a' && c <= 'f') { v = c - 'a'; @@ -273,12 +277,46 @@ Error Expression::_get_token(Token &r_token) { } break; } + // Parse UTF-16 pair. + if ((res & 0xfffffc00) == 0xd800) { + if (prev == 0) { + prev = res; + continue; + } else { + _set_error("Invalid UTF-16 sequence in string, unpaired lead surrogate"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + } else if ((res & 0xfffffc00) == 0xdc00) { + if (prev == 0) { + _set_error("Invalid UTF-16 sequence in string, unpaired trail surrogate"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } else { + res = (prev << 10UL) + res - ((0xd800 << 10UL) + 0xdc00 - 0x10000); + prev = 0; + } + } + if (prev != 0) { + _set_error("Invalid UTF-16 sequence in string, unpaired lead surrogate"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } str += res; - } else { + if (prev != 0) { + _set_error("Invalid UTF-16 sequence in string, unpaired lead surrogate"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } str += ch; } } + if (prev != 0) { + _set_error("Invalid UTF-16 sequence in string, unpaired lead surrogate"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } r_token.type = TK_CONSTANT; r_token.value = str; @@ -291,39 +329,67 @@ Error Expression::_get_token(Token &r_token) { } char32_t next_char = (str_ofs >= expression.length()) ? 0 : expression[str_ofs]; - if (_is_number(cchar) || (cchar == '.' && _is_number(next_char))) { + if (is_digit(cchar) || (cchar == '.' && is_digit(next_char))) { //a number String num; #define READING_SIGN 0 #define READING_INT 1 -#define READING_DEC 2 -#define READING_EXP 3 -#define READING_DONE 4 +#define READING_HEX 2 +#define READING_BIN 3 +#define READING_DEC 4 +#define READING_EXP 5 +#define READING_DONE 6 int reading = READING_INT; char32_t c = cchar; bool exp_sign = false; bool exp_beg = false; + bool bin_beg = false; + bool hex_beg = false; bool is_float = false; + bool is_first_char = true; while (true) { switch (reading) { case READING_INT: { - if (_is_number(c)) { - //pass + if (is_digit(c)) { + if (is_first_char && c == '0') { + if (next_char == 'b') { + reading = READING_BIN; + } else if (next_char == 'x') { + reading = READING_HEX; + } + } } else if (c == '.') { reading = READING_DEC; is_float = true; } else if (c == 'e') { reading = READING_EXP; + is_float = true; } else { reading = READING_DONE; } } break; + case READING_BIN: { + if (bin_beg && !is_binary_digit(c)) { + reading = READING_DONE; + } else if (c == 'b') { + bin_beg = true; + } + + } break; + case READING_HEX: { + if (hex_beg && !is_hex_digit(c)) { + reading = READING_DONE; + } else if (c == 'x') { + hex_beg = true; + } + + } break; case READING_DEC: { - if (_is_number(c)) { + if (is_digit(c)) { } else if (c == 'e') { reading = READING_EXP; @@ -333,13 +399,10 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_EXP: { - if (_is_number(c)) { + if (is_digit(c)) { exp_beg = true; } else if ((c == '-' || c == '+') && !exp_sign && !exp_beg) { - if (c == '-') { - is_float = true; - } exp_sign = true; } else { @@ -353,6 +416,7 @@ Error Expression::_get_token(Token &r_token) { } num += String::chr(c); c = GET_CHAR(); + is_first_char = false; } str_ofs--; @@ -361,16 +425,20 @@ Error Expression::_get_token(Token &r_token) { if (is_float) { r_token.value = num.to_float(); + } else if (bin_beg) { + r_token.value = num.bin_to_int(); + } else if (hex_beg) { + r_token.value = num.hex_to_int(); } else { r_token.value = num.to_int(); } return OK; - } else if ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_') { + } else if (is_ascii_char(cchar) || is_underscore(cchar)) { String id; bool first = true; - while ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_' || (!first && _is_number(cchar))) { + while (is_ascii_char(cchar) || is_underscore(cchar) || (!first && is_digit(cchar))) { id += String::chr(cchar); cchar = GET_CHAR(); first = false; @@ -410,6 +478,14 @@ 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; + } + } + if (Variant::has_utility_function(id)) { r_token.type = TK_BUILTIN_FUNC; r_token.value = id; @@ -471,6 +547,7 @@ const char *Expression::token_name[TK_MAX] = { "OP MUL", "OP DIV", "OP MOD", + "OP POW", "OP SHIFT LEFT", "OP SHIFT RIGHT", "OP BIT AND", @@ -814,7 +891,7 @@ Expression::ENode *Expression::_parse_expression() { case TK_PERIOD: { //named indexing or function call _get_token(tk); - if (tk.type != TK_IDENTIFIER) { + if (tk.type != TK_IDENTIFIER && tk.type != TK_BUILTIN_FUNC) { _set_error("Expected identifier after '.'"); return nullptr; } @@ -942,6 +1019,9 @@ Expression::ENode *Expression::_parse_expression() { case TK_OP_MOD: op = Variant::OP_MODULE; break; + case TK_OP_POW: + op = Variant::OP_POWER; + break; case TK_OP_SHIFT_LEFT: op = Variant::OP_SHIFT_LEFT; break; @@ -995,35 +1075,38 @@ Expression::ENode *Expression::_parse_expression() { bool unary = false; switch (expression[i].op) { - case Variant::OP_BIT_NEGATE: + case Variant::OP_POWER: priority = 0; + break; + case Variant::OP_BIT_NEGATE: + priority = 1; unary = true; break; case Variant::OP_NEGATE: - priority = 1; + priority = 2; unary = true; break; case Variant::OP_MULTIPLY: case Variant::OP_DIVIDE: case Variant::OP_MODULE: - priority = 2; + priority = 3; break; case Variant::OP_ADD: case Variant::OP_SUBTRACT: - priority = 3; + priority = 4; break; case Variant::OP_SHIFT_LEFT: case Variant::OP_SHIFT_RIGHT: - priority = 4; + priority = 5; break; case Variant::OP_BIT_AND: - priority = 5; + priority = 6; break; case Variant::OP_BIT_XOR: - priority = 6; + priority = 7; break; case Variant::OP_BIT_OR: - priority = 7; + priority = 8; break; case Variant::OP_LESS: case Variant::OP_LESS_EQUAL: @@ -1031,20 +1114,20 @@ Expression::ENode *Expression::_parse_expression() { case Variant::OP_GREATER_EQUAL: case Variant::OP_EQUAL: case Variant::OP_NOT_EQUAL: - priority = 8; + priority = 9; break; case Variant::OP_IN: - priority = 10; + priority = 11; break; case Variant::OP_NOT: - priority = 11; + priority = 12; unary = true; break; case Variant::OP_AND: - priority = 12; + priority = 13; break; case Variant::OP_OR: - priority = 13; + priority = 14; break; default: { _set_error("Parser bug, invalid operator in expression: " + itos(expression[i].op)); @@ -1087,7 +1170,7 @@ Expression::ENode *Expression::_parse_expression() { op->nodes[1] = nullptr; expression.write[i].is_op = false; expression.write[i].node = op; - expression.remove(i + 1); + expression.remove_at(i + 1); } } else { @@ -1119,8 +1202,8 @@ Expression::ENode *Expression::_parse_expression() { //replace all 3 nodes by this operator and make it an expression expression.write[next_op - 1].node = op; - expression.remove(next_op); - expression.remove(next_op); + expression.remove_at(next_op); + expression.remove_at(next_op); } } @@ -1157,12 +1240,12 @@ bool Expression::_compile_expression() { return false; } -bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str) { +bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, bool p_const_calls_only, 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); + r_error_str = vformat(RTR("Invalid input %d (not passed) in expression"), in->index); return true; } r_ret = p_inputs[in->index]; @@ -1183,7 +1266,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, op->nodes[0], a, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1191,7 +1274,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: Variant b; if (op->nodes[1]) { - ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str); + ret = _execute(p_inputs, p_instance, op->nodes[1], b, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1209,14 +1292,14 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, index->base, base, p_const_calls_only, r_error_str); if (ret) { return true; } Variant idx; - ret = _execute(p_inputs, p_instance, index->index, idx, r_error_str); + ret = _execute(p_inputs, p_instance, index->index, idx, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1233,7 +1316,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, index->base, base, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1253,7 +1336,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, array->array[i], value, p_const_calls_only, r_error_str); if (ret) { return true; @@ -1270,14 +1353,14 @@ 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); + bool ret = _execute(p_inputs, p_instance, dictionary->dict[i + 0], key, p_const_calls_only, r_error_str); if (ret) { return true; } Variant value; - ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, r_error_str); + ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1297,7 +1380,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, constructor->arguments[i], value, p_const_calls_only, r_error_str); if (ret) { return true; @@ -1325,7 +1408,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, bifunc->arguments[i], value, p_const_calls_only, r_error_str); if (ret) { return true; } @@ -1346,7 +1429,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: 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); + bool ret = _execute(p_inputs, p_instance, call->base, base, p_const_calls_only, r_error_str); if (ret) { return true; @@ -1359,7 +1442,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: for (int i = 0; i < call->arguments.size(); i++) { Variant value; - ret = _execute(p_inputs, p_instance, call->arguments[i], value, r_error_str); + ret = _execute(p_inputs, p_instance, call->arguments[i], value, p_const_calls_only, r_error_str); if (ret) { return true; @@ -1369,7 +1452,11 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } Callable::CallError ce; - base.call(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce); + if (p_const_calls_only) { + base.call_const(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce); + } else { + base.callp(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce); + } if (ce.error != Callable::CallError::CALL_OK) { r_error_str = vformat(RTR("On call to '%s':"), String(call->method)); @@ -1408,13 +1495,13 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu return OK; } -Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) { +Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error, bool p_const_calls_only) { ERR_FAIL_COND_V_MSG(error_set, Variant(), "There was previously a parse error: " + error_str + "."); execution_error = false; Variant output; String error_txt; - bool err = _execute(p_inputs, p_base, root, output, error_txt); + bool err = _execute(p_inputs, p_base, root, output, p_const_calls_only, error_txt); if (err) { execution_error = true; error_str = error_txt; @@ -1434,7 +1521,7 @@ 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("execute", "inputs", "base_instance", "show_error", "const_calls_only"), &Expression::execute, DEFVAL(Array()), DEFVAL(Variant()), DEFVAL(true), DEFVAL(false)); ClassDB::bind_method(D_METHOD("has_execute_failed"), &Expression::has_execute_failed); ClassDB::bind_method(D_METHOD("get_error_text"), &Expression::get_error_text); } diff --git a/core/math/expression.h b/core/math/expression.h index aecf662d0a..2d58915996 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -85,6 +85,7 @@ private: TK_OP_MUL, TK_OP_DIV, TK_OP_MOD, + TK_OP_POW, TK_OP_SHIFT_LEFT, TK_OP_SHIFT_RIGHT, TK_OP_BIT_AND, @@ -147,7 +148,7 @@ private: bool is_op = false; union { Variant::Operator op; - ENode *node; + ENode *node = nullptr; }; }; @@ -256,14 +257,14 @@ private: Vector<String> input_names; bool execution_error = false; - bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str); + bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, bool p_const_calls_only, String &r_error_str); protected: static void _bind_methods(); public: Error parse(const String &p_expression, const Vector<String> &p_input_names = Vector<String>()); - Variant execute(Array p_inputs = Array(), Object *p_base = nullptr, bool p_show_error = true); + Variant execute(Array p_inputs = Array(), Object *p_base = nullptr, bool p_show_error = true, bool p_const_calls_only = false); bool has_execute_failed() const; String get_error_text() const; diff --git a/core/math/face3.cpp b/core/math/face3.cpp index 31a853e1a9..fb92f6b0df 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -42,7 +42,7 @@ 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 + if (p_plane.has_point(vertex[i], (real_t)CMP_EPSILON)) { // point is in plane ERR_FAIL_COND_V(above_count >= 4, 0); above[above_count++] = vertex[i]; @@ -117,7 +117,7 @@ bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vect bool Face3::is_degenerate() const { Vector3 normal = vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]); - return (normal.length_squared() < CMP_EPSILON2); + return (normal.length_squared() < (real_t)CMP_EPSILON2); } Face3::Side Face3::get_side_of(const Face3 &p_face, ClockDirection p_clock_dir) const { @@ -157,7 +157,7 @@ Vector3 Face3::get_random_point_inside() const { SWAP(a, b); } - return vertex[0] * a + vertex[1] * (b - a) + vertex[2] * (1.0 - b); + return vertex[0] * a + vertex[1] * (b - a) + vertex[2] * (1.0f - b); } Plane Face3::get_plane(ClockDirection p_dir) const { @@ -165,11 +165,11 @@ Plane Face3::get_plane(ClockDirection p_dir) const { } Vector3 Face3::get_median_point() const { - return (vertex[0] + vertex[1] + vertex[2]) / 3.0; + return (vertex[0] + vertex[1] + vertex[2]) / 3.0f; } real_t Face3::get_area() const { - return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length() * 0.5; + return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length() * 0.5f; } ClockDirection Face3::get_clock_dir() const { @@ -208,7 +208,7 @@ bool Face3::intersects_aabb(const AABB &p_aabb) const { /** TEST ALL EDGES **/ - Vector3 edge_norms[3] = { + const Vector3 edge_norms[3] = { vertex[0] - vertex[1], vertex[1] - vertex[2], vertex[2] - vertex[0], @@ -223,7 +223,7 @@ 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.0001f) { continue; // coplanar } axis.normalize(); @@ -260,8 +260,8 @@ void Face3::project_range(const Vector3 &p_normal, const Transform3D &p_transfor } 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 + constexpr double face_support_threshold = 0.98; + constexpr double edge_support_threshold = 0.05; if (p_max <= 0) { return; @@ -270,7 +270,7 @@ void Face3::get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 n = p_transform.basis.xform_inv(p_normal); /** TEST FACE AS SUPPORT **/ - if (get_plane().normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (get_plane().normal.dot(n) > face_support_threshold) { *p_count = MIN(3, p_max); for (int i = 0; i < *p_count; i++) { @@ -304,7 +304,7 @@ void Face3::get_support(const Vector3 &p_normal, const Transform3D &p_transform, // 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) { + if (dot < edge_support_threshold) { *p_count = MIN(2, p_max); for (int j = 0; j < *p_count; j++) { diff --git a/core/math/face3.h b/core/math/face3.h index 9e9026e54e..23260336fa 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,8 +36,7 @@ #include "core/math/transform_3d.h" #include "core/math/vector3.h" -class Face3 { -public: +struct _NO_DISCARD_ Face3 { enum Side { SIDE_OVER, SIDE_UNDER, @@ -48,14 +47,11 @@ public: Vector3 vertex[3]; /** - * - * @param p_plane plane used to split the face - * @param p_res array of at least 3 faces, amount used in function return - * @param p_is_point_over array of at least 3 booleans, determining which face is over the plane, amount used in function return - * @param _epsilon constant used for numerical error rounding, to add "thickness" to the plane (so coplanar points can happen) - * @return amount of faces generated by the split, either 0 (means no split possible), 2 or 3 - */ - + * @param p_plane plane used to split the face + * @param p_res array of at least 3 faces, amount used in function return + * @param p_is_point_over array of at least 3 booleans, determining which face is over the plane, amount used in function return + * @return amount of faces generated by the split, either 0 (means no split possible), 2 or 3 + */ int split_by_plane(const Plane &p_plane, Face3 *p_res, bool *p_is_point_over) const; Plane get_plane(ClockDirection p_dir = CLOCKWISE) const; @@ -99,7 +95,7 @@ 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; + Vector3 half_extents = p_aabb.size * 0.5f; Vector3 ofs = p_aabb.position + half_extents; Vector3 sup = Vector3( @@ -137,7 +133,7 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { #undef TEST_AXIS - Vector3 edge_norms[3] = { + const Vector3 edge_norms[3] = { vertex[0] - vertex[1], vertex[1] - vertex[2], vertex[2] - vertex[0], @@ -210,7 +206,7 @@ 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.0001f) { continue; // coplanar } //axis.normalize(); diff --git a/core/math/geometry_2d.cpp b/core/math/geometry_2d.cpp index 7b2630b4ff..31fade5a99 100644 --- a/core/math/geometry_2d.cpp +++ b/core/math/geometry_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -74,14 +74,14 @@ Vector<Vector<Vector2>> Geometry2D::decompose_polygon_in_convex(Vector<Point2> p struct _AtlasWorkRect { Size2i s; Point2i p; - int idx; + int idx = 0; _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; + int max_w = 0; + int max_h = 0; }; void Geometry2D::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size) { @@ -218,10 +218,10 @@ Vector<Vector<Point2>> Geometry2D::_polypaths_do_operation(PolyBooleanOperation // 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); + path_a << IntPoint(p_polypath_a[i].x * (real_t)SCALE_FACTOR, p_polypath_a[i].y * (real_t)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); + path_b << IntPoint(p_polypath_b[i].x * (real_t)SCALE_FACTOR, p_polypath_b[i].y * (real_t)SCALE_FACTOR); } Clipper clp; clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. @@ -246,8 +246,8 @@ Vector<Vector<Point2>> Geometry2D::_polypaths_do_operation(PolyBooleanOperation 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)); + static_cast<real_t>(scaled_path[j].X) / (real_t)SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / (real_t)SCALE_FACTOR)); } polypaths.push_back(polypath); } @@ -290,17 +290,17 @@ Vector<Vector<Point2>> Geometry2D::_polypath_offset(const Vector<Point2> &p_poly et = etOpenRound; break; } - ClipperOffset co(2.0, 0.25 * SCALE_FACTOR); // Defaults from ClipperOffset. + ClipperOffset co(2.0, 0.25f * (real_t)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); + path << IntPoint(p_polypath[i].x * (real_t)SCALE_FACTOR, p_polypath[i].y * (real_t)SCALE_FACTOR); } co.AddPath(path, jt, et); Paths paths; - co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. + co.Execute(paths, p_delta * (real_t)SCALE_FACTOR); // Inflate/deflate. // Have to scale points down now. Vector<Vector<Point2>> polypaths; @@ -312,8 +312,8 @@ Vector<Vector<Point2>> Geometry2D::_polypath_offset(const Vector<Point2> &p_poly 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)); + static_cast<real_t>(scaled_path[j].X) / (real_t)SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / (real_t)SCALE_FACTOR)); } polypaths.push_back(polypath); } diff --git a/core/math/geometry_2d.h b/core/math/geometry_2d.h index 6010159597..62786d69be 100644 --- a/core/math/geometry_2d.h +++ b/core/math/geometry_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,11 @@ #define GEOMETRY_2D_H #include "core/math/delaunay_2d.h" +#include "core/math/math_funcs.h" #include "core/math/triangulate.h" +#include "core/math/vector2.h" +#include "core/math/vector2i.h" +#include "core/math/vector3.h" #include "core/math/vector3i.h" #include "core/templates/vector.h" @@ -47,31 +51,31 @@ public: 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) { + if (a <= (real_t)CMP_EPSILON && e <= (real_t)CMP_EPSILON) { // Both segments degenerate into points. c1 = p1; c2 = p2; return Math::sqrt((c1 - c2).dot(c1 - c2)); } - if (a <= CMP_EPSILON) { + if (a <= (real_t)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); + t = CLAMP(t, 0.0f, 1.0f); } else { real_t c = d1.dot(r); - if (e <= CMP_EPSILON) { + if (e <= (real_t)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 + s = CLAMP(-c / a, 0.0f, 1.0f); // 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); + if (denom != 0.0f) { + s = CLAMP((b * f - c * e) / denom, 0.0f, 1.0f); } else { s = 0.0; } @@ -82,12 +86,12 @@ public: //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) { + if (t < 0.0f) { t = 0.0; - s = CLAMP(-c / a, 0.0, 1.0); - } else if (t > 1.0) { + s = CLAMP(-c / a, 0.0f, 1.0f); + } else if (t > 1.0f) { t = 1.0; - s = CLAMP((b - c) / a, 0.0, 1.0); + s = CLAMP((b - c) / a, 0.0f, 1.0f); } } } @@ -100,15 +104,15 @@ public: Vector2 p = p_point - p_segment[0]; Vector2 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) { + if (l2 < 1e-20f) { return p_segment[0]; // Both points are the same, just give any. } real_t d = n.dot(p) / l2; - if (d <= 0.0) { + if (d <= 0.0f) { return p_segment[0]; // Before first point. - } else if (d >= 1.0) { + } else if (d >= 1.0f) { return p_segment[1]; // After first point. } else { return p_segment[0] + n * d; // Inside. @@ -133,7 +137,7 @@ public: Vector2 p = p_point - p_segment[0]; Vector2 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) { + if (l2 < 1e-20f) { return p_segment[0]; // Both points are the same, just give any. } @@ -181,8 +185,7 @@ public: D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y); // Fail if C x B and D x B have the same sign (segments don't intersect). - // (equivalent to condition (C.y < 0 && D.y < CMP_EPSILON) || (C.y > 0 && D.y > CMP_EPSILON)) - if (C.y * D.y > CMP_EPSILON) { + if ((C.y < (real_t)-CMP_EPSILON && D.y < (real_t)-CMP_EPSILON) || (C.y > (real_t)CMP_EPSILON && D.y > (real_t)CMP_EPSILON)) { return false; } @@ -195,7 +198,7 @@ public: 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) { + if ((ABpos < 0) || (ABpos > 1)) { return false; } diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp index 88d2656025..ec96753c79 100644 --- a/core/math/geometry_3d.cpp +++ b/core/math/geometry_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,7 @@ #include "thirdparty/misc/polypartition.h" void Geometry3D::MeshData::optimize_vertices() { - Map<int, int> vtx_remap; + HashMap<int, int> vtx_remap; for (int i = 0; i < faces.size(); i++) { for (int j = 0; j < faces[i].indices.size(); j++) { @@ -124,8 +124,8 @@ static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { 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 (vi1.distance_to(vj1) < 0.00001f && + vi2.distance_to(vj2) < 0.00001f) { if (p_faces[i].links[k].face != -1) { ERR_PRINT("already linked\n"); error = true; @@ -281,16 +281,16 @@ static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int int div_y = len_y > 1 ? 2 : 1; int div_z = len_z > 1 ? 2 : 1; -#define _SPLIT(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \ - if (m_div == 1) { \ - m_new_v = m_v; \ - m_new_len_v = 1; \ - } else if (m_i == 0) { \ - m_new_v = m_v; \ - m_new_len_v = m_len_v / 2; \ - } else { \ - m_new_v = m_v + m_len_v / 2; \ - m_new_len_v = m_len_v - m_len_v / 2; \ +#define SPLIT_DIV(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \ + if (m_div == 1) { \ + m_new_v = m_v; \ + m_new_len_v = 1; \ + } else if (m_i == 0) { \ + m_new_v = m_v; \ + m_new_len_v = m_len_v / 2; \ + } else { \ + m_new_v = m_v + m_len_v / 2; \ + m_new_len_v = m_len_v - m_len_v / 2; \ } int new_x; @@ -301,18 +301,20 @@ 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); + SPLIT_DIV(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); + SPLIT_DIV(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); + SPLIT_DIV(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); } } } + +#undef SPLIT_DIV } 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) { @@ -491,11 +493,10 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i } 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(); const Face3 *faces = p_array.ptr(); + constexpr double min_size = 1.0; + constexpr int max_length = 20; AABB global_aabb; @@ -507,27 +508,27 @@ Vector<Face3> Geometry3D::wrap_geometry(Vector<Face3> p_array, real_t *p_error) } } - global_aabb.grow_by(0.01); // Avoid numerical error. + global_aabb.grow_by(0.01f); // Avoid numerical error. // Determine amount of cells in grid axis. int div_x, div_y, div_z; - if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) { - div_x = (int)(global_aabb.size.x / _MIN_SIZE) + 1; + if (global_aabb.size.x / min_size < max_length) { + div_x = (int)(global_aabb.size.x / min_size) + 1; } else { - div_x = _MAX_LENGTH; + div_x = max_length; } - if (global_aabb.size.y / _MIN_SIZE < _MAX_LENGTH) { - div_y = (int)(global_aabb.size.y / _MIN_SIZE) + 1; + if (global_aabb.size.y / min_size < max_length) { + div_y = (int)(global_aabb.size.y / min_size) + 1; } else { - div_y = _MAX_LENGTH; + div_y = max_length; } - if (global_aabb.size.z / _MIN_SIZE < _MAX_LENGTH) { - div_z = (int)(global_aabb.size.z / _MIN_SIZE) + 1; + if (global_aabb.size.z / min_size < max_length) { + div_z = (int)(global_aabb.size.z / min_size) + 1; } else { - div_z = _MAX_LENGTH; + div_z = max_length; } Vector3 voxelsize = global_aabb.size; @@ -637,21 +638,22 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes Vector3 ref = Vector3(0.0, 1.0, 0.0); - if (ABS(p.normal.dot(ref)) > 0.95) { + if (ABS(p.normal.dot(ref)) > 0.95f) { 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.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); - vertices.push_back(center + up * subplane_size - right * subplane_size); - vertices.push_back(center + up * subplane_size + right * subplane_size); + Vector<Vector3> vertices = { + center - up * subplane_size + right * subplane_size, + center - up * subplane_size - right * subplane_size, + center + up * subplane_size - right * subplane_size, + center + up * subplane_size + right * subplane_size + }; for (int j = 0; j < p_planes.size(); j++) { if (j == i) { @@ -661,7 +663,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes Vector<Vector3> new_vertices; Plane clip = p_planes[j]; - if (clip.normal.dot(p.normal) > 0.95) { + if (clip.normal.dot(p.normal) > 0.95f) { continue; } @@ -714,7 +716,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes 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) { + if (mesh.vertices[k].distance_to(vertices[j]) < 0.001f) { idx = k; break; } @@ -762,14 +764,14 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_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)); - planes.push_back(Plane(Vector3(0, 1, 0), p_extents.y)); - planes.push_back(Plane(Vector3(0, -1, 0), p_extents.y)); - planes.push_back(Plane(Vector3(0, 0, 1), p_extents.z)); - planes.push_back(Plane(Vector3(0, 0, -1), p_extents.z)); + Vector<Plane> planes = { + Plane(Vector3(1, 0, 0), p_extents.x), + Plane(Vector3(-1, 0, 0), p_extents.x), + Plane(Vector3(0, 1, 0), p_extents.y), + Plane(Vector3(0, -1, 0), p_extents.y), + Plane(Vector3(0, 0, 1), p_extents.z), + Plane(Vector3(0, 0, -1), p_extents.z) + }; return planes; } @@ -791,8 +793,8 @@ Vector<Plane> Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height Vector3 axis; axis[p_axis] = 1.0; - planes.push_back(Plane(axis, p_height * 0.5)); - planes.push_back(Plane(-axis, p_height * 0.5)); + planes.push_back(Plane(axis, p_height * 0.5f)); + planes.push_back(Plane(-axis, p_height * 0.5f)); return planes; } @@ -851,7 +853,7 @@ Vector<Plane> Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, for (int j = 1; j <= p_lats; j++) { Vector3 plane_normal = normal.lerp(axis, j / (real_t)p_lats).normalized(); - Vector3 position = axis * p_height * 0.5 + plane_normal * p_radius; + Vector3 position = axis * p_height * 0.5f + plane_normal * p_radius; planes.push_back(Plane(plane_normal, position)); planes.push_back(Plane(plane_normal * axis_neg, position * axis_neg)); } @@ -877,7 +879,7 @@ Vector<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, in 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) { + if (dp - p_planes[n].d > (real_t)CMP_EPSILON) { excluded = true; break; } @@ -902,8 +904,8 @@ Vector<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, in /* 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]; + int *v = reinterpret_cast<int *>(&(d[n])); + float *z = reinterpret_cast<float *>(&v[n]); int k = 0; v[0] = 0; diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h index 6a59b34585..59c56906f4 100644 --- a/core/math/geometry_3d.h +++ b/core/math/geometry_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -76,16 +76,16 @@ public: 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. + if (D < (real_t)CMP_EPSILON) { // The lines are almost parallel. + sN = 0.0f; // Force using point P0 on segment S1 + sD = 1.0f; // 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; + if (sN < 0.0f) { // sc < 0 => the s=0 edge is visible. + sN = 0.0f; tN = e; tD = c; } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. @@ -95,11 +95,11 @@ public: } } - if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. - tN = 0.0; + if (tN < 0.0f) { // tc < 0 => the t=0 edge is visible. + tN = 0.0f; // Recompute sc for this edge. - if (-d < 0.0) { - sN = 0.0; + if (-d < 0.0f) { + sN = 0.0f; } else if (-d > a) { sN = sD; } else { @@ -109,7 +109,7 @@ public: } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. tN = tD; // Recompute sc for this edge. - if ((-d + b) < 0.0) { + if ((-d + b) < 0.0f) { sN = 0; } else if ((-d + b) > a) { sN = sD; @@ -119,8 +119,8 @@ public: } } // 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); + sc = (Math::is_zero_approx(sN) ? 0.0f : sN / sD); + tc = (Math::is_zero_approx(tN) ? 0.0f : tN / tD); // Get the difference of the two closest points. Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) @@ -137,12 +137,12 @@ public: return false; } - real_t f = 1.0 / a; + real_t f = 1.0f / a; Vector3 s = p_from - p_v0; real_t u = f * s.dot(h); - if (u < 0.0 || u > 1.0) { + if ((u < 0.0f) || (u > 1.0f)) { return false; } @@ -150,7 +150,7 @@ public: real_t v = f * p_dir.dot(q); - if (v < 0.0 || u + v > 1.0) { + if ((v < 0.0f) || (u + v > 1.0f)) { return false; } @@ -158,7 +158,7 @@ public: // the intersection point is on the line. real_t t = f * e2.dot(q); - if (t > 0.00001) { // ray intersection + if (t > 0.00001f) { // ray intersection if (r_res) { *r_res = p_from + p_dir * t; } @@ -178,12 +178,12 @@ public: return false; } - real_t f = 1.0 / a; + real_t f = 1.0f / a; Vector3 s = p_from - p_v0; real_t u = f * s.dot(h); - if (u < 0.0 || u > 1.0) { + if ((u < 0.0f) || (u > 1.0f)) { return false; } @@ -191,7 +191,7 @@ public: real_t v = f * rel.dot(q); - if (v < 0.0 || u + v > 1.0) { + if ((v < 0.0f) || (u + v > 1.0f)) { return false; } @@ -199,7 +199,7 @@ public: // the intersection point is on the line. real_t t = f * e2.dot(q); - if (t > CMP_EPSILON && t <= 1.0) { // Ray intersection. + if (t > (real_t)CMP_EPSILON && t <= 1.0f) { // Ray intersection. if (r_res) { *r_res = p_from + rel * t; } @@ -213,7 +213,7 @@ public: Vector3 sphere_pos = p_sphere_pos - p_from; Vector3 rel = (p_to - p_from); real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) { + if (rel_l < (real_t)CMP_EPSILON) { return false; // Both points are the same. } Vector3 normal = rel / rel_l; @@ -229,7 +229,7 @@ public: real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance; real_t inters_d = sphere_d; - if (inters_d2 >= CMP_EPSILON) { + if (inters_d2 >= (real_t)CMP_EPSILON) { inters_d -= Math::sqrt(inters_d2); } @@ -253,14 +253,14 @@ public: 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) { + if (rel_l < (real_t)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; + cylinder_axis[p_cylinder_axis] = 1.0f; // First check if they are parallel. Vector3 normal = (rel / rel_l); @@ -269,9 +269,9 @@ public: Vector3 axis_dir; - if (crs_l < CMP_EPSILON) { + if (crs_l < (real_t)CMP_EPSILON) { Vector3 side_axis; - side_axis[(p_cylinder_axis + 1) % 3] = 1.0; // Any side axis OK. + side_axis[(p_cylinder_axis + 1) % 3] = 1.0f; // Any side axis OK. axis_dir = side_axis; } else { axis_dir = crs / crs_l; @@ -285,10 +285,10 @@ public: // Convert to 2D. real_t w2 = p_radius * p_radius - dist * dist; - if (w2 < CMP_EPSILON) { + if (w2 < (real_t)CMP_EPSILON) { return false; // Avoid numerical error. } - Size2 size(Math::sqrt(w2), p_height * 0.5); + Size2 size(Math::sqrt(w2), p_height * 0.5f); Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized(); @@ -366,7 +366,7 @@ public: Vector3 rel = p_to - p_from; real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) { + if (rel_l < (real_t)CMP_EPSILON) { return false; } @@ -379,7 +379,7 @@ public: real_t den = p.normal.dot(dir); - if (Math::abs(den) <= CMP_EPSILON) { + if (Math::abs(den) <= (real_t)CMP_EPSILON) { continue; // Ignore parallel plane. } @@ -417,15 +417,15 @@ public: Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) { + if (l2 < 1e-20f) { return p_segment[0]; // Both points are the same, just give any. } real_t d = n.dot(p) / l2; - if (d <= 0.0) { + if (d <= 0.0f) { return p_segment[0]; // Before first point. - } else if (d >= 1.0) { + } else if (d >= 1.0f) { return p_segment[1]; // After first point. } else { return p_segment[0] + n * d; // Inside. @@ -436,7 +436,7 @@ public: Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; real_t l2 = n.length_squared(); - if (l2 < 1e-20) { + if (l2 < 1e-20f) { return p_segment[0]; // Both points are the same, just give any. } @@ -564,11 +564,11 @@ public: for (int a = 0; a < polygon.size(); a++) { real_t dist = p_plane.distance_to(polygon[a]); - if (dist < -CMP_POINT_IN_PLANE_EPSILON) { + if (dist < (real_t)-CMP_POINT_IN_PLANE_EPSILON) { location_cache[a] = LOC_INSIDE; inside_count++; } else { - if (dist > CMP_POINT_IN_PLANE_EPSILON) { + if (dist > (real_t)CMP_POINT_IN_PLANE_EPSILON) { location_cache[a] = LOC_OUTSIDE; outside_count++; } else { @@ -907,9 +907,9 @@ public: _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); + Vector2 f = p_uv * 2.0f - Vector2(1.0f, 1.0f); 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); + float t = CLAMP(-n.z, 0.0f, 1.0f); n.x += n.x >= 0 ? -t : t; n.y += n.y >= 0 ? -t : t; return n.normalized(); diff --git a/core/math/math_defs.h b/core/math/math_defs.h index c3a8f910c0..b8b82f2ff4 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -68,37 +68,38 @@ enum Orientation { VERTICAL }; -enum HAlign { - HALIGN_LEFT, - HALIGN_CENTER, - HALIGN_RIGHT, - HALIGN_FILL, +enum HorizontalAlignment { + HORIZONTAL_ALIGNMENT_LEFT, + HORIZONTAL_ALIGNMENT_CENTER, + HORIZONTAL_ALIGNMENT_RIGHT, + HORIZONTAL_ALIGNMENT_FILL, }; -enum VAlign { - VALIGN_TOP, - VALIGN_CENTER, - VALIGN_BOTTOM +enum VerticalAlignment { + VERTICAL_ALIGNMENT_TOP, + VERTICAL_ALIGNMENT_CENTER, + VERTICAL_ALIGNMENT_BOTTOM, + VERTICAL_ALIGNMENT_FILL, }; -enum InlineAlign { +enum InlineAlignment { // Image alignment points. - INLINE_ALIGN_TOP_TO = 0b0000, - INLINE_ALIGN_CENTER_TO = 0b0001, - INLINE_ALIGN_BOTTOM_TO = 0b0010, - INLINE_ALIGN_IMAGE_MASK = 0b0011, + INLINE_ALIGNMENT_TOP_TO = 0b0000, + INLINE_ALIGNMENT_CENTER_TO = 0b0001, + INLINE_ALIGNMENT_BOTTOM_TO = 0b0010, + INLINE_ALIGNMENT_IMAGE_MASK = 0b0011, // Text alignment points. - INLINE_ALIGN_TO_TOP = 0b0000, - INLINE_ALIGN_TO_CENTER = 0b0100, - INLINE_ALIGN_TO_BASELINE = 0b1000, - INLINE_ALIGN_TO_BOTTOM = 0b1100, - INLINE_ALIGN_TEXT_MASK = 0b1100, + INLINE_ALIGNMENT_TO_TOP = 0b0000, + INLINE_ALIGNMENT_TO_CENTER = 0b0100, + INLINE_ALIGNMENT_TO_BASELINE = 0b1000, + INLINE_ALIGNMENT_TO_BOTTOM = 0b1100, + INLINE_ALIGNMENT_TEXT_MASK = 0b1100, // Presets. - INLINE_ALIGN_TOP = INLINE_ALIGN_TOP_TO | INLINE_ALIGN_TO_TOP, - INLINE_ALIGN_CENTER = INLINE_ALIGN_CENTER_TO | INLINE_ALIGN_TO_CENTER, - INLINE_ALIGN_BOTTOM = INLINE_ALIGN_BOTTOM_TO | INLINE_ALIGN_TO_BOTTOM + INLINE_ALIGNMENT_TOP = INLINE_ALIGNMENT_TOP_TO | INLINE_ALIGNMENT_TO_TOP, + INLINE_ALIGNMENT_CENTER = INLINE_ALIGNMENT_CENTER_TO | INLINE_ALIGNMENT_TO_CENTER, + INLINE_ALIGNMENT_BOTTOM = INLINE_ALIGNMENT_BOTTOM_TO | INLINE_ALIGNMENT_TO_BOTTOM }; enum Side { @@ -116,10 +117,10 @@ enum Corner { }; /** - * The "Real" type is an abstract type used for real numbers, such as 1.5, - * in contrast to integer numbers. Precision can be controlled with the - * presence or absence of the REAL_T_IS_DOUBLE define. - */ + * The "Real" type is an abstract type used for real numbers, such as 1.5, + * in contrast to integer numbers. Precision can be controlled with the + * presence or absence of the REAL_T_IS_DOUBLE define. + */ #ifdef REAL_T_IS_DOUBLE typedef double real_t; #else diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index 570c57e254..208f89f449 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -76,6 +76,36 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } + case Variant::VECTOR3I: { + SETUP_TYPE(Vector3i) + + /**/ TRY_TRANSFER_FIELD("x", x) + else TRY_TRANSFER_FIELD("y", y) + else TRY_TRANSFER_FIELD("z", z) + + return target; + } + case Variant::VECTOR4: { + SETUP_TYPE(Vector4) + + /**/ TRY_TRANSFER_FIELD("x", x) + else TRY_TRANSFER_FIELD("y", y) + else TRY_TRANSFER_FIELD("z", z) + else TRY_TRANSFER_FIELD("w", w) + + return target; + } + case Variant::VECTOR4I: { + SETUP_TYPE(Vector4i) + + /**/ TRY_TRANSFER_FIELD("x", x) + else TRY_TRANSFER_FIELD("y", y) + else TRY_TRANSFER_FIELD("z", z) + else TRY_TRANSFER_FIELD("w", w) + + return target; + } + case Variant::PLANE: { SETUP_TYPE(Plane) @@ -115,12 +145,12 @@ 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]) - else TRY_TRANSFER_FIELD("xy", elements[0][1]) - else TRY_TRANSFER_FIELD("yx", elements[1][0]) - else TRY_TRANSFER_FIELD("yy", elements[1][1]) - else TRY_TRANSFER_FIELD("ox", elements[2][0]) - else TRY_TRANSFER_FIELD("oy", elements[2][1]) + /**/ TRY_TRANSFER_FIELD("xx", columns[0][0]) + else TRY_TRANSFER_FIELD("xy", columns[0][1]) + else TRY_TRANSFER_FIELD("yx", columns[1][0]) + else TRY_TRANSFER_FIELD("yy", columns[1][1]) + else TRY_TRANSFER_FIELD("ox", columns[2][0]) + else TRY_TRANSFER_FIELD("oy", columns[2][1]) return target; } @@ -128,15 +158,15 @@ 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]) - else TRY_TRANSFER_FIELD("xy", elements[0][1]) - else TRY_TRANSFER_FIELD("xz", elements[0][2]) - else TRY_TRANSFER_FIELD("yx", elements[1][0]) - else TRY_TRANSFER_FIELD("yy", elements[1][1]) - else TRY_TRANSFER_FIELD("yz", elements[1][2]) - else TRY_TRANSFER_FIELD("zx", elements[2][0]) - else TRY_TRANSFER_FIELD("zy", elements[2][1]) - else TRY_TRANSFER_FIELD("zz", elements[2][2]) + /**/ TRY_TRANSFER_FIELD("xx", rows[0][0]) + else TRY_TRANSFER_FIELD("xy", rows[0][1]) + else TRY_TRANSFER_FIELD("xz", rows[0][2]) + else TRY_TRANSFER_FIELD("yx", rows[1][0]) + else TRY_TRANSFER_FIELD("yy", rows[1][1]) + else TRY_TRANSFER_FIELD("yz", rows[1][2]) + else TRY_TRANSFER_FIELD("zx", rows[2][0]) + else TRY_TRANSFER_FIELD("zy", rows[2][1]) + else TRY_TRANSFER_FIELD("zz", rows[2][2]) return target; } @@ -144,15 +174,15 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const case Variant::TRANSFORM3D: { SETUP_TYPE(Transform3D) - /**/ TRY_TRANSFER_FIELD("xx", basis.elements[0][0]) - else TRY_TRANSFER_FIELD("xy", basis.elements[0][1]) - else TRY_TRANSFER_FIELD("xz", basis.elements[0][2]) - else TRY_TRANSFER_FIELD("yx", basis.elements[1][0]) - else TRY_TRANSFER_FIELD("yy", basis.elements[1][1]) - else TRY_TRANSFER_FIELD("yz", basis.elements[1][2]) - else TRY_TRANSFER_FIELD("zx", basis.elements[2][0]) - else TRY_TRANSFER_FIELD("zy", basis.elements[2][1]) - else TRY_TRANSFER_FIELD("zz", basis.elements[2][2]) + /**/ TRY_TRANSFER_FIELD("xx", basis.rows[0][0]) + else TRY_TRANSFER_FIELD("xy", basis.rows[0][1]) + else TRY_TRANSFER_FIELD("xz", basis.rows[0][2]) + else TRY_TRANSFER_FIELD("yx", basis.rows[1][0]) + else TRY_TRANSFER_FIELD("yy", basis.rows[1][1]) + else TRY_TRANSFER_FIELD("yz", basis.rows[1][2]) + else TRY_TRANSFER_FIELD("zx", basis.rows[2][0]) + else TRY_TRANSFER_FIELD("zy", basis.rows[2][1]) + else TRY_TRANSFER_FIELD("zz", basis.rows[2][2]) else TRY_TRANSFER_FIELD("xo", origin.x) else TRY_TRANSFER_FIELD("yo", origin.y) else TRY_TRANSFER_FIELD("zo", origin.z) diff --git a/core/math/math_fieldwise.h b/core/math/math_fieldwise.h index fe44d09900..f8a5b7cbb2 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index bbed257f60..614828d7cb 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -53,6 +53,10 @@ uint32_t Math::rand() { return default_rand.rand(); } +double Math::randfn(double mean, double deviation) { + return default_rand.randfn(mean, deviation); +} + int Math::step_decimals(double p_step) { static const int maxn = 10; static const double sd[maxn] = { diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 4e4f566517..463e119add 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -64,7 +64,7 @@ public: static _ALWAYS_INLINE_ float sinc(float p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } static _ALWAYS_INLINE_ double sinc(double p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } - static _ALWAYS_INLINE_ float sincn(float p_x) { return sinc(Math_PI * p_x); } + static _ALWAYS_INLINE_ float sincn(float p_x) { return sinc((float)Math_PI * p_x); } static _ALWAYS_INLINE_ double sincn(double p_x) { return sinc(Math_PI * p_x); } static _ALWAYS_INLINE_ double cosh(double p_x) { return ::cosh(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 log1p(double p_x) { return ::log1p(p_x); } + static _ALWAYS_INLINE_ float log1p(float p_x) { return ::log1pf(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); } @@ -159,7 +162,7 @@ public: } ieee754; ieee754.f = p_val; return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && - ((unsigned)ieee754.u == 0); + ((unsigned)ieee754.u == 0); #else return isinf(p_val); #endif @@ -187,7 +190,7 @@ public: static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) { double value = Math::fmod(p_x, p_y); - if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { + if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) { value += p_y; } value += 0.0; @@ -195,10 +198,10 @@ public: } static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) { float value = Math::fmod(p_x, p_y); - if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { + if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) { value += p_y; } - value += 0.0; + value += 0.0f; return value; } static _ALWAYS_INLINE_ float fposmodp(float p_x, float p_y) { @@ -206,7 +209,7 @@ public: if (value < 0) { value += p_y; } - value += 0.0; + value += 0.0f; return value; } static _ALWAYS_INLINE_ double fposmodp(double p_x, double p_y) { @@ -220,21 +223,80 @@ public: 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)) { + 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_ float deg2rad(float p_y) { return p_y * (float)(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_ float rad2deg(float p_y) { return p_y * (float)(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; } + static _ALWAYS_INLINE_ double cubic_interpolate(double p_from, double p_to, double p_pre, double p_post, double p_weight) { + return 0.5 * + ((p_from * 2.0) + + (-p_pre + p_to) * p_weight + + (2.0 * p_pre - 5.0 * p_from + 4.0 * p_to - p_post) * (p_weight * p_weight) + + (-p_pre + 3.0 * p_from - 3.0 * p_to + p_post) * (p_weight * p_weight * p_weight)); + } + static _ALWAYS_INLINE_ float cubic_interpolate(float p_from, float p_to, float p_pre, float p_post, float p_weight) { + return 0.5f * + ((p_from * 2.0f) + + (-p_pre + p_to) * p_weight + + (2.0f * p_pre - 5.0f * p_from + 4.0f * p_to - p_post) * (p_weight * p_weight) + + (-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight)); + } + + static _ALWAYS_INLINE_ double cubic_interpolate_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight, + double p_to_t, double p_pre_t, double p_post_t) { + /* Barry-Goldman method */ + double t = Math::lerp(0.0, p_to_t, p_weight); + double a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0 : (t - p_pre_t) / -p_pre_t); + double a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5 : t / p_to_t); + double a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0 : (t - p_to_t) / (p_post_t - p_to_t)); + double b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0 : (t - p_pre_t) / (p_to_t - p_pre_t)); + double b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t); + return Math::lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t); + } + static _ALWAYS_INLINE_ float cubic_interpolate_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight, + float p_to_t, float p_pre_t, float p_post_t) { + /* Barry-Goldman method */ + float t = Math::lerp(0.0f, p_to_t, p_weight); + float a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0f : (t - p_pre_t) / -p_pre_t); + float a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5f : t / p_to_t); + float a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0f : (t - p_to_t) / (p_post_t - p_to_t)); + float b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0f : (t - p_pre_t) / (p_to_t - p_pre_t)); + float b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0f : t / p_post_t); + return Math::lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_t); + } + + static _ALWAYS_INLINE_ double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) { + /* Formula from Wikipedia article on Bezier curves. */ + double omt = (1.0 - p_t); + double omt2 = omt * omt; + double omt3 = omt2 * omt; + double t2 = p_t * p_t; + double t3 = t2 * p_t; + + return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; + } + static _ALWAYS_INLINE_ float bezier_interpolate(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) { + /* Formula from Wikipedia article on Bezier curves. */ + float omt = (1.0f - p_t); + float omt2 = omt * omt; + float omt3 = omt2 * omt; + float t2 = p_t * p_t; + float t3 = t2 * p_t; + + return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0f + p_control_2 * omt * t2 * 3.0f + p_end * t3; + } + static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) { double difference = fmod(p_to - p_from, Math_TAU); double distance = fmod(2.0 * difference, Math_TAU) - difference; @@ -266,14 +328,14 @@ public: 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; } + 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 + SIGN(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 + SIGN(p_to - p_from) * p_delta; } static _ALWAYS_INLINE_ double linear2db(double p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } - static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } + static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log(p_linear) * (float)8.6858896380650365530225783783321; } 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_ float db2linear(float p_db) { return Math::exp(p_db * (float)0.11512925464970228420089957273422); } static _ALWAYS_INLINE_ double round(double p_val) { return ::round(p_val); } static _ALWAYS_INLINE_ float round(float p_val) { return ::roundf(p_val); } @@ -284,17 +346,38 @@ public: } static _ALWAYS_INLINE_ double wrapf(double value, double min, double max) { double range = max - min; - return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); + double result = is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); + if (is_equal_approx(result, max)) { + return min; + } + return result; } static _ALWAYS_INLINE_ float wrapf(float value, float min, float max) { float range = max - min; - return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); + float result = is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); + if (is_equal_approx(result, max)) { + return min; + } + return result; + } + + static _ALWAYS_INLINE_ float fract(float value) { + return value - floor(value); + } + static _ALWAYS_INLINE_ double fract(double value) { + return value - floor(value); + } + static _ALWAYS_INLINE_ float pingpong(float value, float length) { + return (length != 0.0f) ? abs(fract((value - length) / (length * 2.0f)) * length * 2.0f - length) : 0.0f; + } + static _ALWAYS_INLINE_ double pingpong(double value, double length) { + return (length != 0.0) ? abs(fract((value - length) / (length * 2.0)) * length * 2.0 - length) : 0.0; } // double only, as these functions are mainly used by the editor and not performance-critical, 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 int range_step_decimals(double p_step); // For editor use only. static double snapped(double p_value, double p_step); static uint32_t larger_prime(uint32_t p_val); @@ -305,6 +388,7 @@ public: static uint32_t rand(); 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 randfn(double mean, double deviation); static double random(double from, double to); static float random(float from, float to); @@ -316,9 +400,9 @@ public: return true; } // Then check for approximate equality. - float tolerance = CMP_EPSILON * abs(a); - if (tolerance < CMP_EPSILON) { - tolerance = CMP_EPSILON; + float tolerance = (float)CMP_EPSILON * abs(a); + if (tolerance < (float)CMP_EPSILON) { + tolerance = (float)CMP_EPSILON; } return abs(a - b) < tolerance; } @@ -333,7 +417,7 @@ public: } static _ALWAYS_INLINE_ bool is_zero_approx(float s) { - return abs(s) < CMP_EPSILON; + return abs(s) < (float)CMP_EPSILON; } static _ALWAYS_INLINE_ bool is_equal_approx(double a, double b) { @@ -444,16 +528,16 @@ public: uint32_t x = ci.ui; uint32_t sign = (unsigned short)(x >> 31); uint32_t mantissa; - uint32_t exp; + uint32_t exponent; uint16_t hf; // get mantissa mantissa = x & ((1 << 23) - 1); // get exponent bits - exp = x & (0xFF << 23); - if (exp >= 0x47800000) { + exponent = x & (0xFF << 23); + if (exponent >= 0x47800000) { // check if the original single precision float number is a NaN - if (mantissa && (exp == (0xFF << 23))) { + if (mantissa && (exponent == (0xFF << 23))) { // we have a single precision NaN mantissa = (1 << 23) - 1; } else { @@ -461,21 +545,22 @@ public: mantissa = 0; } hf = (((uint16_t)sign) << 15) | (uint16_t)((0x1F << 10)) | - (uint16_t)(mantissa >> 13); + (uint16_t)(mantissa >> 13); } // check if exponent is <= -15 - else if (exp <= 0x38000000) { - /*// store a denorm half-float value or zero - exp = (0x38000000 - exp) >> 23; - mantissa >>= (14 + exp); - - hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa); - */ + else if (exponent <= 0x38000000) { + /* + // store a denorm half-float value or zero + exponent = (0x38000000 - exponent) >> 23; + mantissa >>= (14 + exponent); + + hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa); + */ hf = 0; //denormals do not work for 3D, convert to zero } else { hf = (((uint16_t)sign) << 15) | - (uint16_t)((exp - 0x38000000) >> 13) | - (uint16_t)(mantissa >> 13); + (uint16_t)((exponent - 0x38000000) >> 13) | + (uint16_t)(mantissa >> 13); } return hf; diff --git a/core/math/octree.h b/core/math/octree.h deleted file mode 100644 index 493a63aa2e..0000000000 --- a/core/math/octree.h +++ /dev/null @@ -1,1292 +0,0 @@ -/*************************************************************************/ -/* octree.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 OCTREE_H -#define OCTREE_H - -#include "core/math/aabb.h" -#include "core/math/geometry_3d.h" -#include "core/math/vector3.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; - -#define OCTREE_ELEMENT_INVALID_ID 0 -#define OCTREE_SIZE_LIMIT 1e15 - -template <class T, bool use_pairs = false, class AL = DefaultAllocator> -class Octree { -public: - typedef void *(*PairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int); - typedef void (*UnpairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int, void *); - -private: - enum { - NEG = 0, - POS = 1, - }; - - enum { - OCTANT_NX_NY_NZ, - OCTANT_PX_NY_NZ, - OCTANT_NX_PY_NZ, - OCTANT_PX_PY_NZ, - OCTANT_NX_NY_PZ, - OCTANT_PX_NY_PZ, - OCTANT_NX_PY_PZ, - OCTANT_PX_PY_PZ - }; - - struct PairKey { - union { - struct { - OctreeElementID A; - OctreeElementID B; - }; - uint64_t key; - }; - - _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; - } - } - - _FORCE_INLINE_ PairKey() {} - }; - - struct Element; - - struct Octant { - // cached for FAST plane check - AABB aabb; - - uint64_t last_pass = 0; - Octant *parent = nullptr; - Octant *children[8] = { nullptr }; - - 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() {} - ~Octant() {} - }; - - struct PairData; - - struct Element { - Octree *octree = nullptr; - - T *userdata = nullptr; - int subindex = 0; - bool pairable = false; - uint32_t pairable_mask = 0; - uint32_t pairable_type = 0; - - uint64_t last_pass = 0; - OctreeElementID _id = 0; - Octant *common_parent = nullptr; - - AABB aabb; - AABB container_aabb; - - 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() {} - }; - - struct PairData { - int refcount; - bool intersect; - Element *A, *B; - void *ud; - typename List<PairData *, AL>::Element *eA, *eB; - }; - - typedef Map<OctreeElementID, Element, Comparator<OctreeElementID>, AL> ElementMap; - typedef Map<PairKey, PairData, Comparator<PairKey>, AL> PairMap; - ElementMap element_map; - PairMap pair_map; - - PairCallback pair_callback; - UnpairCallback unpair_callback; - void *pair_callback_userdata; - void *unpair_callback_userdata; - - OctreeElementID last_element_id; - uint64_t pass; - - real_t unit_size; - Octant *root; - int octant_count; - 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); - } - pair_count--; - } - - p_pair->intersect = intersect; - } - } - - _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)) { - return; - } - - if (!(p_A->pairable_type & p_B->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; - pdata.B = p_B; - pdata.intersect = false; - E = pair_map.insert(key, pdata); - E->get().eA = p_A->pair_list.push_back(&E->get()); - E->get().eB = p_B->pair_list.push_back(&E->get()); - - /* - if (pair_callback) - 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) { - return; - } - - PairKey key(p_A->_id, p_B->_id); - typename PairMap::Element *E = pair_map.find(key); - if (!E) { - return; // no pair - } - - E->get().refcount--; - - if (E->get().refcount == 0) { - // bye pair - - if (E->get().intersect) { - if (unpair_callback) { - unpair_callback(pair_callback_userdata, p_A->_id, p_A->userdata, p_A->subindex, p_B->_id, p_B->userdata, p_B->subindex, E->get().ud); - } - - pair_count--; - } - - if (p_A == E->get().B) { - //may be reaching inverted - SWAP(p_A, p_B); - } - - p_A->pair_list.erase(E->get().eA); - p_B->pair_list.erase(E->get().eB); - pair_map.erase(E); - } - } - - _FORCE_INLINE_ void _element_check_pairs(Element *p_element) { - typename List<PairData *, AL>::Element *E = p_element->pair_list.front(); - while (E) { - _pair_check(E->get()); - E = E->next(); - } - } - - _FORCE_INLINE_ void _optimize() { - while (root && root->children_count < 2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) { - Octant *new_root = nullptr; - if (root->children_count == 1) { - for (int i = 0; i < 8; i++) { - if (root->children[i]) { - new_root = root->children[i]; - root->children[i] = nullptr; - break; - } - } - ERR_FAIL_COND(!new_root); - new_root->parent = nullptr; - new_root->parent_index = -1; - } - - memdelete_allocator<Octant, AL>(root); - octant_count--; - root = new_root; - } - } - - 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 = 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; - uint32_t mask; - }; - - void _cull_convex(Octant *p_octant, _CullConvexData *p_cull); - void _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); - void _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); - 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) { - return; - } - - for (int i = 0; i < 8; i++) { - if (p_octant->children[i]) { - _remove_tree(p_octant->children[i]); - } - } - - memdelete_allocator<Octant, AL>(p_octant); - } - -public: - OctreeElementID create(T *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); - void move(OctreeElementID p_id, const AABB &p_aabb); - void set_pairable(OctreeElementID p_id, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); - void erase(OctreeElementID p_id); - - bool is_pairable(OctreeElementID p_id) const; - T *get(OctreeElementID p_id) const; - 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 = 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 = 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); - - int get_octant_count() const { return octant_count; } - int get_pair_count() const { return pair_count; } - Octree(real_t p_unit_size = 1.0); - ~Octree() { _remove_tree(root); } -}; - -/* PRIVATE FUNCTIONS */ - -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, 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; -} - -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; -} - -#define OCTREE_DIVISOR 4 - -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 == nullptr) { - p_element->common_parent = p_octant; - p_element->container_aabb = p_octant->aabb; - } else { - p_element->container_aabb.merge_with(p_octant->aabb); - } - - 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]); - } - } - } - } else { - /* not big enough, send it to subitems */ - int splits = 0; - 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)) { - _insert_element(p_element, p_octant->children[i]); - splits++; - } - } else { - /* check against AABB where child should be */ - - AABB aabb = p_octant->aabb; - aabb.size *= 0.5; - - if (i & 1) { - aabb.position.x += aabb.size.x; - } - if (i & 2) { - aabb.position.y += aabb.size.y; - } - if (i & 4) { - aabb.position.z += aabb.size.z; - } - - if (aabb.intersects_inclusive(p_element->aabb)) { - /* if actually intersects, create the child */ - - Octant *child = memnew_allocator(Octant, AL); - p_octant->children[i] = child; - child->parent = p_octant; - child->parent_index = i; - - child->aabb = aabb; - - p_octant->children_count++; - - _insert_element(p_element, child); - octant_count++; - splits++; - } - } - } - - 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) { - _pair_reference(p_element, E->get()); - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - _pair_reference(p_element, E->get()); - E = E->next(); - } - } - } -} - -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; - } else { - base.position -= base.size; - base.size *= 2.0; - } - } - - root = memnew_allocator(Octant, AL); - - 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); - octant_count++; - root->parent = gp; - - if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { - /* grow towards positive */ - base.size *= 2.0; - gp->aabb = base; - gp->children[0] = root; - root->parent_index = 0; - } else { - base.position -= base.size; - base.size *= 2.0; - gp->aabb = base; - gp->children[(1 << 0) | (1 << 1) | (1 << 2)] = root; // add at all-positive - root->parent_index = (1 << 0) | (1 << 1) | (1 << 2); - } - - gp->children_count = 1; - root = gp; - } - } -} - -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 - return octant_removed; - } - - bool unpaired = false; - - if (use_pairs && p_octant->last_pass != pass) { - // check whether we should unpair stuff - // always test pairable - typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front(); - while (E) { - _pair_unreference(p_element, E->get()); - E = E->next(); - } - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - _pair_unreference(p_element, E->get()); - E = E->next(); - } - } - p_octant->last_pass = pass; - unpaired = true; - } - - bool removed = false; - - Octant *parent = p_octant->parent; - - 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 = nullptr; - } else { - ERR_FAIL_INDEX_V(p_octant->parent_index, 8, octant_removed); - - parent->children[p_octant->parent_index] = nullptr; - parent->children_count--; - } - - memdelete_allocator<Octant, AL>(p_octant); - octant_count--; - removed = true; - octant_removed = true; - } - - if (!removed && !unpaired) { - return octant_removed; // no reason to keep going up anymore! was already visited and was not removed - } - - p_octant = parent; - } - - return octant_removed; -} - -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) { - if (E->get()->last_pass != pass) { // only remove ONE reference - _pair_unreference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - if (E->get()->last_pass != pass) { // only remove ONE reference - _pair_unreference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - } - - p_octant->last_pass = pass; - - if (p_octant->children_count == 0) { - return; // small optimization for leafs - } - - for (int i = 0; i < 8; 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; - } - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->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; - } - E = E->next(); - } - } - p_octant->last_pass = pass; - - if (p_octant->children_count == 0) { - return; // small optimization for leafs - } - - for (int i = 0; i < 8; 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 - o->elements.erase(I->get().E); - } - - _remove_element_from_octant(p_element, o); - } - - /* THEN remove going down */ - - 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]) { - _unpair_element(p_element, o->children[i]); - } - } - - if (p_element->pairable) { - o->pairable_elements.erase(I->get().E); - } 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); - } -} - -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); - ERR_FAIL_COND_V(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15, 0); - ERR_FAIL_COND_V(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15, 0); - ERR_FAIL_COND_V(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0); - ERR_FAIL_COND_V(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0); - ERR_FAIL_COND_V(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.x), 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.y), 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.z), 0); - -#endif - typename ElementMap::Element *E = element_map.insert(last_element_id++, - Element()); - Element &e = E->get(); - - e.aabb = p_aabb; - e.userdata = p_userdata; - e.subindex = p_subindex; - e.last_pass = 0; - e.octree = this; - e.pairable = p_pairable; - e.pairable_type = p_pairable_type; - e.pairable_mask = p_pairable_mask; - e._id = last_element_id - 1; - - if (!e.aabb.has_no_surface()) { - _ensure_valid_root(p_aabb); - _insert_element(&e, root); - if (use_pairs) { - _element_check_pairs(&e); - } - } - - return last_element_id - 1; -} - -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); - ERR_FAIL_COND(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15); - ERR_FAIL_COND(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15); - ERR_FAIL_COND(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0); - ERR_FAIL_COND(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0); - ERR_FAIL_COND(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.x)); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.y)); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.z)); -#endif - typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - Element &e = E->get(); - - bool old_has_surf = !e.aabb.has_no_surface(); - bool new_has_surf = !p_aabb.has_no_surface(); - - if (old_has_surf != new_has_surf) { - if (old_has_surf) { - _remove_element(&e); // removing - e.common_parent = nullptr; - e.aabb = AABB(); - _optimize(); - } else { - _ensure_valid_root(p_aabb); // inserting - e.common_parent = nullptr; - e.aabb = p_aabb; - _insert_element(&e, root); - if (use_pairs) { - _element_check_pairs(&e); - } - } - - return; - } - - 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) { - _element_check_pairs(&e); // must check pairs anyway - } - - return; - } - - AABB combined = e.aabb; - combined.merge_with(p_aabb); - _ensure_valid_root(combined); - - ERR_FAIL_COND(e.octant_owners.front() == nullptr); - - /* FIND COMMON PARENT */ - - List<typename Element::OctantOwner, AL> owners = e.octant_owners; // save the octant owners - Octant *common_parent = e.common_parent; - ERR_FAIL_COND(!common_parent); - - //src is now the place towards where insertion is going to happen - pass++; - - 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 = nullptr; - e.aabb = p_aabb; - - _insert_element(&e, common_parent); // reinsert from this point - - 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(); - - /* - if (!use_pairs) - o->elements.erase( F->get().E ); - */ - - if (use_pairs && e.pairable) { - o->pairable_elements.erase(F->get().E); - } else { - o->elements.erase(F->get().E); - } - - if (_remove_element_from_octant(&e, o, common_parent->parent)) { - owners.erase(F); - } - - F = N; - } - - 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]) { - _unpair_element(&e, o->children[i]); - } - } - } - - _element_check_pairs(&e); - } - - _optimize(); -} - -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) { - return; // no changes, return - } - - if (!e.aabb.has_no_surface()) { - _remove_element(&e); - } - - e.pairable = p_pairable; - e.pairable_type = p_pairable_type; - e.pairable_mask = p_pairable_mask; - e.common_parent = nullptr; - - if (!e.aabb.has_no_surface()) { - _ensure_valid_root(e.aabb); - _insert_element(&e, root); - 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); - } - - element_map.erase(p_id); - _optimize(); -} - -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) { - return; //pointless - } - - 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))) { - continue; - } - e->last_pass = pass; - - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - if (*p_cull->result_idx < p_cull->result_max) { - p_cull->result_array[*p_cull->result_idx] = e->userdata; - (*p_cull->result_idx)++; - } else { - return; // pointless to continue - } - } - } - } - - 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))) { - continue; - } - e->last_pass = pass; - - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - if (*p_cull->result_idx < p_cull->result_max) { - p_cull->result_array[*p_cull->result_idx] = e->userdata; - (*p_cull->result_idx)++; - } else { - return; // pointless to continue - } - } - } - } - - for (int i = 0; i < 8; i++) { - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - _cull_convex(p_octant->children[i], p_cull); - } - } -} - -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) { - return; //pointless - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - - (*p_result_idx)++; - } else { - return; // pointless to continue - } - } - } - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - (*p_result_idx)++; - } else { - return; // pointless to continue - } - } - } - } - - 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); - } - } -} - -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) { - return; //pointless - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - (*p_result_idx)++; - - } else { - return; // pointless to continue - } - } - } - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - - (*p_result_idx)++; - - } else { - return; // pointless to continue - } - } - } - } - - 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); - } - } -} - -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) { - return; //pointless - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - (*p_result_idx)++; - - } else { - return; // pointless to continue - } - } - } - } - - 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))) { - 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) { - p_subindex_array[*p_result_idx] = e->subindex; - } - - (*p_result_idx)++; - - } else { - return; // pointless to continue - } - } - } - } - - 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); - } - } -} - -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; - } - - 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; - cdata.mask = p_mask; - - _cull_convex(root, &cdata); - - return result_count; -} - -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) { - return 0; - } - - int result_count = 0; - pass++; - _cull_aabb(root, p_aabb, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -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) { - return 0; - } - - int result_count = 0; - pass++; - _cull_segment(root, p_from, p_to, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -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) { - return 0; - } - - int result_count = 0; - pass++; - _cull_point(root, p_point, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -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 = nullptr; - - octant_count = 0; - pair_count = 0; - - pair_callback = nullptr; - unpair_callback = nullptr; - pair_callback_userdata = nullptr; - unpair_callback_userdata = nullptr; -} - -#endif // OCTREE_H diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 3c78b55b90..6881ad4014 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,7 +58,7 @@ Vector3 Plane::get_any_perpendicular_normal() const { 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.99f) { // if too similar to p1 p = p2; // use p2 } else { p = p1; // use p1 @@ -88,7 +88,7 @@ bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r *r_result = ((vec3_cross(normal1, normal2) * p_plane0.d) + (vec3_cross(normal2, normal0) * p_plane1.d) + (vec3_cross(normal0, normal1) * p_plane2.d)) / - denom; + denom; } return true; @@ -106,7 +106,7 @@ bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 real_t dist = (normal.dot(p_from) - d) / den; //printf("dist is %i\n",dist); - if (dist > CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist + if (dist > (real_t)CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist return false; } @@ -129,7 +129,7 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec real_t dist = (normal.dot(p_begin) - d) / den; //printf("dist is %i\n",dist); - if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { + if (dist < (real_t)-CMP_EPSILON || dist > (1.0f + (real_t)CMP_EPSILON)) { return false; } diff --git a/core/math/plane.h b/core/math/plane.h index 18be5d5d12..73babfa496 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -35,13 +35,12 @@ class Variant; -class Plane { -public: +struct _NO_DISCARD_ Plane { Vector3 normal; 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 + _FORCE_INLINE_ Vector3 get_normal() const { return normal; }; void normalize(); Plane normalized() const; @@ -53,7 +52,7 @@ public: _FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane _FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const; - _FORCE_INLINE_ bool has_point(const Vector3 &p_point, real_t _epsilon = CMP_EPSILON) const; + _FORCE_INLINE_ bool has_point(const Vector3 &p_point, real_t p_tolerance = CMP_EPSILON) const; /* intersections */ @@ -98,10 +97,10 @@ 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 { +bool Plane::has_point(const Vector3 &p_point, real_t p_tolerance) const { real_t dist = normal.dot(p_point) - d; dist = ABS(dist); - return (dist <= _epsilon); + return (dist <= p_tolerance); } Plane::Plane(const Vector3 &p_normal, real_t p_d) : diff --git a/core/math/camera_matrix.cpp b/core/math/projection.cpp index 8066a59281..edf8bf36cd 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/projection.cpp @@ -1,12 +1,12 @@ /*************************************************************************/ -/* camera_matrix.cpp */ +/* projection.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). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,27 +28,31 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "camera_matrix.h" +#include "projection.h" +#include "core/math/aabb.h" #include "core/math/math_funcs.h" +#include "core/math/plane.h" +#include "core/math/rect2.h" +#include "core/math/transform_3d.h" #include "core/string/print_string.h" -float CameraMatrix::determinant() const { +float Projection::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() { + 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 Projection::set_identity() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { matrix[i][j] = (i == j) ? 1 : 0; @@ -56,7 +60,7 @@ void CameraMatrix::set_identity() { } } -void CameraMatrix::set_zero() { +void Projection::set_zero() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { matrix[i][j] = 0; @@ -64,7 +68,7 @@ void CameraMatrix::set_zero() { } } -Plane CameraMatrix::xform4(const Plane &p_vec4) const { +Plane Projection::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; @@ -74,7 +78,22 @@ Plane CameraMatrix::xform4(const Plane &p_vec4) const { return ret; } -void CameraMatrix::adjust_perspective_znear(real_t p_new_znear) { +Vector4 Projection::xform(const Vector4 &p_vec4) const { + return Vector4( + matrix[0][0] * p_vec4.x + matrix[1][0] * p_vec4.y + matrix[2][0] * p_vec4.z + matrix[3][0] * p_vec4.w, + matrix[0][1] * p_vec4.x + matrix[1][1] * p_vec4.y + matrix[2][1] * p_vec4.z + matrix[3][1] * p_vec4.w, + matrix[0][2] * p_vec4.x + matrix[1][2] * p_vec4.y + matrix[2][2] * p_vec4.z + matrix[3][2] * p_vec4.w, + matrix[0][3] * p_vec4.x + matrix[1][3] * p_vec4.y + matrix[2][3] * p_vec4.z + matrix[3][3] * p_vec4.w); +} +Vector4 Projection::xform_inv(const Vector4 &p_vec4) const { + return Vector4( + matrix[0][0] * p_vec4.x + matrix[0][1] * p_vec4.y + matrix[0][2] * p_vec4.z + matrix[0][3] * p_vec4.w, + matrix[1][0] * p_vec4.x + matrix[1][1] * p_vec4.y + matrix[1][2] * p_vec4.z + matrix[1][3] * p_vec4.w, + matrix[2][0] * p_vec4.x + matrix[2][1] * p_vec4.y + matrix[2][2] * p_vec4.z + matrix[2][3] * p_vec4.w, + matrix[3][0] * p_vec4.x + matrix[3][1] * p_vec4.y + matrix[3][2] * p_vec4.z + matrix[3][3] * p_vec4.w); +} + +void Projection::adjust_perspective_znear(real_t p_new_znear) { real_t zfar = get_z_far(); real_t znear = p_new_znear; @@ -83,7 +102,154 @@ void CameraMatrix::adjust_perspective_znear(real_t p_new_znear) { 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) { +Projection Projection::create_depth_correction(bool p_flip_y) { + Projection proj; + proj.set_depth_correction(p_flip_y); + return proj; +} + +Projection Projection::create_light_atlas_rect(const Rect2 &p_rect) { + Projection proj; + proj.set_light_atlas_rect(p_rect); + return proj; +} + +Projection Projection::create_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) { + Projection proj; + proj.set_perspective(p_fovy_degrees, p_aspect, p_z_near, p_z_far, p_flip_fov); + return proj; +} + +Projection Projection::create_perspective_hmd(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) { + Projection proj; + proj.set_perspective(p_fovy_degrees, p_aspect, p_z_near, p_z_far, p_flip_fov, p_eye, p_intraocular_dist, p_convergence_dist); + return proj; +} + +Projection Projection::create_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) { + Projection proj; + proj.set_for_hmd(p_eye, p_aspect, p_intraocular_dist, p_display_width, p_display_to_lens, p_oversample, p_z_near, p_z_far); + return proj; +} + +Projection Projection::create_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) { + Projection proj; + proj.set_orthogonal(p_left, p_right, p_bottom, p_top, p_zfar, p_zfar); + return proj; +} + +Projection Projection::create_orthogonal_aspect(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) { + Projection proj; + proj.set_orthogonal(p_size, p_aspect, p_znear, p_zfar, p_flip_fov); + return proj; +} + +Projection Projection::create_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) { + Projection proj; + proj.set_frustum(p_left, p_right, p_bottom, p_top, p_near, p_far); + return proj; +} + +Projection Projection::create_frustum_aspect(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) { + Projection proj; + proj.set_frustum(p_size, p_aspect, p_offset, p_near, p_far, p_flip_fov); + return proj; +} + +Projection Projection::create_fit_aabb(const AABB &p_aabb) { + Projection proj; + proj.scale_translate_to_fit(p_aabb); + return proj; +} + +Projection Projection::perspective_znear_adjusted(real_t p_new_znear) const { + Projection proj = *this; + proj.adjust_perspective_znear(p_new_znear); + return proj; +} + +Plane Projection::get_projection_plane(Planes p_plane) const { + const real_t *matrix = (const real_t *)this->matrix; + + switch (p_plane) { + case PLANE_NEAR: { + Plane new_plane = Plane(matrix[3] + matrix[2], + matrix[7] + matrix[6], + matrix[11] + matrix[10], + matrix[15] + matrix[14]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + case PLANE_FAR: { + Plane new_plane = Plane(matrix[3] - matrix[2], + matrix[7] - matrix[6], + matrix[11] - matrix[10], + matrix[15] - matrix[14]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + case PLANE_LEFT: { + Plane new_plane = Plane(matrix[3] + matrix[0], + matrix[7] + matrix[4], + matrix[11] + matrix[8], + matrix[15] + matrix[12]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + case PLANE_TOP: { + Plane new_plane = Plane(matrix[3] - matrix[1], + matrix[7] - matrix[5], + matrix[11] - matrix[9], + matrix[15] - matrix[13]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + case PLANE_RIGHT: { + Plane new_plane = Plane(matrix[3] - matrix[0], + matrix[7] - matrix[4], + matrix[11] - matrix[8], + matrix[15] - matrix[12]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + case PLANE_BOTTOM: { + Plane new_plane = Plane(matrix[3] + matrix[1], + matrix[7] + matrix[5], + matrix[11] + matrix[9], + matrix[15] + matrix[13]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + return new_plane; + } break; + } + + return Plane(); +} + +Projection Projection::flipped_y() const { + Projection proj = *this; + proj.flip_y(); + return proj; +} + +Projection Projection ::jitter_offseted(const Vector2 &p_offset) const { + Projection proj = *this; + proj.add_jitter_offset(p_offset); + return proj; +} + +void Projection::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) { if (p_flip_fov) { p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect); } @@ -109,7 +275,7 @@ void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_ matrix[3][3] = 0; } -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, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist) { +void Projection::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) { if (p_flip_fov) { p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect); } @@ -141,13 +307,13 @@ void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_ set_frustum(left, right, -ymax, ymax, p_z_near, p_z_far); // translate matrix by (modeltranslation, 0.0, 0.0) - CameraMatrix cm; + Projection cm; cm.set_identity(); cm.matrix[3][0] = modeltranslation; *this = *this * cm; } -void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) { +void Projection::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) { // we first calculate our base frustum on our values without taking our lens magnification into account. real_t f1 = (p_intraocular_dist * 0.5) / p_display_to_lens; real_t f2 = ((p_display_width - p_intraocular_dist) * 0.5) / p_display_to_lens; @@ -175,7 +341,7 @@ void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_ } } -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) { +void Projection::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); @@ -187,7 +353,7 @@ void CameraMatrix::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom matrix[3][3] = 1.0; } -void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) { +void Projection::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; } @@ -195,7 +361,7 @@ void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear set_orthogonal(-p_size / 2, +p_size / 2, -p_size / p_aspect / 2, +p_size / p_aspect / 2, p_znear, p_zfar); } -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) { +void Projection::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); @@ -227,7 +393,7 @@ void CameraMatrix::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, r te[15] = 0; } -void CameraMatrix::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) { +void Projection::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) { if (!p_flip_fov) { p_size *= p_aspect; } @@ -235,7 +401,7 @@ void CameraMatrix::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, set_frustum(-p_size / 2 + p_offset.x, +p_size / 2 + p_offset.x, -p_size / p_aspect / 2 + p_offset.y, +p_size / p_aspect / 2 + p_offset.y, p_near, p_far); } -real_t CameraMatrix::get_z_far() const { +real_t Projection::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], @@ -248,7 +414,7 @@ real_t CameraMatrix::get_z_far() const { return new_plane.d; } -real_t CameraMatrix::get_z_near() const { +real_t Projection::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], @@ -259,7 +425,7 @@ real_t CameraMatrix::get_z_near() const { return new_plane.d; } -Vector2 CameraMatrix::get_viewport_half_extents() const { +Vector2 Projection::get_viewport_half_extents() const { const real_t *matrix = (const real_t *)this->matrix; ///////--- Near Plane ---/////// Plane near_plane = Plane(matrix[3] + matrix[2], @@ -287,7 +453,7 @@ Vector2 CameraMatrix::get_viewport_half_extents() const { return Vector2(res.x, res.y); } -Vector2 CameraMatrix::get_far_plane_half_extents() const { +Vector2 Projection::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], @@ -315,7 +481,7 @@ Vector2 CameraMatrix::get_far_plane_half_extents() const { return Vector2(res.x, res.y); } -bool CameraMatrix::get_endpoints(const Transform3D &p_transform, Vector3 *p_8points) const { +bool Projection::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 }, @@ -338,7 +504,7 @@ bool CameraMatrix::get_endpoints(const Transform3D &p_transform, Vector3 *p_8poi return true; } -Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform) const { +Vector<Plane> Projection::get_projection_planes(const Transform3D &p_transform) const { /** Fast Plane Extraction from combined modelview/projection matrices. * References: * https://web.archive.org/web/20011221205252/https://www.markmorley.com/opengl/frustumculling.html @@ -346,6 +512,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform */ Vector<Plane> planes; + planes.resize(6); const real_t *matrix = (const real_t *)this->matrix; @@ -360,7 +527,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[0] = p_transform.xform(new_plane); ///////--- Far Plane ---/////// new_plane = Plane(matrix[3] - matrix[2], @@ -371,7 +538,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[1] = p_transform.xform(new_plane); ///////--- Left Plane ---/////// new_plane = Plane(matrix[3] + matrix[0], @@ -382,7 +549,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[2] = p_transform.xform(new_plane); ///////--- Top Plane ---/////// new_plane = Plane(matrix[3] - matrix[1], @@ -393,7 +560,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[3] = p_transform.xform(new_plane); ///////--- Right Plane ---/////// new_plane = Plane(matrix[3] - matrix[0], @@ -404,7 +571,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[4] = p_transform.xform(new_plane); ///////--- Bottom Plane ---/////// new_plane = Plane(matrix[3] + matrix[1], @@ -415,25 +582,23 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform new_plane.normal = -new_plane.normal; new_plane.normalize(); - planes.push_back(p_transform.xform(new_plane)); + planes.write[5] = p_transform.xform(new_plane); return planes; } -CameraMatrix CameraMatrix::inverse() const { - CameraMatrix cm = *this; +Projection Projection::inverse() const { + Projection cm = *this; cm.invert(); return cm; } -void CameraMatrix::invert() { +void Projection::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 */ real_t hold; /* Temporary storage */ - real_t determinat; /* Determinant */ - - determinat = 1.0; + real_t determinant = 1.0f; for (k = 0; k < 4; k++) { /** Locate k'th pivot element **/ pvt_val = matrix[k][k]; /** Initialize for search **/ @@ -441,7 +606,7 @@ void CameraMatrix::invert() { pvt_j[k] = k; for (i = k; i < 4; i++) { for (j = k; j < 4; j++) { - if (Math::absd(matrix[i][j]) > Math::absd(pvt_val)) { + if (Math::abs(matrix[i][j]) > Math::abs(pvt_val)) { pvt_i[k] = i; pvt_j[k] = j; pvt_val = matrix[i][j]; @@ -450,9 +615,9 @@ void CameraMatrix::invert() { } /** Product of pivots, gives determinant when finished **/ - determinat *= pvt_val; - if (Math::absd(determinat) < 1e-7) { - return; //(false); /** Matrix is singular (zero determinant). **/ + determinant *= pvt_val; + if (Math::is_zero_approx(determinant)) { + return; /** Matrix is singular (zero determinant). **/ } /** "Interchange" rows (with sign change stuff) **/ @@ -526,18 +691,18 @@ void CameraMatrix::invert() { } } -void CameraMatrix::flip_y() { +void Projection::flip_y() { for (int i = 0; i < 4; i++) { matrix[1][i] = -matrix[1][i]; } } -CameraMatrix::CameraMatrix() { +Projection::Projection() { set_identity(); } -CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { - CameraMatrix new_matrix; +Projection Projection::operator*(const Projection &p_matrix) const { + Projection new_matrix; for (int j = 0; j < 4; j++) { for (int i = 0; i < 4; i++) { @@ -552,7 +717,7 @@ CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { return new_matrix; } -void CameraMatrix::set_depth_correction(bool p_flip_y) { +void Projection::set_depth_correction(bool p_flip_y) { real_t *m = &matrix[0][0]; m[0] = 1; @@ -573,7 +738,7 @@ void CameraMatrix::set_depth_correction(bool p_flip_y) { m[15] = 1.0; } -void CameraMatrix::set_light_bias() { +void Projection::set_light_bias() { real_t *m = &matrix[0][0]; m[0] = 0.5; @@ -594,7 +759,7 @@ void CameraMatrix::set_light_bias() { m[15] = 1.0; } -void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { +void Projection::set_light_atlas_rect(const Rect2 &p_rect) { real_t *m = &matrix[0][0]; m[0] = p_rect.size.width; @@ -615,7 +780,7 @@ void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { m[15] = 1.0; } -CameraMatrix::operator String() const { +Projection::operator String() const { String str; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { @@ -626,22 +791,22 @@ CameraMatrix::operator String() const { return str; } -real_t CameraMatrix::get_aspect() const { +real_t Projection::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 { +int Projection::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 { +bool Projection::is_orthogonal() const { return matrix[3][3] == 1.0; } -real_t CameraMatrix::get_fov() const { +real_t Projection::get_fov() const { const real_t *matrix = (const real_t *)this->matrix; Plane right_plane = Plane(matrix[3] - matrix[0], @@ -664,7 +829,7 @@ real_t CameraMatrix::get_fov() const { } } -float CameraMatrix::get_lod_multiplier() const { +float Projection::get_lod_multiplier() const { if (is_orthogonal()) { return get_viewport_half_extents().x; } else { @@ -675,14 +840,14 @@ float CameraMatrix::get_lod_multiplier() const { //usage is lod_size / (lod_distance * multiplier) < threshold } -void CameraMatrix::make_scale(const Vector3 &p_scale) { +void Projection::make_scale(const Vector3 &p_scale) { set_identity(); matrix[0][0] = p_scale.x; matrix[1][1] = p_scale.y; matrix[2][2] = p_scale.z; } -void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { +void Projection::scale_translate_to_fit(const AABB &p_aabb) { Vector3 min = p_aabb.position; Vector3 max = p_aabb.position + p_aabb.size; @@ -707,21 +872,26 @@ void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { matrix[3][3] = 1; } -CameraMatrix::operator Transform3D() const { +void Projection::add_jitter_offset(const Vector2 &p_offset) { + matrix[3][0] += p_offset.x; + matrix[3][1] += p_offset.y; +} + +Projection::operator Transform3D() const { Transform3D tr; const real_t *m = &matrix[0][0]; - tr.basis.elements[0][0] = m[0]; - tr.basis.elements[1][0] = m[1]; - tr.basis.elements[2][0] = m[2]; + tr.basis.rows[0][0] = m[0]; + tr.basis.rows[1][0] = m[1]; + tr.basis.rows[2][0] = m[2]; - tr.basis.elements[0][1] = m[4]; - tr.basis.elements[1][1] = m[5]; - tr.basis.elements[2][1] = m[6]; + tr.basis.rows[0][1] = m[4]; + tr.basis.rows[1][1] = m[5]; + tr.basis.rows[2][1] = m[6]; - tr.basis.elements[0][2] = m[8]; - tr.basis.elements[1][2] = m[9]; - tr.basis.elements[2][2] = m[10]; + tr.basis.rows[0][2] = m[8]; + tr.basis.rows[1][2] = m[9]; + tr.basis.rows[2][2] = m[10]; tr.origin.x = m[12]; tr.origin.y = m[13]; @@ -729,22 +899,27 @@ CameraMatrix::operator Transform3D() const { return tr; } - -CameraMatrix::CameraMatrix(const Transform3D &p_transform) { +Projection::Projection(const Vector4 &p_x, const Vector4 &p_y, const Vector4 &p_z, const Vector4 &p_w) { + matrix[0] = p_x; + matrix[1] = p_y; + matrix[2] = p_z; + matrix[3] = p_w; +} +Projection::Projection(const Transform3D &p_transform) { const Transform3D &tr = p_transform; real_t *m = &matrix[0][0]; - m[0] = tr.basis.elements[0][0]; - m[1] = tr.basis.elements[1][0]; - m[2] = tr.basis.elements[2][0]; + m[0] = tr.basis.rows[0][0]; + m[1] = tr.basis.rows[1][0]; + m[2] = tr.basis.rows[2][0]; m[3] = 0.0; - m[4] = tr.basis.elements[0][1]; - m[5] = tr.basis.elements[1][1]; - m[6] = tr.basis.elements[2][1]; + m[4] = tr.basis.rows[0][1]; + m[5] = tr.basis.rows[1][1]; + m[6] = tr.basis.rows[2][1]; m[7] = 0.0; - m[8] = tr.basis.elements[0][2]; - m[9] = tr.basis.elements[1][2]; - m[10] = tr.basis.elements[2][2]; + m[8] = tr.basis.rows[0][2]; + m[9] = tr.basis.rows[1][2]; + m[10] = tr.basis.rows[2][2]; m[11] = 0.0; m[12] = tr.origin.x; m[13] = tr.origin.y; @@ -752,5 +927,5 @@ CameraMatrix::CameraMatrix(const Transform3D &p_transform) { m[15] = 1.0; } -CameraMatrix::~CameraMatrix() { +Projection::~Projection() { } diff --git a/core/math/camera_matrix.h b/core/math/projection.h index 786d46055a..a3d2d7720b 100644 --- a/core/math/camera_matrix.h +++ b/core/math/projection.h @@ -1,12 +1,12 @@ /*************************************************************************/ -/* camera_matrix.h */ +/* projection.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). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,13 +28,21 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef CAMERA_MATRIX_H -#define CAMERA_MATRIX_H +#ifndef PROJECTION_H +#define PROJECTION_H -#include "core/math/rect2.h" -#include "core/math/transform_3d.h" +#include "core/math/math_defs.h" +#include "core/math/vector3.h" +#include "core/math/vector4.h" +#include "core/templates/vector.h" -struct CameraMatrix { +struct AABB; +struct Plane; +struct Rect2; +struct Transform3D; +struct Vector2; + +struct Projection { enum Planes { PLANE_NEAR, PLANE_FAR, @@ -44,13 +52,24 @@ struct CameraMatrix { PLANE_BOTTOM }; - real_t matrix[4][4]; + Vector4 matrix[4]; + + _FORCE_INLINE_ const Vector4 &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 4); + return matrix[p_axis]; + } + + _FORCE_INLINE_ Vector4 &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 4); + return matrix[p_axis]; + } 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); @@ -61,6 +80,21 @@ struct CameraMatrix { void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false); void adjust_perspective_znear(real_t p_new_znear); + static Projection create_depth_correction(bool p_flip_y); + static Projection create_light_atlas_rect(const Rect2 &p_rect); + static Projection create_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); + static Projection create_perspective_hmd(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); + static Projection create_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far); + static Projection create_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); + static Projection create_orthogonal_aspect(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false); + static Projection create_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); + static Projection create_frustum_aspect(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false); + static Projection create_fit_aabb(const AABB &p_aabb); + Projection perspective_znear_adjusted(real_t p_new_znear) const; + Plane get_projection_plane(Planes p_plane) const; + Projection flipped_y() const; + Projection jitter_offseted(const Vector2 &p_offset) const; + 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); } @@ -78,23 +112,27 @@ struct CameraMatrix { Vector2 get_far_plane_half_extents() const; void invert(); - CameraMatrix inverse() const; + Projection inverse() const; - CameraMatrix operator*(const CameraMatrix &p_matrix) const; + Projection operator*(const Projection &p_matrix) const; Plane xform4(const Plane &p_vec4) const; _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vec3) const; + Vector4 xform(const Vector4 &p_vec4) const; + Vector4 xform_inv(const Vector4 &p_vec4) const; + operator String() const; void scale_translate_to_fit(const AABB &p_aabb); + void add_jitter_offset(const Vector2 &p_offset); void make_scale(const Vector3 &p_scale); int get_pixels_per_meter(int p_for_pixel_width) const; operator Transform3D() const; void flip_y(); - bool operator==(const CameraMatrix &p_cam) const { + bool operator==(const Projection &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]) { @@ -105,18 +143,19 @@ struct CameraMatrix { return true; } - bool operator!=(const CameraMatrix &p_cam) const { + bool operator!=(const Projection &p_cam) const { return !(*this == p_cam); } float get_lod_multiplier() const; - CameraMatrix(); - CameraMatrix(const Transform3D &p_transform); - ~CameraMatrix(); + Projection(); + Projection(const Vector4 &p_x, const Vector4 &p_y, const Vector4 &p_z, const Vector4 &p_w); + Projection(const Transform3D &p_transform); + ~Projection(); }; -Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const { +Vector3 Projection::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]; @@ -125,4 +164,4 @@ Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const { return ret / w; } -#endif // CAMERA_MATRIX_H +#endif // PROJECTION_H diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 944474686a..c836a82e37 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -102,6 +102,23 @@ Quaternion Quaternion::inverse() const { return Quaternion(-x, -y, -z, w); } +Quaternion Quaternion::log() const { + Quaternion src = *this; + Vector3 src_v = src.get_axis() * src.get_angle(); + return Quaternion(src_v.x, src_v.y, src_v.z, 0); +} + +Quaternion Quaternion::exp() const { + Quaternion src = *this; + Vector3 src_v = Vector3(src.x, src.y, src.z); + real_t theta = src_v.length(); + src_v = src_v.normalized(); + if (theta < CMP_EPSILON || !src_v.is_normalized()) { + return Quaternion(0, 0, 0, 1); + } + return Quaternion(src_v, theta); +} + 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."); @@ -114,22 +131,16 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con cosom = dot(p_to); // adjust signs (if necessary) - if (cosom < 0.0) { + if (cosom < 0.0f) { cosom = -cosom; - to1.x = -p_to.x; - to1.y = -p_to.y; - to1.z = -p_to.z; - to1.w = -p_to.w; + to1 = -p_to; } else { - to1.x = p_to.x; - to1.y = p_to.y; - to1.z = p_to.z; - to1.w = p_to.w; + to1 = p_to; } // calculate coefficients - if ((1.0 - cosom) > CMP_EPSILON) { + if ((1.0f - cosom) > (real_t)CMP_EPSILON) { // standard case (slerp) omega = Math::acos(cosom); sinom = Math::sin(omega); @@ -138,7 +149,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con } else { // "from" and "to" quaternions are very close // ... so we can do a linear interpolation - scale0 = 1.0 - p_weight; + scale0 = 1.0f - p_weight; scale1 = p_weight; } // calculate final values @@ -158,14 +169,14 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c real_t dot = from.dot(p_to); - if (Math::absf(dot) > 0.9999) { + if (Math::absf(dot) > 0.9999f) { return from; } real_t theta = Math::acos(dot), - sinT = 1.0 / Math::sin(theta), + sinT = 1.0f / Math::sin(theta), newFactor = Math::sin(p_weight * theta) * sinT, - invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; + invFactor = Math::sin((1.0f - p_weight) * theta) * sinT; return Quaternion(invFactor * from.x + newFactor * p_to.x, invFactor * from.y + newFactor * p_to.y, @@ -173,16 +184,105 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c 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 { +Quaternion Quaternion::spherical_cubic_interpolate(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 + Quaternion from_q = *this; + Quaternion pre_q = p_pre_a; + Quaternion to_q = p_b; + Quaternion post_q = p_post_b; + + // Align flip phases. + from_q = Basis(from_q).get_rotation_quaternion(); + pre_q = Basis(pre_q).get_rotation_quaternion(); + to_q = Basis(to_q).get_rotation_quaternion(); + post_q = Basis(post_q).get_rotation_quaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = signbit(from_q.dot(pre_q)); + pre_q = flip1 ? -pre_q : pre_q; + bool flip2 = signbit(from_q.dot(to_q)); + to_q = flip2 ? -to_q : to_q; + bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q)); + post_q = flip3 ? -post_q : post_q; + + // Calc by Expmap in from_q space. + Quaternion ln_from = Quaternion(0, 0, 0, 0); + Quaternion ln_to = (from_q.inverse() * to_q).log(); + Quaternion ln_pre = (from_q.inverse() * pre_q).log(); + Quaternion ln_post = (from_q.inverse() * post_q).log(); + Quaternion ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q1 = from_q * ln.exp(); + + // Calc by Expmap in to_q space. + ln_from = (to_q.inverse() * from_q).log(); + ln_to = Quaternion(0, 0, 0, 0); + ln_pre = (to_q.inverse() * pre_q).log(); + ln_post = (to_q.inverse() * post_q).log(); + ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q2 = to_q * ln.exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.slerp(q2, p_weight); +} + +Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, + const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) 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 from_q = *this; + Quaternion pre_q = p_pre_a; + Quaternion to_q = p_b; + Quaternion post_q = p_post_b; + + // Align flip phases. + from_q = Basis(from_q).get_rotation_quaternion(); + pre_q = Basis(pre_q).get_rotation_quaternion(); + to_q = Basis(to_q).get_rotation_quaternion(); + post_q = Basis(post_q).get_rotation_quaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = signbit(from_q.dot(pre_q)); + pre_q = flip1 ? -pre_q : pre_q; + bool flip2 = signbit(from_q.dot(to_q)); + to_q = flip2 ? -to_q : to_q; + bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q)); + post_q = flip3 ? -post_q : post_q; + + // Calc by Expmap in from_q space. + Quaternion ln_from = Quaternion(0, 0, 0, 0); + Quaternion ln_to = (from_q.inverse() * to_q).log(); + Quaternion ln_pre = (from_q.inverse() * pre_q).log(); + Quaternion ln_post = (from_q.inverse() * post_q).log(); + Quaternion ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + Quaternion q1 = from_q * ln.exp(); + + // Calc by Expmap in to_q space. + ln_from = (to_q.inverse() * from_q).log(); + ln_to = Quaternion(0, 0, 0, 0); + ln_pre = (to_q.inverse() * pre_q).log(); + ln_post = (to_q.inverse() * post_q).log(); + ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + Quaternion q2 = to_q * ln.exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.slerp(q2, p_weight); } Quaternion::operator String() const { @@ -190,11 +290,14 @@ Quaternion::operator String() const { } Vector3 Quaternion::get_axis() const { + if (Math::abs(w) > 1 - CMP_EPSILON) { + return Vector3(x, y, z); + } real_t r = ((real_t)1) / Math::sqrt(1 - w * w); return Vector3(x * r, y * r, z * r); } -float Quaternion::get_angle() const { +real_t Quaternion::get_angle() const { return 2 * Math::acos(w); } @@ -209,8 +312,8 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { 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 sin_angle = Math::sin(p_angle * 0.5f); + real_t cos_angle = Math::cos(p_angle * 0.5f); real_t s = sin_angle / d; x = p_axis.x * s; y = p_axis.y * s; @@ -224,9 +327,9 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { // 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; + real_t half_a1 = p_euler.y * 0.5f; + real_t half_a2 = p_euler.x * 0.5f; + real_t half_a3 = p_euler.z * 0.5f; // 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) diff --git a/core/math/quaternion.h b/core/math/quaternion.h index e20ea74eb4..43d7bffcfc 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,8 +36,7 @@ #include "core/math/vector3.h" #include "core/string/ustring.h" -class Quaternion { -public: +struct _NO_DISCARD_ Quaternion { union { struct { real_t x; @@ -61,6 +60,8 @@ public: Quaternion normalized() const; bool is_normalized() const; Quaternion inverse() const; + Quaternion log() const; + Quaternion exp() const; _FORCE_INLINE_ real_t dot(const Quaternion &p_q) const; real_t angle_to(const Quaternion &p_to) const; @@ -70,10 +71,11 @@ public: 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; + Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const; + Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; Vector3 get_axis() const; - float get_angle() const; + real_t get_angle() const; _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { r_angle = 2 * Math::acos(w); @@ -86,13 +88,6 @@ public: 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."); @@ -141,12 +136,11 @@ public: w(p_q.w) { } - Quaternion &operator=(const Quaternion &p_q) { + void 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 @@ -154,19 +148,19 @@ public: Vector3 c = v0.cross(v1); real_t d = v0.dot(v1); - if (d < -1.0 + CMP_EPSILON) { + if (d < -1.0f + (real_t)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; + real_t s = Math::sqrt((1.0f + d) * 2.0f); + real_t rs = 1.0f / s; x = c.x * rs; y = c.y * rs; z = c.z * rs; - w = s * 0.5; + w = s * 0.5f; } } }; @@ -201,7 +195,7 @@ void Quaternion::operator*=(const real_t &s) { } void Quaternion::operator/=(const real_t &s) { - *this *= 1.0 / s; + *this *= 1.0f / s; } Quaternion Quaternion::operator+(const Quaternion &q2) const { @@ -224,7 +218,7 @@ Quaternion Quaternion::operator*(const real_t &s) const { } Quaternion Quaternion::operator/(const real_t &s) const { - return *this * (1.0 / s); + return *this * (1.0f / s); } bool Quaternion::operator==(const Quaternion &p_quaternion) const { diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index d438a9a377..c7727a44a1 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,7 +30,7 @@ #include "quick_hull.h" -#include "core/templates/map.h" +#include "core/templates/rb_map.h" uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF; @@ -52,7 +52,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ Vector<bool> valid_points; valid_points.resize(p_points.size()); - Set<Vector3> valid_cache; + HashSet<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)); @@ -237,7 +237,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ //find lit faces and lit edges List<List<Face>::Element *> lit_faces; //lit face is a death sentence - Map<Edge, FaceConnect> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot + HashMap<Edge, FaceConnect, Edge> 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) { @@ -248,15 +248,15 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ uint32_t b = E->get().vertices[(i + 1) % 3]; Edge e(a, b); - Map<Edge, FaceConnect>::Element *F = lit_edges.find(e); + HashMap<Edge, FaceConnect, Edge>::Iterator F = lit_edges.find(e); if (!F) { F = lit_edges.insert(e, FaceConnect()); } if (e.vertices[0] == a) { //left - F->get().left = E; + F->value.left = E; } else { - F->get().right = E; + F->value.right = E; } } } @@ -333,7 +333,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ /* CREATE MESHDATA */ //make a map of edges again - Map<Edge, RetFaceConnect> ret_edges; + HashMap<Edge, RetFaceConnect, Edge> ret_edges; List<Geometry3D::MeshData::Face> ret_faces; for (const Face &E : faces) { @@ -351,15 +351,15 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ uint32_t b = E.vertices[(i + 1) % 3]; Edge e(a, b); - Map<Edge, RetFaceConnect>::Element *G = ret_edges.find(e); + HashMap<Edge, RetFaceConnect, Edge>::Iterator G = ret_edges.find(e); if (!G) { G = ret_edges.insert(e, RetFaceConnect()); } if (e.vertices[0] == a) { //left - G->get().left = F; + G->value.left = F; } else { - G->get().right = F; + G->value.right = F; } } } @@ -374,17 +374,16 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ int b = E->get().indices[(i + 1) % f.indices.size()]; Edge e(a, b); - Map<Edge, RetFaceConnect>::Element *F = ret_edges.find(e); + HashMap<Edge, RetFaceConnect, Edge>::Iterator F = ret_edges.find(e); ERR_CONTINUE(!F); - List<Geometry3D::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left; + List<Geometry3D::MeshData::Face>::Element *O = F->value.left == E ? F->value.right : F->value.left; ERR_CONTINUE(O == E); ERR_CONTINUE(O == nullptr); if (O->get().plane.is_equal_approx(f.plane)) { //merge and delete edge and contiguous face, while repointing edges (uuugh!) int ois = O->get().indices.size(); - int merged = 0; for (int j = 0; j < ois; j++) { //search a @@ -399,17 +398,16 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ if (idx != a) { f.indices.insert(i + 1, idx); i++; - merged++; } Edge e2(idx, idxn); - Map<Edge, RetFaceConnect>::Element *F2 = ret_edges.find(e2); + HashMap<Edge, RetFaceConnect, Edge>::Iterator F2 = ret_edges.find(e2); ERR_CONTINUE(!F2); //change faceconnect, point to this face instead - if (F2->get().left == O) { - F2->get().left = E; - } else if (F2->get().right == O) { - F2->get().right = E; + if (F2->value.left == O) { + F2->value.left = E; + } else if (F2->value.right == O) { + F2->value.right = E; } } @@ -428,7 +426,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ } } - ret_edges.erase(F); //remove the edge + ret_edges.remove(F); //remove the edge ret_faces.erase(O); //remove the face } } diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index 48ea139cc9..6783743fc2 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,8 +33,8 @@ #include "core/math/aabb.h" #include "core/math/geometry_3d.h" +#include "core/templates/hash_set.h" #include "core/templates/list.h" -#include "core/templates/set.h" class QuickHull { public: @@ -44,9 +44,16 @@ public: uint64_t id = 0; }; + static uint32_t hash(const Edge &p_edge) { + return hash_one_uint64(p_edge.id); + } + bool operator<(const Edge &p_edge) const { return id < p_edge.id; } + bool operator==(const Edge &p_edge) const { + return id == p_edge.id; + } Edge(int p_vtx_a = 0, int p_vtx_b = 0) { if (p_vtx_a > p_vtx_b) { diff --git a/core/math/random_number_generator.cpp b/core/math/random_number_generator.cpp index b40d010219..31eeed4399 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/random_number_generator.h b/core/math/random_number_generator.h index 06cd3999f3..9352bae0a6 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/random_pcg.cpp b/core/math/random_pcg.cpp index 681c2a9717..c69986e6df 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/random_pcg.h b/core/math/random_pcg.h index 5a03b758ce..a088b30d17 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -61,8 +61,8 @@ static int __bsr_clz32(uint32_t x) { class RandomPCG { pcg32_random_t pcg; - uint64_t current_seed; // The seed the current generator state started from. - uint64_t current_inc; + uint64_t current_seed = 0; // The seed the current generator state started from. + uint64_t current_inc = 0; public: static const uint64_t DEFAULT_SEED = 12047754176567800795U; @@ -129,7 +129,7 @@ public: return p_mean + p_deviation * (cos(Math_TAU * randd()) * sqrt(-2.0 * log(randd()))); // Box-Muller transform } _FORCE_INLINE_ float randfn(float p_mean, float p_deviation) { - return p_mean + p_deviation * (cos(Math_TAU * randf()) * sqrt(-2.0 * log(randf()))); // Box-Muller transform + return p_mean + p_deviation * (cos((float)Math_TAU * randf()) * sqrt(-2.0 * log(randf()))); // Box-Muller transform } double random(double p_from, double p_to); diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index f64bf560c8..9e78ead816 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,13 +28,22 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "core/math/transform_2d.h" // Includes rect2.h but Rect2 needs Transform2D +#include "rect2.h" + +#include "core/math/rect2i.h" +#include "core/math/transform_2d.h" +#include "core/string/ustring.h" 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 { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif real_t min = 0, max = 1; int axis = 0; real_t sign = 0; @@ -95,6 +104,11 @@ bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 } bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif //SAT intersection between local and transformed rect2 Vector2 xf_points[4] = { @@ -187,33 +201,33 @@ next4: Vector2(position.x + size.x, position.y + size.y), }; - real_t maxa = p_xform.elements[0].dot(xf_points2[0]); + real_t maxa = p_xform.columns[0].dot(xf_points2[0]); real_t mina = maxa; - real_t dp = p_xform.elements[0].dot(xf_points2[1]); + real_t dp = p_xform.columns[0].dot(xf_points2[1]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - dp = p_xform.elements[0].dot(xf_points2[2]); + dp = p_xform.columns[0].dot(xf_points2[2]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - dp = p_xform.elements[0].dot(xf_points2[3]); + dp = p_xform.columns[0].dot(xf_points2[3]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - real_t maxb = p_xform.elements[0].dot(xf_points[0]); + real_t maxb = p_xform.columns[0].dot(xf_points[0]); real_t minb = maxb; - dp = p_xform.elements[0].dot(xf_points[1]); + dp = p_xform.columns[0].dot(xf_points[1]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); - dp = p_xform.elements[0].dot(xf_points[2]); + dp = p_xform.columns[0].dot(xf_points[2]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); - dp = p_xform.elements[0].dot(xf_points[3]); + dp = p_xform.columns[0].dot(xf_points[3]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); @@ -224,33 +238,33 @@ next4: return false; } - maxa = p_xform.elements[1].dot(xf_points2[0]); + maxa = p_xform.columns[1].dot(xf_points2[0]); mina = maxa; - dp = p_xform.elements[1].dot(xf_points2[1]); + dp = p_xform.columns[1].dot(xf_points2[1]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - dp = p_xform.elements[1].dot(xf_points2[2]); + dp = p_xform.columns[1].dot(xf_points2[2]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - dp = p_xform.elements[1].dot(xf_points2[3]); + dp = p_xform.columns[1].dot(xf_points2[3]); maxa = MAX(dp, maxa); mina = MIN(dp, mina); - maxb = p_xform.elements[1].dot(xf_points[0]); + maxb = p_xform.columns[1].dot(xf_points[0]); minb = maxb; - dp = p_xform.elements[1].dot(xf_points[1]); + dp = p_xform.columns[1].dot(xf_points[1]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); - dp = p_xform.elements[1].dot(xf_points[2]); + dp = p_xform.columns[1].dot(xf_points[2]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); - dp = p_xform.elements[1].dot(xf_points[3]); + dp = p_xform.columns[1].dot(xf_points[3]); maxb = MAX(dp, maxb); minb = MIN(dp, minb); @@ -268,6 +282,6 @@ Rect2::operator String() const { return "[P: " + position.operator String() + ", S: " + size + "]"; } -Rect2i::operator String() const { - return "[P: " + position.operator String() + ", S: " + size + "]"; +Rect2::operator Rect2i() const { + return Rect2i(position, size); } diff --git a/core/math/rect2.h b/core/math/rect2.h index 2557959fa2..679af933c2 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,11 +31,14 @@ #ifndef RECT2_H #define RECT2_H -#include "core/math/vector2.h" // also includes math_funcs and ustring +#include "core/error/error_macros.h" +#include "core/math/vector2.h" +class String; +struct Rect2i; struct Transform2D; -struct Rect2 { +struct _NO_DISCARD_ Rect2 { Point2 position; Size2 size; @@ -46,9 +49,14 @@ struct Rect2 { real_t get_area() const { return size.width * size.height; } - _FORCE_INLINE_ Vector2 get_center() const { return position + (size * 0.5); } + _FORCE_INLINE_ Vector2 get_center() const { return position + (size * 0.5f); } inline bool intersects(const Rect2 &p_rect, const bool p_include_borders = false) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif if (p_include_borders) { if (position.x > (p_rect.position.x + p_rect.size.width)) { return false; @@ -81,6 +89,11 @@ struct Rect2 { } inline real_t distance_to(const Vector2 &p_point) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif real_t dist = 0.0; bool inside = true; @@ -117,9 +130,14 @@ struct Rect2 { bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = nullptr, Point2 *r_normal = nullptr) const; inline bool encloses(const Rect2 &p_rect) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) && - ((p_rect.position.x + p_rect.size.x) <= (position.x + size.x)) && - ((p_rect.position.y + p_rect.size.y) <= (position.y + size.y)); + ((p_rect.position.x + p_rect.size.x) <= (position.x + size.x)) && + ((p_rect.position.y + p_rect.size.y) <= (position.y + size.y)); } _FORCE_INLINE_ bool has_no_area() const { @@ -147,7 +165,11 @@ struct Rect2 { } inline Rect2 merge(const Rect2 &p_rect) const { ///< return a merged rect - +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif Rect2 new_rect; new_rect.position.x = MIN(p_rect.position.x, position.x); @@ -160,7 +182,13 @@ struct Rect2 { return new_rect; } + inline bool has_point(const Point2 &p_point) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif if (p_point.x < position.x) { return false; } @@ -177,6 +205,7 @@ struct Rect2 { return true; } + bool is_equal_approx(const Rect2 &p_rect) const; bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; } @@ -225,7 +254,11 @@ struct Rect2 { } inline void expand_to(const Vector2 &p_vector) { //in place function for speed - +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); + } +#endif Vector2 begin = position; Vector2 end = position + size; @@ -252,12 +285,12 @@ struct Rect2 { } Vector2 get_support(const Vector2 &p_normal) const { - Vector2 half_extents = size * 0.5; + Vector2 half_extents = size * 0.5f; 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; + ofs; } _FORCE_INLINE_ bool intersects_filled_polygon(const Vector2 *p_points, int p_point_count) const { @@ -274,14 +307,14 @@ struct Rect2 { Vector2 r = (b - a); float l = r.length(); - if (l == 0.0) { + if (l == 0.0f) { continue; } //check inside Vector2 tg = r.orthogonal(); float s = tg.dot(center) - tg.dot(a); - if (s < 0.0) { + if (s < 0.0f) { side_plus++; } else { side_minus++; @@ -289,7 +322,7 @@ struct Rect2 { //check ray box r /= l; - Vector2 ir(1.0 / r.x, 1.0 / r.y); + Vector2 ir(1.0f / r.x, 1.0f / r.y); // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner // r.org is origin of ray @@ -323,6 +356,7 @@ struct Rect2 { } operator String() const; + operator Rect2i() const; Rect2() {} Rect2(real_t p_x, real_t p_y, real_t p_width, real_t p_height) : @@ -335,190 +369,4 @@ struct Rect2 { } }; -struct Rect2i { - Point2i position; - Size2i size; - - const Point2i &get_position() const { return position; } - void set_position(const Point2i &p_position) { position = p_position; } - const Size2i &get_size() const { return size; } - void set_size(const Size2i &p_size) { size = p_size; } - - int get_area() const { return size.width * size.height; } - - _FORCE_INLINE_ Vector2i get_center() const { return position + (size / 2); } - - inline bool intersects(const Rect2i &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; - } - - 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); - } - - // 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)) { - return Rect2i(); - } - - new_rect.position.x = MAX(p_rect.position.x, position.x); - new_rect.position.y = MAX(p_rect.position.y, position.y); - - 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); - - return new_rect; - } - - inline Rect2i merge(const Rect2i &p_rect) const { ///< return a merged rect - - Rect2i new_rect; - - new_rect.position.x = MIN(p_rect.position.x, position.x); - new_rect.position.y = MIN(p_rect.position.y, position.y); - - new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); - new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); - - new_rect.size = new_rect.size - new_rect.position; //make relative again - - return new_rect; - } - bool has_point(const Point2i &p_point) const { - if (p_point.x < position.x) { - return false; - } - if (p_point.y < position.y) { - return false; - } - - if (p_point.x >= (position.x + size.x)) { - return false; - } - if (p_point.y >= (position.y + size.y)) { - return false; - } - - return true; - } - - 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_amount) const { - Rect2i g = *this; - 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_side(Side p_side, int p_amount) const { - Rect2i g = *this; - 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_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; - g.size.width += p_left + p_right; - g.size.height += p_top + p_bottom; - - return g; - } - - _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) { - begin.x = p_vector.x; - } - if (p_vector.y < begin.y) { - begin.y = p_vector.y; - } - - if (p_vector.x > end.x) { - end.x = p_vector.x; - } - if (p_vector.y > end.y) { - end.y = p_vector.y; - } - - position = begin; - size = end - begin; - } - - _FORCE_INLINE_ Rect2i abs() const { - return Rect2i(Point2i(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); - } - - _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(int p_x, int p_y, int p_width, int p_height) : - position(Point2i(p_x, p_y)), - size(Size2i(p_width, p_height)) { - } - Rect2i(const Point2i &p_pos, const Size2i &p_size) : - position(p_pos), - size(p_size) { - } -}; - #endif // RECT2_H diff --git a/core/math/rect2i.cpp b/core/math/rect2i.cpp new file mode 100644 index 0000000000..0782c450d0 --- /dev/null +++ b/core/math/rect2i.cpp @@ -0,0 +1,42 @@ +/*************************************************************************/ +/* rect2i.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 "rect2i.h" + +#include "core/math/rect2.h" +#include "core/string/ustring.h" + +Rect2i::operator String() const { + return "[P: " + position.operator String() + ", S: " + size + "]"; +} + +Rect2i::operator Rect2() const { + return Rect2(position, size); +} diff --git a/core/math/rect2i.h b/core/math/rect2i.h new file mode 100644 index 0000000000..db1459a3e6 --- /dev/null +++ b/core/math/rect2i.h @@ -0,0 +1,245 @@ +/*************************************************************************/ +/* rect2i.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 RECT2I_H +#define RECT2I_H + +#include "core/error/error_macros.h" +#include "core/math/vector2i.h" + +class String; +struct Rect2; + +struct _NO_DISCARD_ Rect2i { + Point2i position; + Size2i size; + + const Point2i &get_position() const { return position; } + void set_position(const Point2i &p_position) { position = p_position; } + const Size2i &get_size() const { return size; } + void set_size(const Size2i &p_size) { size = p_size; } + + int get_area() const { return size.width * size.height; } + + _FORCE_INLINE_ Vector2i get_center() const { return position + (size / 2); } + + inline bool intersects(const Rect2i &p_rect) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size."); + } +#endif + 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 bool encloses(const Rect2i &p_rect) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size."); + } +#endif + 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); + } + + // 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)) { + return Rect2i(); + } + + new_rect.position.x = MAX(p_rect.position.x, position.x); + new_rect.position.y = MAX(p_rect.position.y, position.y); + + Point2i p_rect_end = p_rect.position + p_rect.size; + Point2i end = position + size; + + new_rect.size.x = MIN(p_rect_end.x, end.x) - new_rect.position.x; + new_rect.size.y = MIN(p_rect_end.y, end.y) - new_rect.position.y; + + return new_rect; + } + + inline Rect2i merge(const Rect2i &p_rect) const { ///< return a merged rect +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) { + ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size."); + } +#endif + Rect2i new_rect; + + new_rect.position.x = MIN(p_rect.position.x, position.x); + new_rect.position.y = MIN(p_rect.position.y, position.y); + + new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); + new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); + + new_rect.size = new_rect.size - new_rect.position; //make relative again + + return new_rect; + } + bool has_point(const Point2i &p_point) const { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size."); + } +#endif + if (p_point.x < position.x) { + return false; + } + if (p_point.y < position.y) { + return false; + } + + if (p_point.x >= (position.x + size.x)) { + return false; + } + if (p_point.y >= (position.y + size.y)) { + return false; + } + + return true; + } + + 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_amount) const { + Rect2i g = *this; + 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_side(Side p_side, int p_amount) const { + Rect2i g = *this; + 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_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; + g.size.width += p_left + p_right; + g.size.height += p_top + p_bottom; + + return g; + } + + _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) { +#ifdef MATH_CHECKS + if (unlikely(size.x < 0 || size.y < 0)) { + ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size."); + } +#endif + Point2i begin = position; + Point2i end = position + size; + + if (p_vector.x < begin.x) { + begin.x = p_vector.x; + } + if (p_vector.y < begin.y) { + begin.y = p_vector.y; + } + + if (p_vector.x > end.x) { + end.x = p_vector.x; + } + if (p_vector.y > end.y) { + end.y = p_vector.y; + } + + position = begin; + size = end - begin; + } + + _FORCE_INLINE_ Rect2i abs() const { + return Rect2i(Point2i(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); + } + + _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; + + Rect2i() {} + Rect2i(int p_x, int p_y, int p_width, int p_height) : + position(Point2i(p_x, p_y)), + size(Size2i(p_width, p_height)) { + } + Rect2i(const Point2i &p_pos, const Size2i &p_size) : + position(p_pos), + size(p_size) { + } +}; + +#endif // RECT2I_H diff --git a/core/math/static_raycaster.cpp b/core/math/static_raycaster.cpp index da05d49428..2510138d90 100644 --- a/core/math/static_raycaster.cpp +++ b/core/math/static_raycaster.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/core/math/static_raycaster.h b/core/math/static_raycaster.h index 3759c788a7..bc6511c073 100644 --- a/core/math/static_raycaster.h +++ b/core/math/static_raycaster.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -102,7 +102,7 @@ public: virtual void add_mesh(const PackedVector3Array &p_vertices, const PackedInt32Array &p_indices, unsigned int p_id) = 0; virtual void commit() = 0; - virtual void set_mesh_filter(const Set<int> &p_mesh_ids) = 0; + virtual void set_mesh_filter(const HashSet<int> &p_mesh_ids) = 0; virtual void clear_mesh_filter() = 0; static Ref<StaticRaycaster> create(); diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index 496a557844..226076029b 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,11 +30,13 @@ #include "transform_2d.h" +#include "core/string/ustring.h" + void Transform2D::invert() { // FIXME: this function assumes the basis is a rotation matrix, with no scaling. // Transform2D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that. - SWAP(elements[0][1], elements[1][0]); - elements[2] = basis_xform(-elements[2]); + SWAP(columns[0][1], columns[1][0]); + columns[2] = basis_xform(-columns[2]); } Transform2D Transform2D::inverse() const { @@ -48,13 +50,13 @@ void Transform2D::affine_invert() { #ifdef MATH_CHECKS ERR_FAIL_COND(det == 0); #endif - real_t idet = 1.0 / det; + real_t idet = 1.0f / det; - SWAP(elements[0][0], elements[1][1]); - elements[0] *= Vector2(idet, -idet); - elements[1] *= Vector2(-idet, idet); + SWAP(columns[0][0], columns[1][1]); + columns[0] *= Vector2(idet, -idet); + columns[1] *= Vector2(-idet, idet); - elements[2] = basis_xform(-elements[2]); + columns[2] = basis_xform(-columns[2]); } Transform2D Transform2D::affine_inverse() const { @@ -63,97 +65,97 @@ Transform2D Transform2D::affine_inverse() const { return inv; } -void Transform2D::rotate(const real_t p_phi) { - *this = Transform2D(p_phi, Vector2()) * (*this); +void Transform2D::rotate(const real_t p_angle) { + *this = Transform2D(p_angle, Vector2()) * (*this); } real_t Transform2D::get_skew() const { real_t det = basis_determinant(); - return Math::acos(elements[0].normalized().dot(SGN(det) * elements[1].normalized())) - Math_PI * 0.5; + return Math::acos(columns[0].normalized().dot(SIGN(det) * columns[1].normalized())) - (real_t)Math_PI * 0.5f; } void Transform2D::set_skew(const real_t p_angle) { real_t det = basis_determinant(); - elements[1] = SGN(det) * elements[0].rotated((Math_PI * 0.5 + p_angle)).normalized() * elements[1].length(); + columns[1] = SIGN(det) * columns[0].rotated(((real_t)Math_PI * 0.5f + p_angle)).normalized() * columns[1].length(); } real_t Transform2D::get_rotation() const { - return Math::atan2(elements[0].y, elements[0].x); + return Math::atan2(columns[0].y, columns[0].x); } void Transform2D::set_rotation(const real_t p_rot) { Size2 scale = get_scale(); real_t cr = Math::cos(p_rot); real_t sr = Math::sin(p_rot); - elements[0][0] = cr; - elements[0][1] = sr; - elements[1][0] = -sr; - elements[1][1] = cr; + columns[0][0] = cr; + columns[0][1] = sr; + columns[1][0] = -sr; + columns[1][1] = cr; set_scale(scale); } Transform2D::Transform2D(const 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; - elements[0][1] = sr; - elements[1][0] = -sr; - elements[1][1] = cr; - elements[2] = p_pos; + columns[0][0] = cr; + columns[0][1] = sr; + columns[1][0] = -sr; + columns[1][1] = cr; + columns[2] = p_pos; } Transform2D::Transform2D(const real_t p_rot, const Size2 &p_scale, const real_t p_skew, const Vector2 &p_pos) { - 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; - elements[2] = p_pos; + columns[0][0] = Math::cos(p_rot) * p_scale.x; + columns[1][1] = Math::cos(p_rot + p_skew) * p_scale.y; + columns[1][0] = -Math::sin(p_rot + p_skew) * p_scale.y; + columns[0][1] = Math::sin(p_rot) * p_scale.x; + columns[2] = p_pos; } Size2 Transform2D::get_scale() const { - real_t det_sign = SGN(basis_determinant()); - return Size2(elements[0].length(), det_sign * elements[1].length()); + real_t det_sign = SIGN(basis_determinant()); + return Size2(columns[0].length(), det_sign * columns[1].length()); } void Transform2D::set_scale(const Size2 &p_scale) { - elements[0].normalize(); - elements[1].normalize(); - elements[0] *= p_scale.x; - elements[1] *= p_scale.y; + columns[0].normalize(); + columns[1].normalize(); + columns[0] *= p_scale.x; + columns[1] *= p_scale.y; } void Transform2D::scale(const Size2 &p_scale) { scale_basis(p_scale); - elements[2] *= p_scale; + columns[2] *= 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; + columns[0][0] *= p_scale.x; + columns[0][1] *= p_scale.y; + columns[1][0] *= p_scale.x; + columns[1][1] *= p_scale.y; } -void Transform2D::translate(const real_t p_tx, const real_t p_ty) { - translate(Vector2(p_tx, p_ty)); +void Transform2D::translate_local(const real_t p_tx, const real_t p_ty) { + translate_local(Vector2(p_tx, p_ty)); } -void Transform2D::translate(const Vector2 &p_translation) { - elements[2] += basis_xform(p_translation); +void Transform2D::translate_local(const Vector2 &p_translation) { + columns[2] += basis_xform(p_translation); } void Transform2D::orthonormalize() { // Gram-Schmidt Process - Vector2 x = elements[0]; - Vector2 y = elements[1]; + Vector2 x = columns[0]; + Vector2 y = columns[1]; x.normalize(); y = (y - x * (x.dot(y))); y.normalize(); - elements[0] = x; - elements[1] = y; + columns[0] = x; + columns[1] = y; } Transform2D Transform2D::orthonormalized() const { @@ -163,7 +165,7 @@ Transform2D Transform2D::orthonormalized() const { } 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]); + return columns[0].is_equal_approx(p_transform.columns[0]) && columns[1].is_equal_approx(p_transform.columns[1]) && columns[2].is_equal_approx(p_transform.columns[2]); } Transform2D Transform2D::looking_at(const Vector2 &p_target) const { @@ -175,7 +177,7 @@ Transform2D Transform2D::looking_at(const Vector2 &p_target) const { bool Transform2D::operator==(const Transform2D &p_transform) const { for (int i = 0; i < 3; i++) { - if (elements[i] != p_transform.elements[i]) { + if (columns[i] != p_transform.columns[i]) { return false; } } @@ -185,7 +187,7 @@ bool Transform2D::operator==(const Transform2D &p_transform) const { bool Transform2D::operator!=(const Transform2D &p_transform) const { for (int i = 0; i < 3; i++) { - if (elements[i] != p_transform.elements[i]) { + if (columns[i] != p_transform.columns[i]) { return true; } } @@ -194,19 +196,19 @@ bool Transform2D::operator!=(const Transform2D &p_transform) const { } void Transform2D::operator*=(const Transform2D &p_transform) { - elements[2] = xform(p_transform.elements[2]); + columns[2] = xform(p_transform.columns[2]); real_t x0, x1, y0, y1; - x0 = tdotx(p_transform.elements[0]); - x1 = tdoty(p_transform.elements[0]); - y0 = tdotx(p_transform.elements[1]); - y1 = tdoty(p_transform.elements[1]); + x0 = tdotx(p_transform.columns[0]); + x1 = tdoty(p_transform.columns[0]); + y0 = tdotx(p_transform.columns[1]); + y1 = tdoty(p_transform.columns[1]); - elements[0][0] = x0; - elements[0][1] = x1; - elements[1][0] = y0; - elements[1][1] = y1; + columns[0][0] = x0; + columns[0][1] = x1; + columns[1][0] = y0; + columns[1][1] = y1; } Transform2D Transform2D::operator*(const Transform2D &p_transform) const { @@ -215,38 +217,52 @@ Transform2D Transform2D::operator*(const Transform2D &p_transform) const { return t; } -Transform2D Transform2D::scaled(const Size2 &p_scale) const { +Transform2D Transform2D::basis_scaled(const Size2 &p_scale) const { Transform2D copy = *this; - copy.scale(p_scale); + copy.scale_basis(p_scale); return copy; } -Transform2D Transform2D::basis_scaled(const Size2 &p_scale) const { +Transform2D Transform2D::scaled(const Size2 &p_scale) const { + // Equivalent to left multiplication Transform2D copy = *this; - copy.scale_basis(p_scale); + copy.scale(p_scale); return copy; } +Transform2D Transform2D::scaled_local(const Size2 &p_scale) const { + // Equivalent to right multiplication + return Transform2D(columns[0] * p_scale.x, columns[1] * p_scale.y, columns[2]); +} + Transform2D Transform2D::untranslated() const { Transform2D copy = *this; - copy.elements[2] = Vector2(); + copy.columns[2] = Vector2(); return copy; } Transform2D Transform2D::translated(const Vector2 &p_offset) const { - Transform2D copy = *this; - copy.translate(p_offset); - return copy; + // Equivalent to left multiplication + return Transform2D(columns[0], columns[1], columns[2] + p_offset); } -Transform2D Transform2D::rotated(const real_t p_phi) const { - Transform2D copy = *this; - copy.rotate(p_phi); - return copy; +Transform2D Transform2D::translated_local(const Vector2 &p_offset) const { + // Equivalent to right multiplication + return Transform2D(columns[0], columns[1], columns[2] + basis_xform(p_offset)); +} + +Transform2D Transform2D::rotated(const real_t p_angle) const { + // Equivalent to left multiplication + return Transform2D(p_angle, Vector2()) * (*this); +} + +Transform2D Transform2D::rotated_local(const real_t p_angle) const { + // Equivalent to right multiplication + return (*this) * Transform2D(p_angle, Vector2()); // Could be optimized, because origin transform can be skipped. } real_t Transform2D::basis_determinant() const { - return elements[0].x * elements[1].y - elements[0].y * elements[1].x; + return columns[0].x * columns[1].y - columns[0].y * columns[1].x; } Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const real_t p_c) const { @@ -266,11 +282,11 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const real_t dot = v1.dot(v2); - dot = CLAMP(dot, -1.0, 1.0); + dot = CLAMP(dot, -1.0f, 1.0f); Vector2 v; - if (dot > 0.9995) { + if (dot > 0.9995f) { v = v1.lerp(v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues } else { real_t angle = p_c * Math::acos(dot); @@ -285,9 +301,9 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const } void Transform2D::operator*=(const real_t p_val) { - elements[0] *= p_val; - elements[1] *= p_val; - elements[2] *= p_val; + columns[0] *= p_val; + columns[1] *= p_val; + columns[2] *= p_val; } Transform2D Transform2D::operator*(const real_t p_val) const { @@ -297,7 +313,7 @@ Transform2D Transform2D::operator*(const real_t p_val) const { } Transform2D::operator String() const { - return "[X: " + elements[0].operator String() + - ", Y: " + elements[1].operator String() + - ", O: " + elements[2].operator String() + "]"; + return "[X: " + columns[0].operator String() + + ", Y: " + columns[1].operator String() + + ", O: " + columns[2].operator String() + "]"; } diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index 6ed3af2ba7..f23f32867a 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,36 +31,32 @@ #ifndef TRANSFORM_2D_H #define TRANSFORM_2D_H -#include "core/math/rect2.h" // also includes vector2, math_funcs, and ustring +#include "core/math/math_funcs.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" +#include "core/templates/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": - // M = (elements[0][0] elements[1][0]) - // (elements[0][1] elements[1][1]) - // This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as elements[i]. - // Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to elements[1][0] here. +class String; + +struct _NO_DISCARD_ Transform2D { + // Warning #1: basis of Transform2D is stored differently from Basis. In terms of columns array, the basis matrix looks like "on paper": + // M = (columns[0][0] columns[1][0]) + // (columns[0][1] columns[1][1]) + // This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as columns[i]. + // Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to columns[1][0] here. // This requires additional care when working with explicit indices. // See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading. // Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down, // and angle is measure from +X to +Y in a clockwise-fashion. - Vector2 elements[3]; - - _FORCE_INLINE_ real_t tdotx(const Vector2 &v) const { return elements[0][0] * v.x + elements[1][0] * v.y; } - _FORCE_INLINE_ real_t tdoty(const Vector2 &v) const { return elements[0][1] * v.x + elements[1][1] * v.y; } + Vector2 columns[3]; - const Vector2 &operator[](int p_idx) const { return elements[p_idx]; } - Vector2 &operator[](int p_idx) { return elements[p_idx]; } + _FORCE_INLINE_ real_t tdotx(const Vector2 &v) const { return columns[0][0] * v.x + columns[1][0] * v.y; } + _FORCE_INLINE_ real_t tdoty(const Vector2 &v) const { return columns[0][1] * v.x + columns[1][1] * v.y; } - _FORCE_INLINE_ Vector2 get_axis(int p_axis) const { - ERR_FAIL_INDEX_V(p_axis, 3, Vector2()); - return elements[p_axis]; - } - _FORCE_INLINE_ void set_axis(int p_axis, const Vector2 &p_vec) { - ERR_FAIL_INDEX(p_axis, 3); - elements[p_axis] = p_vec; - } + const Vector2 &operator[](int p_idx) const { return columns[p_idx]; } + Vector2 &operator[](int p_idx) { return columns[p_idx]; } void invert(); Transform2D inverse() const; @@ -74,25 +70,28 @@ struct Transform2D { void set_skew(const real_t p_angle); _FORCE_INLINE_ void set_rotation_and_scale(const real_t p_rot, const Size2 &p_scale); _FORCE_INLINE_ void set_rotation_scale_and_skew(const real_t p_rot, const Size2 &p_scale, const real_t p_skew); - void rotate(const real_t p_phi); + void rotate(const real_t p_angle); void scale(const Size2 &p_scale); void scale_basis(const Size2 &p_scale); - void translate(const real_t p_tx, const real_t p_ty); - void translate(const Vector2 &p_translation); + void translate_local(const real_t p_tx, const real_t p_ty); + void translate_local(const Vector2 &p_translation); real_t basis_determinant() const; Size2 get_scale() const; void set_scale(const Size2 &p_scale); - _FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; } - _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; } + _FORCE_INLINE_ const Vector2 &get_origin() const { return columns[2]; } + _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { columns[2] = p_origin; } - Transform2D scaled(const Size2 &p_scale) const; Transform2D basis_scaled(const Size2 &p_scale) const; + Transform2D scaled(const Size2 &p_scale) const; + Transform2D scaled_local(const Size2 &p_scale) const; Transform2D translated(const Vector2 &p_offset) const; - Transform2D rotated(const real_t p_phi) const; + Transform2D translated_local(const Vector2 &p_offset) const; + Transform2D rotated(const real_t p_angle) const; + Transform2D rotated_local(const real_t p_angle) const; Transform2D untranslated() const; @@ -124,18 +123,18 @@ struct Transform2D { operator String() const; Transform2D(const real_t xx, const real_t xy, const real_t yx, const real_t yy, const real_t ox, const real_t oy) { - elements[0][0] = xx; - elements[0][1] = xy; - elements[1][0] = yx; - elements[1][1] = yy; - elements[2][0] = ox; - elements[2][1] = oy; + columns[0][0] = xx; + columns[0][1] = xy; + columns[1][0] = yx; + columns[1][1] = yy; + columns[2][0] = ox; + columns[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; + columns[0] = p_x; + columns[1] = p_y; + columns[2] = p_origin; } Transform2D(const real_t p_rot, const Vector2 &p_pos); @@ -143,8 +142,8 @@ struct Transform2D { Transform2D(const real_t p_rot, const Size2 &p_scale, const real_t p_skew, const Vector2 &p_pos); Transform2D() { - elements[0][0] = 1.0; - elements[1][1] = 1.0; + columns[0][0] = 1.0; + columns[1][1] = 1.0; } }; @@ -156,28 +155,28 @@ Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const { Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const { return Vector2( - elements[0].dot(p_vec), - elements[1].dot(p_vec)); + columns[0].dot(p_vec), + columns[1].dot(p_vec)); } Vector2 Transform2D::xform(const Vector2 &p_vec) const { return Vector2( tdotx(p_vec), tdoty(p_vec)) + - elements[2]; + columns[2]; } Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const { - Vector2 v = p_vec - elements[2]; + Vector2 v = p_vec - columns[2]; return Vector2( - elements[0].dot(v), - elements[1].dot(v)); + columns[0].dot(v), + columns[1].dot(v)); } 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 x = columns[0] * p_rect.size.x; + Vector2 y = columns[1] * p_rect.size.y; Vector2 pos = xform(p_rect.position); Rect2 new_rect; @@ -189,17 +188,17 @@ Rect2 Transform2D::xform(const Rect2 &p_rect) const { } void Transform2D::set_rotation_and_scale(const 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; + columns[0][0] = Math::cos(p_rot) * p_scale.x; + columns[1][1] = Math::cos(p_rot) * p_scale.y; + columns[1][0] = -Math::sin(p_rot) * p_scale.y; + columns[0][1] = Math::sin(p_rot) * p_scale.x; } void Transform2D::set_rotation_scale_and_skew(const real_t p_rot, const Size2 &p_scale, const real_t 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; + columns[0][0] = Math::cos(p_rot) * p_scale.x; + columns[1][1] = Math::cos(p_rot + p_skew) * p_scale.y; + columns[1][0] = -Math::sin(p_rot + p_skew) * p_scale.y; + columns[0][1] = Math::sin(p_rot) * p_scale.x; } Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index 4f4943c8ef..a634faca9a 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -57,32 +57,48 @@ Transform3D Transform3D::inverse() const { return ret; } -void Transform3D::rotate(const Vector3 &p_axis, real_t p_phi) { - *this = rotated(p_axis, p_phi); +void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) { + *this = rotated(p_axis, p_angle); } -Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_phi) const { - return Transform3D(Basis(p_axis, p_phi), Vector3()) * (*this); +Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const { + // Equivalent to left multiplication + Basis p_basis(p_axis, p_angle); + return Transform3D(p_basis * basis, p_basis.xform(origin)); } -void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_phi) { - basis.rotate(p_axis, p_phi); +Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) const { + // Equivalent to right multiplication + Basis p_basis(p_axis, p_angle); + return Transform3D(basis * p_basis, origin); +} + +void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) { + basis.rotate(p_axis, p_angle); } Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(origin.is_equal_approx(p_target), Transform3D(), "The transform's origin and target can't be equal."); +#endif Transform3D t = *this; t.basis = Basis::looking_at(p_target - origin, p_up); return t; } void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal."); +#endif basis = Basis::looking_at(p_target - p_eye, p_up); origin = p_eye; } -Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { +Transform3D Transform3D::spherical_interpolate_with(const Transform3D &p_transform, real_t p_c) const { /* not sure if very "efficient" but good enough? */ + Transform3D interp; + Vector3 src_scale = basis.get_scale(); Quaternion src_rot = basis.get_rotation_quaternion(); Vector3 src_loc = origin; @@ -91,42 +107,58 @@ Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t Quaternion dst_rot = p_transform.basis.get_rotation_quaternion(); Vector3 dst_loc = p_transform.origin; - 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; } +Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { + Transform3D interp; + + interp.basis = basis.lerp(p_transform.basis, p_c); + interp.origin = origin.lerp(p_transform.origin, p_c); + + return interp; +} + void Transform3D::scale(const Vector3 &p_scale) { basis.scale(p_scale); origin *= p_scale; } Transform3D Transform3D::scaled(const Vector3 &p_scale) const { - Transform3D t = *this; - t.scale(p_scale); - return t; + // Equivalent to left multiplication + return Transform3D(basis.scaled(p_scale), origin * p_scale); +} + +Transform3D Transform3D::scaled_local(const Vector3 &p_scale) const { + // Equivalent to right multiplication + return Transform3D(basis.scaled_local(p_scale), origin); } void Transform3D::scale_basis(const Vector3 &p_scale) { basis.scale(p_scale); } -void Transform3D::translate(real_t p_tx, real_t p_ty, real_t p_tz) { - translate(Vector3(p_tx, p_ty, p_tz)); +void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) { + translate_local(Vector3(p_tx, p_ty, p_tz)); } -void Transform3D::translate(const Vector3 &p_translation) { +void Transform3D::translate_local(const Vector3 &p_translation) { for (int i = 0; i < 3; i++) { origin[i] += basis[i].dot(p_translation); } } Transform3D Transform3D::translated(const Vector3 &p_translation) const { - Transform3D t = *this; - t.translate(p_translation); - return t; + // Equivalent to left multiplication + return Transform3D(basis, origin + p_translation); +} + +Transform3D Transform3D::translated_local(const Vector3 &p_translation) const { + // Equivalent to right multiplication + return Transform3D(basis, origin + basis.xform(p_translation)); } void Transform3D::orthonormalize() { @@ -139,6 +171,16 @@ Transform3D Transform3D::orthonormalized() const { return _copy; } +void Transform3D::orthogonalize() { + basis.orthogonalize(); +} + +Transform3D Transform3D::orthogonalized() const { + Transform3D _copy = *this; + _copy.orthogonalize(); + return _copy; +} + 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); } @@ -174,10 +216,10 @@ Transform3D Transform3D::operator*(const real_t p_val) const { } 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() + "]"; + return "[X: " + basis.get_column(0).operator String() + + ", Y: " + basis.get_column(1).operator String() + + ", Z: " + basis.get_column(2).operator String() + + ", O: " + origin.operator String() + "]"; } Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) : @@ -187,9 +229,9 @@ Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) : 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); + basis.set_column(0, p_x); + basis.set_column(1, p_y); + basis.set_column(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) { diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h index 345e0fade0..b572e90859 100644 --- a/core/math/transform_3d.h +++ b/core/math/transform_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,15 +28,14 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef TRANSFORM_H -#define TRANSFORM_H +#ifndef TRANSFORM_3D_H +#define TRANSFORM_3D_H #include "core/math/aabb.h" #include "core/math/basis.h" #include "core/math/plane.h" -class Transform3D { -public: +struct _NO_DISCARD_ Transform3D { Basis basis; Vector3 origin; @@ -46,20 +45,23 @@ public: void affine_invert(); Transform3D affine_inverse() const; - Transform3D rotated(const Vector3 &p_axis, real_t p_phi) const; + Transform3D rotated(const Vector3 &p_axis, real_t p_angle) const; + Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const; - void rotate(const Vector3 &p_axis, real_t p_phi); - void rotate_basis(const Vector3 &p_axis, real_t p_phi); + void rotate(const Vector3 &p_axis, real_t p_angle); + void rotate_basis(const Vector3 &p_axis, real_t p_angle); 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); Transform3D scaled(const Vector3 &p_scale) const; + Transform3D scaled_local(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); + void translate_local(real_t p_tx, real_t p_ty, real_t p_tz); + void translate_local(const Vector3 &p_translation); Transform3D translated(const Vector3 &p_translation) const; + Transform3D translated_local(const Vector3 &p_translation) const; const Basis &get_basis() const { return basis; } void set_basis(const Basis &p_basis) { basis = p_basis; } @@ -69,6 +71,8 @@ public: void orthonormalize(); Transform3D orthonormalized() const; + void orthogonalize(); + Transform3D orthogonalized() const; bool is_equal_approx(const Transform3D &p_transform) const; bool operator==(const Transform3D &p_transform) const; @@ -99,6 +103,7 @@ public: void operator*=(const real_t p_val); Transform3D operator*(const real_t p_val) const; + Transform3D spherical_interpolate_with(const Transform3D &p_transform, real_t p_c) const; Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const; _FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const { @@ -133,9 +138,9 @@ _FORCE_INLINE_ Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const { Vector3 v = p_vector - origin; return Vector3( - (basis.elements[0][0] * v.x) + (basis.elements[1][0] * v.y) + (basis.elements[2][0] * v.z), - (basis.elements[0][1] * v.x) + (basis.elements[1][1] * v.y) + (basis.elements[2][1] * v.z), - (basis.elements[0][2] * v.x) + (basis.elements[1][2] * v.y) + (basis.elements[2][2] * v.z)); + (basis.rows[0][0] * v.x) + (basis.rows[1][0] * v.y) + (basis.rows[2][0] * v.z), + (basis.rows[0][1] * v.x) + (basis.rows[1][1] * v.y) + (basis.rows[2][1] * v.z), + (basis.rows[0][2] * v.x) + (basis.rows[1][2] * v.y) + (basis.rows[2][2] * v.z)); } // Neither the plane regular xform or xform_inv are particularly efficient, @@ -262,4 +267,4 @@ _FORCE_INLINE_ Plane Transform3D::xform_inv_fast(const Plane &p_plane, const Tra return Plane(normal, d); } -#endif // TRANSFORM_H +#endif // TRANSFORM_3D_H diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 2f3da0b6a8..4433559e6d 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -104,9 +104,11 @@ void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const { } } -void TriangleMesh::create(const Vector<Vector3> &p_faces) { +void TriangleMesh::create(const Vector<Vector3> &p_faces, const Vector<int32_t> &p_surface_indices) { valid = false; + ERR_FAIL_COND(p_surface_indices.size() && p_surface_indices.size() != p_faces.size()); + int fc = p_faces.size(); ERR_FAIL_COND(!fc || ((fc % 3) != 0)); fc /= 3; @@ -121,8 +123,9 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { //goes in-place. const Vector3 *r = p_faces.ptr(); + const int32_t *si = p_surface_indices.ptr(); Triangle *w = triangles.ptrw(); - Map<Vector3, int> db; + HashMap<Vector3, int> db; for (int i = 0; i < fc; i++) { Triangle &f = w[i]; @@ -131,9 +134,9 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { 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); + HashMap<Vector3, int>::Iterator E = db.find(vs); if (E) { - vidx = E->get(); + vidx = E->value; } else { vidx = db.size(); db[vs] = vidx; @@ -148,6 +151,7 @@ void TriangleMesh::create(const Vector<Vector3> &p_faces) { } f.normal = Face3(r[i * 3 + 0], r[i * 3 + 1], r[i * 3 + 2]).get_plane().get_normal(); + f.surface_index = si ? si[i] : 0; bw[i].left = -1; bw[i].right = -1; @@ -231,14 +235,14 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { } case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; level++; + stack[level] = b.left | TEST_AABB_BIT; continue; } case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; level++; + stack[level] = b.right | TEST_AABB_BIT; continue; } case VISIT_DONE_BIT: { @@ -264,7 +268,7 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { return n; } -bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { +bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -317,6 +321,9 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en d = nd; r_point = res; r_normal = f3.get_plane().get_normal(); + if (r_surf_index) { + *r_surf_index = s.surface_index; + } inters = true; } } @@ -331,14 +338,14 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en } case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; level++; + stack[level] = b.left | TEST_AABB_BIT; continue; } case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; level++; + stack[level] = b.right | TEST_AABB_BIT; continue; } case VISIT_DONE_BIT: { @@ -366,7 +373,7 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en return inters; } -bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const { +bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { @@ -417,6 +424,9 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V d = nd; r_point = res; r_normal = f3.get_plane().get_normal(); + if (r_surf_index) { + *r_surf_index = s.surface_index; + } inters = true; } } @@ -431,14 +441,14 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V } case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; level++; + stack[level] = b.left | TEST_AABB_BIT; continue; } case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; level++; + stack[level] = b.right | TEST_AABB_BIT; continue; } case VISIT_DONE_BIT: { @@ -551,14 +561,14 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou } case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; level++; + stack[level] = b.left | TEST_AABB_BIT; continue; } case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; level++; + stack[level] = b.right | TEST_AABB_BIT; continue; } case VISIT_DONE_BIT: { @@ -644,14 +654,14 @@ bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, } case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; level++; + stack[level] = b.left | TEST_AABB_BIT; continue; } case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; level++; + stack[level] = b.right | TEST_AABB_BIT; continue; } case VISIT_DONE_BIT: { diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 2d3b4db4bb..166b4adb7a 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-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,6 +41,7 @@ public: struct Triangle { Vector3 normal; int indices[3]; + int32_t surface_index; }; private: @@ -81,8 +82,8 @@ private: 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_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index = nullptr) const; + bool intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index = nullptr) 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; @@ -92,7 +93,7 @@ public: const Vector<Vector3> &get_vertices() const { return vertices; } void get_indices(Vector<int> *r_triangles_indices) const; - void create(const Vector<Vector3> &p_faces); + void create(const Vector<Vector3> &p_faces, const Vector<int32_t> &p_surface_indices = Vector<int32_t>()); TriangleMesh(); }; diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index fa1588dbc5..0a9872ae08 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,21 +39,16 @@ real_t Triangulate::get_area(const Vector<Vector2> &contour) { for (int p = n - 1, q = 0; q < n; p = q++) { A += c[p].cross(c[q]); } - return A * 0.5; + return A * 0.5f; } -/* - is_inside_triangle decides if a point P is Inside of the triangle - defined by A, B, C. - */ - +/* `is_inside_triangle` decides if a point P is inside the triangle + * defined by A, B, C. */ bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, real_t Bx, real_t By, real_t Cx, real_t Cy, real_t Px, real_t Py, - bool include_edges) - -{ + bool include_edges) { real_t ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; real_t cCROSSap, bCROSScp, aCROSSbp; @@ -75,9 +70,9 @@ bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, bCROSScp = bx * cpy - by * cpx; if (include_edges) { - return ((aCROSSbp > 0.0) && (bCROSScp > 0.0) && (cCROSSap > 0.0)); + return ((aCROSSbp > 0.0f) && (bCROSScp > 0.0f) && (cCROSSap > 0.0f)); } else { - return ((aCROSSbp >= 0.0) && (bCROSScp >= 0.0) && (cCROSSap >= 0.0)); + return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); } } @@ -133,7 +128,7 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul /* we want a counter-clockwise polygon in V */ - if (0.0 < get_area(contour)) { + if (0.0f < get_area(contour)) { for (int v = 0; v < n; v++) { V.write[v] = v; } diff --git a/core/math/triangulate.h b/core/math/triangulate.h index 249ca6238f..0bfcfcb978 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,6 +32,7 @@ #define TRIANGULATE_H #include "core/math/vector2.h" +#include "core/templates/vector.h" /* https://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index 16e43d7d06..d9b5d55454 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,6 +30,9 @@ #include "vector2.h" +#include "core/math/vector2i.h" +#include "core/string/ustring.h" + real_t Vector2::angle() const { return Math::atan2(y, x); } @@ -79,7 +82,7 @@ real_t Vector2::angle_to(const Vector2 &p_vector2) const { } real_t Vector2::angle_to_point(const Vector2 &p_vector2) const { - return (*this - p_vector2).angle(); + return (p_vector2 - *this).angle(); } real_t Vector2::dot(const Vector2 &p_other) const { @@ -91,7 +94,7 @@ real_t Vector2::cross(const Vector2 &p_other) const { } Vector2 Vector2::sign() const { - return Vector2(SGN(x), SGN(y)); + return Vector2(SIGN(x), SIGN(y)); } Vector2 Vector2::floor() const { @@ -149,29 +152,11 @@ Vector2 Vector2::limit_length(const 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, const 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_weight; - real_t t2 = t * t; - real_t t3 = t2 * t; - - Vector2 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; -} - Vector2 Vector2::move_toward(const Vector2 &p_to, const real_t p_delta) const { Vector2 v = *this; Vector2 vd = p_to - v; real_t len = vd.length(); - return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta; + return len <= p_delta || len < (real_t)CMP_EPSILON ? p_to : v + vd / len * p_delta; } // slide returns the component of the vector along the given plane, specified by its normal vector. @@ -190,7 +175,7 @@ Vector2 Vector2::reflect(const Vector2 &p_normal) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector2(), "The normal Vector2 must be normalized."); #endif - return 2.0 * p_normal * this->dot(p_normal) - *this; + return 2.0f * p_normal * this->dot(p_normal) - *this; } bool Vector2::is_equal_approx(const Vector2 &p_v) const { @@ -201,83 +186,6 @@ Vector2::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ")"; } -/* Vector2i */ - -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) { - x += p_v.x; - y += p_v.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) { - 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 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 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 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 { - return x != p_vec2.x || y != p_vec2.y; -} - -Vector2i::operator String() const { - return "(" + itos(x) + ", " + itos(y) + ")"; +Vector2::operator Vector2i() const { + return Vector2i(x, y); } diff --git a/core/math/vector2.h b/core/math/vector2.h index 332c0475fa..caa6b226e7 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 VECTOR2_H #define VECTOR2_H +#include "core/error/error_macros.h" #include "core/math/math_funcs.h" -#include "core/string/ustring.h" +class String; struct Vector2i; -struct Vector2 { +struct _NO_DISCARD_ Vector2 { static const int AXIS_COUNT = 2; enum Axis { @@ -60,22 +61,24 @@ struct Vector2 { }; _FORCE_INLINE_ real_t &operator[](int p_idx) { - return p_idx ? y : x; + DEV_ASSERT((unsigned int)p_idx < 2); + return coord[p_idx]; } _FORCE_INLINE_ const real_t &operator[](int p_idx) const { - return p_idx ? y : x; + DEV_ASSERT((unsigned int)p_idx < 2); + return coord[p_idx]; } _FORCE_INLINE_ void set_all(const real_t p_value) { x = y = p_value; } - _FORCE_INLINE_ int min_axis() const { - return x < y ? 0 : 1; + _FORCE_INLINE_ Vector2::Axis min_axis_index() const { + return x < y ? Vector2::AXIS_X : Vector2::AXIS_Y; } - _FORCE_INLINE_ int max_axis() const { - return x < y ? 1 : 0; + _FORCE_INLINE_ Vector2::Axis max_axis_index() const { + return x < y ? Vector2::AXIS_Y : Vector2::AXIS_X; } void normalize(); @@ -110,7 +113,10 @@ struct Vector2 { _FORCE_INLINE_ Vector2 lerp(const Vector2 &p_to, const real_t p_weight) const; _FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, const real_t p_weight) const; - Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const; + _FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const; + _FORCE_INLINE_ Vector2 cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; + _FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const; + Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const; Vector2 slide(const Vector2 &p_normal) const; @@ -167,6 +173,7 @@ struct Vector2 { real_t aspect() const { return width / height; } operator String() const; + operator Vector2i() const; _FORCE_INLINE_ Vector2() {} _FORCE_INLINE_ Vector2(const real_t p_x, const real_t p_y) { @@ -179,22 +186,6 @@ _FORCE_INLINE_ Vector2 Vector2::plane_project(const real_t p_d, const Vector2 &p return p_vec - *this * (dot(p_vec) - p_d); } -_FORCE_INLINE_ Vector2 operator*(const float p_scalar, const Vector2 &p_vec) { - return p_vec * p_scalar; -} - -_FORCE_INLINE_ Vector2 operator*(const double p_scalar, const Vector2 &p_vec) { - return p_vec * p_scalar; -} - -_FORCE_INLINE_ Vector2 operator*(const int32_t p_scalar, const Vector2 &p_vec) { - return p_vec * p_scalar; -} - -_FORCE_INLINE_ Vector2 operator*(const 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); } @@ -261,126 +252,71 @@ Vector2 Vector2::lerp(const Vector2 &p_to, const real_t p_weight) const { } Vector2 Vector2::slerp(const Vector2 &p_to, const 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_to); - return rotated(theta * p_weight); + real_t start_length_sq = length_squared(); + real_t end_length_sq = p_to.length_squared(); + if (unlikely(start_length_sq == 0.0f || end_length_sq == 0.0f)) { + // Zero length vectors have no angle, so the best we can do is either lerp or throw an error. + return lerp(p_to, p_weight); + } + real_t start_length = Math::sqrt(start_length_sq); + real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight); + real_t angle = angle_to(p_to); + return rotated(angle * p_weight) * (result_length / start_length); } -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::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const { + Vector2 res = *this; + res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight); + res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight); + return res; } -typedef Vector2 Size2; -typedef Vector2 Point2; - -/* INTEGER STUFF */ - -struct Vector2i { - enum Axis { - AXIS_X, - AXIS_Y, - }; - - union { - int32_t x = 0; - int32_t width; - }; - union { - int32_t y = 0; - int32_t height; - }; - - _FORCE_INLINE_ int32_t &operator[](int p_idx) { - return p_idx ? y : x; - } - _FORCE_INLINE_ const int32_t &operator[](int p_idx) const { - return p_idx ? y : x; - } - - _FORCE_INLINE_ int min_axis() const { - return x < y ? 0 : 1; - } - - _FORCE_INLINE_ int max_axis() const { - return x < y ? 1 : 0; - } - - 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 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 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); } +Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector2 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} - bool operator==(const Vector2i &p_vec2) const; - bool operator!=(const Vector2i &p_vec2) const; +Vector2 Vector2::bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const { + Vector2 res = *this; - 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; + /* Formula from Wikipedia article on Bezier curves. */ + real_t omt = (1.0 - p_t); + real_t omt2 = omt * omt; + real_t omt3 = omt2 * omt; + real_t t2 = p_t * p_t; + real_t t3 = t2 * p_t; - operator String() const; + return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; +} - operator Vector2() const { return Vector2(x, y); } +Vector2 Vector2::direction_to(const Vector2 &p_to) const { + Vector2 ret(p_to.x - x, p_to.y - y); + ret.normalize(); + return ret; +} - inline Vector2i() {} - inline Vector2i(const Vector2 &p_vec2) { - x = (int32_t)p_vec2.x; - y = (int32_t)p_vec2.y; - } - inline Vector2i(const int32_t p_x, const int32_t p_y) { - x = p_x; - y = p_y; - } -}; +// Multiplication operators required to workaround issues with LLVM using implicit conversion +// to Vector2i instead for integers where it should not. -_FORCE_INLINE_ Vector2i operator*(const int32_t &p_scalar, const Vector2i &p_vector) { - return p_vector * p_scalar; +_FORCE_INLINE_ Vector2 operator*(const float p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; } -_FORCE_INLINE_ Vector2i operator*(const int64_t &p_scalar, const Vector2i &p_vector) { - return p_vector * p_scalar; +_FORCE_INLINE_ Vector2 operator*(const double p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; } -_FORCE_INLINE_ Vector2i operator*(const float &p_scalar, const Vector2i &p_vector) { - return p_vector * p_scalar; +_FORCE_INLINE_ Vector2 operator*(const int32_t p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; } -_FORCE_INLINE_ Vector2i operator*(const double &p_scalar, const Vector2i &p_vector) { - return p_vector * p_scalar; +_FORCE_INLINE_ Vector2 operator*(const int64_t p_scalar, const Vector2 &p_vec) { + return p_vec * p_scalar; } -typedef Vector2i Size2i; -typedef Vector2i Point2i; +typedef Vector2 Size2; +typedef Vector2 Point2; #endif // VECTOR2_H diff --git a/core/math/vector2i.cpp b/core/math/vector2i.cpp new file mode 100644 index 0000000000..dfed42e4d6 --- /dev/null +++ b/core/math/vector2i.cpp @@ -0,0 +1,125 @@ +/*************************************************************************/ +/* vector2i.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 "vector2i.h" + +#include "core/math/vector2.h" +#include "core/string/ustring.h" + +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)); +} + +int64_t Vector2i::length_squared() const { + return x * (int64_t)x + y * (int64_t)y; +} + +double Vector2i::length() const { + return Math::sqrt((double)length_squared()); +} + +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) { + x += p_v.x; + y += p_v.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) { + 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 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 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 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 { + return x != p_vec2.x || y != p_vec2.y; +} + +Vector2i::operator String() const { + return "(" + itos(x) + ", " + itos(y) + ")"; +} + +Vector2i::operator Vector2() const { + return Vector2((int32_t)x, (int32_t)y); +} diff --git a/core/math/vector2i.h b/core/math/vector2i.h new file mode 100644 index 0000000000..13b70031bd --- /dev/null +++ b/core/math/vector2i.h @@ -0,0 +1,152 @@ +/*************************************************************************/ +/* vector2i.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 VECTOR2I_H +#define VECTOR2I_H + +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" + +class String; +struct Vector2; + +struct _NO_DISCARD_ Vector2i { + enum Axis { + AXIS_X, + AXIS_Y, + }; + + union { + struct { + union { + int32_t x; + int32_t width; + }; + union { + int32_t y; + int32_t height; + }; + }; + + int32_t coord[2] = { 0 }; + }; + + _FORCE_INLINE_ int32_t &operator[](int p_idx) { + DEV_ASSERT((unsigned int)p_idx < 2); + return coord[p_idx]; + } + _FORCE_INLINE_ const int32_t &operator[](int p_idx) const { + DEV_ASSERT((unsigned int)p_idx < 2); + return coord[p_idx]; + } + + _FORCE_INLINE_ Vector2i::Axis min_axis_index() const { + return x < y ? Vector2i::AXIS_X : Vector2i::AXIS_Y; + } + + _FORCE_INLINE_ Vector2i::Axis max_axis_index() const { + return x < y ? Vector2i::AXIS_Y : Vector2i::AXIS_X; + } + + Vector2i min(const Vector2i &p_vector2i) const { + return Vector2i(MIN(x, p_vector2i.x), MIN(y, p_vector2i.y)); + } + + Vector2i max(const Vector2i &p_vector2i) const { + return Vector2i(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 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 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; + + int64_t length_squared() const; + double length() const; + + real_t aspect() const { return width / (real_t)height; } + Vector2i sign() const { return Vector2i(SIGN(x), SIGN(y)); } + Vector2i abs() const { return Vector2i(ABS(x), ABS(y)); } + Vector2i clamp(const Vector2i &p_min, const Vector2i &p_max) const; + + operator String() const; + operator Vector2() const; + + inline Vector2i() {} + inline Vector2i(const int32_t p_x, const int32_t p_y) { + x = p_x; + y = p_y; + } +}; + +// Multiplication operators required to workaround issues with LLVM using implicit conversion. + +_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; + +#endif // VECTOR2I_H diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index fa212c178a..fdbbb8cb5c 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,17 @@ #include "vector3.h" #include "core/math/basis.h" +#include "core/math/vector2.h" +#include "core/math/vector3i.h" +#include "core/string/ustring.h" -void Vector3::rotate(const Vector3 &p_axis, const real_t p_phi) { - *this = Basis(p_axis, p_phi).xform(*this); +void Vector3::rotate(const Vector3 &p_axis, const real_t p_angle) { + *this = Basis(p_axis, p_angle).xform(*this); } -Vector3 Vector3::rotated(const Vector3 &p_axis, const real_t p_phi) const { +Vector3 Vector3::rotated(const Vector3 &p_axis, const real_t p_angle) const { Vector3 r = *this; - r.rotate(p_axis, p_phi); + r.rotate(p_axis, p_angle); return r; } @@ -82,35 +85,58 @@ Vector3 Vector3::limit_length(const real_t p_len) const { return v; } -Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const 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_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.0 * p2 - p3) * t2 + - (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3); - return out; -} - Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const { Vector3 v = *this; Vector3 vd = p_to - v; real_t len = vd.length(); - return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta; + return len <= p_delta || len < (real_t)CMP_EPSILON ? p_to : v + vd / len * p_delta; } -Basis Vector3::outer(const Vector3 &p_b) const { - Vector3 row0(x * p_b.x, x * p_b.y, x * p_b.z); - Vector3 row1(y * p_b.x, y * p_b.y, y * p_b.z); - Vector3 row2(z * p_b.x, z * p_b.y, z * p_b.z); +Vector2 Vector3::octahedron_encode() const { + Vector3 n = *this; + n /= Math::abs(n.x) + Math::abs(n.y) + Math::abs(n.z); + Vector2 o; + if (n.z >= 0.0f) { + o.x = n.x; + o.y = n.y; + } else { + o.x = (1.0f - Math::abs(n.y)) * (n.x >= 0.0f ? 1.0f : -1.0f); + o.y = (1.0f - Math::abs(n.x)) * (n.y >= 0.0f ? 1.0f : -1.0f); + } + o.x = o.x * 0.5f + 0.5f; + o.y = o.y * 0.5f + 0.5f; + return o; +} + +Vector3 Vector3::octahedron_decode(const Vector2 &p_oct) { + Vector2 f(p_oct.x * 2.0f - 1.0f, p_oct.y * 2.0f - 1.0f); + Vector3 n(f.x, f.y, 1.0f - Math::abs(f.x) - Math::abs(f.y)); + float t = CLAMP(-n.z, 0.0f, 1.0f); + n.x += n.x >= 0 ? -t : t; + n.y += n.y >= 0 ? -t : t; + return n.normalized(); +} + +Vector2 Vector3::octahedron_tangent_encode(const float sign) const { + Vector2 res = this->octahedron_encode(); + res.y = res.y * 0.5f + 0.5f; + res.y = sign >= 0.0f ? res.y : 1 - res.y; + return res; +} + +Vector3 Vector3::octahedron_tangent_decode(const Vector2 &p_oct, float *sign) { + Vector2 oct_compressed = p_oct; + oct_compressed.y = oct_compressed.y * 2 - 1; + *sign = oct_compressed.y >= 0.0f ? 1.0f : -1.0f; + oct_compressed.y = Math::abs(oct_compressed.y); + Vector3 res = Vector3::octahedron_decode(oct_compressed); + return res; +} + +Basis Vector3::outer(const Vector3 &p_with) const { + Vector3 row0(x * p_with.x, x * p_with.y, x * p_with.z); + Vector3 row1(y * p_with.x, y * p_with.y, y * p_with.z); + Vector3 row2(z * p_with.x, z * p_with.y, z * p_with.z); return Basis(row0, row1, row2); } @@ -122,3 +148,7 @@ bool Vector3::is_equal_approx(const Vector3 &p_v) const { Vector3::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")"; } + +Vector3::operator Vector3i() const { + return Vector3i(x, y, z); +} diff --git a/core/math/vector3.h b/core/math/vector3.h index dc9aa60458..7cae6e2481 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,13 +31,15 @@ #ifndef VECTOR3_H #define VECTOR3_H +#include "core/error/error_macros.h" #include "core/math/math_funcs.h" -#include "core/math/vector2.h" -#include "core/math/vector3i.h" -#include "core/string/ustring.h" -class Basis; -struct Vector3 { +class String; +struct Basis; +struct Vector2; +struct Vector3i; + +struct _NO_DISCARD_ Vector3 { static const int AXIS_COUNT = 3; enum Axis { @@ -57,10 +59,12 @@ struct Vector3 { }; _FORCE_INLINE_ const real_t &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 3); return coord[p_axis]; } _FORCE_INLINE_ real_t &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 3); return coord[p_axis]; } @@ -71,12 +75,12 @@ struct Vector3 { x = y = z = p_value; } - _FORCE_INLINE_ int min_axis() const { - return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); + _FORCE_INLINE_ Vector3::Axis min_axis_index() const { + return x < y ? (x < z ? Vector3::AXIS_X : Vector3::AXIS_Z) : (y < z ? Vector3::AXIS_Y : Vector3::AXIS_Z); } - _FORCE_INLINE_ int max_axis() const { - return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); + _FORCE_INLINE_ Vector3::Axis max_axis_index() const { + return x < y ? (y < z ? Vector3::AXIS_Z : Vector3::AXIS_Y) : (x < z ? Vector3::AXIS_Z : Vector3::AXIS_X); } _FORCE_INLINE_ real_t length() const; @@ -93,44 +97,27 @@ struct Vector3 { void snap(const Vector3 p_val); Vector3 snapped(const Vector3 p_val) const; - void rotate(const Vector3 &p_axis, const real_t p_phi); - Vector3 rotated(const Vector3 &p_axis, const real_t p_phi) const; + void rotate(const Vector3 &p_axis, const real_t p_angle); + Vector3 rotated(const Vector3 &p_axis, const real_t p_angle) const; /* Static Methods between 2 vector3s */ _FORCE_INLINE_ Vector3 lerp(const Vector3 &p_to, const real_t p_weight) const; _FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, const real_t p_weight) const; - Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const; - Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; + _FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const; + _FORCE_INLINE_ Vector3 cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; + _FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const; - _FORCE_INLINE_ Vector2 octahedron_encode() const { - Vector3 n = *this; - n /= Math::abs(n.x) + Math::abs(n.y) + Math::abs(n.z); - Vector2 o; - if (n.z >= 0.0) { - o.x = n.x; - o.y = n.y; - } else { - o.x = (1.0 - Math::abs(n.y)) * (n.x >= 0.0 ? 1.0 : -1.0); - o.y = (1.0 - Math::abs(n.x)) * (n.y >= 0.0 ? 1.0 : -1.0); - } - o.x = o.x * 0.5 + 0.5; - o.y = o.y * 0.5 + 0.5; - return o; - } + Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; - static _FORCE_INLINE_ Vector3 octahedron_decode(const Vector2 &p_oct) { - Vector2 f(p_oct.x * 2.0 - 1.0, p_oct.y * 2.0 - 1.0); - Vector3 n(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(); - } + Vector2 octahedron_encode() const; + static Vector3 octahedron_decode(const Vector2 &p_oct); + Vector2 octahedron_tangent_encode(const float sign) const; + static Vector3 octahedron_tangent_decode(const Vector2 &p_oct, float *sign); - _FORCE_INLINE_ Vector3 cross(const Vector3 &p_b) const; - _FORCE_INLINE_ real_t dot(const Vector3 &p_b) const; - Basis outer(const Vector3 &p_b) const; + _FORCE_INLINE_ Vector3 cross(const Vector3 &p_with) const; + _FORCE_INLINE_ real_t dot(const Vector3 &p_with) const; + Basis outer(const Vector3 &p_with) const; _FORCE_INLINE_ Vector3 abs() const; _FORCE_INLINE_ Vector3 floor() const; @@ -182,16 +169,9 @@ struct Vector3 { _FORCE_INLINE_ bool operator>=(const Vector3 &p_v) const; operator String() const; - _FORCE_INLINE_ operator Vector3i() const { - return Vector3i(x, y, z); - } + operator Vector3i() const; _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(const real_t p_x, const real_t p_y, const real_t p_z) { x = p_x; y = p_y; @@ -199,17 +179,17 @@ struct Vector3 { } }; -Vector3 Vector3::cross(const Vector3 &p_b) const { +Vector3 Vector3::cross(const Vector3 &p_with) const { Vector3 ret( - (y * p_b.z) - (z * p_b.y), - (z * p_b.x) - (x * p_b.z), - (x * p_b.y) - (y * p_b.x)); + (y * p_with.z) - (z * p_with.y), + (z * p_with.x) - (x * p_with.z), + (x * p_with.y) - (y * p_with.x)); return ret; } -real_t Vector3::dot(const Vector3 &p_b) const { - return x * p_b.x + y * p_b.y + z * p_b.z; +real_t Vector3::dot(const Vector3 &p_with) const { + return x * p_with.x + y * p_with.y + z * p_with.z; } Vector3 Vector3::abs() const { @@ -217,7 +197,7 @@ Vector3 Vector3::abs() const { } Vector3 Vector3::sign() const { - return Vector3(SGN(x), SGN(y), SGN(z)); + return Vector3(SIGN(x), SIGN(y), SIGN(z)); } Vector3 Vector3::floor() const { @@ -240,8 +220,54 @@ Vector3 Vector3::lerp(const Vector3 &p_to, const real_t p_weight) const { } Vector3 Vector3::slerp(const Vector3 &p_to, const real_t p_weight) const { - real_t theta = angle_to(p_to); - return rotated(cross(p_to).normalized(), theta * p_weight); + // This method seems more complicated than it really is, since we write out + // the internals of some methods for efficiency (mainly, checking length). + real_t start_length_sq = length_squared(); + real_t end_length_sq = p_to.length_squared(); + if (unlikely(start_length_sq == 0.0f || end_length_sq == 0.0f)) { + // Zero length vectors have no angle, so the best we can do is either lerp or throw an error. + return lerp(p_to, p_weight); + } + Vector3 axis = cross(p_to); + real_t axis_length_sq = axis.length_squared(); + if (unlikely(axis_length_sq == 0.0f)) { + // Colinear vectors have no rotation axis or angle between them, so the best we can do is lerp. + return lerp(p_to, p_weight); + } + axis /= Math::sqrt(axis_length_sq); + real_t start_length = Math::sqrt(start_length_sq); + real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight); + real_t angle = angle_to(p_to); + return rotated(axis, angle * p_weight) * (result_length / start_length); +} + +Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const { + Vector3 res = *this; + res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight); + res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight); + res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight); + return res; +} + +Vector3 Vector3::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector3 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} + +Vector3 Vector3::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const { + Vector3 res = *this; + + /* Formula from Wikipedia article on Bezier curves. */ + real_t omt = (1.0 - p_t); + real_t omt2 = omt * omt; + real_t omt3 = omt2 * omt; + real_t t2 = p_t * p_t; + real_t t3 = t2 * p_t; + + return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; } real_t Vector3::distance_to(const Vector3 &p_to) const { @@ -334,6 +360,9 @@ Vector3 &Vector3::operator*=(const real_t p_scalar) { return *this; } +// Multiplication operators required to workaround issues with LLVM using implicit conversion +// to Vector3i instead for integers where it should not. + _FORCE_INLINE_ Vector3 operator*(const float p_scalar, const Vector3 &p_vec) { return p_vec * p_scalar; } @@ -465,7 +494,7 @@ bool Vector3::is_normalized() const { } Vector3 Vector3::inverse() const { - return Vector3(1.0 / x, 1.0 / y, 1.0 / z); + return Vector3(1.0f / x, 1.0f / y, 1.0f / z); } void Vector3::zero() { @@ -488,7 +517,7 @@ Vector3 Vector3::reflect(const Vector3 &p_normal) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector3(), "The normal Vector3 must be normalized."); #endif - return 2.0 * p_normal * this->dot(p_normal) - *this; + return 2.0f * p_normal * this->dot(p_normal) - *this; } #endif // VECTOR3_H diff --git a/core/math/vector3i.cpp b/core/math/vector3i.cpp index d3a57af77c..b8e74ea6d2 100644 --- a/core/math/vector3i.cpp +++ b/core/math/vector3i.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,6 +30,9 @@ #include "vector3i.h" +#include "core/math/vector3.h" +#include "core/string/ustring.h" + void Vector3i::set_axis(const int p_axis, const int32_t p_value) { ERR_FAIL_INDEX(p_axis, 3); coord[p_axis] = p_value; @@ -40,12 +43,12 @@ int32_t Vector3i::get_axis(const int p_axis) const { return operator[](p_axis); } -int Vector3i::min_axis() const { - return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); +Vector3i::Axis Vector3i::min_axis_index() const { + return x < y ? (x < z ? Vector3i::AXIS_X : Vector3i::AXIS_Z) : (y < z ? Vector3i::AXIS_Y : Vector3i::AXIS_Z); } -int Vector3i::max_axis() const { - return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); +Vector3i::Axis Vector3i::max_axis_index() const { + return x < y ? (y < z ? Vector3i::AXIS_Z : Vector3i::AXIS_Y) : (x < z ? Vector3i::AXIS_Z : Vector3i::AXIS_X); } Vector3i Vector3i::clamp(const Vector3i &p_min, const Vector3i &p_max) const { @@ -58,3 +61,7 @@ Vector3i Vector3i::clamp(const Vector3i &p_min, const Vector3i &p_max) const { Vector3i::operator String() const { return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ")"; } + +Vector3i::operator Vector3() const { + return Vector3(x, y, z); +} diff --git a/core/math/vector3i.h b/core/math/vector3i.h index 9308d09045..b49c1142ed 100644 --- a/core/math/vector3i.h +++ b/core/math/vector3i.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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,10 +31,13 @@ #ifndef VECTOR3I_H #define VECTOR3I_H -#include "core/string/ustring.h" -#include "core/typedefs.h" +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" -struct Vector3i { +class String; +struct Vector3; + +struct _NO_DISCARD_ Vector3i { enum Axis { AXIS_X, AXIS_Y, @@ -52,18 +55,23 @@ struct Vector3i { }; _FORCE_INLINE_ const int32_t &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 3); return coord[p_axis]; } _FORCE_INLINE_ int32_t &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 3); return coord[p_axis]; } void set_axis(const int p_axis, const int32_t p_value); int32_t get_axis(const int p_axis) const; - int min_axis() const; - int max_axis() const; + Vector3i::Axis min_axis_index() const; + Vector3i::Axis max_axis_index() const; + + _FORCE_INLINE_ int64_t length_squared() const; + _FORCE_INLINE_ double length() const; _FORCE_INLINE_ void zero(); @@ -101,6 +109,7 @@ struct Vector3i { _FORCE_INLINE_ bool operator>=(const Vector3i &p_v) const; operator String() const; + operator Vector3() const; _FORCE_INLINE_ Vector3i() {} _FORCE_INLINE_ Vector3i(const int32_t p_x, const int32_t p_y, const int32_t p_z) { @@ -110,12 +119,20 @@ struct Vector3i { } }; +int64_t Vector3i::length_squared() const { + return x * (int64_t)x + y * (int64_t)y + z * (int64_t)z; +} + +double Vector3i::length() const { + return Math::sqrt((double)length_squared()); +} + Vector3i Vector3i::abs() const { return Vector3i(ABS(x), ABS(y), ABS(z)); } Vector3i Vector3i::sign() const { - return Vector3i(SGN(x), SGN(y), SGN(z)); + return Vector3i(SIGN(x), SIGN(y), SIGN(z)); } /* Operators */ @@ -182,6 +199,12 @@ Vector3i &Vector3i::operator*=(const int32_t p_scalar) { return *this; } +Vector3i Vector3i::operator*(const int32_t p_scalar) const { + return Vector3i(x * p_scalar, y * p_scalar, z * p_scalar); +} + +// Multiplication operators required to workaround issues with LLVM using implicit conversion. + _FORCE_INLINE_ Vector3i operator*(const int32_t p_scalar, const Vector3i &p_vector) { return p_vector * p_scalar; } @@ -198,10 +221,6 @@ _FORCE_INLINE_ Vector3i operator*(const double p_scalar, const Vector3i &p_vecto return p_vector * p_scalar; } -Vector3i Vector3i::operator*(const int32_t p_scalar) const { - return Vector3i(x * p_scalar, y * p_scalar, z * p_scalar); -} - Vector3i &Vector3i::operator/=(const int32_t p_scalar) { x /= p_scalar; y /= p_scalar; diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp new file mode 100644 index 0000000000..273a111891 --- /dev/null +++ b/core/math/vector4.cpp @@ -0,0 +1,185 @@ +/*************************************************************************/ +/* vector4.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 "vector4.h" + +#include "core/math/basis.h" +#include "core/string/print_string.h" + +void Vector4::set_axis(const int p_axis, const real_t p_value) { + ERR_FAIL_INDEX(p_axis, 4); + components[p_axis] = p_value; +} + +real_t Vector4::get_axis(const int p_axis) const { + ERR_FAIL_INDEX_V(p_axis, 4, 0); + return operator[](p_axis); +} + +Vector4::Axis Vector4::min_axis_index() const { + uint32_t min_index = 0; + real_t min_value = x; + for (uint32_t i = 1; i < 4; i++) { + if (operator[](i) <= min_value) { + min_index = i; + min_value = operator[](i); + } + } + return Vector4::Axis(min_index); +} + +Vector4::Axis Vector4::max_axis_index() const { + uint32_t max_index = 0; + real_t max_value = x; + for (uint32_t i = 1; i < 4; i++) { + if (operator[](i) > max_value) { + max_index = i; + max_value = operator[](i); + } + } + return Vector4::Axis(max_index); +} + +bool Vector4::is_equal_approx(const Vector4 &p_vec4) const { + return Math::is_equal_approx(x, p_vec4.x) && Math::is_equal_approx(y, p_vec4.y) && Math::is_equal_approx(z, p_vec4.z) && Math::is_equal_approx(w, p_vec4.w); +} + +real_t Vector4::length() const { + return Math::sqrt(length_squared()); +} + +void Vector4::normalize() { + *this /= length(); +} + +Vector4 Vector4::normalized() const { + return *this / length(); +} + +bool Vector4::is_normalized() const { + return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); // Use less epsilon. +} + +real_t Vector4::distance_to(const Vector4 &p_to) const { + return (p_to - *this).length(); +} + +real_t Vector4::distance_squared_to(const Vector4 &p_to) const { + return (p_to - *this).length_squared(); +} + +Vector4 Vector4::direction_to(const Vector4 &p_to) const { + Vector4 ret(p_to.x - x, p_to.y - y, p_to.z - z, p_to.w - w); + ret.normalize(); + return ret; +} + +Vector4 Vector4::abs() const { + return Vector4(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w)); +} + +Vector4 Vector4::sign() const { + return Vector4(SIGN(x), SIGN(y), SIGN(z), SIGN(w)); +} + +Vector4 Vector4::floor() const { + return Vector4(Math::floor(x), Math::floor(y), Math::floor(z), Math::floor(w)); +} + +Vector4 Vector4::ceil() const { + return Vector4(Math::ceil(x), Math::ceil(y), Math::ceil(z), Math::ceil(w)); +} + +Vector4 Vector4::round() const { + return Vector4(Math::round(x), Math::round(y), Math::round(z), Math::round(w)); +} + +Vector4 Vector4::lerp(const Vector4 &p_to, const real_t p_weight) const { + return Vector4( + x + (p_weight * (p_to.x - x)), + y + (p_weight * (p_to.y - y)), + z + (p_weight * (p_to.z - z)), + w + (p_weight * (p_to.w - w))); +} + +Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const { + Vector4 res = *this; + res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight); + res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight); + res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight); + res.w = Math::cubic_interpolate(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight); + return res; +} + +Vector4 Vector4::cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector4 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.w = Math::cubic_interpolate_in_time(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} + +Vector4 Vector4::posmod(const real_t p_mod) const { + return Vector4(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod), Math::fposmod(w, p_mod)); +} + +Vector4 Vector4::posmodv(const Vector4 &p_modv) const { + return Vector4(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z), Math::fposmod(w, p_modv.w)); +} + +void Vector4::snap(const Vector4 &p_step) { + x = Math::snapped(x, p_step.x); + y = Math::snapped(y, p_step.y); + z = Math::snapped(z, p_step.z); + w = Math::snapped(w, p_step.w); +} + +Vector4 Vector4::snapped(const Vector4 &p_step) const { + Vector4 v = *this; + v.snap(p_step); + return v; +} + +Vector4 Vector4::inverse() const { + return Vector4(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w); +} + +Vector4 Vector4::clamp(const Vector4 &p_min, const Vector4 &p_max) const { + return Vector4( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y), + CLAMP(z, p_min.z, p_max.z), + CLAMP(w, p_min.w, p_max.w)); +} + +Vector4::operator String() const { + return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")"; +} diff --git a/core/math/vector4.h b/core/math/vector4.h new file mode 100644 index 0000000000..17d0de18e1 --- /dev/null +++ b/core/math/vector4.h @@ -0,0 +1,304 @@ +/*************************************************************************/ +/* vector4.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 VECTOR4_H +#define VECTOR4_H + +#include "core/math/math_defs.h" +#include "core/math/math_funcs.h" +#include "core/math/vector3.h" +#include "core/string/ustring.h" + +struct _NO_DISCARD_ Vector4 { + enum Axis { + AXIS_X, + AXIS_Y, + AXIS_Z, + AXIS_W, + }; + + union { + struct { + real_t x; + real_t y; + real_t z; + real_t w; + }; + real_t components[4] = { 0, 0, 0, 0 }; + }; + + _FORCE_INLINE_ real_t &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 4); + return components[p_axis]; + } + _FORCE_INLINE_ const real_t &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 4); + return components[p_axis]; + } + + _FORCE_INLINE_ void set_all(const real_t p_value); + + void set_axis(const int p_axis, const real_t p_value); + real_t get_axis(const int p_axis) const; + + Vector4::Axis min_axis_index() const; + Vector4::Axis max_axis_index() const; + + _FORCE_INLINE_ real_t length_squared() const; + bool is_equal_approx(const Vector4 &p_vec4) const; + real_t length() const; + void normalize(); + Vector4 normalized() const; + bool is_normalized() const; + + real_t distance_to(const Vector4 &p_to) const; + real_t distance_squared_to(const Vector4 &p_to) const; + Vector4 direction_to(const Vector4 &p_to) const; + + Vector4 abs() const; + Vector4 sign() const; + Vector4 floor() const; + Vector4 ceil() const; + Vector4 round() const; + Vector4 lerp(const Vector4 &p_to, const real_t p_weight) const; + Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const; + Vector4 cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; + + Vector4 posmod(const real_t p_mod) const; + Vector4 posmodv(const Vector4 &p_modv) const; + void snap(const Vector4 &p_step); + Vector4 snapped(const Vector4 &p_step) const; + Vector4 clamp(const Vector4 &p_min, const Vector4 &p_max) const; + + Vector4 inverse() const; + _FORCE_INLINE_ real_t dot(const Vector4 &p_vec4) const; + + _FORCE_INLINE_ void operator+=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator-=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator*=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator/=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator*=(const real_t &s); + _FORCE_INLINE_ void operator/=(const real_t &s); + _FORCE_INLINE_ Vector4 operator+(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator-(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator*(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator/(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator-() const; + _FORCE_INLINE_ Vector4 operator*(const real_t &s) const; + _FORCE_INLINE_ Vector4 operator/(const real_t &s) const; + + _FORCE_INLINE_ bool operator==(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator!=(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator>(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator<(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator>=(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator<=(const Vector4 &p_vec4) const; + + operator String() const; + + _FORCE_INLINE_ Vector4() {} + + _FORCE_INLINE_ Vector4(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) { + } + + Vector4(const Vector4 &p_vec4) : + x(p_vec4.x), + y(p_vec4.y), + z(p_vec4.z), + w(p_vec4.w) { + } + + void operator=(const Vector4 &p_vec4) { + x = p_vec4.x; + y = p_vec4.y; + z = p_vec4.z; + w = p_vec4.w; + } +}; + +void Vector4::set_all(const real_t p_value) { + x = y = z = p_value; +} + +real_t Vector4::dot(const Vector4 &p_vec4) const { + return x * p_vec4.x + y * p_vec4.y + z * p_vec4.z + w * p_vec4.w; +} + +real_t Vector4::length_squared() const { + return dot(*this); +} + +void Vector4::operator+=(const Vector4 &p_vec4) { + x += p_vec4.x; + y += p_vec4.y; + z += p_vec4.z; + w += p_vec4.w; +} + +void Vector4::operator-=(const Vector4 &p_vec4) { + x -= p_vec4.x; + y -= p_vec4.y; + z -= p_vec4.z; + w -= p_vec4.w; +} + +void Vector4::operator*=(const Vector4 &p_vec4) { + x *= p_vec4.x; + y *= p_vec4.y; + z *= p_vec4.z; + w *= p_vec4.w; +} + +void Vector4::operator/=(const Vector4 &p_vec4) { + x /= p_vec4.x; + y /= p_vec4.y; + z /= p_vec4.z; + w /= p_vec4.w; +} +void Vector4::operator*=(const real_t &s) { + x *= s; + y *= s; + z *= s; + w *= s; +} + +void Vector4::operator/=(const real_t &s) { + *this *= 1.0f / s; +} + +Vector4 Vector4::operator+(const Vector4 &p_vec4) const { + return Vector4(x + p_vec4.x, y + p_vec4.y, z + p_vec4.z, w + p_vec4.w); +} + +Vector4 Vector4::operator-(const Vector4 &p_vec4) const { + return Vector4(x - p_vec4.x, y - p_vec4.y, z - p_vec4.z, w - p_vec4.w); +} + +Vector4 Vector4::operator*(const Vector4 &p_vec4) const { + return Vector4(x * p_vec4.x, y * p_vec4.y, z * p_vec4.z, w * p_vec4.w); +} + +Vector4 Vector4::operator/(const Vector4 &p_vec4) const { + return Vector4(x / p_vec4.x, y / p_vec4.y, z / p_vec4.z, w / p_vec4.w); +} + +Vector4 Vector4::operator-() const { + return Vector4(-x, -y, -z, -w); +} + +Vector4 Vector4::operator*(const real_t &s) const { + return Vector4(x * s, y * s, z * s, w * s); +} + +Vector4 Vector4::operator/(const real_t &s) const { + return *this * (1.0f / s); +} + +bool Vector4::operator==(const Vector4 &p_vec4) const { + return x == p_vec4.x && y == p_vec4.y && z == p_vec4.z && w == p_vec4.w; +} + +bool Vector4::operator!=(const Vector4 &p_vec4) const { + return x != p_vec4.x || y != p_vec4.y || z != p_vec4.z || w != p_vec4.w; +} + +bool Vector4::operator<(const Vector4 &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w < p_v.w; + } + return z < p_v.z; + } + return y < p_v.y; + } + return x < p_v.x; +} + +bool Vector4::operator>(const Vector4 &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w > p_v.w; + } + return z > p_v.z; + } + return y > p_v.y; + } + return x > p_v.x; +} + +bool Vector4::operator<=(const Vector4 &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w <= p_v.w; + } + return z < p_v.z; + } + return y < p_v.y; + } + return x < p_v.x; +} + +bool Vector4::operator>=(const Vector4 &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w >= p_v.w; + } + return z > p_v.z; + } + return y > p_v.y; + } + return x > p_v.x; +} + +_FORCE_INLINE_ Vector4 operator*(const float p_scalar, const Vector4 &p_vec) { + return p_vec * p_scalar; +} + +_FORCE_INLINE_ Vector4 operator*(const double p_scalar, const Vector4 &p_vec) { + return p_vec * p_scalar; +} + +_FORCE_INLINE_ Vector4 operator*(const int32_t p_scalar, const Vector4 &p_vec) { + return p_vec * p_scalar; +} + +_FORCE_INLINE_ Vector4 operator*(const int64_t p_scalar, const Vector4 &p_vec) { + return p_vec * p_scalar; +} + +#endif // VECTOR4_H diff --git a/core/math/vector4i.cpp b/core/math/vector4i.cpp new file mode 100644 index 0000000000..2dc5b74202 --- /dev/null +++ b/core/math/vector4i.cpp @@ -0,0 +1,91 @@ +/*************************************************************************/ +/* vector4i.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 "vector4i.h" + +#include "core/math/vector4.h" +#include "core/string/ustring.h" + +void Vector4i::set_axis(const int p_axis, const int32_t p_value) { + ERR_FAIL_INDEX(p_axis, 4); + coord[p_axis] = p_value; +} + +int32_t Vector4i::get_axis(const int p_axis) const { + ERR_FAIL_INDEX_V(p_axis, 4, 0); + return operator[](p_axis); +} + +Vector4i::Axis Vector4i::min_axis_index() const { + uint32_t min_index = 0; + int32_t min_value = x; + for (uint32_t i = 1; i < 4; i++) { + if (operator[](i) <= min_value) { + min_index = i; + min_value = operator[](i); + } + } + return Vector4i::Axis(min_index); +} + +Vector4i::Axis Vector4i::max_axis_index() const { + uint32_t max_index = 0; + int32_t max_value = x; + for (uint32_t i = 1; i < 4; i++) { + if (operator[](i) > max_value) { + max_index = i; + max_value = operator[](i); + } + } + return Vector4i::Axis(max_index); +} + +Vector4i Vector4i::clamp(const Vector4i &p_min, const Vector4i &p_max) const { + return Vector4i( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y), + CLAMP(z, p_min.z, p_max.z), + CLAMP(w, p_min.w, p_max.w)); +} + +Vector4i::operator String() const { + return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ", " + itos(w) + ")"; +} + +Vector4i::operator Vector4() const { + return Vector4(x, y, z, w); +} + +Vector4i::Vector4i(const Vector4 &p_vec4) { + x = p_vec4.x; + y = p_vec4.y; + z = p_vec4.z; + w = p_vec4.w; +} diff --git a/core/math/vector4i.h b/core/math/vector4i.h new file mode 100644 index 0000000000..37d905878f --- /dev/null +++ b/core/math/vector4i.h @@ -0,0 +1,338 @@ +/*************************************************************************/ +/* vector4i.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 VECTOR4I_H +#define VECTOR4I_H + +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" + +class String; +struct Vector4; + +struct _NO_DISCARD_ Vector4i { + enum Axis { + AXIS_X, + AXIS_Y, + AXIS_Z, + AXIS_W, + }; + + union { + struct { + int32_t x; + int32_t y; + int32_t z; + int32_t w; + }; + + int32_t coord[4] = { 0 }; + }; + + _FORCE_INLINE_ const int32_t &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 4); + return coord[p_axis]; + } + + _FORCE_INLINE_ int32_t &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 4); + return coord[p_axis]; + } + + void set_axis(const int p_axis, const int32_t p_value); + int32_t get_axis(const int p_axis) const; + + Vector4i::Axis min_axis_index() const; + Vector4i::Axis max_axis_index() const; + + _FORCE_INLINE_ int64_t length_squared() const; + _FORCE_INLINE_ double length() const; + + _FORCE_INLINE_ void zero(); + + _FORCE_INLINE_ Vector4i abs() const; + _FORCE_INLINE_ Vector4i sign() const; + Vector4i clamp(const Vector4i &p_min, const Vector4i &p_max) const; + + /* Operators */ + + _FORCE_INLINE_ Vector4i &operator+=(const Vector4i &p_v); + _FORCE_INLINE_ Vector4i operator+(const Vector4i &p_v) const; + _FORCE_INLINE_ Vector4i &operator-=(const Vector4i &p_v); + _FORCE_INLINE_ Vector4i operator-(const Vector4i &p_v) const; + _FORCE_INLINE_ Vector4i &operator*=(const Vector4i &p_v); + _FORCE_INLINE_ Vector4i operator*(const Vector4i &p_v) const; + _FORCE_INLINE_ Vector4i &operator/=(const Vector4i &p_v); + _FORCE_INLINE_ Vector4i operator/(const Vector4i &p_v) const; + _FORCE_INLINE_ Vector4i &operator%=(const Vector4i &p_v); + _FORCE_INLINE_ Vector4i operator%(const Vector4i &p_v) const; + + _FORCE_INLINE_ Vector4i &operator*=(const int32_t p_scalar); + _FORCE_INLINE_ Vector4i operator*(const int32_t p_scalar) const; + _FORCE_INLINE_ Vector4i &operator/=(const int32_t p_scalar); + _FORCE_INLINE_ Vector4i operator/(const int32_t p_scalar) const; + _FORCE_INLINE_ Vector4i &operator%=(const int32_t p_scalar); + _FORCE_INLINE_ Vector4i operator%(const int32_t p_scalar) const; + + _FORCE_INLINE_ Vector4i operator-() const; + + _FORCE_INLINE_ bool operator==(const Vector4i &p_v) const; + _FORCE_INLINE_ bool operator!=(const Vector4i &p_v) const; + _FORCE_INLINE_ bool operator<(const Vector4i &p_v) const; + _FORCE_INLINE_ bool operator<=(const Vector4i &p_v) const; + _FORCE_INLINE_ bool operator>(const Vector4i &p_v) const; + _FORCE_INLINE_ bool operator>=(const Vector4i &p_v) const; + + operator String() const; + operator Vector4() const; + + _FORCE_INLINE_ Vector4i() {} + Vector4i(const Vector4 &p_vec4); + _FORCE_INLINE_ Vector4i(const int32_t p_x, const int32_t p_y, const int32_t p_z, const int32_t p_w) { + x = p_x; + y = p_y; + z = p_z; + w = p_w; + } +}; + +int64_t Vector4i::length_squared() const { + return x * (int64_t)x + y * (int64_t)y + z * (int64_t)z + w * (int64_t)w; +} + +double Vector4i::length() const { + return Math::sqrt((double)length_squared()); +} + +Vector4i Vector4i::abs() const { + return Vector4i(ABS(x), ABS(y), ABS(z), ABS(w)); +} + +Vector4i Vector4i::sign() const { + return Vector4i(SIGN(x), SIGN(y), SIGN(z), SIGN(w)); +} + +/* Operators */ + +Vector4i &Vector4i::operator+=(const Vector4i &p_v) { + x += p_v.x; + y += p_v.y; + z += p_v.z; + w += p_v.w; + return *this; +} + +Vector4i Vector4i::operator+(const Vector4i &p_v) const { + return Vector4i(x + p_v.x, y + p_v.y, z + p_v.z, w + p_v.w); +} + +Vector4i &Vector4i::operator-=(const Vector4i &p_v) { + x -= p_v.x; + y -= p_v.y; + z -= p_v.z; + w -= p_v.w; + return *this; +} + +Vector4i Vector4i::operator-(const Vector4i &p_v) const { + return Vector4i(x - p_v.x, y - p_v.y, z - p_v.z, w - p_v.w); +} + +Vector4i &Vector4i::operator*=(const Vector4i &p_v) { + x *= p_v.x; + y *= p_v.y; + z *= p_v.z; + w *= p_v.w; + return *this; +} + +Vector4i Vector4i::operator*(const Vector4i &p_v) const { + return Vector4i(x * p_v.x, y * p_v.y, z * p_v.z, w * p_v.w); +} + +Vector4i &Vector4i::operator/=(const Vector4i &p_v) { + x /= p_v.x; + y /= p_v.y; + z /= p_v.z; + w /= p_v.w; + return *this; +} + +Vector4i Vector4i::operator/(const Vector4i &p_v) const { + return Vector4i(x / p_v.x, y / p_v.y, z / p_v.z, w / p_v.w); +} + +Vector4i &Vector4i::operator%=(const Vector4i &p_v) { + x %= p_v.x; + y %= p_v.y; + z %= p_v.z; + w %= p_v.w; + return *this; +} + +Vector4i Vector4i::operator%(const Vector4i &p_v) const { + return Vector4i(x % p_v.x, y % p_v.y, z % p_v.z, w % p_v.w); +} + +Vector4i &Vector4i::operator*=(const int32_t p_scalar) { + x *= p_scalar; + y *= p_scalar; + z *= p_scalar; + w *= p_scalar; + return *this; +} + +Vector4i Vector4i::operator*(const int32_t p_scalar) const { + return Vector4i(x * p_scalar, y * p_scalar, z * p_scalar, w * p_scalar); +} + +// Multiplication operators required to workaround issues with LLVM using implicit conversion. + +_FORCE_INLINE_ Vector4i operator*(const int32_t p_scalar, const Vector4i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector4i operator*(const int64_t p_scalar, const Vector4i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector4i operator*(const float p_scalar, const Vector4i &p_vector) { + return p_vector * p_scalar; +} + +_FORCE_INLINE_ Vector4i operator*(const double p_scalar, const Vector4i &p_vector) { + return p_vector * p_scalar; +} + +Vector4i &Vector4i::operator/=(const int32_t p_scalar) { + x /= p_scalar; + y /= p_scalar; + z /= p_scalar; + w /= p_scalar; + return *this; +} + +Vector4i Vector4i::operator/(const int32_t p_scalar) const { + return Vector4i(x / p_scalar, y / p_scalar, z / p_scalar, w / p_scalar); +} + +Vector4i &Vector4i::operator%=(const int32_t p_scalar) { + x %= p_scalar; + y %= p_scalar; + z %= p_scalar; + w %= p_scalar; + return *this; +} + +Vector4i Vector4i::operator%(const int32_t p_scalar) const { + return Vector4i(x % p_scalar, y % p_scalar, z % p_scalar, w % p_scalar); +} + +Vector4i Vector4i::operator-() const { + return Vector4i(-x, -y, -z, -w); +} + +bool Vector4i::operator==(const Vector4i &p_v) const { + return (x == p_v.x && y == p_v.y && z == p_v.z && w == p_v.w); +} + +bool Vector4i::operator!=(const Vector4i &p_v) const { + return (x != p_v.x || y != p_v.y || z != p_v.z || w != p_v.w); +} + +bool Vector4i::operator<(const Vector4i &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w < p_v.w; + } else { + return z < p_v.z; + } + } else { + return y < p_v.y; + } + } else { + return x < p_v.x; + } +} + +bool Vector4i::operator>(const Vector4i &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w > p_v.w; + } else { + return z > p_v.z; + } + } else { + return y > p_v.y; + } + } else { + return x > p_v.x; + } +} + +bool Vector4i::operator<=(const Vector4i &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w <= p_v.w; + } else { + return z < p_v.z; + } + } else { + return y < p_v.y; + } + } else { + return x < p_v.x; + } +} + +bool Vector4i::operator>=(const Vector4i &p_v) const { + if (x == p_v.x) { + if (y == p_v.y) { + if (z == p_v.z) { + return w >= p_v.w; + } else { + return z > p_v.z; + } + } else { + return y > p_v.y; + } + } else { + return x > p_v.x; + } +} + +void Vector4i::zero() { + x = y = z = w = 0; +} + +#endif // VECTOR4I_H |