diff options
Diffstat (limited to 'core/math')
49 files changed, 2841 insertions, 637 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 41a0848d01..b4281820e2 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -209,8 +209,8 @@ bool AStar3D::has_point(int64_t p_id) const { return points.has(p_id); } -Array AStar3D::get_point_ids() { - Array point_list; +PackedInt64Array AStar3D::get_point_ids() { + PackedInt64Array point_list; for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { point_list.push_back(*(it.key)); @@ -605,7 +605,7 @@ Vector<int64_t> AStar2D::get_point_connections(int64_t p_id) { return astar.get_point_connections(p_id); } -Array AStar2D::get_point_ids() { +PackedInt64Array AStar2D::get_point_ids() { return astar.get_point_ids(); } diff --git a/core/math/a_star.h b/core/math/a_star.h index c1497d133f..a9e2a62bb2 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -133,7 +133,7 @@ public: 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(); + PackedInt64Array get_point_ids(); void set_point_disabled(int64_t p_id, bool p_disabled = true); bool is_point_disabled(int64_t p_id) const; @@ -183,7 +183,7 @@ public: 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(); + PackedInt64Array get_point_ids(); void set_point_disabled(int64_t p_id, bool p_disabled = true); bool is_point_disabled(int64_t p_id) const; diff --git a/core/math/a_star_grid_2d.cpp b/core/math/a_star_grid_2d.cpp new file mode 100644 index 0000000000..c30acf32bb --- /dev/null +++ b/core/math/a_star_grid_2d.cpp @@ -0,0 +1,589 @@ +/*************************************************************************/ +/* a_star_grid_2d.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 "a_star_grid_2d.h" + +#include "core/variant/typed_array.h" + +static real_t heuristic_euclidian(const Vector2i &p_from, const Vector2i &p_to) { + real_t dx = (real_t)ABS(p_to.x - p_from.x); + real_t dy = (real_t)ABS(p_to.y - p_from.y); + return (real_t)Math::sqrt(dx * dx + dy * dy); +} + +static real_t heuristic_manhattan(const Vector2i &p_from, const Vector2i &p_to) { + real_t dx = (real_t)ABS(p_to.x - p_from.x); + real_t dy = (real_t)ABS(p_to.y - p_from.y); + return dx + dy; +} + +static real_t heuristic_octile(const Vector2i &p_from, const Vector2i &p_to) { + real_t dx = (real_t)ABS(p_to.x - p_from.x); + real_t dy = (real_t)ABS(p_to.y - p_from.y); + real_t F = Math_SQRT2 - 1; + return (dx < dy) ? F * dx + dy : F * dy + dx; +} + +static real_t heuristic_chebyshev(const Vector2i &p_from, const Vector2i &p_to) { + real_t dx = (real_t)ABS(p_to.x - p_from.x); + real_t dy = (real_t)ABS(p_to.y - p_from.y); + return MAX(dx, dy); +} + +static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_euclidian, heuristic_manhattan, heuristic_octile, heuristic_chebyshev }; + +void AStarGrid2D::set_size(const Size2i &p_size) { + ERR_FAIL_COND(p_size.x < 0 || p_size.y < 0); + if (p_size != size) { + size = p_size; + dirty = true; + } +} + +Size2i AStarGrid2D::get_size() const { + return size; +} + +void AStarGrid2D::set_offset(const Vector2 &p_offset) { + if (!offset.is_equal_approx(p_offset)) { + offset = p_offset; + dirty = true; + } +} + +Vector2 AStarGrid2D::get_offset() const { + return offset; +} + +void AStarGrid2D::set_cell_size(const Size2 &p_cell_size) { + if (!cell_size.is_equal_approx(p_cell_size)) { + cell_size = p_cell_size; + dirty = true; + } +} + +Size2 AStarGrid2D::get_cell_size() const { + return cell_size; +} + +void AStarGrid2D::update() { + points.clear(); + for (int64_t y = 0; y < size.y; y++) { + LocalVector<Point> line; + for (int64_t x = 0; x < size.x; x++) { + line.push_back(Point(Vector2i(x, y), offset + Vector2(x, y) * cell_size)); + } + points.push_back(line); + } + dirty = false; +} + +bool AStarGrid2D::is_in_bounds(int p_x, int p_y) const { + return p_x >= 0 && p_x < size.width && p_y >= 0 && p_y < size.height; +} + +bool AStarGrid2D::is_in_boundsv(const Vector2i &p_id) const { + return p_id.x >= 0 && p_id.x < size.width && p_id.y >= 0 && p_id.y < size.height; +} + +bool AStarGrid2D::is_dirty() const { + return dirty; +} + +void AStarGrid2D::set_jumping_enabled(bool p_enabled) { + jumping_enabled = p_enabled; +} + +bool AStarGrid2D::is_jumping_enabled() const { + return jumping_enabled; +} + +void AStarGrid2D::set_diagonal_mode(DiagonalMode p_diagonal_mode) { + ERR_FAIL_INDEX((int)p_diagonal_mode, (int)DIAGONAL_MODE_MAX); + diagonal_mode = p_diagonal_mode; +} + +AStarGrid2D::DiagonalMode AStarGrid2D::get_diagonal_mode() const { + return diagonal_mode; +} + +void AStarGrid2D::set_default_heuristic(Heuristic p_heuristic) { + ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX); + default_heuristic = p_heuristic; +} + +AStarGrid2D::Heuristic AStarGrid2D::get_default_heuristic() const { + return default_heuristic; +} + +void AStarGrid2D::set_point_solid(const Vector2i &p_id, bool p_solid) { + ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method."); + ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set if point is disabled. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height)); + points[p_id.y][p_id.x].solid = p_solid; +} + +bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const { + ERR_FAIL_COND_V_MSG(dirty, false, "Grid is not initialized. Call the update method."); + ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), false, vformat("Can't get if point is disabled. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height)); + return points[p_id.y][p_id.x].solid; +} + +AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) { + if (!p_to || p_to->solid) { + return nullptr; + } + if (p_to == end) { + return p_to; + } + + int64_t from_x = p_from->id.x; + int64_t from_y = p_from->id.y; + + int64_t to_x = p_to->id.x; + int64_t to_y = p_to->id.y; + + int64_t dx = to_x - from_x; + int64_t dy = to_y - from_y; + + if (diagonal_mode == DIAGONAL_MODE_ALWAYS || diagonal_mode == DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE) { + if (dx != 0 && dy != 0) { + if ((_is_walkable(to_x - dx, to_y + dy) && !_is_walkable(to_x - dx, to_y)) || (_is_walkable(to_x + dx, to_y - dy) && !_is_walkable(to_x, to_y - dy))) { + return p_to; + } + if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) { + return p_to; + } + if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) { + return p_to; + } + } else { + if (dx != 0) { + if ((_is_walkable(to_x + dx, to_y + 1) && !_is_walkable(to_x, to_y + 1)) || (_is_walkable(to_x + dx, to_y - 1) && !_is_walkable(to_x, to_y - 1))) { + return p_to; + } + } else { + if ((_is_walkable(to_x + 1, to_y + dy) && !_is_walkable(to_x + 1, to_y)) || (_is_walkable(to_x - 1, to_y + dy) && !_is_walkable(to_x - 1, to_y))) { + return p_to; + } + } + } + if (_is_walkable(to_x + dx, to_y + dy) && (diagonal_mode == DIAGONAL_MODE_ALWAYS || (_is_walkable(to_x + dx, to_y) || _is_walkable(to_x, to_y + dy)))) { + return _jump(p_to, _get_point(to_x + dx, to_y + dy)); + } + } else if (diagonal_mode == DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES) { + if (dx != 0 && dy != 0) { + if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) { + return p_to; + } + if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) { + return p_to; + } + if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) { + return p_to; + } + } else { + if (dx != 0) { + if ((_is_walkable(to_x, to_y + 1) && !_is_walkable(to_x - dx, to_y + 1)) || (_is_walkable(to_x, to_y - 1) && !_is_walkable(to_x - dx, to_y - 1))) { + return p_to; + } + } else { + if ((_is_walkable(to_x + 1, to_y) && !_is_walkable(to_x + 1, to_y - dy)) || (_is_walkable(to_x - 1, to_y) && !_is_walkable(to_x - 1, to_y - dy))) { + return p_to; + } + } + } + if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) { + return _jump(p_to, _get_point(to_x + dx, to_y + dy)); + } + } else { // DIAGONAL_MODE_NEVER + if (dx != 0) { + if (!_is_walkable(to_x + dx, to_y)) { + return p_to; + } + if (_jump(p_to, _get_point(to_x, to_y + 1)) != nullptr) { + return p_to; + } + if (_jump(p_to, _get_point(to_x, to_y - 1)) != nullptr) { + return p_to; + } + } else { + if (!_is_walkable(to_x, to_y + dy)) { + return p_to; + } + if (_jump(p_to, _get_point(to_x + 1, to_y)) != nullptr) { + return p_to; + } + if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) { + return p_to; + } + } + if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) { + return _jump(p_to, _get_point(to_x + dx, to_y + dy)); + } + } + return nullptr; +} + +void AStarGrid2D::_get_nbors(Point *p_point, List<Point *> &r_nbors) { + bool ts0 = false, td0 = false, + ts1 = false, td1 = false, + ts2 = false, td2 = false, + ts3 = false, td3 = false; + + Point *left = nullptr; + Point *right = nullptr; + Point *top = nullptr; + Point *bottom = nullptr; + + Point *top_left = nullptr; + Point *top_right = nullptr; + Point *bottom_left = nullptr; + Point *bottom_right = nullptr; + + { + bool has_left = false; + bool has_right = false; + + if (p_point->id.x - 1 >= 0) { + left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y); + has_left = true; + } + if (p_point->id.x + 1 < size.width) { + right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y); + has_right = true; + } + if (p_point->id.y - 1 >= 0) { + top = _get_point_unchecked(p_point->id.x, p_point->id.y - 1); + if (has_left) { + top_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y - 1); + } + if (has_right) { + top_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y - 1); + } + } + if (p_point->id.y + 1 < size.height) { + bottom = _get_point_unchecked(p_point->id.x, p_point->id.y + 1); + if (has_left) { + bottom_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y + 1); + } + if (has_right) { + bottom_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y + 1); + } + } + } + + if (top && !top->solid) { + r_nbors.push_back(top); + ts0 = true; + } + if (right && !right->solid) { + r_nbors.push_back(right); + ts1 = true; + } + if (bottom && !bottom->solid) { + r_nbors.push_back(bottom); + ts2 = true; + } + if (left && !left->solid) { + r_nbors.push_back(left); + ts3 = true; + } + + switch (diagonal_mode) { + case DIAGONAL_MODE_ALWAYS: { + td0 = true; + td1 = true; + td2 = true; + td3 = true; + } break; + case DIAGONAL_MODE_NEVER: { + } break; + case DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE: { + td0 = ts3 || ts0; + td1 = ts0 || ts1; + td2 = ts1 || ts2; + td3 = ts2 || ts3; + } break; + case DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES: { + td0 = ts3 && ts0; + td1 = ts0 && ts1; + td2 = ts1 && ts2; + td3 = ts2 && ts3; + } break; + default: + break; + } + + if (td0 && (top_left && !top_left->solid)) { + r_nbors.push_back(top_left); + } + if (td1 && (top_right && !top_right->solid)) { + r_nbors.push_back(top_right); + } + if (td2 && (bottom_right && !bottom_right->solid)) { + r_nbors.push_back(bottom_right); + } + if (td3 && (bottom_left && !bottom_left->solid)) { + r_nbors.push_back(bottom_left); + } +} + +bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) { + pass++; + + if (p_end_point->solid) { + return false; + } + + bool found_route = false; + + Vector<Point *> open_list; + SortArray<Point *, SortPoints> sorter; + + p_begin_point->g_score = 0; + p_begin_point->f_score = _estimate_cost(p_begin_point->id, p_end_point->id); + open_list.push_back(p_begin_point); + end = p_end_point; + + while (!open_list.is_empty()) { + Point *p = open_list[0]; // The currently processed point. + + if (p == p_end_point) { + found_route = true; + break; + } + + sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list. + open_list.remove_at(open_list.size() - 1); + p->closed_pass = pass; // Mark the point as closed. + + List<Point *> nbors; + _get_nbors(p, nbors); + for (List<Point *>::Element *E = nbors.front(); E; E = E->next()) { + Point *e = E->get(); // The neighbour point. + if (jumping_enabled) { + e = _jump(p, e); + if (!e || e->closed_pass == pass) { + continue; + } + } else { + if (e->solid || e->closed_pass == pass) { + continue; + } + } + + real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id); + bool new_point = false; + + if (e->open_pass != pass) { // The point wasn't inside the open list. + e->open_pass = pass; + open_list.push_back(e); + new_point = true; + } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous. + continue; + } + + e->prev_point = p; + e->g_score = tentative_g_score; + e->f_score = e->g_score + _estimate_cost(e->id, p_end_point->id); + + if (new_point) { // The position of the new points is already known. + sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw()); + } else { + sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw()); + } + } + } + + return found_route; +} + +real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) { + real_t scost; + if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) { + return scost; + } + return heuristics[default_heuristic](p_from_id, p_to_id); +} + +real_t AStarGrid2D::_compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) { + real_t scost; + if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) { + return scost; + } + return heuristics[default_heuristic](p_from_id, p_to_id); +} + +void AStarGrid2D::clear() { + points.clear(); + size = Vector2i(); +} + +Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id) { + ERR_FAIL_COND_V_MSG(dirty, Vector<Vector2>(), "Grid is not initialized. Call the update method."); + ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), Vector<Vector2>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_from_id.x, size.width, p_from_id.y, size.height)); + ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), Vector<Vector2>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_to_id.x, size.width, p_to_id.y, size.height)); + + Point *a = _get_point(p_from_id.x, p_from_id.y); + Point *b = _get_point(p_to_id.x, p_to_id.y); + + if (a == b) { + Vector<Vector2> ret; + ret.push_back(a->pos); + return ret; + } + + Point *begin_point = a; + Point *end_point = b; + + bool found_route = _solve(begin_point, end_point); + if (!found_route) { + return Vector<Vector2>(); + } + + Point *p = end_point; + int64_t pc = 1; + while (p != begin_point) { + pc++; + p = p->prev_point; + } + + Vector<Vector2> path; + path.resize(pc); + + { + Vector2 *w = path.ptrw(); + + p = end_point; + int64_t idx = pc - 1; + while (p != begin_point) { + w[idx--] = p->pos; + p = p->prev_point; + } + + w[0] = p->pos; + } + + return path; +} + +TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id) { + ERR_FAIL_COND_V_MSG(dirty, TypedArray<Vector2i>(), "Grid is not initialized. Call the update method."); + ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_from_id.x, size.width, p_from_id.y, size.height)); + ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_to_id.x, size.width, p_to_id.y, size.height)); + + Point *a = _get_point(p_from_id.x, p_from_id.y); + Point *b = _get_point(p_to_id.x, p_to_id.y); + + if (a == b) { + TypedArray<Vector2i> ret; + ret.push_back(a); + return ret; + } + + Point *begin_point = a; + Point *end_point = b; + + bool found_route = _solve(begin_point, end_point); + if (!found_route) { + return TypedArray<Vector2i>(); + } + + Point *p = end_point; + int64_t pc = 1; + while (p != begin_point) { + pc++; + p = p->prev_point; + } + + TypedArray<Vector2i> path; + path.resize(pc); + + { + p = end_point; + int64_t idx = pc - 1; + while (p != begin_point) { + path[idx--] = p->id; + p = p->prev_point; + } + + path[0] = p->id; + } + + return path; +} + +void AStarGrid2D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_size", "size"), &AStarGrid2D::set_size); + ClassDB::bind_method(D_METHOD("get_size"), &AStarGrid2D::get_size); + ClassDB::bind_method(D_METHOD("set_offset", "offset"), &AStarGrid2D::set_offset); + ClassDB::bind_method(D_METHOD("get_offset"), &AStarGrid2D::get_offset); + ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &AStarGrid2D::set_cell_size); + ClassDB::bind_method(D_METHOD("get_cell_size"), &AStarGrid2D::get_cell_size); + ClassDB::bind_method(D_METHOD("is_in_bounds", "x", "y"), &AStarGrid2D::is_in_bounds); + ClassDB::bind_method(D_METHOD("is_in_boundsv", "id"), &AStarGrid2D::is_in_boundsv); + ClassDB::bind_method(D_METHOD("is_dirty"), &AStarGrid2D::is_dirty); + ClassDB::bind_method(D_METHOD("update"), &AStarGrid2D::update); + ClassDB::bind_method(D_METHOD("set_jumping_enabled", "enabled"), &AStarGrid2D::set_jumping_enabled); + ClassDB::bind_method(D_METHOD("is_jumping_enabled"), &AStarGrid2D::is_jumping_enabled); + ClassDB::bind_method(D_METHOD("set_diagonal_mode", "mode"), &AStarGrid2D::set_diagonal_mode); + ClassDB::bind_method(D_METHOD("get_diagonal_mode"), &AStarGrid2D::get_diagonal_mode); + ClassDB::bind_method(D_METHOD("set_default_heuristic", "heuristic"), &AStarGrid2D::set_default_heuristic); + ClassDB::bind_method(D_METHOD("get_default_heuristic"), &AStarGrid2D::get_default_heuristic); + ClassDB::bind_method(D_METHOD("set_point_solid", "id", "solid"), &AStarGrid2D::set_point_solid, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("is_point_solid", "id"), &AStarGrid2D::is_point_solid); + ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear); + + ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::get_point_path); + ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::get_id_path); + + GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id") + GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id") + + ADD_PROPERTY(PropertyInfo(Variant::VECTOR2I, "size"), "set_size", "get_size"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "offset"), "set_offset", "get_offset"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "cell_size"), "set_cell_size", "get_cell_size"); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "jumping_enabled"), "set_jumping_enabled", "is_jumping_enabled"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "default_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev,Max"), "set_default_heuristic", "get_default_heuristic"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "diagonal_mode", PROPERTY_HINT_ENUM, "Never,Always,At Least One Walkable,Only If No Obstacles,Max"), "set_diagonal_mode", "get_diagonal_mode"); + + BIND_ENUM_CONSTANT(HEURISTIC_EUCLIDEAN); + BIND_ENUM_CONSTANT(HEURISTIC_MANHATTAN); + BIND_ENUM_CONSTANT(HEURISTIC_OCTILE); + BIND_ENUM_CONSTANT(HEURISTIC_CHEBYSHEV); + BIND_ENUM_CONSTANT(HEURISTIC_MAX); + + BIND_ENUM_CONSTANT(DIAGONAL_MODE_ALWAYS); + BIND_ENUM_CONSTANT(DIAGONAL_MODE_NEVER); + BIND_ENUM_CONSTANT(DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE); + BIND_ENUM_CONSTANT(DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES); + BIND_ENUM_CONSTANT(DIAGONAL_MODE_MAX); +} diff --git a/core/math/a_star_grid_2d.h b/core/math/a_star_grid_2d.h new file mode 100644 index 0000000000..1002f18738 --- /dev/null +++ b/core/math/a_star_grid_2d.h @@ -0,0 +1,178 @@ +/*************************************************************************/ +/* a_star_grid_2d.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 A_STAR_GRID_2D_H +#define A_STAR_GRID_2D_H + +#include "core/object/gdvirtual.gen.inc" +#include "core/object/ref_counted.h" +#include "core/object/script_language.h" +#include "core/templates/list.h" +#include "core/templates/local_vector.h" + +class AStarGrid2D : public RefCounted { + GDCLASS(AStarGrid2D, RefCounted); + +public: + enum DiagonalMode { + DIAGONAL_MODE_ALWAYS, + DIAGONAL_MODE_NEVER, + DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE, + DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES, + DIAGONAL_MODE_MAX, + }; + + enum Heuristic { + HEURISTIC_EUCLIDEAN, + HEURISTIC_MANHATTAN, + HEURISTIC_OCTILE, + HEURISTIC_CHEBYSHEV, + HEURISTIC_MAX, + }; + +private: + Size2i size; + Vector2 offset; + Size2 cell_size = Size2(1, 1); + bool dirty = false; + + bool jumping_enabled = false; + DiagonalMode diagonal_mode = DIAGONAL_MODE_ALWAYS; + Heuristic default_heuristic = HEURISTIC_EUCLIDEAN; + + struct Point { + Vector2i id; + + bool solid = false; + Vector2 pos; + + // Used for pathfinding. + Point *prev_point = nullptr; + real_t g_score = 0; + real_t f_score = 0; + uint64_t open_pass = 0; + uint64_t closed_pass = 0; + + Point() {} + + Point(const Vector2i &p_id, const Vector2 &p_pos) : + id(p_id), pos(p_pos) {} + }; + + struct SortPoints { + _FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B. + if (A->f_score > B->f_score) { + return true; + } else if (A->f_score < B->f_score) { + return false; + } else { + return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start. + } + } + }; + + LocalVector<LocalVector<Point>> points; + Point *end = nullptr; + + uint64_t pass = 1; + +private: // Internal routines. + _FORCE_INLINE_ bool _is_walkable(int64_t p_x, int64_t p_y) const { + if (p_x >= 0 && p_y >= 0 && p_x < size.width && p_y < size.height) { + return !points[p_y][p_x].solid; + } + return false; + } + + _FORCE_INLINE_ Point *_get_point(int64_t p_x, int64_t p_y) { + if (p_x >= 0 && p_y >= 0 && p_x < size.width && p_y < size.height) { + return &points[p_y][p_x]; + } + return nullptr; + } + + _FORCE_INLINE_ Point *_get_point_unchecked(int64_t p_x, int64_t p_y) { + return &points[p_y][p_x]; + } + + void _get_nbors(Point *p_point, List<Point *> &r_nbors); + Point *_jump(Point *p_from, Point *p_to); + bool _solve(Point *p_begin_point, Point *p_end_point); + +protected: + static void _bind_methods(); + + virtual real_t _estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id); + virtual real_t _compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id); + + GDVIRTUAL2RC(real_t, _estimate_cost, Vector2i, Vector2i) + GDVIRTUAL2RC(real_t, _compute_cost, Vector2i, Vector2i) + +public: + void set_size(const Size2i &p_size); + Size2i get_size() const; + + void set_offset(const Vector2 &p_offset); + Vector2 get_offset() const; + + void set_cell_size(const Size2 &p_cell_size); + Size2 get_cell_size() const; + + void update(); + + int get_width() const; + int get_height() const; + + bool is_in_bounds(int p_x, int p_y) const; + bool is_in_boundsv(const Vector2i &p_id) const; + bool is_dirty() const; + + void set_jumping_enabled(bool p_enabled); + bool is_jumping_enabled() const; + + void set_diagonal_mode(DiagonalMode p_diagonal_mode); + DiagonalMode get_diagonal_mode() const; + + void set_default_heuristic(Heuristic p_heuristic); + Heuristic get_default_heuristic() const; + + void set_point_solid(const Vector2i &p_id, bool p_solid = true); + bool is_point_solid(const Vector2i &p_id) const; + + void clear(); + + Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to); + TypedArray<Vector2i> get_id_path(const Vector2i &p_from, const Vector2i &p_to); +}; + +VARIANT_ENUM_CAST(AStarGrid2D::DiagonalMode); +VARIANT_ENUM_CAST(AStarGrid2D::Heuristic); + +#endif // A_STAR_GRID_2D_H diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 4c89be7f4d..fcf245d2ad 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -30,7 +30,7 @@ #include "aabb.h" -#include "core/string/print_string.h" +#include "core/string/ustring.h" #include "core/variant/variant.h" real_t AABB::get_volume() const { @@ -76,6 +76,10 @@ bool AABB::is_equal_approx(const AABB &p_aabb) const { return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size); } +bool AABB::is_finite() const { + return position.is_finite() && size.is_finite(); +} + AABB AABB::intersection(const AABB &p_aabb) const { #ifdef MATH_CHECKS if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { @@ -403,6 +407,7 @@ Variant AABB::intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to } return Variant(); } + Variant AABB::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const { Vector3 inters; if (intersects_ray(p_from, p_dir, &inters)) { diff --git a/core/math/aabb.h b/core/math/aabb.h index e88ba33531..9d5837ad37 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -31,7 +31,6 @@ #ifndef AABB_H #define AABB_H -#include "core/math/math_defs.h" #include "core/math/plane.h" #include "core/math/vector3.h" @@ -47,12 +46,12 @@ struct _NO_DISCARD_ AABB { Vector3 size; real_t get_volume() const; - _FORCE_INLINE_ bool has_no_volume() const { - return (size.x <= 0 || size.y <= 0 || size.z <= 0); + _FORCE_INLINE_ bool has_volume() const { + return size.x > 0.0f && size.y > 0.0f && size.z > 0.0f; } - _FORCE_INLINE_ bool has_no_surface() const { - return (size.x <= 0 && size.y <= 0 && size.z <= 0); + _FORCE_INLINE_ bool has_surface() const { + return size.x > 0.0f || size.y > 0.0f || size.z > 0.0f; } const Vector3 &get_position() const { return position; } @@ -64,6 +63,7 @@ struct _NO_DISCARD_ AABB { bool operator!=(const AABB &p_rval) const; bool is_equal_approx(const AABB &p_aabb) const; + bool is_finite() const; _FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap _FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap _FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this @@ -101,7 +101,7 @@ struct _NO_DISCARD_ AABB { _FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */ _FORCE_INLINE_ AABB abs() const { - return AABB(Vector3(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0), position.z + MIN(size.z, 0)), size.abs()); + return AABB(Vector3(position.x + MIN(size.x, (real_t)0), position.y + MIN(size.y, (real_t)0), position.z + MIN(size.z, (real_t)0)), size.abs()); } Variant intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const; diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index b3d63c0094..1a80faaa12 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -48,7 +48,7 @@ static inline float undenormalise(volatile float f) { } static const float AUDIO_PEAK_OFFSET = 0.0000000001f; -static const float AUDIO_MIN_PEAK_DB = -200.0f; // linear2db(AUDIO_PEAK_OFFSET) +static const float AUDIO_MIN_PEAK_DB = -200.0f; // linear_to_db(AUDIO_PEAK_OFFSET) struct AudioFrame { //left and right samples diff --git a/core/math/basis.cpp b/core/math/basis.cpp index f8e7c47107..9b8188eed8 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -31,7 +31,7 @@ #include "basis.h" #include "core/math/math_funcs.h" -#include "core/string/print_string.h" +#include "core/string/ustring.h" #define cofac(row1, col1, row2, col2) \ (rows[row1][col1] * rows[row2][col2] - rows[row1][col2] * rows[row2][col1]) @@ -142,8 +142,8 @@ bool Basis::is_symmetric() const { #endif Basis Basis::diagonalize() { -//NOTE: only implemented for symmetric matrices -//with the Jacobi iterative method +// NOTE: only implemented for symmetric matrices +// with the Jacobi iterative method #ifdef MATH_CHECKS ERR_FAIL_COND_V(!is_symmetric(), Basis()); #endif @@ -691,6 +691,10 @@ bool Basis::is_equal_approx(const Basis &p_basis) const { return rows[0].is_equal_approx(p_basis.rows[0]) && rows[1].is_equal_approx(p_basis.rows[1]) && rows[2].is_equal_approx(p_basis.rows[2]); } +bool Basis::is_finite() const { + return rows[0].is_finite() && rows[1].is_finite() && rows[2].is_finite(); +} + bool Basis::operator==(const Basis &p_matrix) const { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -749,95 +753,33 @@ Quaternion Basis::get_quaternion() const { return Quaternion(temp[0], temp[1], temp[2], temp[3]); } -static const Basis _ortho_bases[24] = { - Basis(1, 0, 0, 0, 1, 0, 0, 0, 1), - Basis(0, -1, 0, 1, 0, 0, 0, 0, 1), - Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1), - Basis(0, 1, 0, -1, 0, 0, 0, 0, 1), - Basis(1, 0, 0, 0, 0, -1, 0, 1, 0), - Basis(0, 0, 1, 1, 0, 0, 0, 1, 0), - Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0), - Basis(0, 0, -1, -1, 0, 0, 0, 1, 0), - Basis(1, 0, 0, 0, -1, 0, 0, 0, -1), - Basis(0, 1, 0, 1, 0, 0, 0, 0, -1), - Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1), - Basis(0, -1, 0, -1, 0, 0, 0, 0, -1), - Basis(1, 0, 0, 0, 0, 1, 0, -1, 0), - Basis(0, 0, -1, 1, 0, 0, 0, -1, 0), - Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0), - Basis(0, 0, 1, -1, 0, 0, 0, -1, 0), - Basis(0, 0, 1, 0, 1, 0, -1, 0, 0), - Basis(0, -1, 0, 0, 0, 1, -1, 0, 0), - Basis(0, 0, -1, 0, -1, 0, -1, 0, 0), - Basis(0, 1, 0, 0, 0, -1, -1, 0, 0), - Basis(0, 0, 1, 0, -1, 0, 1, 0, 0), - Basis(0, 1, 0, 0, 0, 1, 1, 0, 0), - Basis(0, 0, -1, 0, 1, 0, 1, 0, 0), - Basis(0, -1, 0, 0, 0, -1, 1, 0, 0) -}; - -int Basis::get_orthogonal_index() const { - //could be sped up if i come up with a way - Basis orth = *this; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - real_t v = orth[i][j]; - if (v > 0.5f) { - v = 1.0f; - } else if (v < -0.5f) { - v = -1.0f; - } else { - v = 0; - } - - orth[i][j] = v; - } - } - - for (int i = 0; i < 24; i++) { - if (_ortho_bases[i] == orth) { - return i; - } - } - - return 0; -} - -void Basis::set_orthogonal_index(int p_index) { - //there only exist 24 orthogonal bases in r3 - ERR_FAIL_INDEX(p_index, 24); - - *this = _ortho_bases[p_index]; -} - void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { /* checking this is a bad idea, because obtaining from scaled transform is a valid use case #ifdef MATH_CHECKS ERR_FAIL_COND(!is_rotation()); #endif -*/ - real_t angle, x, y, z; // variables for result - real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees - - 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]) < 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 + */ + + // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm + real_t x, y, z; // Variables for result. + if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[2][1])) { + // Singularity found. + // First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms. + if (is_diagonal() && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) { + // This singularity is identity matrix so angle = 0. r_axis = Vector3(0, 1, 0); r_angle = 0; return; } - // otherwise this singularity is angle = 180 - angle = Math_PI; + // Otherwise this singularity is angle = 180. real_t xx = (rows[0][0] + 1) / 2; real_t yy = (rows[1][1] + 1) / 2; real_t zz = (rows[2][2] + 1) / 2; - real_t xy = (rows[1][0] + rows[0][1]) / 4; - real_t xz = (rows[2][0] + rows[0][2]) / 4; - real_t yz = (rows[2][1] + rows[1][2]) / 4; - if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term + real_t xy = (rows[0][1] + rows[1][0]) / 4; + real_t xz = (rows[0][2] + rows[2][0]) / 4; + real_t yz = (rows[1][2] + rows[2][1]) / 4; + + if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term. if (xx < CMP_EPSILON) { x = 0; y = Math_SQRT12; @@ -847,7 +789,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { y = xy / x; z = xz / x; } - } else if (yy > zz) { // rows[1][1] is the largest diagonal term + } else if (yy > zz) { // rows[1][1] is the largest diagonal term. if (yy < CMP_EPSILON) { x = Math_SQRT12; y = 0; @@ -857,7 +799,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { x = xy / y; z = yz / y; } - } else { // rows[2][2] is the largest diagonal term so base result on this + } else { // rows[2][2] is the largest diagonal term so base result on this. if (zz < CMP_EPSILON) { x = Math_SQRT12; y = Math_SQRT12; @@ -869,22 +811,24 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { } } r_axis = Vector3(x, y, z); - r_angle = angle; + r_angle = Math_PI; return; } - // as we have reached here there are no singularities so we can handle normally - real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise + // As we have reached here there are no singularities so we can handle normally. + double s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise. - angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2); - if (angle < 0) { - s = -s; + if (Math::abs(s) < CMP_EPSILON) { + // Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above. + s = 1; } + x = (rows[2][1] - rows[1][2]) / s; y = (rows[0][2] - rows[2][0]) / s; z = (rows[1][0] - rows[0][1]) / s; r_axis = Vector3(x, y, z); - r_angle = angle; + // CLAMP to avoid NaN if the value passed to acos is not in [0,1]. + r_angle = Math::acos(CLAMP((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2, (real_t)0.0, (real_t)1.0)); } void Basis::set_quaternion(const Quaternion &p_quaternion) { @@ -1094,13 +1038,13 @@ void Basis::rotate_sh(real_t *p_values) { Basis Basis::looking_at(const Vector3 &p_target, const Vector3 &p_up) { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(p_target.is_equal_approx(Vector3()), Basis(), "The target vector can't be zero."); - ERR_FAIL_COND_V_MSG(p_up.is_equal_approx(Vector3()), Basis(), "The up vector can't be zero."); + ERR_FAIL_COND_V_MSG(p_target.is_zero_approx(), Basis(), "The target vector can't be zero."); + ERR_FAIL_COND_V_MSG(p_up.is_zero_approx(), Basis(), "The up vector can't be zero."); #endif Vector3 v_z = -p_target.normalized(); Vector3 v_x = p_up.cross(v_z); #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(v_x.is_equal_approx(Vector3()), Basis(), "The target vector and up vector can't be parallel to each other."); + ERR_FAIL_COND_V_MSG(v_x.is_zero_approx(), Basis(), "The target vector and up vector can't be parallel to each other."); #endif v_x.normalize(); Vector3 v_y = v_z.cross(v_x); diff --git a/core/math/basis.h b/core/math/basis.h index 4be325cdd2..69bef5a7be 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -134,6 +134,7 @@ struct _NO_DISCARD_ Basis { } bool is_equal_approx(const Basis &p_basis) const; + bool is_finite() const; bool operator==(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const; @@ -149,9 +150,6 @@ struct _NO_DISCARD_ Basis { _FORCE_INLINE_ void operator*=(const real_t p_val); _FORCE_INLINE_ Basis operator*(const real_t p_val) const; - int get_orthogonal_index() const; - void set_orthogonal_index(int p_index); - bool is_orthogonal() const; bool is_diagonal() const; bool is_rotation() const; @@ -241,10 +239,8 @@ struct _NO_DISCARD_ Basis { Basis(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_angle, p_scale); } static Basis from_scale(const Vector3 &p_scale); - _FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) { - rows[0] = row0; - rows[1] = row1; - rows[2] = row2; + _FORCE_INLINE_ Basis(const Vector3 &p_x_axis, const Vector3 &p_y_axis, const Vector3 &p_z_axis) { + set_columns(p_x_axis, p_y_axis, p_z_axis); } _FORCE_INLINE_ Basis() {} 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_abb.h b/core/math/bvh_abb.h index 8a44f1c4da..699f7de604 100644 --- a/core/math/bvh_abb.h +++ b/core/math/bvh_abb.h @@ -251,7 +251,9 @@ struct BVH_ABB { void expand(real_t p_change) { POINT change; - change.set_all(p_change); + for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) { + change[axis] = p_change; + } grow(change); } @@ -262,7 +264,9 @@ struct BVH_ABB { } void set_to_max_opposite_extents() { - neg_max.set_all(FLT_MAX); + for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) { + neg_max[axis] = FLT_MAX; + } min = neg_max; } 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/bvh_split.inc b/core/math/bvh_split.inc index ff07166d4a..180bbfb511 100644 --- a/core/math/bvh_split.inc +++ b/core/math/bvh_split.inc @@ -13,7 +13,7 @@ void _split_inform_references(uint32_t p_node_id) { void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) { // special case for low leaf sizes .. should static compile out - if (MAX_ITEMS < 4) { + if constexpr (MAX_ITEMS < 4) { uint32_t ind = group_a[0]; // add to b @@ -34,7 +34,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u order[POINT::AXIS_COUNT - 1] = size.max_axis_index(); static_assert(POINT::AXIS_COUNT <= 3, "BVH POINT::AXIS_COUNT has unexpected size"); - if (POINT::AXIS_COUNT == 3) { + if constexpr (POINT::AXIS_COUNT == 3) { order[1] = 3 - (order[0] + order[2]); } diff --git a/core/math/bvh_structs.inc b/core/math/bvh_structs.inc index 58c8f0479a..06f6e5d05d 100644 --- a/core/math/bvh_structs.inc +++ b/core/math/bvh_structs.inc @@ -100,7 +100,11 @@ public: num_items++; return id; } +#ifdef DEV_ENABLED return -1; +#else + ERR_FAIL_V_MSG(0, "BVH request_item error."); +#endif } }; diff --git a/core/math/bvh_tree.h b/core/math/bvh_tree.h index cdb2bb4413..3836e92a83 100644 --- a/core/math/bvh_tree.h +++ b/core/math/bvh_tree.h @@ -43,7 +43,6 @@ #include "core/math/bvh_abb.h" #include "core/math/geometry_3d.h" #include "core/math/vector3.h" -#include "core/string/print_string.h" #include "core/templates/local_vector.h" #include "core/templates/pooled_list.h" #include <limits.h> @@ -235,7 +234,7 @@ private: // no need to keep back references for children at the moment - uint32_t sibling_id; // always a node id, as tnode is never a leaf + uint32_t sibling_id = 0; // always a node id, as tnode is never a leaf bool sibling_present = false; // if there are more children, or this is the root node, don't try and delete diff --git a/core/math/color.cpp b/core/math/color.cpp index 4bdeafd2f2..f223853f6b 100644 --- a/core/math/color.cpp +++ b/core/math/color.cpp @@ -32,85 +32,85 @@ #include "color_names.inc" #include "core/math/math_funcs.h" -#include "core/string/print_string.h" +#include "core/string/ustring.h" #include "core/templates/rb_map.h" #include "thirdparty/misc/ok_color.h" uint32_t Color::to_argb32() const { - uint32_t c = (uint8_t)Math::round(a * 255); + uint32_t c = (uint8_t)Math::round(a * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(r * 255); + c |= (uint8_t)Math::round(r * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(g * 255); + c |= (uint8_t)Math::round(g * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(b * 255); + c |= (uint8_t)Math::round(b * 255.0f); return c; } uint32_t Color::to_abgr32() const { - uint32_t c = (uint8_t)Math::round(a * 255); + uint32_t c = (uint8_t)Math::round(a * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(b * 255); + c |= (uint8_t)Math::round(b * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(g * 255); + c |= (uint8_t)Math::round(g * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(r * 255); + c |= (uint8_t)Math::round(r * 255.0f); return c; } uint32_t Color::to_rgba32() const { - uint32_t c = (uint8_t)Math::round(r * 255); + uint32_t c = (uint8_t)Math::round(r * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(g * 255); + c |= (uint8_t)Math::round(g * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(b * 255); + c |= (uint8_t)Math::round(b * 255.0f); c <<= 8; - c |= (uint8_t)Math::round(a * 255); + c |= (uint8_t)Math::round(a * 255.0f); return c; } uint64_t Color::to_abgr64() const { - uint64_t c = (uint16_t)Math::round(a * 65535); + uint64_t c = (uint16_t)Math::round(a * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(b * 65535); + c |= (uint16_t)Math::round(b * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(g * 65535); + c |= (uint16_t)Math::round(g * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(r * 65535); + c |= (uint16_t)Math::round(r * 65535.0f); return c; } uint64_t Color::to_argb64() const { - uint64_t c = (uint16_t)Math::round(a * 65535); + uint64_t c = (uint16_t)Math::round(a * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(r * 65535); + c |= (uint16_t)Math::round(r * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(g * 65535); + c |= (uint16_t)Math::round(g * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(b * 65535); + c |= (uint16_t)Math::round(b * 65535.0f); return c; } uint64_t Color::to_rgba64() const { - uint64_t c = (uint16_t)Math::round(r * 65535); + uint64_t c = (uint16_t)Math::round(r * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(g * 65535); + c |= (uint16_t)Math::round(g * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(b * 65535); + c |= (uint16_t)Math::round(b * 65535.0f); c <<= 16; - c |= (uint16_t)Math::round(a * 65535); + c |= (uint16_t)Math::round(a * 65535.0f); return c; } String _to_hex(float p_val) { - int v = Math::round(p_val * 255); + int v = Math::round(p_val * 255.0f); v = CLAMP(v, 0, 255); String ret; @@ -150,8 +150,8 @@ float Color::get_h() const { float delta = max - min; - if (delta == 0) { - return 0; + if (delta == 0.0f) { + return 0.0f; } float h; @@ -164,7 +164,7 @@ float Color::get_h() const { } h /= 6.0f; - if (h < 0) { + if (h < 0.0f) { h += 1.0f; } @@ -179,7 +179,7 @@ float Color::get_s() const { float delta = max - min; - return (max != 0) ? (delta / max) : 0; + return (max != 0.0f) ? (delta / max) : 0.0f; } float Color::get_v() const { @@ -193,7 +193,7 @@ void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) { float f, p, q, t; a = p_alpha; - if (p_s == 0) { + if (p_s == 0.0f) { // Achromatic (grey) r = g = b = p_v; return; @@ -204,9 +204,9 @@ void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) { i = Math::floor(p_h); f = p_h - i; - p = p_v * (1 - p_s); - q = p_v * (1 - p_s * f); - t = p_v * (1 - p_s * (1 - f)); + p = p_v * (1.0f - p_s); + q = p_v * (1.0f - p_s * f); + t = p_v * (1.0f - p_s * (1.0f - f)); switch (i) { case 0: // Red is the dominant color @@ -347,7 +347,7 @@ Color Color::html(const String &p_rgba) { ERR_FAIL_V_MSG(Color(), "Invalid color code: " + p_rgba + "."); } - float r, g, b, a = 1.0; + float r, g, b, a = 1.0f; if (is_shorthand) { r = _parse_col4(color, 0) / 15.0f; g = _parse_col4(color, 1) / 15.0f; @@ -363,10 +363,10 @@ Color Color::html(const String &p_rgba) { a = _parse_col8(color, 6) / 255.0f; } } - ERR_FAIL_COND_V_MSG(r < 0, Color(), "Invalid color code: " + p_rgba + "."); - ERR_FAIL_COND_V_MSG(g < 0, Color(), "Invalid color code: " + p_rgba + "."); - ERR_FAIL_COND_V_MSG(b < 0, Color(), "Invalid color code: " + p_rgba + "."); - ERR_FAIL_COND_V_MSG(a < 0, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(r < 0.0f, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(g < 0.0f, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(b < 0.0f, Color(), "Invalid color code: " + p_rgba + "."); + ERR_FAIL_COND_V_MSG(a < 0.0f, Color(), "Invalid color code: " + p_rgba + "."); return Color(r, g, b, a); } @@ -474,7 +474,7 @@ Color Color::from_rgbe9995(uint32_t p_rgbe) { float g = (p_rgbe >> 9) & 0x1ff; float b = (p_rgbe >> 18) & 0x1ff; float e = (p_rgbe >> 27); - float m = Math::pow(2, e - 15.0f - 9.0f); + float m = Math::pow(2.0f, e - 15.0f - 9.0f); float rd = r * m; float gd = g * m; diff --git a/core/math/color.h b/core/math/color.h index 0afa6006a8..a23a4953ce 100644 --- a/core/math/color.h +++ b/core/math/color.h @@ -32,7 +32,8 @@ #define COLOR_H #include "core/math/math_funcs.h" -#include "core/string/ustring.h" + +class String; struct _NO_DISCARD_ Color { union { @@ -55,11 +56,11 @@ struct _NO_DISCARD_ Color { float get_h() const; float get_s() const; float get_v() const; - void set_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0); + void set_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f); float get_ok_hsl_h() const; float get_ok_hsl_s() const; float get_ok_hsl_l() const; - void set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0); + void set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0f); _FORCE_INLINE_ float &operator[](int p_idx) { return components[p_idx]; @@ -175,9 +176,9 @@ struct _NO_DISCARD_ Color { _FORCE_INLINE_ Color srgb_to_linear() const { return Color( - r < 0.04045f ? r * (1.0 / 12.92) : Math::pow((r + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), - g < 0.04045f ? g * (1.0 / 12.92) : Math::pow((g + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), - b < 0.04045f ? b * (1.0 / 12.92) : Math::pow((b + 0.055f) * (float)(1.0 / (1 + 0.055)), 2.4f), + r < 0.04045f ? r * (1.0f / 12.92f) : Math::pow((r + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f), + g < 0.04045f ? g * (1.0f / 12.92f) : Math::pow((g + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f), + b < 0.04045f ? b * (1.0f / 12.92f) : Math::pow((b + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f), a); } _FORCE_INLINE_ Color linear_to_srgb() const { @@ -198,11 +199,11 @@ struct _NO_DISCARD_ Color { static String get_named_color_name(int p_idx); static Color get_named_color(int p_idx); static Color from_string(const String &p_string, const Color &p_default); - static Color from_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0); - static Color from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0); + static Color from_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f); + static Color from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0f); static Color from_rgbe9995(uint32_t p_rgbe); - _FORCE_INLINE_ bool operator<(const Color &p_color) const; //used in set keys + _FORCE_INLINE_ bool operator<(const Color &p_color) const; // Used in set keys. operator String() const; // For the binder. @@ -215,12 +216,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/convex_hull.h b/core/math/convex_hull.h index bd86fe0eba..cc41a794bd 100644 --- a/core/math/convex_hull.h +++ b/core/math/convex_hull.h @@ -28,6 +28,9 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef CONVEX_HULL_H +#define CONVEX_HULL_H + /* Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net This software is provided 'as-is', without any express or implied warranty. @@ -40,9 +43,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_HULL_H -#define CONVEX_HULL_H - #include "core/math/geometry_3d.h" #include "core/math/vector3.h" #include "core/templates/local_vector.h" diff --git a/core/math/delaunay_3d.h b/core/math/delaunay_3d.h index 4ab00e1f34..3f8fe09445 100644 --- a/core/math/delaunay_3d.h +++ b/core/math/delaunay_3d.h @@ -33,9 +33,8 @@ #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" #include "core/templates/oa_hash_map.h" #include "core/templates/vector.h" @@ -184,27 +183,27 @@ 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; - cm.matrix[0][2] = p_points[p_simplex.points[2]].x; - cm.matrix[0][3] = p_points[p_simplex.points[3]].x; + cm.columns[0][0] = p_points[p_simplex.points[0]].x; + cm.columns[0][1] = p_points[p_simplex.points[1]].x; + cm.columns[0][2] = p_points[p_simplex.points[2]].x; + cm.columns[0][3] = p_points[p_simplex.points[3]].x; - cm.matrix[1][0] = p_points[p_simplex.points[0]].y; - cm.matrix[1][1] = p_points[p_simplex.points[1]].y; - cm.matrix[1][2] = p_points[p_simplex.points[2]].y; - cm.matrix[1][3] = p_points[p_simplex.points[3]].y; + cm.columns[1][0] = p_points[p_simplex.points[0]].y; + cm.columns[1][1] = p_points[p_simplex.points[1]].y; + cm.columns[1][2] = p_points[p_simplex.points[2]].y; + cm.columns[1][3] = p_points[p_simplex.points[3]].y; - cm.matrix[2][0] = p_points[p_simplex.points[0]].z; - cm.matrix[2][1] = p_points[p_simplex.points[1]].z; - cm.matrix[2][2] = p_points[p_simplex.points[2]].z; - cm.matrix[2][3] = p_points[p_simplex.points[3]].z; + cm.columns[2][0] = p_points[p_simplex.points[0]].z; + cm.columns[2][1] = p_points[p_simplex.points[1]].z; + cm.columns[2][2] = p_points[p_simplex.points[2]].z; + cm.columns[2][3] = p_points[p_simplex.points[3]].z; - cm.matrix[3][0] = 1.0; - cm.matrix[3][1] = 1.0; - cm.matrix[3][2] = 1.0; - cm.matrix[3][3] = 1.0; + cm.columns[3][0] = 1.0; + cm.columns[3][1] = 1.0; + cm.columns[3][2] = 1.0; + cm.columns[3][3] = 1.0; return ABS(cm.determinant()) <= CMP_EPSILON; } diff --git a/core/math/expression.cpp b/core/math/expression.cpp index e230b69dc9..dcec3929fe 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -560,7 +560,7 @@ const char *Expression::token_name[TK_MAX] = { }; Expression::ENode *Expression::_parse_expression() { - Vector<ExpressionNode> expression; + Vector<ExpressionNode> expression_nodes; while (true) { //keep appending stuff to expression @@ -838,14 +838,14 @@ Expression::ENode *Expression::_parse_expression() { ExpressionNode e; e.is_op = true; e.op = Variant::OP_NEGATE; - expression.push_back(e); + expression_nodes.push_back(e); continue; } break; case TK_OP_NOT: { ExpressionNode e; e.is_op = true; e.op = Variant::OP_NOT; - expression.push_back(e); + expression_nodes.push_back(e); continue; } break; @@ -960,7 +960,7 @@ Expression::ENode *Expression::_parse_expression() { ExpressionNode e; e.is_op = false; e.node = expr; - expression.push_back(e); + expression_nodes.push_back(e); } //ok finally look for an operator @@ -1054,19 +1054,19 @@ Expression::ENode *Expression::_parse_expression() { ExpressionNode e; e.is_op = true; e.op = op; - expression.push_back(e); + expression_nodes.push_back(e); } } /* Reduce the set of expressions and place them in an operator tree, respecting precedence */ - while (expression.size() > 1) { + while (expression_nodes.size() > 1) { int next_op = -1; int min_priority = 0xFFFFF; bool is_unary = false; - for (int i = 0; i < expression.size(); i++) { - if (!expression[i].is_op) { + for (int i = 0; i < expression_nodes.size(); i++) { + if (!expression_nodes[i].is_op) { continue; } @@ -1074,7 +1074,7 @@ Expression::ENode *Expression::_parse_expression() { bool unary = false; - switch (expression[i].op) { + switch (expression_nodes[i].op) { case Variant::OP_POWER: priority = 0; break; @@ -1130,7 +1130,7 @@ Expression::ENode *Expression::_parse_expression() { priority = 14; break; default: { - _set_error("Parser bug, invalid operator in expression: " + itos(expression[i].op)); + _set_error("Parser bug, invalid operator in expression: " + itos(expression_nodes[i].op)); return nullptr; } } @@ -1153,9 +1153,9 @@ Expression::ENode *Expression::_parse_expression() { // OK! create operator.. if (is_unary) { int expr_pos = next_op; - while (expression[expr_pos].is_op) { + while (expression_nodes[expr_pos].is_op) { expr_pos++; - if (expr_pos == expression.size()) { + if (expr_pos == expression_nodes.size()) { //can happen.. _set_error("Unexpected end of expression..."); return nullptr; @@ -1165,29 +1165,29 @@ Expression::ENode *Expression::_parse_expression() { //consecutively do unary operators for (int i = expr_pos - 1; i >= next_op; i--) { OperatorNode *op = alloc_node<OperatorNode>(); - op->op = expression[i].op; - op->nodes[0] = expression[i + 1].node; + op->op = expression_nodes[i].op; + op->nodes[0] = expression_nodes[i + 1].node; op->nodes[1] = nullptr; - expression.write[i].is_op = false; - expression.write[i].node = op; - expression.remove_at(i + 1); + expression_nodes.write[i].is_op = false; + expression_nodes.write[i].node = op; + expression_nodes.remove_at(i + 1); } } else { - if (next_op < 1 || next_op >= (expression.size() - 1)) { + if (next_op < 1 || next_op >= (expression_nodes.size() - 1)) { _set_error("Parser bug..."); ERR_FAIL_V(nullptr); } OperatorNode *op = alloc_node<OperatorNode>(); - op->op = expression[next_op].op; + op->op = expression_nodes[next_op].op; - if (expression[next_op - 1].is_op) { + if (expression_nodes[next_op - 1].is_op) { _set_error("Parser bug..."); ERR_FAIL_V(nullptr); } - if (expression[next_op + 1].is_op) { + if (expression_nodes[next_op + 1].is_op) { // this is not invalid and can really appear // but it becomes invalid anyway because no binary op // can be followed by a unary op in a valid combination, @@ -1197,17 +1197,17 @@ Expression::ENode *Expression::_parse_expression() { return nullptr; } - op->nodes[0] = expression[next_op - 1].node; //expression goes as left - op->nodes[1] = expression[next_op + 1].node; //next expression goes as right + op->nodes[0] = expression_nodes[next_op - 1].node; //expression goes as left + op->nodes[1] = expression_nodes[next_op + 1].node; //next expression goes as right //replace all 3 nodes by this operator and make it an expression - expression.write[next_op - 1].node = op; - expression.remove_at(next_op); - expression.remove_at(next_op); + expression_nodes.write[next_op - 1].node = op; + expression_nodes.remove_at(next_op); + expression_nodes.remove_at(next_op); } } - return expression[0].node; + return expression_nodes[0].node; } bool Expression::_compile_expression() { diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp index ec96753c79..c5871358ed 100644 --- a/core/math/geometry_3d.cpp +++ b/core/math/geometry_3d.cpp @@ -30,11 +30,114 @@ #include "geometry_3d.h" -#include "core/string/print_string.h" - #include "thirdparty/misc/clipper.hpp" #include "thirdparty/misc/polypartition.h" +void Geometry3D::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) { + // Based on David Eberly's Computation of Distance Between Line Segments algorithm. + + Vector3 p = p_p1 - p_p0; + Vector3 q = p_q1 - p_q0; + Vector3 r = p_p0 - p_q0; + + real_t a = p.dot(p); + real_t b = p.dot(q); + real_t c = q.dot(q); + real_t d = p.dot(r); + real_t e = q.dot(r); + + real_t s = 0.0f; + real_t t = 0.0f; + + real_t det = a * c - b * b; + if (det > CMP_EPSILON) { + // Non-parallel segments + real_t bte = b * e; + real_t ctd = c * d; + + if (bte <= ctd) { + // s <= 0.0f + if (e <= 0.0f) { + // t <= 0.0f + s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f)); + t = 0.0f; + } else if (e < c) { + // 0.0f < t < 1 + s = 0.0f; + t = e / c; + } else { + // t >= 1 + s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f)); + t = 1; + } + } else { + // s > 0.0f + s = bte - ctd; + if (s >= det) { + // s >= 1 + if (b + e <= 0.0f) { + // t <= 0.0f + s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1)); + t = 0.0f; + } else if (b + e < c) { + // 0.0f < t < 1 + s = 1; + t = (b + e) / c; + } else { + // t >= 1 + s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1)); + t = 1; + } + } else { + // 0.0f < s < 1 + real_t ate = a * e; + real_t btd = b * d; + + if (ate <= btd) { + // t <= 0.0f + s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a)); + t = 0.0f; + } else { + // t > 0.0f + t = ate - btd; + if (t >= det) { + // t >= 1 + s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a)); + t = 1; + } else { + // 0.0f < t < 1 + s /= det; + t /= det; + } + } + } + } + } else { + // Parallel segments + if (e <= 0.0f) { + s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a)); + t = 0.0f; + } else if (e >= c) { + s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a)); + t = 1; + } else { + s = 0.0f; + t = e / c; + } + } + + r_ps = (1 - s) * p_p0 + s * p_p1; + r_qt = (1 - t) * p_q0 + t * p_q1; +} + +real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) { + Vector3 ps; + Vector3 qt; + get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt); + Vector3 st = qt - ps; + return st.length(); +} + void Geometry3D::MeshData::optimize_vertices() { HashMap<int, int> vtx_remap; diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h index 59c56906f4..e5ace9db72 100644 --- a/core/math/geometry_3d.h +++ b/core/math/geometry_3d.h @@ -37,96 +37,8 @@ class Geometry3D { public: - static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { -// Do the function 'd' as defined by pb. I think it's a dot product of some sort. -#define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) - - // Calculate the parametric position on the 2 curves, mua and mub. - real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); - real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); - - // Clip the value between [0..1] constraining the solution to lie on the original curves. - if (mua < 0) { - mua = 0; - } - if (mub < 0) { - mub = 0; - } - if (mua > 1) { - mua = 1; - } - if (mub > 1) { - mub = 1; - } - c1 = p1.lerp(p2, mua); - c2 = q1.lerp(q2, mub); - } - - static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { - Vector3 u = p_to_a - p_from_a; - Vector3 v = p_to_b - p_from_b; - Vector3 w = p_from_a - p_to_a; - real_t a = u.dot(u); // Always >= 0 - real_t b = u.dot(v); - real_t c = v.dot(v); // Always >= 0 - real_t d = u.dot(w); - real_t e = v.dot(w); - real_t D = a * c - b * b; // Always >= 0 - real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 - real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - - // Compute the line parameters of the two closest points. - if (D < (real_t)CMP_EPSILON) { // The lines are almost parallel. - sN = 0.0f; // Force using point P0 on segment S1 - sD = 1.0f; // to prevent possible division by 0.0 later. - tN = e; - tD = c; - } else { // Get the closest points on the infinite lines - sN = (b * e - c * d); - tN = (a * e - b * d); - if (sN < 0.0f) { // sc < 0 => the s=0 edge is visible. - sN = 0.0f; - tN = e; - tD = c; - } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0.0f) { // tc < 0 => the t=0 edge is visible. - tN = 0.0f; - // Recompute sc for this edge. - if (-d < 0.0f) { - sN = 0.0f; - } else if (-d > a) { - sN = sD; - } else { - sN = -d; - sD = a; - } - } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. - tN = tD; - // Recompute sc for this edge. - if ((-d + b) < 0.0f) { - sN = 0; - } else if ((-d + b) > a) { - sN = sD; - } else { - sN = (-d + b); - sD = a; - } - } - // Finally do the division to get sc and tc. - sc = (Math::is_zero_approx(sN) ? 0.0f : sN / sD); - tc = (Math::is_zero_approx(tN) ? 0.0f : tN / tD); - - // Get the difference of the two closest points. - Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - - return dP.length(); // Return the closest distance. - } + static void get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt); + static real_t get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1); static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { Vector3 e1 = p_v1 - p_v0; diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index 4be4809e3f..7b30b9a98c 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -56,6 +56,15 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } + case Variant::VECTOR2I: { + SETUP_TYPE(Vector2i) + + /**/ TRY_TRANSFER_FIELD("x", x) + else TRY_TRANSFER_FIELD("y", y) + + return target; + } + case Variant::RECT2: { SETUP_TYPE(Rect2) @@ -67,6 +76,17 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } + case Variant::RECT2I: { + SETUP_TYPE(Rect2i) + + /**/ TRY_TRANSFER_FIELD("x", position.x) + else TRY_TRANSFER_FIELD("y", position.y) + else TRY_TRANSFER_FIELD("w", size.x) + else TRY_TRANSFER_FIELD("h", size.y) + + return target; + } + case Variant::VECTOR3: { SETUP_TYPE(Vector3) @@ -77,6 +97,38 @@ 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) @@ -160,6 +212,29 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } + case Variant::PROJECTION: { + SETUP_TYPE(Projection) + + /**/ TRY_TRANSFER_FIELD("xx", columns[0].x) + else TRY_TRANSFER_FIELD("xy", columns[0].y) + else TRY_TRANSFER_FIELD("xz", columns[0].z) + else TRY_TRANSFER_FIELD("xw", columns[0].w) + else TRY_TRANSFER_FIELD("yx", columns[1].x) + else TRY_TRANSFER_FIELD("yy", columns[1].y) + else TRY_TRANSFER_FIELD("yz", columns[1].z) + else TRY_TRANSFER_FIELD("yw", columns[1].w) + else TRY_TRANSFER_FIELD("zx", columns[2].x) + else TRY_TRANSFER_FIELD("zy", columns[2].y) + else TRY_TRANSFER_FIELD("zz", columns[2].z) + else TRY_TRANSFER_FIELD("zw", columns[2].w) + else TRY_TRANSFER_FIELD("xo", columns[3].x) + else TRY_TRANSFER_FIELD("yo", columns[3].y) + else TRY_TRANSFER_FIELD("zo", columns[3].z) + else TRY_TRANSFER_FIELD("wo", columns[3].w) + + return target; + } + default: { ERR_FAIL_V(p_target); } diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 53deb9bd42..0af529ad98 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -184,6 +184,9 @@ public: #endif } + static _ALWAYS_INLINE_ bool is_finite(double p_val) { return isfinite(p_val); } + static _ALWAYS_INLINE_ bool is_finite(float p_val) { return isfinite(p_val); } + static _ALWAYS_INLINE_ double abs(double g) { return absd(g); } static _ALWAYS_INLINE_ float abs(float g) { return absf(g); } static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; } @@ -229,11 +232,11 @@ public: return value; } - static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * (Math_PI / 180.0); } - static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * (float)(Math_PI / 180.0); } + static _ALWAYS_INLINE_ double deg_to_rad(double p_y) { return p_y * (Math_PI / 180.0); } + static _ALWAYS_INLINE_ float deg_to_rad(float p_y) { return p_y * (float)(Math_PI / 180.0); } - static _ALWAYS_INLINE_ double rad2deg(double p_y) { return p_y * (180.0 / Math_PI); } - static _ALWAYS_INLINE_ float rad2deg(float p_y) { return p_y * (float)(180.0 / Math_PI); } + static _ALWAYS_INLINE_ double rad_to_deg(double p_y) { return p_y * (180.0 / Math_PI); } + static _ALWAYS_INLINE_ float rad_to_deg(float p_y) { return p_y * (float)(180.0 / Math_PI); } static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; } static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; } @@ -253,6 +256,92 @@ public: (-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight)); } + static _ALWAYS_INLINE_ double cubic_interpolate_angle(double p_from, double p_to, double p_pre, double p_post, double p_weight) { + double from_rot = fmod(p_from, Math_TAU); + + double pre_diff = fmod(p_pre - from_rot, Math_TAU); + double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff; + + double to_diff = fmod(p_to - from_rot, Math_TAU); + double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff; + + double post_diff = fmod(p_post - to_rot, Math_TAU); + double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff; + + return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight); + } + + static _ALWAYS_INLINE_ float cubic_interpolate_angle(float p_from, float p_to, float p_pre, float p_post, float p_weight) { + float from_rot = fmod(p_from, (float)Math_TAU); + + float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU); + float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff; + + float to_diff = fmod(p_to - from_rot, (float)Math_TAU); + float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff; + + float post_diff = fmod(p_post - to_rot, (float)Math_TAU); + float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff; + + return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight); + } + + static _ALWAYS_INLINE_ double cubic_interpolate_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight, + double p_to_t, double p_pre_t, double p_post_t) { + /* Barry-Goldman method */ + double t = Math::lerp(0.0, p_to_t, p_weight); + double a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0 : (t - p_pre_t) / -p_pre_t); + double a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5 : t / p_to_t); + double a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0 : (t - p_to_t) / (p_post_t - p_to_t)); + double b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0 : (t - p_pre_t) / (p_to_t - p_pre_t)); + double b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t); + return Math::lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t); + } + + static _ALWAYS_INLINE_ float cubic_interpolate_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight, + float p_to_t, float p_pre_t, float p_post_t) { + /* Barry-Goldman method */ + float t = Math::lerp(0.0f, p_to_t, p_weight); + float a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0f : (t - p_pre_t) / -p_pre_t); + float a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5f : t / p_to_t); + float a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0f : (t - p_to_t) / (p_post_t - p_to_t)); + float b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0f : (t - p_pre_t) / (p_to_t - p_pre_t)); + float b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0f : t / p_post_t); + return Math::lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_t); + } + + static _ALWAYS_INLINE_ double cubic_interpolate_angle_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight, + double p_to_t, double p_pre_t, double p_post_t) { + double from_rot = fmod(p_from, Math_TAU); + + double pre_diff = fmod(p_pre - from_rot, Math_TAU); + double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff; + + double to_diff = fmod(p_to - from_rot, Math_TAU); + double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff; + + double post_diff = fmod(p_post - to_rot, Math_TAU); + double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff; + + return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t); + } + + static _ALWAYS_INLINE_ float cubic_interpolate_angle_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight, + float p_to_t, float p_pre_t, float p_post_t) { + float from_rot = fmod(p_from, (float)Math_TAU); + + float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU); + float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff; + + float to_diff = fmod(p_to - from_rot, (float)Math_TAU); + float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff; + + float post_diff = fmod(p_post - to_rot, (float)Math_TAU); + float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff; + + return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t); + } + static _ALWAYS_INLINE_ double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) { /* Formula from Wikipedia article on Bezier curves. */ double omt = (1.0 - p_t); @@ -263,6 +352,7 @@ public: 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); @@ -285,11 +375,19 @@ public: return p_from + distance * p_weight; } - static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) { return (p_value - p_from) / (p_to - p_from); } - static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) { return (p_value - p_from) / (p_to - p_from); } + static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) { + return (p_value - p_from) / (p_to - p_from); + } + static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) { + return (p_value - p_from) / (p_to - p_from); + } - static _ALWAYS_INLINE_ double range_lerp(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } - static _ALWAYS_INLINE_ float range_lerp(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } + static _ALWAYS_INLINE_ double remap(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) { + return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); + } + static _ALWAYS_INLINE_ float remap(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) { + return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); + } static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_s) { if (is_equal_approx(p_from, p_to)) { @@ -305,14 +403,26 @@ public: float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f); return s * s * (3.0f - 2.0f * s); } - static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta; } - static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta; } + static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) { + return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta; + } + static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) { + return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta; + } - static _ALWAYS_INLINE_ double linear2db(double p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } - static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log(p_linear) * (float)8.6858896380650365530225783783321; } + static _ALWAYS_INLINE_ double linear_to_db(double p_linear) { + return Math::log(p_linear) * 8.6858896380650365530225783783321; + } + static _ALWAYS_INLINE_ float linear_to_db(float p_linear) { + return Math::log(p_linear) * (float)8.6858896380650365530225783783321; + } - static _ALWAYS_INLINE_ double db2linear(double p_db) { return Math::exp(p_db * 0.11512925464970228420089957273422); } - static _ALWAYS_INLINE_ float db2linear(float p_db) { return Math::exp(p_db * (float)0.11512925464970228420089957273422); } + static _ALWAYS_INLINE_ double db_to_linear(double p_db) { + return Math::exp(p_db * 0.11512925464970228420089957273422); + } + static _ALWAYS_INLINE_ float db_to_linear(float p_db) { + return Math::exp(p_db * (float)0.11512925464970228420089957273422); + } static _ALWAYS_INLINE_ double round(double p_val) { return ::round(p_val); } static _ALWAYS_INLINE_ float round(float p_val) { return ::roundf(p_val); } diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 6881ad4014..a5d2fe5628 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -147,6 +147,7 @@ Variant Plane::intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) co return Variant(); } } + Variant Plane::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const { Vector3 inters; if (intersects_ray(p_from, p_dir, &inters)) { @@ -155,6 +156,7 @@ Variant Plane::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) return Variant(); } } + Variant Plane::intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const { Vector3 inters; if (intersects_segment(p_begin, p_end, &inters)) { @@ -174,6 +176,10 @@ bool Plane::is_equal_approx(const Plane &p_plane) const { return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d); } +bool Plane::is_finite() const { + return normal.is_finite() && Math::is_finite(d); +} + Plane::operator String() const { return "[N: " + normal.operator String() + ", D: " + String::num_real(d, false) + "]"; } diff --git a/core/math/plane.h b/core/math/plane.h index 66c1741662..77da59fb27 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 */ @@ -74,6 +74,7 @@ struct _NO_DISCARD_ Plane { Plane operator-() const { return Plane(-normal, -d); } bool is_equal_approx(const Plane &p_plane) const; bool is_equal_approx_any_side(const Plane &p_plane) const; + bool is_finite() const; _FORCE_INLINE_ bool operator==(const Plane &p_plane) const; _FORCE_INLINE_ bool operator!=(const Plane &p_plane) const; @@ -97,10 +98,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..70cc9b5f7c 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,72 +28,234 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "camera_matrix.h" +#include "projection.h" #include "core/math/aabb.h" #include "core/math/math_funcs.h" #include "core/math/plane.h" #include "core/math/rect2.h" #include "core/math/transform_3d.h" -#include "core/string/print_string.h" - -float CameraMatrix::determinant() const { - return matrix[0][3] * matrix[1][2] * matrix[2][1] * matrix[3][0] - matrix[0][2] * matrix[1][3] * matrix[2][1] * matrix[3][0] - - matrix[0][3] * matrix[1][1] * matrix[2][2] * matrix[3][0] + matrix[0][1] * matrix[1][3] * matrix[2][2] * matrix[3][0] + - matrix[0][2] * matrix[1][1] * matrix[2][3] * matrix[3][0] - matrix[0][1] * matrix[1][2] * matrix[2][3] * matrix[3][0] - - matrix[0][3] * matrix[1][2] * matrix[2][0] * matrix[3][1] + matrix[0][2] * matrix[1][3] * matrix[2][0] * matrix[3][1] + - matrix[0][3] * matrix[1][0] * matrix[2][2] * matrix[3][1] - matrix[0][0] * matrix[1][3] * matrix[2][2] * matrix[3][1] - - matrix[0][2] * matrix[1][0] * matrix[2][3] * matrix[3][1] + matrix[0][0] * matrix[1][2] * matrix[2][3] * matrix[3][1] + - matrix[0][3] * matrix[1][1] * matrix[2][0] * matrix[3][2] - matrix[0][1] * matrix[1][3] * matrix[2][0] * matrix[3][2] - - matrix[0][3] * matrix[1][0] * matrix[2][1] * matrix[3][2] + matrix[0][0] * matrix[1][3] * matrix[2][1] * matrix[3][2] + - matrix[0][1] * matrix[1][0] * matrix[2][3] * matrix[3][2] - matrix[0][0] * matrix[1][1] * matrix[2][3] * matrix[3][2] - - matrix[0][2] * matrix[1][1] * matrix[2][0] * matrix[3][3] + matrix[0][1] * matrix[1][2] * matrix[2][0] * matrix[3][3] + - matrix[0][2] * matrix[1][0] * matrix[2][1] * matrix[3][3] - matrix[0][0] * matrix[1][2] * matrix[2][1] * matrix[3][3] - - matrix[0][1] * matrix[1][0] * matrix[2][2] * matrix[3][3] + matrix[0][0] * matrix[1][1] * matrix[2][2] * matrix[3][3]; -} - -void CameraMatrix::set_identity() { +#include "core/string/ustring.h" + +float Projection::determinant() const { + return columns[0][3] * columns[1][2] * columns[2][1] * columns[3][0] - columns[0][2] * columns[1][3] * columns[2][1] * columns[3][0] - + columns[0][3] * columns[1][1] * columns[2][2] * columns[3][0] + columns[0][1] * columns[1][3] * columns[2][2] * columns[3][0] + + columns[0][2] * columns[1][1] * columns[2][3] * columns[3][0] - columns[0][1] * columns[1][2] * columns[2][3] * columns[3][0] - + columns[0][3] * columns[1][2] * columns[2][0] * columns[3][1] + columns[0][2] * columns[1][3] * columns[2][0] * columns[3][1] + + columns[0][3] * columns[1][0] * columns[2][2] * columns[3][1] - columns[0][0] * columns[1][3] * columns[2][2] * columns[3][1] - + columns[0][2] * columns[1][0] * columns[2][3] * columns[3][1] + columns[0][0] * columns[1][2] * columns[2][3] * columns[3][1] + + columns[0][3] * columns[1][1] * columns[2][0] * columns[3][2] - columns[0][1] * columns[1][3] * columns[2][0] * columns[3][2] - + columns[0][3] * columns[1][0] * columns[2][1] * columns[3][2] + columns[0][0] * columns[1][3] * columns[2][1] * columns[3][2] + + columns[0][1] * columns[1][0] * columns[2][3] * columns[3][2] - columns[0][0] * columns[1][1] * columns[2][3] * columns[3][2] - + columns[0][2] * columns[1][1] * columns[2][0] * columns[3][3] + columns[0][1] * columns[1][2] * columns[2][0] * columns[3][3] + + columns[0][2] * columns[1][0] * columns[2][1] * columns[3][3] - columns[0][0] * columns[1][2] * columns[2][1] * columns[3][3] - + columns[0][1] * columns[1][0] * columns[2][2] * columns[3][3] + columns[0][0] * columns[1][1] * columns[2][2] * columns[3][3]; +} + +void Projection::set_identity() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { - matrix[i][j] = (i == j) ? 1 : 0; + columns[i][j] = (i == j) ? 1 : 0; } } } -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; + columns[i][j] = 0; } } } -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; - ret.normal.y = matrix[0][1] * p_vec4.normal.x + matrix[1][1] * p_vec4.normal.y + matrix[2][1] * p_vec4.normal.z + matrix[3][1] * p_vec4.d; - ret.normal.z = matrix[0][2] * p_vec4.normal.x + matrix[1][2] * p_vec4.normal.y + matrix[2][2] * p_vec4.normal.z + matrix[3][2] * p_vec4.d; - ret.d = matrix[0][3] * p_vec4.normal.x + matrix[1][3] * p_vec4.normal.y + matrix[2][3] * p_vec4.normal.z + matrix[3][3] * p_vec4.d; + ret.normal.x = columns[0][0] * p_vec4.normal.x + columns[1][0] * p_vec4.normal.y + columns[2][0] * p_vec4.normal.z + columns[3][0] * p_vec4.d; + ret.normal.y = columns[0][1] * p_vec4.normal.x + columns[1][1] * p_vec4.normal.y + columns[2][1] * p_vec4.normal.z + columns[3][1] * p_vec4.d; + ret.normal.z = columns[0][2] * p_vec4.normal.x + columns[1][2] * p_vec4.normal.y + columns[2][2] * p_vec4.normal.z + columns[3][2] * p_vec4.d; + ret.d = columns[0][3] * p_vec4.normal.x + columns[1][3] * p_vec4.normal.y + columns[2][3] * p_vec4.normal.z + columns[3][3] * p_vec4.d; return ret; } -void CameraMatrix::adjust_perspective_znear(real_t p_new_znear) { +Vector4 Projection::xform(const Vector4 &p_vec4) const { + return Vector4( + columns[0][0] * p_vec4.x + columns[1][0] * p_vec4.y + columns[2][0] * p_vec4.z + columns[3][0] * p_vec4.w, + columns[0][1] * p_vec4.x + columns[1][1] * p_vec4.y + columns[2][1] * p_vec4.z + columns[3][1] * p_vec4.w, + columns[0][2] * p_vec4.x + columns[1][2] * p_vec4.y + columns[2][2] * p_vec4.z + columns[3][2] * p_vec4.w, + columns[0][3] * p_vec4.x + columns[1][3] * p_vec4.y + columns[2][3] * p_vec4.z + columns[3][3] * p_vec4.w); +} +Vector4 Projection::xform_inv(const Vector4 &p_vec4) const { + return Vector4( + columns[0][0] * p_vec4.x + columns[0][1] * p_vec4.y + columns[0][2] * p_vec4.z + columns[0][3] * p_vec4.w, + columns[1][0] * p_vec4.x + columns[1][1] * p_vec4.y + columns[1][2] * p_vec4.z + columns[1][3] * p_vec4.w, + columns[2][0] * p_vec4.x + columns[2][1] * p_vec4.y + columns[2][2] * p_vec4.z + columns[2][3] * p_vec4.w, + columns[3][0] * p_vec4.x + columns[3][1] * p_vec4.y + columns[3][2] * p_vec4.z + columns[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; real_t deltaZ = zfar - znear; - matrix[2][2] = -(zfar + znear) / deltaZ; - matrix[3][2] = -2 * znear * zfar / deltaZ; + columns[2][2] = -(zfar + znear) / deltaZ; + columns[3][2] = -2 * znear * zfar / deltaZ; +} + +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 *)columns; + + 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; } -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 ::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); } real_t sine, cotangent, deltaZ; - real_t radians = Math::deg2rad(p_fovy_degrees / 2.0); + real_t radians = Math::deg_to_rad(p_fovy_degrees / 2.0); deltaZ = p_z_far - p_z_near; sine = Math::sin(radians); @@ -105,22 +267,22 @@ void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_ set_identity(); - matrix[0][0] = cotangent / p_aspect; - matrix[1][1] = cotangent; - matrix[2][2] = -(p_z_far + p_z_near) / deltaZ; - matrix[2][3] = -1; - matrix[3][2] = -2 * p_z_near * p_z_far / deltaZ; - matrix[3][3] = 0; + columns[0][0] = cotangent / p_aspect; + columns[1][1] = cotangent; + columns[2][2] = -(p_z_far + p_z_near) / deltaZ; + columns[2][3] = -1; + columns[3][2] = -2 * p_z_near * p_z_far / deltaZ; + columns[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); } real_t left, right, modeltranslation, ymax, xmax, frustumshift; - ymax = p_z_near * tan(Math::deg2rad(p_fovy_degrees / 2.0)); + ymax = p_z_near * tan(Math::deg_to_rad(p_fovy_degrees / 2.0)); xmax = ymax * p_aspect; frustumshift = (p_intraocular_dist / 2.0) * p_z_near / p_convergence_dist; @@ -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; + cm.columns[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,19 +341,19 @@ 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); - matrix[3][0] = -((p_right + p_left) / (p_right - p_left)); - matrix[1][1] = 2.0 / (p_top - p_bottom); - matrix[3][1] = -((p_top + p_bottom) / (p_top - p_bottom)); - matrix[2][2] = -2.0 / (p_zfar - p_znear); - matrix[3][2] = -((p_zfar + p_znear) / (p_zfar - p_znear)); - matrix[3][3] = 1.0; + columns[0][0] = 2.0 / (p_right - p_left); + columns[3][0] = -((p_right + p_left) / (p_right - p_left)); + columns[1][1] = 2.0 / (p_top - p_bottom); + columns[3][1] = -((p_top + p_bottom) / (p_top - p_bottom)); + columns[2][2] = -2.0 / (p_zfar - p_znear); + columns[3][2] = -((p_zfar + p_znear) / (p_zfar - p_znear)); + columns[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,12 +361,12 @@ 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); - real_t *te = &matrix[0][0]; + real_t *te = &columns[0][0]; real_t x = 2 * p_near / (p_right - p_left); real_t y = 2 * p_near / (p_top - p_bottom); @@ -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,8 +401,8 @@ 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 { - const real_t *matrix = (const real_t *)this->matrix; +real_t Projection::get_z_far() const { + const real_t *matrix = (const real_t *)columns; Plane new_plane = Plane(matrix[3] - matrix[2], matrix[7] - matrix[6], matrix[11] - matrix[10], @@ -252,8 +414,8 @@ real_t CameraMatrix::get_z_far() const { return new_plane.d; } -real_t CameraMatrix::get_z_near() const { - const real_t *matrix = (const real_t *)this->matrix; +real_t Projection::get_z_near() const { + const real_t *matrix = (const real_t *)columns; Plane new_plane = Plane(matrix[3] + matrix[2], matrix[7] + matrix[6], matrix[11] + matrix[10], @@ -263,8 +425,8 @@ real_t CameraMatrix::get_z_near() const { return new_plane.d; } -Vector2 CameraMatrix::get_viewport_half_extents() const { - const real_t *matrix = (const real_t *)this->matrix; +Vector2 Projection::get_viewport_half_extents() const { + const real_t *matrix = (const real_t *)columns; ///////--- Near Plane ---/////// Plane near_plane = Plane(matrix[3] + matrix[2], matrix[7] + matrix[6], @@ -291,8 +453,8 @@ Vector2 CameraMatrix::get_viewport_half_extents() const { return Vector2(res.x, res.y); } -Vector2 CameraMatrix::get_far_plane_half_extents() const { - const real_t *matrix = (const real_t *)this->matrix; +Vector2 Projection::get_far_plane_half_extents() const { + const real_t *matrix = (const real_t *)columns; ///////--- Far Plane ---/////// Plane far_plane = Plane(matrix[3] - matrix[2], matrix[7] - matrix[6], @@ -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 }, @@ -334,7 +496,10 @@ bool CameraMatrix::get_endpoints(const Transform3D &p_transform, Vector3 *p_8poi for (int i = 0; i < 8; i++) { Vector3 point; - bool res = planes[intersections[i][0]].intersect_3(planes[intersections[i][1]], planes[intersections[i][2]], &point); + Plane a = planes[intersections[i][0]]; + Plane b = planes[intersections[i][1]]; + Plane c = planes[intersections[i][2]]; + bool res = a.intersect_3(b, c, &point); ERR_FAIL_COND_V(!res, false); p_8points[i] = p_transform.xform(point); } @@ -342,7 +507,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 @@ -352,7 +517,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform3D &p_transform Vector<Plane> planes; planes.resize(6); - const real_t *matrix = (const real_t *)this->matrix; + const real_t *matrix = (const real_t *)columns; Plane new_plane; @@ -425,13 +590,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 */ @@ -439,15 +604,15 @@ void CameraMatrix::invert() { real_t determinant = 1.0f; for (k = 0; k < 4; k++) { /** Locate k'th pivot element **/ - pvt_val = matrix[k][k]; /** Initialize for search **/ + pvt_val = columns[k][k]; /** Initialize for search **/ pvt_i[k] = k; pvt_j[k] = k; for (i = k; i < 4; i++) { for (j = k; j < 4; j++) { - if (Math::abs(matrix[i][j]) > Math::abs(pvt_val)) { + if (Math::abs(columns[i][j]) > Math::abs(pvt_val)) { pvt_i[k] = i; pvt_j[k] = j; - pvt_val = matrix[i][j]; + pvt_val = columns[i][j]; } } } @@ -462,9 +627,9 @@ void CameraMatrix::invert() { i = pvt_i[k]; if (i != k) { /** If rows are different **/ for (j = 0; j < 4; j++) { - hold = -matrix[k][j]; - matrix[k][j] = matrix[i][j]; - matrix[i][j] = hold; + hold = -columns[k][j]; + columns[k][j] = columns[i][j]; + columns[i][j] = hold; } } @@ -472,25 +637,25 @@ void CameraMatrix::invert() { j = pvt_j[k]; if (j != k) { /** If columns are different **/ for (i = 0; i < 4; i++) { - hold = -matrix[i][k]; - matrix[i][k] = matrix[i][j]; - matrix[i][j] = hold; + hold = -columns[i][k]; + columns[i][k] = columns[i][j]; + columns[i][j] = hold; } } /** Divide column by minus pivot value **/ for (i = 0; i < 4; i++) { if (i != k) { - matrix[i][k] /= (-pvt_val); + columns[i][k] /= (-pvt_val); } } /** Reduce the matrix **/ for (i = 0; i < 4; i++) { - hold = matrix[i][k]; + hold = columns[i][k]; for (j = 0; j < 4; j++) { if (i != k && j != k) { - matrix[i][j] += hold * matrix[k][j]; + columns[i][j] += hold * columns[k][j]; } } } @@ -498,12 +663,12 @@ void CameraMatrix::invert() { /** Divide row by pivot **/ for (j = 0; j < 4; j++) { if (j != k) { - matrix[k][j] /= pvt_val; + columns[k][j] /= pvt_val; } } /** Replace pivot by reciprocal (at last we can touch it). **/ - matrix[k][k] = 1.0 / pvt_val; + columns[k][k] = 1.0 / pvt_val; } /* That was most of the work, one final pass of row/column interchange */ @@ -512,51 +677,51 @@ void CameraMatrix::invert() { i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */ if (i != k) { /* If rows are different */ for (j = 0; j < 4; j++) { - hold = matrix[k][j]; - matrix[k][j] = -matrix[i][j]; - matrix[i][j] = hold; + hold = columns[k][j]; + columns[k][j] = -columns[i][j]; + columns[i][j] = hold; } } j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */ if (j != k) { /* If columns are different */ for (i = 0; i < 4; i++) { - hold = matrix[i][k]; - matrix[i][k] = -matrix[i][j]; - matrix[i][j] = hold; + hold = columns[i][k]; + columns[i][k] = -columns[i][j]; + columns[i][j] = hold; } } } } -void CameraMatrix::flip_y() { +void Projection::flip_y() { for (int i = 0; i < 4; i++) { - matrix[1][i] = -matrix[1][i]; + columns[1][i] = -columns[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++) { real_t ab = 0; for (int k = 0; k < 4; k++) { - ab += matrix[k][i] * p_matrix.matrix[j][k]; + ab += columns[k][i] * p_matrix.columns[j][k]; } - new_matrix.matrix[j][i] = ab; + new_matrix.columns[j][i] = ab; } } return new_matrix; } -void CameraMatrix::set_depth_correction(bool p_flip_y) { - real_t *m = &matrix[0][0]; +void Projection::set_depth_correction(bool p_flip_y) { + real_t *m = &columns[0][0]; m[0] = 1; m[1] = 0.0; @@ -576,8 +741,8 @@ void CameraMatrix::set_depth_correction(bool p_flip_y) { m[15] = 1.0; } -void CameraMatrix::set_light_bias() { - real_t *m = &matrix[0][0]; +void Projection::set_light_bias() { + real_t *m = &columns[0][0]; m[0] = 0.5; m[1] = 0.0; @@ -597,8 +762,8 @@ void CameraMatrix::set_light_bias() { m[15] = 1.0; } -void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { - real_t *m = &matrix[0][0]; +void Projection::set_light_atlas_rect(const Rect2 &p_rect) { + real_t *m = &columns[0][0]; m[0] = p_rect.size.width; m[1] = 0.0; @@ -618,34 +783,34 @@ 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++) { - str += String((j > 0) ? ", " : "\n") + rtos(matrix[i][j]); + str += String((j > 0) ? ", " : "\n") + rtos(columns[i][j]); } } 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 { - return matrix[3][3] == 1.0; +bool Projection::is_orthogonal() const { + return columns[3][3] == 1.0; } -real_t CameraMatrix::get_fov() const { - const real_t *matrix = (const real_t *)this->matrix; +real_t Projection::get_fov() const { + const real_t *matrix = (const real_t *)columns; Plane right_plane = Plane(matrix[3] - matrix[0], matrix[7] - matrix[4], @@ -654,7 +819,7 @@ real_t CameraMatrix::get_fov() const { right_plane.normalize(); if ((matrix[8] == 0) && (matrix[9] == 0)) { - return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))) * 2.0; + return Math::rad_to_deg(Math::acos(Math::abs(right_plane.normal.x))) * 2.0; } else { // our frustum is asymmetrical need to calculate the left planes angle separately.. Plane left_plane = Plane(matrix[3] + matrix[0], @@ -663,11 +828,11 @@ real_t CameraMatrix::get_fov() const { matrix[15] + matrix[12]); left_plane.normalize(); - return Math::rad2deg(Math::acos(Math::abs(left_plane.normal.x))) + Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))); + return Math::rad_to_deg(Math::acos(Math::abs(left_plane.normal.x))) + Math::rad_to_deg(Math::acos(Math::abs(right_plane.normal.x))); } } -float CameraMatrix::get_lod_multiplier() const { +float Projection::get_lod_multiplier() const { if (is_orthogonal()) { return get_viewport_half_extents().x; } else { @@ -676,48 +841,49 @@ float CameraMatrix::get_lod_multiplier() const { return 1.0 / (zn / width); } - //usage is lod_size / (lod_distance * multiplier) < threshold + // 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; + columns[0][0] = p_scale.x; + columns[1][1] = p_scale.y; + columns[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; - matrix[0][0] = 2 / (max.x - min.x); - matrix[1][0] = 0; - matrix[2][0] = 0; - matrix[3][0] = -(max.x + min.x) / (max.x - min.x); + columns[0][0] = 2 / (max.x - min.x); + columns[1][0] = 0; + columns[2][0] = 0; + columns[3][0] = -(max.x + min.x) / (max.x - min.x); - matrix[0][1] = 0; - matrix[1][1] = 2 / (max.y - min.y); - matrix[2][1] = 0; - matrix[3][1] = -(max.y + min.y) / (max.y - min.y); + columns[0][1] = 0; + columns[1][1] = 2 / (max.y - min.y); + columns[2][1] = 0; + columns[3][1] = -(max.y + min.y) / (max.y - min.y); - matrix[0][2] = 0; - matrix[1][2] = 0; - matrix[2][2] = 2 / (max.z - min.z); - matrix[3][2] = -(max.z + min.z) / (max.z - min.z); + columns[0][2] = 0; + columns[1][2] = 0; + columns[2][2] = 2 / (max.z - min.z); + columns[3][2] = -(max.z + min.z) / (max.z - min.z); - matrix[0][3] = 0; - matrix[1][3] = 0; - matrix[2][3] = 0; - matrix[3][3] = 1; + columns[0][3] = 0; + columns[1][3] = 0; + columns[2][3] = 0; + columns[3][3] = 1; } -void CameraMatrix::add_jitter_offset(const Vector2 &p_offset) { - matrix[3][0] += p_offset.x; - matrix[3][1] += p_offset.y; +void Projection::add_jitter_offset(const Vector2 &p_offset) { + columns[3][0] += p_offset.x; + columns[3][1] += p_offset.y; } -CameraMatrix::operator Transform3D() const { +Projection::operator Transform3D() const { Transform3D tr; - const real_t *m = &matrix[0][0]; + const real_t *m = &columns[0][0]; tr.basis.rows[0][0] = m[0]; tr.basis.rows[1][0] = m[1]; @@ -738,9 +904,16 @@ 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) { + columns[0] = p_x; + columns[1] = p_y; + columns[2] = p_z; + columns[3] = p_w; +} + +Projection::Projection(const Transform3D &p_transform) { const Transform3D &tr = p_transform; - real_t *m = &matrix[0][0]; + real_t *m = &columns[0][0]; m[0] = tr.basis.rows[0][0]; m[1] = tr.basis.rows[1][0]; @@ -760,5 +933,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..38fb9781ae 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,12 +28,14 @@ /* 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/templates/vector.h" +#include "core/math/vector4.h" + +template <class T> +class Vector; struct AABB; struct Plane; @@ -41,7 +43,7 @@ struct Rect2; struct Transform3D; struct Vector2; -struct CameraMatrix { +struct _NO_DISCARD_ Projection { enum Planes { PLANE_NEAR, PLANE_FAR, @@ -51,13 +53,24 @@ struct CameraMatrix { PLANE_BOTTOM }; - real_t matrix[4][4]; + Vector4 columns[4]; + + _FORCE_INLINE_ const Vector4 &operator[](const int p_axis) const { + DEV_ASSERT((unsigned int)p_axis < 4); + return columns[p_axis]; + } + + _FORCE_INLINE_ Vector4 &operator[](const int p_axis) { + DEV_ASSERT((unsigned int)p_axis < 4); + return columns[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,8 +81,23 @@ 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); + return Math::rad_to_deg(Math::atan(p_aspect * Math::tan(Math::deg_to_rad(p_fovx) * 0.5)) * 2.0); } real_t get_z_far() const; @@ -85,13 +113,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,10 +133,10 @@ 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]) { + if (columns[i][j] != p_cam.columns[i][j]) { return false; } } @@ -113,24 +144,25 @@ 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]; - ret.z = matrix[0][2] * p_vec3.x + matrix[1][2] * p_vec3.y + matrix[2][2] * p_vec3.z + matrix[3][2]; - real_t w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3]; + ret.x = columns[0][0] * p_vec3.x + columns[1][0] * p_vec3.y + columns[2][0] * p_vec3.z + columns[3][0]; + ret.y = columns[0][1] * p_vec3.x + columns[1][1] * p_vec3.y + columns[2][1] * p_vec3.z + columns[3][1]; + ret.z = columns[0][2] * p_vec3.x + columns[1][2] * p_vec3.y + columns[2][2] * p_vec3.z + columns[3][2]; + real_t w = columns[0][3] * p_vec3.x + columns[1][3] * p_vec3.y + columns[2][3] * p_vec3.z + columns[3][3]; return ret / w; } -#endif // CAMERA_MATRIX_H +#endif // PROJECTION_H diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 11bfcc1a6f..6a5f29f3d8 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -31,7 +31,7 @@ #include "quaternion.h" #include "core/math/basis.h" -#include "core/string/print_string.h" +#include "core/string/ustring.h" real_t Quaternion::angle_to(const Quaternion &p_to) const { real_t d = dot(p_to); @@ -79,6 +79,10 @@ bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const { return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w); } +bool Quaternion::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w); +} + real_t Quaternion::length() const { return Math::sqrt(length_squared()); } @@ -111,11 +115,12 @@ 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(); - if (theta < CMP_EPSILON) { + real_t theta = src_v.length(); + src_v = src_v.normalized(); + if (theta < CMP_EPSILON || !src_v.is_normalized()) { return Quaternion(0, 0, 0, 1); } - return Quaternion(src_v.normalized(), theta); + return Quaternion(src_v, theta); } Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const { @@ -132,15 +137,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 +188,105 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c invFactor * from.w + newFactor * p_to.w); } -Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const { +Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + Quaternion from_q = *this; + Quaternion pre_q = p_pre_a; + Quaternion to_q = p_b; + Quaternion post_q = p_post_b; + + // Align flip phases. + from_q = Basis(from_q).get_rotation_quaternion(); + pre_q = Basis(pre_q).get_rotation_quaternion(); + to_q = Basis(to_q).get_rotation_quaternion(); + post_q = Basis(post_q).get_rotation_quaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = signbit(from_q.dot(pre_q)); + pre_q = flip1 ? -pre_q : pre_q; + bool flip2 = signbit(from_q.dot(to_q)); + to_q = flip2 ? -to_q : to_q; + bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q)); + post_q = flip3 ? -post_q : post_q; + + // Calc by Expmap in from_q space. + Quaternion ln_from = Quaternion(0, 0, 0, 0); + Quaternion ln_to = (from_q.inverse() * to_q).log(); + Quaternion ln_pre = (from_q.inverse() * pre_q).log(); + Quaternion ln_post = (from_q.inverse() * post_q).log(); + Quaternion ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q1 = from_q * ln.exp(); + + // Calc by Expmap in to_q space. + ln_from = (to_q.inverse() * from_q).log(); + ln_to = Quaternion(0, 0, 0, 0); + ln_pre = (to_q.inverse() * pre_q).log(); + ln_post = (to_q.inverse() * post_q).log(); + ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q2 = to_q * ln.exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.slerp(q2, p_weight); +} + +Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, + const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized."); #endif - //the only way to do slerp :| - real_t t2 = (1.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_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + Quaternion q1 = from_q * ln.exp(); + + // Calc by Expmap in to_q space. + ln_from = (to_q.inverse() * from_q).log(); + ln_to = Quaternion(0, 0, 0, 0); + ln_pre = (to_q.inverse() * pre_q).log(); + ln_post = (to_q.inverse() * post_q).log(); + ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + Quaternion q2 = to_q * ln.exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.slerp(q2, p_weight); } Quaternion::operator String() const { @@ -213,7 +301,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..7aa400aa8c 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -31,10 +31,10 @@ #ifndef QUATERNION_H #define QUATERNION_H -#include "core/math/math_defs.h" #include "core/math/math_funcs.h" #include "core/math/vector3.h" -#include "core/string/ustring.h" + +class String; struct _NO_DISCARD_ Quaternion { union { @@ -55,6 +55,7 @@ struct _NO_DISCARD_ Quaternion { } _FORCE_INLINE_ real_t length_squared() const; bool is_equal_approx(const Quaternion &p_quaternion) const; + bool is_finite() const; real_t length() const; void normalize(); Quaternion normalized() const; @@ -71,10 +72,11 @@ 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; + Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; Vector3 get_axis() const; - float get_angle() const; + real_t get_angle() const; _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { r_angle = 2 * Math::acos(w); @@ -142,8 +144,7 @@ struct _NO_DISCARD_ Quaternion { w = p_q.w; } - Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc - { + Quaternion(const Vector3 &v0, const Vector3 &v1) { // Shortest arc. Vector3 c = v0.cross(v1); real_t d = v0.dot(v1); diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index 9e78ead816..facf4eb3c4 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -38,6 +38,10 @@ bool Rect2::is_equal_approx(const Rect2 &p_rect) const { return position.is_equal_approx(p_rect.position) && size.is_equal_approx(p_rect.size); } +bool Rect2::is_finite() const { + return position.is_finite() && size.is_finite(); +} + bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const { #ifdef MATH_CHECKS if (unlikely(size.x < 0 || size.y < 0)) { diff --git a/core/math/rect2.h b/core/math/rect2.h index 679af933c2..9863405d8e 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -140,8 +140,8 @@ struct _NO_DISCARD_ Rect2 { ((p_rect.position.y + p_rect.size.y) <= (position.y + size.y)); } - _FORCE_INLINE_ bool has_no_area() const { - return (size.x <= 0 || size.y <= 0); + _FORCE_INLINE_ bool has_area() const { + return size.x > 0.0f && size.y > 0.0f; } // Returns the instersection between two Rect2s or an empty Rect2 if there is no intersection @@ -178,7 +178,7 @@ struct _NO_DISCARD_ Rect2 { new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); - new_rect.size = new_rect.size - new_rect.position; //make relative again + new_rect.size = new_rect.size - new_rect.position; // Make relative again. return new_rect; } @@ -207,6 +207,7 @@ struct _NO_DISCARD_ Rect2 { } bool is_equal_approx(const Rect2 &p_rect) const; + bool is_finite() const; bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } @@ -253,7 +254,7 @@ struct _NO_DISCARD_ Rect2 { return r; } - inline void expand_to(const Vector2 &p_vector) { //in place function for speed + inline void expand_to(const Vector2 &p_vector) { // In place function for speed. #ifdef MATH_CHECKS if (unlikely(size.x < 0 || size.y < 0)) { ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size."); @@ -281,7 +282,7 @@ struct _NO_DISCARD_ Rect2 { } _FORCE_INLINE_ Rect2 abs() const { - return Rect2(Point2(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); + return Rect2(Point2(position.x + MIN(size.x, (real_t)0), position.y + MIN(size.y, (real_t)0)), size.abs()); } Vector2 get_support(const Vector2 &p_normal) const { @@ -311,7 +312,7 @@ struct _NO_DISCARD_ Rect2 { continue; } - //check inside + // Check inside. Vector2 tg = r.orthogonal(); float s = tg.dot(center) - tg.dot(a); if (s < 0.0f) { @@ -320,7 +321,7 @@ struct _NO_DISCARD_ Rect2 { side_minus++; } - //check ray box + // Check ray box. r /= l; Vector2 ir(1.0f / r.x, 1.0f / r.y); @@ -341,7 +342,7 @@ struct _NO_DISCARD_ Rect2 { } if (side_plus * side_minus == 0) { - return true; //all inside + return true; // All inside. } else { return false; } diff --git a/core/math/rect2i.h b/core/math/rect2i.h index db1459a3e6..c92f2cae02 100644 --- a/core/math/rect2i.h +++ b/core/math/rect2i.h @@ -83,8 +83,8 @@ struct _NO_DISCARD_ Rect2i { ((p_rect.position.y + p_rect.size.y) <= (position.y + size.y)); } - _FORCE_INLINE_ bool has_no_area() const { - return (size.x <= 0 || size.y <= 0); + _FORCE_INLINE_ bool has_area() const { + return size.x > 0 && size.y > 0; } // Returns the instersection between two Rect2is or an empty Rect2i if there is no intersection @@ -121,7 +121,7 @@ struct _NO_DISCARD_ Rect2i { new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); - new_rect.size = new_rect.size - new_rect.position; //make relative again + new_rect.size = new_rect.size - new_rect.position; // Make relative again. return new_rect; } diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index cbd2fd3fa1..548a82d254 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); } @@ -168,6 +168,10 @@ bool Transform2D::is_equal_approx(const Transform2D &p_transform) const { return columns[0].is_equal_approx(p_transform.columns[0]) && columns[1].is_equal_approx(p_transform.columns[1]) && columns[2].is_equal_approx(p_transform.columns[2]); } +bool Transform2D::is_finite() const { + return columns[0].is_finite() && columns[1].is_finite() && columns[2].is_finite(); +} + Transform2D Transform2D::looking_at(const Vector2 &p_target) const { Transform2D return_trans = Transform2D(get_rotation(), get_origin()); Vector2 target_position = affine_inverse().xform(p_target); @@ -217,18 +221,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 +246,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 { @@ -268,7 +286,7 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const real_t dot = v1.dot(v2); - dot = CLAMP(dot, -1.0f, 1.0f); + dot = CLAMP(dot, (real_t)-1.0, (real_t)1.0); Vector2 v; diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index 72d34a5d4c..2b11f36535 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,16 +85,20 @@ 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; void orthonormalize(); Transform2D orthonormalized() const; bool is_equal_approx(const Transform2D &p_transform) const; + bool is_finite() const; Transform2D looking_at(const Vector2 &p_target) const; diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index 76b31daa76..3285cbd664 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -31,7 +31,7 @@ #include "transform_3d.h" #include "core/math/math_funcs.h" -#include "core/string/print_string.h" +#include "core/string/ustring.h" void Transform3D::affine_invert() { basis.invert(); @@ -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,19 +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 { - /* not sure if very "efficient" but good enough? */ - +Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { Transform3D interp; Vector3 src_scale = basis.get_scale(); @@ -99,44 +111,43 @@ Transform3D Transform3D::sphere_interpolate_with(const Transform3D &p_transform, return interp; } -Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { - Transform3D interp; - - interp.basis = basis.lerp(p_transform.basis, p_c); - interp.origin = origin.lerp(p_transform.origin, p_c); - - return interp; -} - void Transform3D::scale(const Vector3 &p_scale) { basis.scale(p_scale); origin *= p_scale; } Transform3D Transform3D::scaled(const Vector3 &p_scale) const { - Transform3D t = *this; - t.scale(p_scale); - return t; + // Equivalent to left multiplication + return Transform3D(basis.scaled(p_scale), origin * p_scale); +} + +Transform3D Transform3D::scaled_local(const Vector3 &p_scale) const { + // Equivalent to right multiplication + return Transform3D(basis.scaled_local(p_scale), origin); } void Transform3D::scale_basis(const Vector3 &p_scale) { basis.scale(p_scale); } -void Transform3D::translate(real_t p_tx, real_t p_ty, real_t p_tz) { - translate(Vector3(p_tx, p_ty, p_tz)); +void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) { + translate_local(Vector3(p_tx, p_ty, p_tz)); } -void Transform3D::translate(const Vector3 &p_translation) { +void Transform3D::translate_local(const Vector3 &p_translation) { for (int i = 0; i < 3; i++) { origin[i] += basis[i].dot(p_translation); } } Transform3D Transform3D::translated(const Vector3 &p_translation) const { - Transform3D t = *this; - t.translate(p_translation); - return t; + // Equivalent to left multiplication + return Transform3D(basis, origin + p_translation); +} + +Transform3D Transform3D::translated_local(const Vector3 &p_translation) const { + // Equivalent to right multiplication + return Transform3D(basis, origin + basis.xform(p_translation)); } void Transform3D::orthonormalize() { @@ -163,6 +174,10 @@ bool Transform3D::is_equal_approx(const Transform3D &p_transform) const { return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin); } +bool Transform3D::is_finite() const { + return basis.is_finite() && origin.is_finite(); +} + bool Transform3D::operator==(const Transform3D &p_transform) const { return (basis == p_transform.basis && origin == p_transform.origin); } diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h index 25832434cd..cb347aa1c1 100644 --- a/core/math/transform_3d.h +++ b/core/math/transform_3d.h @@ -34,6 +34,7 @@ #include "core/math/aabb.h" #include "core/math/basis.h" #include "core/math/plane.h" +#include "core/templates/vector.h" struct _NO_DISCARD_ Transform3D { Basis basis; @@ -46,6 +47,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 +57,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; } @@ -71,6 +75,7 @@ struct _NO_DISCARD_ Transform3D { void orthogonalize(); Transform3D orthogonalized() const; bool is_equal_approx(const Transform3D &p_transform) const; + bool is_finite() const; bool operator==(const Transform3D &p_transform) const; bool operator!=(const Transform3D &p_transform) const; @@ -100,7 +105,6 @@ 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 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 4433559e6d..6515c55a85 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -215,10 +215,8 @@ Vector3 TriangleMesh::get_area_normal(const AABB &p_aabb) const { switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects(p_aabb); - if (!valid) { + if (!b.aabb.intersects(p_aabb)) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - } else { if (b.face_index >= 0) { const Triangle &s = triangleptr[b.face_index]; @@ -302,12 +300,8 @@ bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_segment(p_begin, p_end); - //bool valid = b.aabb.intersects(ray_aabb); - - if (!valid) { + if (!b.aabb.intersects_segment(p_begin, p_end)) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - } else { if (b.face_index >= 0) { const Triangle &s = triangleptr[b.face_index]; @@ -407,10 +401,8 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_ray(p_begin, p_dir); - if (!valid) { + if (!b.aabb.intersects_ray(p_begin, p_dir)) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - } else { if (b.face_index >= 0) { const Triangle &s = triangleptr[b.face_index]; @@ -508,10 +500,8 @@ bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_cou switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count); - if (!valid) { + if (!b.aabb.intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count)) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - } else { if (b.face_index >= 0) { const Triangle &s = triangleptr[b.face_index]; diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index d9b5d55454..5366587126 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -182,6 +182,14 @@ bool Vector2::is_equal_approx(const Vector2 &p_v) const { return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y); } +bool Vector2::is_zero_approx() const { + return Math::is_zero_approx(x) && Math::is_zero_approx(y); +} + +bool Vector2::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y); +} + Vector2::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ")"; } diff --git a/core/math/vector2.h b/core/math/vector2.h index 91d3d3a56b..5775d8e735 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -69,10 +69,6 @@ struct _NO_DISCARD_ Vector2 { return coord[p_idx]; } - _FORCE_INLINE_ void set_all(const real_t p_value) { - x = y = p_value; - } - _FORCE_INLINE_ Vector2::Axis min_axis_index() const { return x < y ? Vector2::AXIS_X : Vector2::AXIS_Y; } @@ -114,6 +110,7 @@ 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; _FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const; + _FORCE_INLINE_ Vector2 cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; _FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const; Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const; @@ -123,6 +120,8 @@ struct _NO_DISCARD_ Vector2 { Vector2 reflect(const Vector2 &p_normal) const; bool is_equal_approx(const Vector2 &p_v) const; + bool is_zero_approx() const; + bool is_finite() const; Vector2 operator+(const Vector2 &p_v) const; void operator+=(const Vector2 &p_v); @@ -270,6 +269,13 @@ Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, c return res; } +Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector2 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} + 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; diff --git a/core/math/vector2i.h b/core/math/vector2i.h index 13b70031bd..e131bdea94 100644 --- a/core/math/vector2i.h +++ b/core/math/vector2i.h @@ -38,6 +38,8 @@ class String; struct Vector2; struct _NO_DISCARD_ Vector2i { + static const int AXIS_COUNT = 2; + enum Axis { AXIS_X, AXIS_Y, @@ -115,7 +117,7 @@ struct _NO_DISCARD_ Vector2i { real_t aspect() const { return width / (real_t)height; } Vector2i sign() const { return Vector2i(SIGN(x), SIGN(y)); } - Vector2i abs() const { return Vector2i(ABS(x), ABS(y)); } + Vector2i abs() const { return Vector2i(Math::abs(x), Math::abs(y)); } Vector2i clamp(const Vector2i &p_min, const Vector2i &p_max) const; operator String() const; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index d71d365053..b106200c4a 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -45,16 +45,6 @@ Vector3 Vector3::rotated(const Vector3 &p_axis, const real_t p_angle) const { return r; } -void Vector3::set_axis(const int p_axis, const real_t p_value) { - ERR_FAIL_INDEX(p_axis, 3); - coord[p_axis] = p_value; -} - -real_t Vector3::get_axis(const int p_axis) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - return operator[](p_axis); -} - Vector3 Vector3::clamp(const Vector3 &p_min, const Vector3 &p_max) const { return Vector3( CLAMP(x, p_min.x, p_max.x), @@ -117,18 +107,42 @@ Vector3 Vector3::octahedron_decode(const Vector2 &p_oct) { return n.normalized(); } -Basis Vector3::outer(const Vector3 &p_with) const { - Vector3 row0(x * p_with.x, x * p_with.y, x * p_with.z); - Vector3 row1(y * p_with.x, y * p_with.y, y * p_with.z); - Vector3 row2(z * p_with.x, z * p_with.y, z * p_with.z); +Vector2 Vector3::octahedron_tangent_encode(const float sign) const { + Vector2 res = this->octahedron_encode(); + res.y = res.y * 0.5f + 0.5f; + res.y = sign >= 0.0f ? res.y : 1 - res.y; + return res; +} - return Basis(row0, row1, row2); +Vector3 Vector3::octahedron_tangent_decode(const Vector2 &p_oct, float *sign) { + Vector2 oct_compressed = p_oct; + oct_compressed.y = oct_compressed.y * 2 - 1; + *sign = oct_compressed.y >= 0.0f ? 1.0f : -1.0f; + oct_compressed.y = Math::abs(oct_compressed.y); + Vector3 res = Vector3::octahedron_decode(oct_compressed); + return res; +} + +Basis Vector3::outer(const Vector3 &p_with) const { + Basis basis; + basis.rows[0] = Vector3(x * p_with.x, x * p_with.y, x * p_with.z); + basis.rows[1] = Vector3(y * p_with.x, y * p_with.y, y * p_with.z); + basis.rows[2] = Vector3(z * p_with.x, z * p_with.y, z * p_with.z); + return basis; } bool Vector3::is_equal_approx(const Vector3 &p_v) const { return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z); } +bool Vector3::is_zero_approx() const { + return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z); +} + +bool Vector3::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z); +} + Vector3::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")"; } diff --git a/core/math/vector3.h b/core/math/vector3.h index 970416234d..19771eb312 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -68,13 +68,6 @@ struct _NO_DISCARD_ Vector3 { return coord[p_axis]; } - void set_axis(const int p_axis, const real_t p_value); - real_t get_axis(const int p_axis) const; - - _FORCE_INLINE_ void set_all(const real_t p_value) { - x = y = z = p_value; - } - _FORCE_INLINE_ Vector3::Axis min_axis_index() const { return x < y ? (x < z ? Vector3::AXIS_X : Vector3::AXIS_Z) : (y < z ? Vector3::AXIS_Y : Vector3::AXIS_Z); } @@ -105,12 +98,15 @@ 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; _FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const; + _FORCE_INLINE_ Vector3 cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; _FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const; Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; Vector2 octahedron_encode() const; static Vector3 octahedron_decode(const Vector2 &p_oct); + Vector2 octahedron_tangent_encode(const float sign) const; + static Vector3 octahedron_tangent_decode(const Vector2 &p_oct, float *sign); _FORCE_INLINE_ Vector3 cross(const Vector3 &p_with) const; _FORCE_INLINE_ real_t dot(const Vector3 &p_with) const; @@ -139,6 +135,8 @@ struct _NO_DISCARD_ Vector3 { _FORCE_INLINE_ Vector3 reflect(const Vector3 &p_normal) const; bool is_equal_approx(const Vector3 &p_v) const; + bool is_zero_approx() const; + bool is_finite() const; /* Operators */ @@ -217,16 +215,25 @@ 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 { @@ -237,6 +244,14 @@ Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, c return res; } +Vector3 Vector3::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector3 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} + Vector3 Vector3::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const { Vector3 res = *this; diff --git a/core/math/vector3i.cpp b/core/math/vector3i.cpp index b8e74ea6d2..b248f35035 100644 --- a/core/math/vector3i.cpp +++ b/core/math/vector3i.cpp @@ -33,16 +33,6 @@ #include "core/math/vector3.h" #include "core/string/ustring.h" -void Vector3i::set_axis(const int p_axis, const int32_t p_value) { - ERR_FAIL_INDEX(p_axis, 3); - coord[p_axis] = p_value; -} - -int32_t Vector3i::get_axis(const int p_axis) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - return operator[](p_axis); -} - Vector3i::Axis Vector3i::min_axis_index() const { return x < y ? (x < z ? Vector3i::AXIS_X : Vector3i::AXIS_Z) : (y < z ? Vector3i::AXIS_Y : Vector3i::AXIS_Z); } diff --git a/core/math/vector3i.h b/core/math/vector3i.h index b49c1142ed..710fd96376 100644 --- a/core/math/vector3i.h +++ b/core/math/vector3i.h @@ -38,6 +38,8 @@ class String; struct Vector3; struct _NO_DISCARD_ Vector3i { + static const int AXIS_COUNT = 3; + enum Axis { AXIS_X, AXIS_Y, @@ -64,9 +66,6 @@ struct _NO_DISCARD_ Vector3i { 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; - Vector3i::Axis min_axis_index() const; Vector3i::Axis max_axis_index() const; @@ -128,7 +127,7 @@ double Vector3i::length() const { } Vector3i Vector3i::abs() const { - return Vector3i(ABS(x), ABS(y), ABS(z)); + return Vector3i(Math::abs(x), Math::abs(y), Math::abs(z)); } Vector3i Vector3i::sign() const { diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp new file mode 100644 index 0000000000..3b189f7ed4 --- /dev/null +++ b/core/math/vector4.cpp @@ -0,0 +1,195 @@ +/*************************************************************************/ +/* 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/string/ustring.h" + +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); +} + +bool Vector4::is_zero_approx() const { + return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z) && Math::is_zero_approx(w); +} + +bool Vector4::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w); +} + +real_t Vector4::length() const { + return Math::sqrt(length_squared()); +} + +void Vector4::normalize() { + real_t lengthsq = length_squared(); + if (lengthsq == 0) { + x = y = z = w = 0; + } else { + real_t length = Math::sqrt(lengthsq); + x /= length; + y /= length; + z /= length; + w /= length; + } +} + +Vector4 Vector4::normalized() const { + Vector4 v = *this; + v.normalize(); + return v; +} + +bool Vector4::is_normalized() const { + return Math::is_equal_approx(length_squared(), (real_t)1, (real_t)UNIT_EPSILON); +} + +real_t Vector4::distance_to(const Vector4 &p_to) const { + return (p_to - *this).length(); +} + +real_t Vector4::distance_squared_to(const Vector4 &p_to) const { + return (p_to - *this).length_squared(); +} + +Vector4 Vector4::direction_to(const Vector4 &p_to) const { + Vector4 ret(p_to.x - x, p_to.y - y, p_to.z - z, p_to.w - w); + ret.normalize(); + return ret; +} + +Vector4 Vector4::abs() const { + return Vector4(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w)); +} + +Vector4 Vector4::sign() const { + return Vector4(SIGN(x), SIGN(y), SIGN(z), SIGN(w)); +} + +Vector4 Vector4::floor() const { + return Vector4(Math::floor(x), Math::floor(y), Math::floor(z), Math::floor(w)); +} + +Vector4 Vector4::ceil() const { + return Vector4(Math::ceil(x), Math::ceil(y), Math::ceil(z), Math::ceil(w)); +} + +Vector4 Vector4::round() const { + return Vector4(Math::round(x), Math::round(y), Math::round(z), Math::round(w)); +} + +Vector4 Vector4::lerp(const Vector4 &p_to, const real_t p_weight) const { + return Vector4( + x + (p_weight * (p_to.x - x)), + y + (p_weight * (p_to.y - y)), + z + (p_weight * (p_to.z - z)), + w + (p_weight * (p_to.w - w))); +} + +Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const { + Vector4 res = *this; + res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight); + res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight); + res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight); + res.w = Math::cubic_interpolate(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight); + return res; +} + +Vector4 Vector4::cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const { + Vector4 res = *this; + res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + res.w = Math::cubic_interpolate_in_time(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight, p_b_t, p_pre_a_t, p_post_b_t); + return res; +} + +Vector4 Vector4::posmod(const real_t p_mod) const { + return Vector4(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod), Math::fposmod(w, p_mod)); +} + +Vector4 Vector4::posmodv(const Vector4 &p_modv) const { + return Vector4(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z), Math::fposmod(w, p_modv.w)); +} + +void Vector4::snap(const Vector4 &p_step) { + x = Math::snapped(x, p_step.x); + y = Math::snapped(y, p_step.y); + z = Math::snapped(z, p_step.z); + w = Math::snapped(w, p_step.w); +} + +Vector4 Vector4::snapped(const Vector4 &p_step) const { + Vector4 v = *this; + v.snap(p_step); + return v; +} + +Vector4 Vector4::inverse() const { + return Vector4(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w); +} + +Vector4 Vector4::clamp(const Vector4 &p_min, const Vector4 &p_max) const { + return Vector4( + CLAMP(x, p_min.x, p_max.x), + CLAMP(y, p_min.y, p_max.y), + CLAMP(z, p_min.z, p_max.z), + CLAMP(w, p_min.w, p_max.w)); +} + +Vector4::operator String() const { + return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")"; +} + +static_assert(sizeof(Vector4) == 4 * sizeof(real_t)); diff --git a/core/math/vector4.h b/core/math/vector4.h new file mode 100644 index 0000000000..7c4bdc1788 --- /dev/null +++ b/core/math/vector4.h @@ -0,0 +1,299 @@ +/*************************************************************************/ +/* 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/error/error_macros.h" +#include "core/math/math_funcs.h" + +class String; + +struct _NO_DISCARD_ Vector4 { + static const int AXIS_COUNT = 4; + + 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]; + } + + 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; + bool is_zero_approx() const; + bool is_finite() const; + real_t length() const; + void normalize(); + Vector4 normalized() const; + bool is_normalized() const; + + real_t distance_to(const Vector4 &p_to) const; + real_t distance_squared_to(const Vector4 &p_to) const; + Vector4 direction_to(const Vector4 &p_to) const; + + Vector4 abs() const; + Vector4 sign() const; + Vector4 floor() const; + Vector4 ceil() const; + Vector4 round() const; + Vector4 lerp(const Vector4 &p_to, const real_t p_weight) const; + Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const; + Vector4 cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; + + Vector4 posmod(const real_t p_mod) const; + Vector4 posmodv(const Vector4 &p_modv) const; + void snap(const Vector4 &p_step); + Vector4 snapped(const Vector4 &p_step) const; + Vector4 clamp(const Vector4 &p_min, const Vector4 &p_max) const; + + Vector4 inverse() const; + _FORCE_INLINE_ real_t dot(const Vector4 &p_vec4) const; + + _FORCE_INLINE_ void operator+=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator-=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator*=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator/=(const Vector4 &p_vec4); + _FORCE_INLINE_ void operator*=(const real_t &s); + _FORCE_INLINE_ void operator/=(const real_t &s); + _FORCE_INLINE_ Vector4 operator+(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator-(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator*(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator/(const Vector4 &p_vec4) const; + _FORCE_INLINE_ Vector4 operator-() const; + _FORCE_INLINE_ Vector4 operator*(const real_t &s) const; + _FORCE_INLINE_ Vector4 operator/(const real_t &s) const; + + _FORCE_INLINE_ bool operator==(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator!=(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator>(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator<(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator>=(const Vector4 &p_vec4) const; + _FORCE_INLINE_ bool operator<=(const Vector4 &p_vec4) const; + + operator String() const; + + _FORCE_INLINE_ Vector4() {} + + _FORCE_INLINE_ Vector4(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : + x(p_x), + y(p_y), + z(p_z), + w(p_w) { + } + + Vector4(const Vector4 &p_vec4) : + x(p_vec4.x), + y(p_vec4.y), + z(p_vec4.z), + w(p_vec4.w) { + } + + void operator=(const Vector4 &p_vec4) { + x = p_vec4.x; + y = p_vec4.y; + z = p_vec4.z; + w = p_vec4.w; + } +}; + +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..77f6fbd5b7 --- /dev/null +++ b/core/math/vector4i.cpp @@ -0,0 +1,83 @@ +/*************************************************************************/ +/* 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" + +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 = (int32_t)p_vec4.x; + y = (int32_t)p_vec4.y; + z = (int32_t)p_vec4.z; + w = (int32_t)p_vec4.w; +} + +static_assert(sizeof(Vector4i) == 4 * sizeof(int32_t)); diff --git a/core/math/vector4i.h b/core/math/vector4i.h new file mode 100644 index 0000000000..a32414bb18 --- /dev/null +++ b/core/math/vector4i.h @@ -0,0 +1,337 @@ +/*************************************************************************/ +/* 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 { + static const int AXIS_COUNT = 4; + + 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]; + } + + 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(Math::abs(x), Math::abs(y), Math::abs(z), Math::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 |