diff options
Diffstat (limited to 'core/math')
33 files changed, 1624 insertions, 1614 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index efa970c681..41a0848d01 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -33,19 +33,19 @@ #include "core/math/geometry_3d.h" #include "core/object/script_language.h" -int AStar3D::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 AStar3D::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 < 0.0, vformat("Can't add a point with weight scale less than 0.0: %f.", p_weight_scale)); @@ -68,7 +68,7 @@ void AStar3D::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { } } -Vector3 AStar3D::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)); @@ -76,7 +76,7 @@ Vector3 AStar3D::get_point_position(int p_id) const { return p->pos; } -void AStar3D::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)); @@ -84,7 +84,7 @@ void AStar3D::set_point_position(int p_id, const Vector3 &p_pos) { p->pos = p_pos; } -real_t AStar3D::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)); @@ -92,7 +92,7 @@ real_t AStar3D::get_point_weight_scale(int p_id) const { return p->weight_scale; } -void AStar3D::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)); @@ -101,12 +101,12 @@ void AStar3D::set_point_weight_scale(int p_id, real_t p_weight_scale) { p->weight_scale = p_weight_scale; } -void AStar3D::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); @@ -114,7 +114,7 @@ void AStar3D::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); @@ -127,7 +127,7 @@ void AStar3D::remove_point(int p_id) { last_free_id = p_id; } -void AStar3D::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; @@ -165,7 +165,7 @@ void AStar3D::connect_points(int p_id, int p_with_id, bool bidirectional) { segments.insert(s); } -void AStar3D::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)); @@ -175,7 +175,7 @@ void AStar3D::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; HashSet<Segment, Segment>::Iterator element = segments.find(s); if (element) { @@ -205,35 +205,35 @@ void AStar3D::disconnect_points(int p_id, int p_with_id, bool bidirectional) { } } -bool AStar3D::has_point(int p_id) const { +bool AStar3D::has_point(int64_t p_id) const { return points.has(p_id); } Array AStar3D::get_point_ids() { Array 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> AStar3D::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 AStar3D::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 HashSet<Segment, Segment>::Iterator element = segments.find(s); @@ -243,32 +243,32 @@ bool AStar3D::are_points_connected(int p_id, int p_with_id, bool bidirectional) 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 AStar3D::get_point_count() const { +int64_t AStar3D::get_point_count() const { return points.get_num_elements(); } -int AStar3D::get_point_capacity() const { +int64_t AStar3D::get_point_capacity() const { return points.get_capacity(); } -void AStar3D::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 AStar3D::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. } @@ -276,7 +276,7 @@ int AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabled) // 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; @@ -295,8 +295,8 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const { for (const Segment &E : segments) { Point *from_point = nullptr, *to_point = nullptr; - points.lookup(E.u, from_point); - points.lookup(E.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; @@ -346,7 +346,7 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point) { 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) { @@ -380,7 +380,7 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point) { return found_route; } -real_t AStar3D::_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; @@ -397,7 +397,7 @@ real_t AStar3D::_estimate_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -real_t AStar3D::_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; @@ -414,7 +414,7 @@ real_t AStar3D::_compute_cost(int p_from_id, int p_to_id) { return from_point->pos.distance_to(to_point->pos); } -Vector<Vector3> AStar3D::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)); @@ -438,7 +438,7 @@ Vector<Vector3> AStar3D::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; @@ -451,7 +451,7 @@ Vector<Vector3> AStar3D::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; @@ -463,17 +463,17 @@ Vector<Vector3> AStar3D::get_point_path(int p_from_id, int p_to_id) { return path; } -Vector<int> AStar3D::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; } @@ -483,24 +483,24 @@ Vector<int> AStar3D::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; @@ -512,7 +512,7 @@ Vector<int> AStar3D::get_id_path(int p_from_id, int p_to_id) { return path; } -void AStar3D::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)); @@ -520,7 +520,7 @@ void AStar3D::set_point_disabled(int p_id, bool p_disabled) { p->enabled = !p_disabled; } -bool AStar3D::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)); @@ -568,40 +568,40 @@ AStar3D::~AStar3D() { ///////////////////////////////////////////////////////////// -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); } @@ -609,31 +609,31 @@ Array 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(); } @@ -641,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); } @@ -654,7 +654,7 @@ 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; @@ -671,7 +671,7 @@ real_t AStar2D::_estimate_cost(int p_from_id, int 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; @@ -688,7 +688,7 @@ real_t AStar2D::_compute_cost(int p_from_id, int 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) { +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)); @@ -711,7 +711,7 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { } AStar3D::Point *p = end_point; - int pc = 1; // Begin point + int64_t pc = 1; // Begin point while (p != begin_point) { pc++; p = p->prev_point; @@ -724,7 +724,7 @@ Vector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { Vector2 *w = path.ptrw(); AStar3D::Point *p2 = end_point; - int idx = pc - 1; + int64_t idx = pc - 1; while (p2 != begin_point) { w[idx--] = Vector2(p2->pos.x, p2->pos.y); p2 = p2->prev_point; @@ -736,17 +736,17 @@ 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) { +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)); 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; } @@ -756,24 +756,24 @@ Vector<int> AStar2D::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>(); } AStar3D::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; @@ -813,7 +813,7 @@ bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) { open_list.remove_at(open_list.size() - 1); p->closed_pass = astar.pass; // Mark the point as closed - for (OAHashMap<int, AStar3D::Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + 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) { @@ -863,8 +863,8 @@ void AStar2D::_bind_methods() { 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 e2f75ad18c..c1497d133f 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -47,13 +47,13 @@ class AStar3D : public RefCounted { 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; @@ -76,13 +76,7 @@ class AStar3D : 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, @@ -93,28 +87,28 @@ class AStar3D : public RefCounted { unsigned char direction = NONE; static uint32_t hash(const Segment &p_seg) { - return hash_one_uint64(p_seg.key); + 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; + OAHashMap<int64_t, Point *> points; HashSet<Segment, Segment> segments; bool _solve(Point *begin_point, Point *end_point); @@ -122,42 +116,42 @@ class AStar3D : public RefCounted { 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); + 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); Array get_point_ids(); - void set_point_disabled(int p_id, bool p_disabled = true); - bool is_point_disabled(int p_id) const; + void set_point_disabled(int64_t p_id, bool p_disabled = true); + bool is_point_disabled(int64_t 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; + 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; - int get_point_count() const; - int get_point_capacity() const; - void reserve_space(int p_num_nodes); + 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); AStar3D() {} ~AStar3D(); @@ -172,42 +166,42 @@ class AStar2D : public RefCounted { 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); + 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); Array get_point_ids(); - void set_point_disabled(int p_id, bool p_disabled = true); - bool is_point_disabled(int p_id) const; + void set_point_disabled(int64_t p_id, bool p_disabled = true); + bool is_point_disabled(int64_t 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; + 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; - int get_point_count() const; - int get_point_capacity() const; - void reserve_space(int p_num_nodes); + 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/audio_frame.h b/core/math/audio_frame.h index 8b244e9fe4..b3d63c0094 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -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; } diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 65353d8118..f8e7c47107 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -365,12 +365,12 @@ 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 { @@ -817,14 +817,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(rows[1][0] - rows[0][1]) < epsilon) && (Math::abs(rows[2][0] - rows[0][2]) < epsilon) && (Math::abs(rows[2][1] - rows[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(rows[1][0] + rows[0][1]) < epsilon2) && (Math::abs(rows[2][0] + rows[0][2]) < epsilon2) && (Math::abs(rows[2][1] + rows[1][2]) < epsilon2) && (Math::abs(rows[0][0] + rows[1][1] + rows[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; @@ -839,7 +838,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { 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 < epsilon) { + if (xx < CMP_EPSILON) { x = 0; y = Math_SQRT12; z = Math_SQRT12; @@ -849,7 +848,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { z = xz / x; } } else if (yy > zz) { // rows[1][1] is the largest diagonal term - if (yy < epsilon) { + if (yy < CMP_EPSILON) { x = Math_SQRT12; y = 0; z = Math_SQRT12; @@ -859,7 +858,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { z = yz / y; } } else { // rows[2][2] is the largest diagonal term so base result on this - if (zz < epsilon) { + if (zz < CMP_EPSILON) { x = Math_SQRT12; y = Math_SQRT12; z = 0; @@ -935,9 +934,9 @@ void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Ve 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) { diff --git a/core/math/basis.h b/core/math/basis.h index 9cce22510b..4be325cdd2 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -56,6 +56,15 @@ struct _NO_DISCARD_ Basis { _FORCE_INLINE_ real_t determinant() const; + enum EulerOrder { + EULER_ORDER_XYZ, + EULER_ORDER_XZY, + EULER_ORDER_YXZ, + EULER_ORDER_YZX, + EULER_ORDER_ZXY, + EULER_ORDER_ZYX + }; + void from_z(const Vector3 &p_z); void rotate(const Vector3 &p_axis, real_t p_angle); @@ -64,21 +73,12 @@ struct _NO_DISCARD_ Basis { 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); - Basis rotated(const Vector3 &p_euler) 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; - enum EulerOrder { - EULER_ORDER_XYZ, - EULER_ORDER_XZY, - EULER_ORDER_YXZ, - EULER_ORDER_YZX, - EULER_ORDER_ZXY, - EULER_ORDER_ZYX - }; - 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; @@ -119,7 +119,7 @@ struct _NO_DISCARD_ Basis { Vector3 get_scale_local() const; 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); + 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 diff --git a/core/math/bvh.h b/core/math/bvh.h index 9f6ab9f736..b5f5eda3e6 100644 --- a/core/math/bvh.h +++ b/core/math/bvh.h @@ -302,7 +302,7 @@ public: tree.update(); _check_for_collisions(); #ifdef BVH_INTEGRITY_CHECKS - tree.integrity_check_all(); + tree._integrity_check_all(); #endif } diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc index 36b0bfeb13..fc1c67a21b 100644 --- a/core/math/bvh_public.inc +++ b/core/math/bvh_public.inc @@ -2,7 +2,7 @@ public: 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 @@ -78,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 diff --git a/core/math/color.h b/core/math/color.h index 0afa6006a8..65036f74cc 100644 --- a/core/math/color.h +++ b/core/math/color.h @@ -215,12 +215,12 @@ struct _NO_DISCARD_ Color { _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_ok_hsl_h(float p_h) { set_ok_hsl(p_h, get_ok_hsl_s(), get_ok_hsl_l()); } - _FORCE_INLINE_ void set_ok_hsl_s(float p_s) { set_ok_hsl(get_ok_hsl_h(), p_s, get_ok_hsl_l()); } - _FORCE_INLINE_ void set_ok_hsl_l(float p_l) { set_ok_hsl(get_ok_hsl_h(), get_ok_hsl_s(), p_l); } + _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() {} diff --git a/core/math/delaunay_3d.h b/core/math/delaunay_3d.h index f8a10ec87e..898c3c2d91 100644 --- a/core/math/delaunay_3d.h +++ b/core/math/delaunay_3d.h @@ -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; diff --git a/core/math/expression.cpp b/core/math/expression.cpp index 5a90f68b66..e230b69dc9 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -891,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; } @@ -1240,7 +1240,7 @@ 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); @@ -1266,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; } @@ -1274,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; } @@ -1292,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; } @@ -1316,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; } @@ -1336,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; @@ -1353,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; } @@ -1380,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; @@ -1408,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; } @@ -1429,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; @@ -1442,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; @@ -1452,7 +1452,11 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: } Callable::CallError ce; - base.callp(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)); @@ -1491,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; @@ -1517,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 6ea3c1611f..2d58915996 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -257,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/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index 4be4809e3f..208f89f449 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -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) diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 068bc0397e..53deb9bd42 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -253,6 +253,27 @@ public: (-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight)); } + 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; @@ -302,11 +323,19 @@ 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) { diff --git a/core/math/octree.h b/core/math/octree.h deleted file mode 100644 index 8dd103f109..0000000000 --- a/core/math/octree.h +++ /dev/null @@ -1,1271 +0,0 @@ -/*************************************************************************/ -/* octree.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 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/rb_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 children (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 = nullptr; - 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 = nullptr; - typename List<PairData *, AL>::Element *eA, *eB; - }; - - typedef HashMap<OctreeElementID, Element, Comparator<OctreeElementID>, AL> ElementMap; - typedef HashMap<PairKey, PairData, Comparator<PairKey>, AL> PairMap; - ElementMap element_map; - PairMap pair_map; - - PairCallback pair_callback = nullptr; - UnpairCallback unpair_callback = nullptr; - void *pair_callback_userdata = nullptr; - void *unpair_callback_userdata = nullptr; - - OctreeElementID last_element_id = 1; - uint64_t pass = 1; - - real_t unit_size = 1.0; - Octant *root = nullptr; - int octant_count = 0; - int pair_count = 0; - - _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()); - } 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 = nullptr; - 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 && 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) { - unit_size = p_unit_size; -} - -#endif // OCTREE_H diff --git a/core/math/plane.h b/core/math/plane.h index 66c1741662..73babfa496 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -52,7 +52,7 @@ struct _NO_DISCARD_ Plane { _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 */ @@ -97,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 57c53b0adb..edf8bf36cd 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/projection.cpp @@ -1,5 +1,5 @@ /*************************************************************************/ -/* camera_matrix.cpp */ +/* projection.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,7 +28,7 @@ /* 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" @@ -37,7 +37,7 @@ #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] - @@ -52,7 +52,7 @@ float CameraMatrix::determinant() const { 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() { +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; @@ -60,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; @@ -68,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; @@ -78,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; @@ -87,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); } @@ -113,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); } @@ -145,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; @@ -179,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); @@ -191,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; } @@ -199,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); @@ -231,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; } @@ -239,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], @@ -252,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], @@ -263,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], @@ -291,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], @@ -319,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 }, @@ -342,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 @@ -425,13 +587,13 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform 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 */ @@ -529,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++) { @@ -555,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; @@ -576,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; @@ -597,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; @@ -618,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++) { @@ -629,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], @@ -667,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 { @@ -678,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; @@ -710,12 +872,12 @@ void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { matrix[3][3] = 1; } -void CameraMatrix::add_jitter_offset(const Vector2 &p_offset) { +void Projection::add_jitter_offset(const Vector2 &p_offset) { matrix[3][0] += p_offset.x; matrix[3][1] += p_offset.y; } -CameraMatrix::operator Transform3D() const { +Projection::operator Transform3D() const { Transform3D tr; const real_t *m = &matrix[0][0]; @@ -737,8 +899,13 @@ 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]; @@ -760,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 a4051cee3b..a3d2d7720b 100644 --- a/core/math/camera_matrix.h +++ b/core/math/projection.h @@ -1,5 +1,5 @@ /*************************************************************************/ -/* camera_matrix.h */ +/* projection.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,11 +28,12 @@ /* 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/math_defs.h" #include "core/math/vector3.h" +#include "core/math/vector4.h" #include "core/templates/vector.h" struct AABB; @@ -41,7 +42,7 @@ struct Rect2; struct Transform3D; struct Vector2; -struct CameraMatrix { +struct Projection { enum Planes { PLANE_NEAR, PLANE_FAR, @@ -51,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); @@ -68,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); } @@ -85,13 +112,16 @@ 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); @@ -102,7 +132,7 @@ struct CameraMatrix { 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]) { @@ -113,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]; @@ -133,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 11bfcc1a6f..c681c60694 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -111,7 +111,7 @@ Quaternion Quaternion::log() const { Quaternion Quaternion::exp() const { Quaternion src = *this; Vector3 src_v = Vector3(src.x, src.y, src.z); - float theta = src_v.length(); + real_t theta = src_v.length(); if (theta < CMP_EPSILON) { return Quaternion(0, 0, 0, 1); } @@ -132,15 +132,9 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con // adjust signs (if necessary) 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 @@ -189,16 +183,54 @@ 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 - //the only way to do slerp :| - real_t t2 = (1.0f - 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(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::operator String() const { @@ -213,7 +245,7 @@ Vector3 Quaternion::get_axis() const { return Vector3(x * r, y * r, z * r); } -float Quaternion::get_angle() const { +real_t Quaternion::get_angle() const { return 2 * Math::acos(w); } diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 9801746659..cb54a6f540 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -71,10 +71,10 @@ struct _NO_DISCARD_ Quaternion { 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; 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); diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index cbd2fd3fa1..226076029b 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -136,11 +136,11 @@ void Transform2D::scale_basis(const Size2 &p_scale) { 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) { +void Transform2D::translate_local(const Vector2 &p_translation) { columns[2] += basis_xform(p_translation); } @@ -217,18 +217,24 @@ 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.columns[2] = Vector2(); @@ -236,15 +242,23 @@ Transform2D Transform2D::untranslated() const { } 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::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 { - Transform2D copy = *this; - copy.rotate(p_angle); - return copy; + // 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 { diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index 72d34a5d4c..f23f32867a 100644 --- a/core/math/transform_2d.h +++ b/core/math/transform_2d.h @@ -74,8 +74,8 @@ struct _NO_DISCARD_ Transform2D { 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; @@ -85,10 +85,13 @@ struct _NO_DISCARD_ Transform2D { _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 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; diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index 76b31daa76..a634faca9a 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -62,7 +62,15 @@ void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) { } Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const { - return Transform3D(Basis(p_axis, p_angle), Vector3()) * (*this); + // Equivalent to left multiplication + Basis p_basis(p_axis, p_angle); + return Transform3D(p_basis * basis, p_basis.xform(origin)); +} + +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) { @@ -70,17 +78,23 @@ void Transform3D::rotate_basis(const Vector3 &p_axis, real_t 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::sphere_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; @@ -114,29 +128,37 @@ void Transform3D::scale(const Vector3 &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() { diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h index 25832434cd..b572e90859 100644 --- a/core/math/transform_3d.h +++ b/core/math/transform_3d.h @@ -46,6 +46,7 @@ struct _NO_DISCARD_ Transform3D { Transform3D affine_inverse() 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_angle); void rotate_basis(const Vector3 &p_axis, real_t p_angle); @@ -55,10 +56,12 @@ struct _NO_DISCARD_ Transform3D { 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; } @@ -100,7 +103,7 @@ struct _NO_DISCARD_ Transform3D { void operator*=(const real_t p_val); Transform3D operator*(const real_t p_val) const; - Transform3D sphere_interpolate_with(const Transform3D &p_transform, real_t p_c) 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 { diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 54461bf70f..4433559e6d 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -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,6 +123,7 @@ 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(); HashMap<Vector3, int> db; @@ -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; @@ -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; } } @@ -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; } } diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 1b99945698..166b4adb7a 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -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/vector2.cpp b/core/math/vector2.cpp index a27227905c..d9b5d55454 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -152,13 +152,6 @@ 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 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; -} - Vector2 Vector2::move_toward(const Vector2 &p_to, const real_t p_delta) const { Vector2 v = *this; Vector2 vd = p_to - v; diff --git a/core/math/vector2.h b/core/math/vector2.h index bd67299f33..91d3d3a56b 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -113,7 +113,9 @@ struct _NO_DISCARD_ 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 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; @@ -261,6 +263,26 @@ Vector2 Vector2::slerp(const Vector2 &p_to, const real_t p_weight) const { return rotated(angle * p_weight) * (result_length / start_length); } +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; +} + +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; + + /* 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; +} + Vector2 Vector2::direction_to(const Vector2 &p_to) const { Vector2 ret(p_to.x - x, p_to.y - y); ret.normalize(); diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index f94f39b7f2..d71d365053 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -85,14 +85,6 @@ 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 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::move_toward(const Vector3 &p_to, const real_t p_delta) const { Vector3 v = *this; Vector3 vd = p_to - v; diff --git a/core/math/vector3.h b/core/math/vector3.h index 8891532f42..4ce01da60e 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -104,7 +104,9 @@ struct _NO_DISCARD_ Vector3 { _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; + _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 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const; + Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; Vector2 octahedron_encode() const; @@ -215,16 +217,46 @@ 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 { + // 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(cross(p_to).normalized(), angle * p_weight) * (result_length / start_length); + 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::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 { diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp new file mode 100644 index 0000000000..4697c311b4 --- /dev/null +++ b/core/math/vector4.cpp @@ -0,0 +1,172 @@ +/*************************************************************************/ +/* 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(); +} + +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::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..373a6a1218 --- /dev/null +++ b/core/math/vector4.h @@ -0,0 +1,302 @@ +/*************************************************************************/ +/* 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; + 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 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 |