summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/a_star.cpp6
-rw-r--r--core/math/a_star.h4
-rw-r--r--core/math/a_star_grid_2d.cpp589
-rw-r--r--core/math/a_star_grid_2d.h178
-rw-r--r--core/math/aabb.h8
-rw-r--r--core/math/audio_frame.h2
-rw-r--r--core/math/basis.cpp67
-rw-r--r--core/math/basis.h9
-rw-r--r--core/math/geometry_3d.cpp105
-rw-r--r--core/math/geometry_3d.h92
-rw-r--r--core/math/math_funcs.h103
-rw-r--r--core/math/projection.cpp8
-rw-r--r--core/math/projection.h2
-rw-r--r--core/math/quaternion.cpp56
-rw-r--r--core/math/quaternion.h1
-rw-r--r--core/math/rect2.h4
-rw-r--r--core/math/rect2i.h4
-rw-r--r--core/math/transform_3d.cpp13
-rw-r--r--core/math/transform_3d.h1
-rw-r--r--core/math/vector2.cpp4
-rw-r--r--core/math/vector2.h9
-rw-r--r--core/math/vector2i.h2
-rw-r--r--core/math/vector3.cpp30
-rw-r--r--core/math/vector3.h12
-rw-r--r--core/math/vector3i.h2
-rw-r--r--core/math/vector4.cpp36
-rw-r--r--core/math/vector4.h3
-rw-r--r--core/math/vector4i.cpp10
-rw-r--r--core/math/vector4i.h2
29 files changed, 1142 insertions, 220 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..23d7e379ee
--- /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"
+
+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_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_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_manhattan, heuristic_euclidian, 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;
+}
+
+Vector<Vector2> AStarGrid2D::get_id_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(Vector2((float)a->id.x, (float)a->id.y));
+ 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--] = Vector2((float)p->id.x, (float)p->id.y);
+ p = p->prev_point;
+ }
+
+ w[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, "Manhattan,Euclidean,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..bf6363aa01
--- /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);
+ Vector<Vector2> 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.h b/core/math/aabb.h
index e88ba33531..acf903eeba 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -47,12 +47,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; }
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..0eb6320ac6 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -749,67 +749,6 @@ 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
@@ -1094,13 +1033,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..cc2924f5ff 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -149,9 +149,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 +238,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/geometry_3d.cpp b/core/math/geometry_3d.cpp
index ec96753c79..9238293b48 100644
--- a/core/math/geometry_3d.cpp
+++ b/core/math/geometry_3d.cpp
@@ -35,6 +35,111 @@
#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_funcs.h b/core/math/math_funcs.h
index 53deb9bd42..656fc9f798 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -229,11 +229,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 +253,89 @@ 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);
@@ -288,8 +371,8 @@ public:
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)) {
@@ -308,11 +391,11 @@ public:
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/projection.cpp b/core/math/projection.cpp
index edf8bf36cd..863fe6ee79 100644
--- a/core/math/projection.cpp
+++ b/core/math/projection.cpp
@@ -255,7 +255,7 @@ void Projection::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t
}
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);
@@ -282,7 +282,7 @@ void Projection::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t
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;
@@ -816,7 +816,7 @@ real_t Projection::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],
@@ -825,7 +825,7 @@ real_t Projection::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)));
}
}
diff --git a/core/math/projection.h b/core/math/projection.h
index a3d2d7720b..f149d307b3 100644
--- a/core/math/projection.h
+++ b/core/math/projection.h
@@ -96,7 +96,7 @@ struct Projection {
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;
diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp
index c681c60694..c836a82e37 100644
--- a/core/math/quaternion.cpp
+++ b/core/math/quaternion.cpp
@@ -112,10 +112,11 @@ Quaternion Quaternion::exp() const {
Quaternion src = *this;
Vector3 src_v = Vector3(src.x, src.y, src.z);
real_t theta = src_v.length();
- if (theta < CMP_EPSILON) {
+ 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 {
@@ -233,6 +234,57 @@ Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const
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
+ 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 {
return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
}
diff --git a/core/math/quaternion.h b/core/math/quaternion.h
index cb54a6f540..43d7bffcfc 100644
--- a/core/math/quaternion.h
+++ b/core/math/quaternion.h
@@ -72,6 +72,7 @@ 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 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;
real_t get_angle() const;
diff --git a/core/math/rect2.h b/core/math/rect2.h
index 679af933c2..2d1be3d4f3 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
diff --git a/core/math/rect2i.h b/core/math/rect2i.h
index db1459a3e6..2b58dcdd98 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
diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp
index a634faca9a..2de9e81b38 100644
--- a/core/math/transform_3d.cpp
+++ b/core/math/transform_3d.cpp
@@ -94,9 +94,7 @@ void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, con
origin = p_eye;
}
-Transform3D Transform3D::spherical_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();
@@ -113,15 +111,6 @@ Transform3D Transform3D::spherical_interpolate_with(const Transform3D &p_transfo
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;
diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h
index b572e90859..c62e4a7b0e 100644
--- a/core/math/transform_3d.h
+++ b/core/math/transform_3d.h
@@ -103,7 +103,6 @@ struct _NO_DISCARD_ Transform3D {
void operator*=(const real_t p_val);
Transform3D operator*(const real_t p_val) const;
- Transform3D spherical_interpolate_with(const Transform3D &p_transform, real_t p_c) const;
Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const;
_FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index d9b5d55454..56dbba393a 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -182,6 +182,10 @@ 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);
+}
+
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..9441f84087 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -114,6 +114,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 +124,7 @@ 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;
Vector2 operator+(const Vector2 &p_v) const;
void operator+=(const Vector2 &p_v);
@@ -270,6 +272,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..0245900a3b 100644
--- a/core/math/vector2i.h
+++ b/core/math/vector2i.h
@@ -115,7 +115,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..4db45fe798 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -117,18 +117,38 @@ 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);
+}
+
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 4ce01da60e..3944afa92e 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -105,12 +105,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 +142,7 @@ 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;
/* Operators */
@@ -246,6 +250,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.h b/core/math/vector3i.h
index b49c1142ed..825ce40318 100644
--- a/core/math/vector3i.h
+++ b/core/math/vector3i.h
@@ -128,7 +128,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
index 4697c311b4..3c25f454a3 100644
--- a/core/math/vector4.cpp
+++ b/core/math/vector4.cpp
@@ -71,26 +71,45 @@ 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);
+}
+
real_t Vector4::length() const {
return Math::sqrt(length_squared());
}
void Vector4::normalize() {
- *this /= length();
+ 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 {
- return *this / length();
+ Vector4 v = *this;
+ v.normalize();
+ return v;
}
bool Vector4::is_normalized() const {
- return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); // Use less epsilon.
+ 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();
@@ -134,6 +153,15 @@ Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, c
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));
}
@@ -170,3 +198,5 @@ Vector4 Vector4::clamp(const Vector4 &p_min, const Vector4 &p_max) const {
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
index 373a6a1218..f964264108 100644
--- a/core/math/vector4.h
+++ b/core/math/vector4.h
@@ -73,12 +73,14 @@ struct _NO_DISCARD_ Vector4 {
_FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Vector4 &p_vec4) const;
+ bool is_zero_approx() 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;
@@ -88,6 +90,7 @@ struct _NO_DISCARD_ Vector4 {
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;
diff --git a/core/math/vector4i.cpp b/core/math/vector4i.cpp
index 2dc5b74202..a89b802675 100644
--- a/core/math/vector4i.cpp
+++ b/core/math/vector4i.cpp
@@ -84,8 +84,10 @@ Vector4i::operator Vector4() const {
}
Vector4i::Vector4i(const Vector4 &p_vec4) {
- x = p_vec4.x;
- y = p_vec4.y;
- z = p_vec4.z;
- w = p_vec4.w;
+ 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
index 37d905878f..d08e40d754 100644
--- a/core/math/vector4i.h
+++ b/core/math/vector4i.h
@@ -132,7 +132,7 @@ double Vector4i::length() const {
}
Vector4i Vector4i::abs() const {
- return Vector4i(ABS(x), ABS(y), ABS(z), ABS(w));
+ return Vector4i(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w));
}
Vector4i Vector4i::sign() const {
generated by cgit v1.2.3 (git 2.25.1) at 2025-04-14 10:45:01 +0000