summaryrefslogtreecommitdiffstats
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/SCsub1
-rw-r--r--core/math/a_star.cpp44
-rw-r--r--core/math/a_star.h8
-rw-r--r--core/math/a_star_grid_2d.cpp225
-rw-r--r--core/math/a_star_grid_2d.h29
-rw-r--r--core/math/aabb.h23
-rw-r--r--core/math/basis.h10
-rw-r--r--core/math/color.h67
-rw-r--r--core/math/convex_hull.cpp2
-rw-r--r--core/math/expression.cpp12
-rw-r--r--core/math/geometry_2d.cpp14
-rw-r--r--core/math/geometry_2d.h12
-rw-r--r--core/math/math_funcs.h15
-rw-r--r--core/math/plane.h2
-rw-r--r--core/math/random_pcg.cpp5
-rw-r--r--core/math/rect2.cpp2
-rw-r--r--core/math/rect2.h16
-rw-r--r--core/math/transform_2d.h13
-rw-r--r--core/math/transform_interpolator.cpp338
-rw-r--r--core/math/transform_interpolator.h51
-rw-r--r--core/math/vector2.cpp2
-rw-r--r--core/math/vector3.cpp2
-rw-r--r--core/math/vector4.h6
23 files changed, 703 insertions, 196 deletions
diff --git a/core/math/SCsub b/core/math/SCsub
index c8fdac207e..6ea3ab6b12 100644
--- a/core/math/SCsub
+++ b/core/math/SCsub
@@ -1,4 +1,5 @@
#!/usr/bin/env python
+from misc.utility.scons_hints import *
Import("env")
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 4497604947..c85201a973 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -319,11 +319,11 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const {
return closest_point;
}
-bool AStar3D::_solve(Point *begin_point, Point *end_point) {
+bool AStar3D::_solve(Point *begin_point, Point *end_point, bool p_allow_partial_path) {
last_closest_point = nullptr;
pass++;
- if (!end_point->enabled) {
+ if (!end_point->enabled && !p_allow_partial_path) {
return false;
}
@@ -391,9 +391,9 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point) {
return found_route;
}
-real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
+real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
real_t scost;
- if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
+ if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
return scost;
}
@@ -401,11 +401,11 @@ real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
bool from_exists = points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
- Point *to_point = nullptr;
- bool to_exists = points.lookup(p_to_id, to_point);
- ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
+ Point *end_point = nullptr;
+ bool end_exists = points.lookup(p_end_id, end_point);
+ ERR_FAIL_COND_V_MSG(!end_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
- return from_point->pos.distance_to(to_point->pos);
+ return from_point->pos.distance_to(end_point->pos);
}
real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
@@ -443,7 +443,7 @@ Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool
Point *begin_point = a;
Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || last_closest_point == nullptr) {
return Vector<Vector3>();
@@ -497,7 +497,7 @@ Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_
Point *begin_point = a;
Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || last_closest_point == nullptr) {
return Vector<int64_t>();
@@ -579,7 +579,7 @@ void AStar3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_point_path, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_id_path, DEFVAL(false));
- GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
+ GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
}
@@ -675,9 +675,9 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
return Vector2(p.x, p.y);
}
-real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
+real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
real_t scost;
- if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
+ if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
return scost;
}
@@ -685,11 +685,11 @@ real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
bool from_exists = astar.points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
- AStar3D::Point *to_point = nullptr;
- bool to_exists = astar.points.lookup(p_to_id, to_point);
- ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
+ AStar3D::Point *end_point = nullptr;
+ bool to_exists = astar.points.lookup(p_end_id, end_point);
+ ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
- return from_point->pos.distance_to(to_point->pos);
+ return from_point->pos.distance_to(end_point->pos);
}
real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
@@ -726,7 +726,7 @@ Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool
AStar3D::Point *begin_point = a;
AStar3D::Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
return Vector<Vector2>();
@@ -780,7 +780,7 @@ Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_
AStar3D::Point *begin_point = a;
AStar3D::Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
return Vector<int64_t>();
@@ -816,11 +816,11 @@ Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_
return path;
}
-bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) {
+bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point, bool p_allow_partial_path) {
astar.last_closest_point = nullptr;
astar.pass++;
- if (!end_point->enabled) {
+ if (!end_point->enabled && !p_allow_partial_path) {
return false;
}
@@ -918,6 +918,6 @@ void AStar2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_point_path, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_id_path, DEFVAL(false));
- GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
+ GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
}
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 8e054c4789..cbaafc1018 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -115,12 +115,12 @@ class AStar3D : public RefCounted {
HashSet<Segment, Segment> segments;
Point *last_closest_point = nullptr;
- bool _solve(Point *begin_point, Point *end_point);
+ bool _solve(Point *begin_point, Point *end_point, bool p_allow_partial_path);
protected:
static void _bind_methods();
- virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_to_id);
+ virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_end_id);
virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id);
GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
@@ -171,12 +171,12 @@ class AStar2D : public RefCounted {
GDCLASS(AStar2D, RefCounted);
AStar3D astar;
- bool _solve(AStar3D::Point *begin_point, AStar3D::Point *end_point);
+ bool _solve(AStar3D::Point *begin_point, AStar3D::Point *end_point, bool p_allow_partial_path);
protected:
static void _bind_methods();
- virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_to_id);
+ virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_end_id);
virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id);
GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
diff --git a/core/math/a_star_grid_2d.cpp b/core/math/a_star_grid_2d.cpp
index 984bb1c9c1..7e0e982c62 100644
--- a/core/math/a_star_grid_2d.cpp
+++ b/core/math/a_star_grid_2d.cpp
@@ -127,13 +127,18 @@ void AStarGrid2D::update() {
}
points.clear();
+ solid_mask.clear();
const int32_t end_x = region.get_end().x;
const int32_t end_y = region.get_end().y;
const Vector2 half_cell_size = cell_size / 2;
+ for (int32_t x = region.position.x; x < end_x + 2; x++) {
+ solid_mask.push_back(true);
+ }
for (int32_t y = region.position.y; y < end_y; y++) {
LocalVector<Point> line;
+ solid_mask.push_back(true);
for (int32_t x = region.position.x; x < end_x; x++) {
Vector2 v = offset;
switch (cell_shape) {
@@ -150,10 +155,16 @@ void AStarGrid2D::update() {
break;
}
line.push_back(Point(Vector2i(x, y), v));
+ solid_mask.push_back(false);
}
+ solid_mask.push_back(true);
points.push_back(line);
}
+ for (int32_t x = region.position.x; x < end_x + 2; x++) {
+ solid_mask.push_back(true);
+ }
+
dirty = false;
}
@@ -207,13 +218,13 @@ AStarGrid2D::Heuristic AStarGrid2D::get_default_estimate_heuristic() const {
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 %s out of bounds %s.", p_id, region));
- _get_point_unchecked(p_id)->solid = p_solid;
+ _set_solid_unchecked(p_id, 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 %s out of bounds %s.", p_id, region));
- return _get_point_unchecked(p_id)->solid;
+ return _get_solid_unchecked(p_id);
}
void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
@@ -238,7 +249,7 @@ void AStarGrid2D::fill_solid_region(const Rect2i &p_region, bool p_solid) {
for (int32_t y = safe_region.position.y; y < end_y; y++) {
for (int32_t x = safe_region.position.x; x < end_x; x++) {
- _get_point_unchecked(x, y)->solid = p_solid;
+ _set_solid_unchecked(x, y, p_solid);
}
}
}
@@ -259,13 +270,6 @@ void AStarGrid2D::fill_weight_scale_region(const Rect2i &p_region, real_t p_weig
}
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;
- }
-
int32_t from_x = p_from->id.x;
int32_t from_y = p_from->id.y;
@@ -276,72 +280,109 @@ AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
int32_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 (dx == 0 || dy == 0) {
+ return _forced_successor(to_x, to_y, dx, dy);
+ }
+
+ while (_is_walkable(to_x, to_y) && (diagonal_mode == DIAGONAL_MODE_ALWAYS || _is_walkable(to_x, to_y - dy) || _is_walkable(to_x - dx, to_y))) {
+ if (end->id.x == to_x && end->id.y == to_y) {
+ return end;
}
- if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
- return p_to;
+
+ 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 _get_point_unchecked(to_x, to_y);
}
- } 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 (_forced_successor(to_x + dx, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y + dy, 0, dy) != nullptr) {
+ return _get_point_unchecked(to_x, to_y);
}
+
+ to_x += dx;
+ to_y += dy;
}
- 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 (dx == 0 || dy == 0) {
+ return _forced_successor(from_x, from_y, dx, dy, true);
+ }
+
+ while (_is_walkable(to_x, to_y) && _is_walkable(to_x, to_y - dy) && _is_walkable(to_x - dx, to_y)) {
+ if (end->id.x == to_x && end->id.y == to_y) {
+ return end;
}
- if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
- return p_to;
+
+ if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
+ return _get_point_unchecked(to_x, to_y);
}
- } 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 (_forced_successor(to_x, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y, 0, dy) != nullptr) {
+ return _get_point_unchecked(to_x, to_y);
}
+
+ to_x += dx;
+ to_y += dy;
}
- 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, 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;
+ if (dy == 0) {
+ return _forced_successor(from_x, from_y, dx, 0, true);
+ }
+
+ while (_is_walkable(to_x, to_y)) {
+ if (end->id.x == to_x && end->id.y == to_y) {
+ return end;
}
- } else if (dy != 0) {
+
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 (_jump(p_to, _get_point(to_x + 1, to_y)) != nullptr) {
- return p_to;
+ return _get_point_unchecked(to_x, to_y);
}
- if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) {
- return p_to;
+
+ if (_forced_successor(to_x, to_y, 1, 0, true) != nullptr || _forced_successor(to_x, to_y, -1, 0, true) != nullptr) {
+ return _get_point_unchecked(to_x, to_y);
}
+
+ to_y += dy;
}
- return _jump(p_to, _get_point(to_x + dx, to_y + dy));
+ }
+
+ return nullptr;
+}
+
+AStarGrid2D::Point *AStarGrid2D::_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive) {
+ // Remembering previous results can improve performance.
+ bool l_prev = false, r_prev = false, l = false, r = false;
+
+ int32_t o_x = p_x, o_y = p_y;
+ if (p_inclusive) {
+ o_x += p_dx;
+ o_y += p_dy;
+ }
+
+ int32_t l_x = p_x - p_dy, l_y = p_y - p_dx;
+ int32_t r_x = p_x + p_dy, r_y = p_y + p_dx;
+
+ while (_is_walkable(o_x, o_y)) {
+ if (end->id.x == o_x && end->id.y == o_y) {
+ return end;
+ }
+
+ l_prev = l || _is_walkable(l_x, l_y);
+ r_prev = r || _is_walkable(r_x, r_y);
+
+ l_x += p_dx;
+ l_y += p_dy;
+ r_x += p_dx;
+ r_y += p_dy;
+
+ l = _is_walkable(l_x, l_y);
+ r = _is_walkable(r_x, r_y);
+
+ if ((l && !l_prev) || (r && !r_prev)) {
+ return _get_point_unchecked(o_x, o_y);
+ }
+
+ o_x += p_dx;
+ o_y += p_dy;
}
return nullptr;
}
@@ -394,19 +435,19 @@ void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
}
}
- if (top && !top->solid) {
+ if (top && !_get_solid_unchecked(top->id)) {
r_nbors.push_back(top);
ts0 = true;
}
- if (right && !right->solid) {
+ if (right && !_get_solid_unchecked(right->id)) {
r_nbors.push_back(right);
ts1 = true;
}
- if (bottom && !bottom->solid) {
+ if (bottom && !_get_solid_unchecked(bottom->id)) {
r_nbors.push_back(bottom);
ts2 = true;
}
- if (left && !left->solid) {
+ if (left && !_get_solid_unchecked(left->id)) {
r_nbors.push_back(left);
ts3 = true;
}
@@ -436,25 +477,25 @@ void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
break;
}
- if (td0 && (top_left && !top_left->solid)) {
+ if (td0 && (top_left && !_get_solid_unchecked(top_left->id))) {
r_nbors.push_back(top_left);
}
- if (td1 && (top_right && !top_right->solid)) {
+ if (td1 && (top_right && !_get_solid_unchecked(top_right->id))) {
r_nbors.push_back(top_right);
}
- if (td2 && (bottom_right && !bottom_right->solid)) {
+ if (td2 && (bottom_right && !_get_solid_unchecked(bottom_right->id))) {
r_nbors.push_back(bottom_right);
}
- if (td3 && (bottom_left && !bottom_left->solid)) {
+ if (td3 && (bottom_left && !_get_solid_unchecked(bottom_left->id))) {
r_nbors.push_back(bottom_left);
}
}
-bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
+bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point, bool p_allow_partial_path) {
last_closest_point = nullptr;
pass++;
- if (p_end_point->solid) {
+ if (_get_solid_unchecked(p_end_point->id) && !p_allow_partial_path) {
return false;
}
@@ -500,7 +541,7 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
continue;
}
} else {
- if (e->solid || e->closed_pass == pass) {
+ if (_get_solid_unchecked(e->id) || e->closed_pass == pass) {
continue;
}
weight_scale = e->weight_scale;
@@ -535,12 +576,12 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
return found_route;
}
-real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_end_id) {
real_t scost;
- if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
+ if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
return scost;
}
- return heuristics[default_estimate_heuristic](p_from_id, p_to_id);
+ return heuristics[default_estimate_heuristic](p_from_id, p_end_id);
}
real_t AStarGrid2D::_compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
@@ -562,6 +603,33 @@ Vector2 AStarGrid2D::get_point_position(const Vector2i &p_id) const {
return _get_point_unchecked(p_id)->pos;
}
+TypedArray<Dictionary> AStarGrid2D::get_point_data_in_region(const Rect2i &p_region) const {
+ ERR_FAIL_COND_V_MSG(dirty, TypedArray<Dictionary>(), "Grid is not initialized. Call the update method.");
+ const Rect2i inter_region = region.intersection(p_region);
+
+ const int32_t start_x = inter_region.position.x - region.position.x;
+ const int32_t start_y = inter_region.position.y - region.position.y;
+ const int32_t end_x = inter_region.get_end().x - region.position.x;
+ const int32_t end_y = inter_region.get_end().y - region.position.y;
+
+ TypedArray<Dictionary> data;
+
+ for (int32_t y = start_y; y < end_y; y++) {
+ for (int32_t x = start_x; x < end_x; x++) {
+ const Point &p = points[y][x];
+
+ Dictionary dict;
+ dict["id"] = p.id;
+ dict["position"] = p.pos;
+ dict["solid"] = _get_solid_unchecked(p.id);
+ dict["weight_scale"] = p.weight_scale;
+ data.push_back(dict);
+ }
+ }
+
+ return data;
+}
+
Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id, bool p_allow_partial_path) {
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 %s out of bounds %s.", p_from_id, region));
@@ -579,7 +647,7 @@ Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vec
Point *begin_point = a;
Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || last_closest_point == nullptr) {
return Vector<Vector2>();
@@ -632,7 +700,7 @@ TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const V
Point *begin_point = a;
Point *end_point = b;
- bool found_route = _solve(begin_point, end_point);
+ bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
if (!found_route) {
if (!p_allow_partial_path || last_closest_point == nullptr) {
return TypedArray<Vector2i>();
@@ -698,10 +766,11 @@ void AStarGrid2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStarGrid2D::get_point_position);
+ ClassDB::bind_method(D_METHOD("get_point_data_in_region", "region"), &AStarGrid2D::get_point_data_in_region);
ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_point_path, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_id_path, DEFVAL(false));
- GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
+ GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
ADD_PROPERTY(PropertyInfo(Variant::RECT2I, "region"), "set_region", "get_region");
diff --git a/core/math/a_star_grid_2d.h b/core/math/a_star_grid_2d.h
index 1a9f6dcc11..0536b8109b 100644
--- a/core/math/a_star_grid_2d.h
+++ b/core/math/a_star_grid_2d.h
@@ -78,7 +78,6 @@ private:
struct Point {
Vector2i id;
- bool solid = false;
Vector2 pos;
real_t weight_scale = 1.0;
@@ -111,6 +110,7 @@ private:
}
};
+ LocalVector<bool> solid_mask;
LocalVector<LocalVector<Point>> points;
Point *end = nullptr;
Point *last_closest_point = nullptr;
@@ -118,11 +118,12 @@ private:
uint64_t pass = 1;
private: // Internal routines.
+ _FORCE_INLINE_ size_t _to_mask_index(int32_t p_x, int32_t p_y) const {
+ return ((p_y - region.position.y + 1) * (region.size.x + 2)) + p_x - region.position.x + 1;
+ }
+
_FORCE_INLINE_ bool _is_walkable(int32_t p_x, int32_t p_y) const {
- if (region.has_point(Vector2i(p_x, p_y))) {
- return !points[p_y - region.position.y][p_x - region.position.x].solid;
- }
- return false;
+ return !solid_mask[_to_mask_index(p_x, p_y)];
}
_FORCE_INLINE_ Point *_get_point(int32_t p_x, int32_t p_y) {
@@ -132,6 +133,18 @@ private: // Internal routines.
return nullptr;
}
+ _FORCE_INLINE_ void _set_solid_unchecked(int32_t p_x, int32_t p_y, bool p_solid) {
+ solid_mask[_to_mask_index(p_x, p_y)] = p_solid;
+ }
+
+ _FORCE_INLINE_ void _set_solid_unchecked(const Vector2i &p_id, bool p_solid) {
+ solid_mask[_to_mask_index(p_id.x, p_id.y)] = p_solid;
+ }
+
+ _FORCE_INLINE_ bool _get_solid_unchecked(const Vector2i &p_id) const {
+ return solid_mask[_to_mask_index(p_id.x, p_id.y)];
+ }
+
_FORCE_INLINE_ Point *_get_point_unchecked(int32_t p_x, int32_t p_y) {
return &points[p_y - region.position.y][p_x - region.position.x];
}
@@ -146,12 +159,13 @@ private: // Internal routines.
void _get_nbors(Point *p_point, LocalVector<Point *> &r_nbors);
Point *_jump(Point *p_from, Point *p_to);
- bool _solve(Point *p_begin_point, Point *p_end_point);
+ bool _solve(Point *p_begin_point, Point *p_end_point, bool p_allow_partial_path);
+ Point *_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive = false);
protected:
static void _bind_methods();
- virtual real_t _estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id);
+ virtual real_t _estimate_cost(const Vector2i &p_from_id, const Vector2i &p_end_id);
virtual real_t _compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id);
GDVIRTUAL2RC(real_t, _estimate_cost, Vector2i, Vector2i)
@@ -209,6 +223,7 @@ public:
void clear();
Vector2 get_point_position(const Vector2i &p_id) const;
+ TypedArray<Dictionary> get_point_data_in_region(const Rect2i &p_region) const;
Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
TypedArray<Vector2i> get_id_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
};
diff --git a/core/math/aabb.h b/core/math/aabb.h
index cb358ca7ef..7a5581b5d4 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -85,7 +85,7 @@ struct [[nodiscard]] AABB {
bool intersects_plane(const Plane &p_plane) const;
_FORCE_INLINE_ bool has_point(const Vector3 &p_point) const;
- _FORCE_INLINE_ Vector3 get_support(const Vector3 &p_normal) const;
+ _FORCE_INLINE_ Vector3 get_support(const Vector3 &p_direction) const;
Vector3 get_longest_axis() const;
int get_longest_axis_index() const;
@@ -212,15 +212,18 @@ inline bool AABB::encloses(const AABB &p_aabb) const {
(src_max.z >= dst_max.z));
}
-Vector3 AABB::get_support(const Vector3 &p_normal) const {
- Vector3 half_extents = size * 0.5f;
- Vector3 ofs = position + half_extents;
-
- return Vector3(
- (p_normal.x > 0) ? half_extents.x : -half_extents.x,
- (p_normal.y > 0) ? half_extents.y : -half_extents.y,
- (p_normal.z > 0) ? half_extents.z : -half_extents.z) +
- ofs;
+Vector3 AABB::get_support(const Vector3 &p_direction) const {
+ Vector3 support = position;
+ if (p_direction.x > 0.0f) {
+ support.x += size.x;
+ }
+ if (p_direction.y > 0.0f) {
+ support.y += size.y;
+ }
+ if (p_direction.z > 0.0f) {
+ support.z += size.z;
+ }
+ return support;
}
Vector3 AABB::get_endpoint(int p_point) const {
diff --git a/core/math/basis.h b/core/math/basis.h
index 5c1a5fbdda..2d4994de19 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -41,11 +41,11 @@ struct [[nodiscard]] Basis {
Vector3(0, 0, 1)
};
- _FORCE_INLINE_ const Vector3 &operator[](int p_axis) const {
- return rows[p_axis];
+ _FORCE_INLINE_ const Vector3 &operator[](int p_row) const {
+ return rows[p_row];
}
- _FORCE_INLINE_ Vector3 &operator[](int p_axis) {
- return rows[p_axis];
+ _FORCE_INLINE_ Vector3 &operator[](int p_row) {
+ return rows[p_row];
}
void invert();
@@ -223,7 +223,7 @@ struct [[nodiscard]] Basis {
static Basis looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0), bool p_use_model_front = false);
- Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); };
+ Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); }
Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, p_scale); }
Basis(const Vector3 &p_axis, real_t p_angle) { set_axis_angle(p_axis, p_angle); }
diff --git a/core/math/color.h b/core/math/color.h
index e17b8c9fd7..70fad78acb 100644
--- a/core/math/color.h
+++ b/core/math/color.h
@@ -129,33 +129,46 @@ struct [[nodiscard]] Color {
}
_FORCE_INLINE_ uint32_t to_rgbe9995() const {
- const float pow2to9 = 512.0f;
- const float B = 15.0f;
- const float N = 9.0f;
-
- float sharedexp = 65408.000f; // Result of: ((pow2to9 - 1.0f) / pow2to9) * powf(2.0f, 31.0f - 15.0f)
-
- float cRed = MAX(0.0f, MIN(sharedexp, r));
- float cGreen = MAX(0.0f, MIN(sharedexp, g));
- float cBlue = MAX(0.0f, MIN(sharedexp, b));
-
- float cMax = MAX(cRed, MAX(cGreen, cBlue));
-
- float expp = MAX(-B - 1.0f, floor(Math::log(cMax) / (real_t)Math_LN2)) + 1.0f + B;
-
- float sMax = (float)floor((cMax / Math::pow(2.0f, expp - B - N)) + 0.5f);
-
- float exps = expp + 1.0f;
-
- if (0.0f <= sMax && sMax < pow2to9) {
- exps = expp;
- }
-
- float sRed = Math::floor((cRed / pow(2.0f, exps - B - N)) + 0.5f);
- float sGreen = Math::floor((cGreen / pow(2.0f, exps - B - N)) + 0.5f);
- float sBlue = Math::floor((cBlue / pow(2.0f, exps - B - N)) + 0.5f);
-
- return (uint32_t(Math::fast_ftoi(sRed)) & 0x1FF) | ((uint32_t(Math::fast_ftoi(sGreen)) & 0x1FF) << 9) | ((uint32_t(Math::fast_ftoi(sBlue)) & 0x1FF) << 18) | ((uint32_t(Math::fast_ftoi(exps)) & 0x1F) << 27);
+ // https://github.com/microsoft/DirectX-Graphics-Samples/blob/v10.0.19041.0/MiniEngine/Core/Color.cpp
+ static const float kMaxVal = float(0x1FF << 7);
+ static const float kMinVal = float(1.f / (1 << 16));
+
+ // Clamp RGB to [0, 1.FF*2^16]
+ const float _r = CLAMP(r, 0.0f, kMaxVal);
+ const float _g = CLAMP(g, 0.0f, kMaxVal);
+ const float _b = CLAMP(b, 0.0f, kMaxVal);
+
+ // Compute the maximum channel, no less than 1.0*2^-15
+ const float MaxChannel = MAX(MAX(_r, _g), MAX(_b, kMinVal));
+
+ // Take the exponent of the maximum channel (rounding up the 9th bit) and
+ // add 15 to it. When added to the channels, it causes the implicit '1.0'
+ // bit and the first 8 mantissa bits to be shifted down to the low 9 bits
+ // of the mantissa, rounding the truncated bits.
+ union {
+ float f;
+ int32_t i;
+ } R, G, B, E;
+
+ E.f = MaxChannel;
+ E.i += 0x07804000; // Add 15 to the exponent and 0x4000 to the mantissa
+ E.i &= 0x7F800000; // Zero the mantissa
+
+ // This shifts the 9-bit values we need into the lowest bits, rounding as
+ // needed. Note that if the channel has a smaller exponent than the max
+ // channel, it will shift even more. This is intentional.
+ R.f = _r + E.f;
+ G.f = _g + E.f;
+ B.f = _b + E.f;
+
+ // Convert the Bias to the correct exponent in the upper 5 bits.
+ E.i <<= 4;
+ E.i += 0x10000000;
+
+ // Combine the fields. RGB floats have unwanted data in the upper 9
+ // bits. Only red needs to mask them off because green and blue shift
+ // it out to the left.
+ return E.i | (B.i << 18) | (G.i << 9) | (R.i & 511);
}
_FORCE_INLINE_ Color blend(const Color &p_over) const {
diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp
index 80662c1b07..620a7541e4 100644
--- a/core/math/convex_hull.cpp
+++ b/core/math/convex_hull.cpp
@@ -204,7 +204,7 @@ public:
static Int128 mul(uint64_t a, uint64_t b);
Int128 operator-() const {
- return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
+ return Int128(uint64_t(-int64_t(low)), ~high + (low == 0));
}
Int128 operator+(const Int128 &b) const {
diff --git a/core/math/expression.cpp b/core/math/expression.cpp
index 636c2c16bf..35303fe9ac 100644
--- a/core/math/expression.cpp
+++ b/core/math/expression.cpp
@@ -30,12 +30,7 @@
#include "expression.h"
-#include "core/io/marshalls.h"
-#include "core/math/math_funcs.h"
#include "core/object/class_db.h"
-#include "core/object/ref_counted.h"
-#include "core/os/os.h"
-#include "core/variant/variant_parser.h"
Error Expression::_get_token(Token &r_token) {
while (true) {
@@ -392,7 +387,6 @@ Error Expression::_get_token(Token &r_token) {
if (is_digit(c)) {
} else if (c == 'e') {
reading = READING_EXP;
-
} else {
reading = READING_DONE;
}
@@ -419,7 +413,9 @@ Error Expression::_get_token(Token &r_token) {
is_first_char = false;
}
- str_ofs--;
+ if (c != 0) {
+ str_ofs--;
+ }
r_token.type = TK_CONSTANT;
@@ -1495,7 +1491,7 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu
}
Variant Expression::execute(const Array &p_inputs, Object *p_base, bool p_show_error, bool p_const_calls_only) {
- ERR_FAIL_COND_V_MSG(error_set, Variant(), "There was previously a parse error: " + error_str + ".");
+ ERR_FAIL_COND_V_MSG(error_set, Variant(), vformat("There was previously a parse error: %s.", error_str));
execution_error = false;
Variant output;
diff --git a/core/math/geometry_2d.cpp b/core/math/geometry_2d.cpp
index d60619b27f..376d5d0b43 100644
--- a/core/math/geometry_2d.cpp
+++ b/core/math/geometry_2d.cpp
@@ -35,7 +35,8 @@
#define STB_RECT_PACK_IMPLEMENTATION
#include "thirdparty/misc/stb_rect_pack.h"
-#define PRECISION 5 // Based on CMP_EPSILON.
+const int clipper_precision = 5; // Based on CMP_EPSILON.
+const double clipper_scale = Math::pow(10.0, clipper_precision);
Vector<Vector<Vector2>> Geometry2D::decompose_polygon_in_convex(const Vector<Point2> &polygon) {
Vector<Vector<Vector2>> decomp;
@@ -75,7 +76,7 @@ struct _AtlasWorkRect {
Size2i s;
Point2i p;
int idx = 0;
- _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; };
+ _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }
};
struct _AtlasWorkRectResult {
@@ -224,7 +225,7 @@ Vector<Vector<Point2>> Geometry2D::_polypaths_do_operation(PolyBooleanOperation
path_b[i] = PointD(p_polypath_b[i].x, p_polypath_b[i].y);
}
- ClipperD clp(PRECISION); // Scale points up internally to attain the desired precision.
+ ClipperD clp(clipper_precision); // Scale points up internally to attain the desired precision.
clp.PreserveCollinear(false); // Remove redundant vertices.
if (is_a_open) {
clp.AddOpenSubject({ path_a });
@@ -298,9 +299,10 @@ Vector<Vector<Point2>> Geometry2D::_polypath_offset(const Vector<Point2> &p_poly
}
// Inflate/deflate.
- PathsD paths = InflatePaths({ polypath }, p_delta, jt, et, 2.0, PRECISION, 0.0);
- // Here the miter_limit = 2.0 and arc_tolerance = 0.0 are Clipper2 defaults,
- // and the PRECISION is used to scale points up internally, to attain the desired precision.
+ PathsD paths = InflatePaths({ polypath }, p_delta, jt, et, 2.0, clipper_precision, 0.25 * clipper_scale);
+ // Here the points are scaled up internally and
+ // the arc_tolerance is scaled accordingly
+ // to attain the desired precision.
Vector<Vector<Point2>> polypaths;
for (PathsD::size_type i = 0; i < paths.size(); ++i) {
diff --git a/core/math/geometry_2d.h b/core/math/geometry_2d.h
index 83ebdc5a84..abd395d8df 100644
--- a/core/math/geometry_2d.h
+++ b/core/math/geometry_2d.h
@@ -451,17 +451,17 @@ public:
return H;
}
- static Vector<Point2i> bresenham_line(const Point2i &p_start, const Point2i &p_end) {
+ static Vector<Point2i> bresenham_line(const Point2i &p_from, const Point2i &p_to) {
Vector<Point2i> points;
- Vector2i delta = (p_end - p_start).abs() * 2;
- Vector2i step = (p_end - p_start).sign();
- Vector2i current = p_start;
+ Vector2i delta = (p_to - p_from).abs() * 2;
+ Vector2i step = (p_to - p_from).sign();
+ Vector2i current = p_from;
if (delta.x > delta.y) {
int err = delta.x / 2;
- for (; current.x != p_end.x; current.x += step.x) {
+ for (; current.x != p_to.x; current.x += step.x) {
points.push_back(current);
err -= delta.y;
@@ -473,7 +473,7 @@ public:
} else {
int err = delta.y / 2;
- for (; current.y != p_end.y; current.y += step.y) {
+ for (; current.y != p_to.y; current.y += step.y) {
points.push_back(current);
err -= delta.x;
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 3060f31970..1afc5f4bbb 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -105,6 +105,9 @@ public:
static _ALWAYS_INLINE_ double fmod(double p_x, double p_y) { return ::fmod(p_x, p_y); }
static _ALWAYS_INLINE_ float fmod(float p_x, float p_y) { return ::fmodf(p_x, p_y); }
+ static _ALWAYS_INLINE_ double modf(double p_x, double *r_y) { return ::modf(p_x, r_y); }
+ static _ALWAYS_INLINE_ float modf(float p_x, float *r_y) { return ::modff(p_x, r_y); }
+
static _ALWAYS_INLINE_ double floor(double p_x) { return ::floor(p_x); }
static _ALWAYS_INLINE_ float floor(float p_x) { return ::floorf(p_x); }
@@ -447,14 +450,22 @@ public:
static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_s) {
if (is_equal_approx(p_from, p_to)) {
- return p_from;
+ if (likely(p_from <= p_to)) {
+ return p_s <= p_from ? 0.0 : 1.0;
+ } else {
+ return p_s <= p_to ? 1.0 : 0.0;
+ }
}
double s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0, 1.0);
return s * s * (3.0 - 2.0 * s);
}
static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_s) {
if (is_equal_approx(p_from, p_to)) {
- return p_from;
+ if (likely(p_from <= p_to)) {
+ return p_s <= p_from ? 0.0f : 1.0f;
+ } else {
+ return p_s <= p_to ? 1.0f : 0.0f;
+ }
}
float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f);
return s * s * (3.0f - 2.0f * s);
diff --git a/core/math/plane.h b/core/math/plane.h
index 6529fea60a..65783ff4cf 100644
--- a/core/math/plane.h
+++ b/core/math/plane.h
@@ -40,7 +40,7 @@ struct [[nodiscard]] Plane {
real_t d = 0;
void set_normal(const Vector3 &p_normal);
- _FORCE_INLINE_ Vector3 get_normal() const { return normal; };
+ _FORCE_INLINE_ Vector3 get_normal() const { return normal; }
void normalize();
Plane normalized() const;
diff --git a/core/math/random_pcg.cpp b/core/math/random_pcg.cpp
index 55787a0b57..c286a60421 100644
--- a/core/math/random_pcg.cpp
+++ b/core/math/random_pcg.cpp
@@ -60,6 +60,11 @@ int64_t RandomPCG::rand_weighted(const Vector<float> &p_weights) {
}
}
+ for (int64_t i = weights_size - 1; i >= 0; --i) {
+ if (weights[i] > 0) {
+ return i;
+ }
+ }
return -1;
}
diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp
index c55226a57e..7f77b0786c 100644
--- a/core/math/rect2.cpp
+++ b/core/math/rect2.cpp
@@ -283,7 +283,7 @@ next4:
}
Rect2::operator String() const {
- return "[P: " + position.operator String() + ", S: " + size + "]";
+ return "[P: " + position.operator String() + ", S: " + size.operator String() + "]";
}
Rect2::operator Rect2i() const {
diff --git a/core/math/rect2.h b/core/math/rect2.h
index 9cb341b689..817923c134 100644
--- a/core/math/rect2.h
+++ b/core/math/rect2.h
@@ -285,13 +285,15 @@ struct [[nodiscard]] Rect2 {
return Rect2(position.round(), size.round());
}
- Vector2 get_support(const Vector2 &p_normal) const {
- Vector2 half_extents = size * 0.5f;
- Vector2 ofs = position + half_extents;
- return Vector2(
- (p_normal.x > 0) ? -half_extents.x : half_extents.x,
- (p_normal.y > 0) ? -half_extents.y : half_extents.y) +
- ofs;
+ Vector2 get_support(const Vector2 &p_direction) const {
+ Vector2 support = position;
+ if (p_direction.x > 0.0f) {
+ support.x += size.x;
+ }
+ if (p_direction.y > 0.0f) {
+ support.y += size.y;
+ }
+ return support;
}
_FORCE_INLINE_ bool intersects_filled_polygon(const Vector2 *p_points, int p_point_count) const {
diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h
index 476577508f..1ee7d3d84f 100644
--- a/core/math/transform_2d.h
+++ b/core/math/transform_2d.h
@@ -39,16 +39,19 @@
class String;
struct [[nodiscard]] Transform2D {
- // Warning #1: basis of Transform2D is stored differently from Basis. In terms of columns array, the basis matrix looks like "on paper":
+ // WARNING: The basis of Transform2D is stored differently from Basis.
+ // In terms of columns array, the basis matrix looks like "on paper":
// M = (columns[0][0] columns[1][0])
// (columns[0][1] columns[1][1])
- // This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as columns[i].
- // Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to columns[1][0] here.
+ // This is such that the columns, which can be interpreted as basis vectors
+ // of the coordinate system "painted" on the object, can be accessed as columns[i].
+ // NOTE: This is the opposite of the indices in mathematical texts,
+ // meaning: $M_{12}$ in a math book corresponds to columns[1][0] here.
// This requires additional care when working with explicit indices.
// See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading.
- // Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down,
- // and angle is measure from +X to +Y in a clockwise-fashion.
+ // WARNING: Be aware that unlike 3D code, 2D code uses a left-handed coordinate system:
+ // Y-axis points down, and angle is measure from +X to +Y in a clockwise-fashion.
Vector2 columns[3];
diff --git a/core/math/transform_interpolator.cpp b/core/math/transform_interpolator.cpp
index 6a564b0ca7..1cd35b3d1a 100644
--- a/core/math/transform_interpolator.cpp
+++ b/core/math/transform_interpolator.cpp
@@ -31,6 +31,7 @@
#include "transform_interpolator.h"
#include "core/math/transform_2d.h"
+#include "core/math/transform_3d.h"
void TransformInterpolator::interpolate_transform_2d(const Transform2D &p_prev, const Transform2D &p_curr, Transform2D &r_result, real_t p_fraction) {
// Special case for physics interpolation, if flipping, don't interpolate basis.
@@ -44,3 +45,340 @@ void TransformInterpolator::interpolate_transform_2d(const Transform2D &p_prev,
r_result = p_prev.interpolate_with(p_curr, p_fraction);
}
+
+void TransformInterpolator::interpolate_transform_3d(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction) {
+ r_result.origin = p_prev.origin + ((p_curr.origin - p_prev.origin) * p_fraction);
+ interpolate_basis(p_prev.basis, p_curr.basis, r_result.basis, p_fraction);
+}
+
+void TransformInterpolator::interpolate_basis(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction) {
+ Method method = find_method(p_prev, p_curr);
+ interpolate_basis_via_method(p_prev, p_curr, r_result, p_fraction, method);
+}
+
+void TransformInterpolator::interpolate_transform_3d_via_method(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction, Method p_method) {
+ r_result.origin = p_prev.origin + ((p_curr.origin - p_prev.origin) * p_fraction);
+ interpolate_basis_via_method(p_prev.basis, p_curr.basis, r_result.basis, p_fraction, p_method);
+}
+
+void TransformInterpolator::interpolate_basis_via_method(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction, Method p_method) {
+ switch (p_method) {
+ default: {
+ interpolate_basis_linear(p_prev, p_curr, r_result, p_fraction);
+ } break;
+ case INTERP_SLERP: {
+ r_result = _basis_slerp_unchecked(p_prev, p_curr, p_fraction);
+ } break;
+ case INTERP_SCALED_SLERP: {
+ interpolate_basis_scaled_slerp(p_prev, p_curr, r_result, p_fraction);
+ } break;
+ }
+}
+
+Quaternion TransformInterpolator::_basis_to_quat_unchecked(const Basis &p_basis) {
+ Basis m = p_basis;
+ real_t trace = m.rows[0][0] + m.rows[1][1] + m.rows[2][2];
+ real_t temp[4];
+
+ if (trace > 0.0) {
+ real_t s = Math::sqrt(trace + 1.0f);
+ temp[3] = (s * 0.5f);
+ s = 0.5f / s;
+
+ temp[0] = ((m.rows[2][1] - m.rows[1][2]) * s);
+ temp[1] = ((m.rows[0][2] - m.rows[2][0]) * s);
+ temp[2] = ((m.rows[1][0] - m.rows[0][1]) * s);
+ } else {
+ int i = m.rows[0][0] < m.rows[1][1]
+ ? (m.rows[1][1] < m.rows[2][2] ? 2 : 1)
+ : (m.rows[0][0] < m.rows[2][2] ? 2 : 0);
+ int j = (i + 1) % 3;
+ int k = (i + 2) % 3;
+
+ real_t s = Math::sqrt(m.rows[i][i] - m.rows[j][j] - m.rows[k][k] + 1.0f);
+ temp[i] = s * 0.5f;
+ s = 0.5f / s;
+
+ temp[3] = (m.rows[k][j] - m.rows[j][k]) * s;
+ temp[j] = (m.rows[j][i] + m.rows[i][j]) * s;
+ temp[k] = (m.rows[k][i] + m.rows[i][k]) * s;
+ }
+
+ return Quaternion(temp[0], temp[1], temp[2], temp[3]);
+}
+
+Quaternion TransformInterpolator::_quat_slerp_unchecked(const Quaternion &p_from, const Quaternion &p_to, real_t p_fraction) {
+ Quaternion to1;
+ real_t omega, cosom, sinom, scale0, scale1;
+
+ // Calculate cosine.
+ cosom = p_from.dot(p_to);
+
+ // 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;
+ } else {
+ to1.x = p_to.x;
+ to1.y = p_to.y;
+ to1.z = p_to.z;
+ to1.w = p_to.w;
+ }
+
+ // Calculate coefficients.
+
+ // This check could possibly be removed as we dealt with this
+ // case in the find_method() function, but is left for safety, it probably
+ // isn't a bottleneck.
+ if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
+ // standard case (slerp)
+ omega = Math::acos(cosom);
+ sinom = Math::sin(omega);
+ scale0 = Math::sin((1.0f - p_fraction) * omega) / sinom;
+ scale1 = Math::sin(p_fraction * omega) / sinom;
+ } else {
+ // "from" and "to" quaternions are very close
+ // ... so we can do a linear interpolation
+ scale0 = 1.0f - p_fraction;
+ scale1 = p_fraction;
+ }
+ // Calculate final values.
+ return Quaternion(
+ scale0 * p_from.x + scale1 * to1.x,
+ scale0 * p_from.y + scale1 * to1.y,
+ scale0 * p_from.z + scale1 * to1.z,
+ scale0 * p_from.w + scale1 * to1.w);
+}
+
+Basis TransformInterpolator::_basis_slerp_unchecked(Basis p_from, Basis p_to, real_t p_fraction) {
+ Quaternion from = _basis_to_quat_unchecked(p_from);
+ Quaternion to = _basis_to_quat_unchecked(p_to);
+
+ Basis b(_quat_slerp_unchecked(from, to, p_fraction));
+ return b;
+}
+
+void TransformInterpolator::interpolate_basis_scaled_slerp(Basis p_prev, Basis p_curr, Basis &r_result, real_t p_fraction) {
+ // Normalize both and find lengths.
+ Vector3 lengths_prev = _basis_orthonormalize(p_prev);
+ Vector3 lengths_curr = _basis_orthonormalize(p_curr);
+
+ r_result = _basis_slerp_unchecked(p_prev, p_curr, p_fraction);
+
+ // Now the result is unit length basis, we need to scale.
+ Vector3 lengths_lerped = lengths_prev + ((lengths_curr - lengths_prev) * p_fraction);
+
+ // Keep a note that the column / row order of the basis is weird,
+ // so keep an eye for bugs with this.
+ r_result[0] *= lengths_lerped;
+ r_result[1] *= lengths_lerped;
+ r_result[2] *= lengths_lerped;
+}
+
+void TransformInterpolator::interpolate_basis_linear(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction) {
+ // Interpolate basis.
+ r_result = p_prev.lerp(p_curr, p_fraction);
+
+ // It turns out we need to guard against zero scale basis.
+ // This is kind of silly, as we should probably fix the bugs elsewhere in Godot that can't deal with
+ // zero scale, but until that time...
+ for (int n = 0; n < 3; n++) {
+ Vector3 &axis = r_result[n];
+
+ // Not ok, this could cause errors due to bugs elsewhere,
+ // so we will bodge set this to a small value.
+ const real_t smallest = 0.0001f;
+ const real_t smallest_squared = smallest * smallest;
+ if (axis.length_squared() < smallest_squared) {
+ // Setting a different component to the smallest
+ // helps prevent the situation where all the axes are pointing in the same direction,
+ // which could be a problem for e.g. cross products...
+ axis[n] = smallest;
+ }
+ }
+}
+
+// Returns length.
+real_t TransformInterpolator::_vec3_normalize(Vector3 &p_vec) {
+ real_t lengthsq = p_vec.length_squared();
+ if (lengthsq == 0.0f) {
+ p_vec.x = p_vec.y = p_vec.z = 0.0f;
+ return 0.0f;
+ }
+ real_t length = Math::sqrt(lengthsq);
+ p_vec.x /= length;
+ p_vec.y /= length;
+ p_vec.z /= length;
+ return length;
+}
+
+// Returns lengths.
+Vector3 TransformInterpolator::_basis_orthonormalize(Basis &r_basis) {
+ // Gram-Schmidt Process.
+
+ Vector3 x = r_basis.get_column(0);
+ Vector3 y = r_basis.get_column(1);
+ Vector3 z = r_basis.get_column(2);
+
+ Vector3 lengths;
+
+ lengths.x = _vec3_normalize(x);
+ y = (y - x * (x.dot(y)));
+ lengths.y = _vec3_normalize(y);
+ z = (z - x * (x.dot(z)) - y * (y.dot(z)));
+ lengths.z = _vec3_normalize(z);
+
+ r_basis.set_column(0, x);
+ r_basis.set_column(1, y);
+ r_basis.set_column(2, z);
+
+ return lengths;
+}
+
+TransformInterpolator::Method TransformInterpolator::_test_basis(Basis p_basis, bool r_needed_normalize, Quaternion &r_quat) {
+ // Axis lengths.
+ Vector3 al = Vector3(p_basis.get_column(0).length_squared(),
+ p_basis.get_column(1).length_squared(),
+ p_basis.get_column(2).length_squared());
+
+ // Non unit scale?
+ if (r_needed_normalize || !_vec3_is_equal_approx(al, Vector3(1.0, 1.0, 1.0), (real_t)0.001f)) {
+ // If the basis is not normalized (at least approximately), it will fail the checks needed for slerp.
+ // So we try to detect a scaled (but not sheared) basis, which we *can* slerp by normalizing first,
+ // and lerping the scales separately.
+
+ // If any of the axes are really small, it is unlikely to be a valid rotation, or is scaled too small to deal with float error.
+ const real_t sl_epsilon = 0.00001f;
+ if ((al.x < sl_epsilon) ||
+ (al.y < sl_epsilon) ||
+ (al.z < sl_epsilon)) {
+ return INTERP_LERP;
+ }
+
+ // Normalize the basis.
+ Basis norm_basis = p_basis;
+
+ al.x = Math::sqrt(al.x);
+ al.y = Math::sqrt(al.y);
+ al.z = Math::sqrt(al.z);
+
+ norm_basis.set_column(0, norm_basis.get_column(0) / al.x);
+ norm_basis.set_column(1, norm_basis.get_column(1) / al.y);
+ norm_basis.set_column(2, norm_basis.get_column(2) / al.z);
+
+ // This doesn't appear necessary, as the later checks will catch it.
+ // if (!_basis_is_orthogonal_any_scale(norm_basis)) {
+ // return INTERP_LERP;
+ // }
+
+ p_basis = norm_basis;
+
+ // Orthonormalize not necessary as normal normalization(!) works if the
+ // axes are orthonormal.
+ // p_basis.orthonormalize();
+
+ // If we needed to normalize one of the two bases, we will need to normalize both,
+ // regardless of whether the 2nd needs it, just to make sure it takes the path to return
+ // INTERP_SCALED_LERP on the 2nd call of _test_basis.
+ r_needed_normalize = true;
+ }
+
+ // Apply less stringent tests than the built in slerp, the standard Godot slerp
+ // is too susceptible to float error to be useful.
+ real_t det = p_basis.determinant();
+ if (!Math::is_equal_approx(det, 1, (real_t)0.01f)) {
+ return INTERP_LERP;
+ }
+
+ if (!_basis_is_orthogonal(p_basis)) {
+ return INTERP_LERP;
+ }
+
+ // TODO: This could possibly be less stringent too, check this.
+ r_quat = _basis_to_quat_unchecked(p_basis);
+ if (!r_quat.is_normalized()) {
+ return INTERP_LERP;
+ }
+
+ return r_needed_normalize ? INTERP_SCALED_SLERP : INTERP_SLERP;
+}
+
+// This check doesn't seem to be needed but is preserved in case of bugs.
+bool TransformInterpolator::_basis_is_orthogonal_any_scale(const Basis &p_basis) {
+ Vector3 cross = p_basis.get_column(0).cross(p_basis.get_column(1));
+ real_t l = _vec3_normalize(cross);
+ // Too small numbers, revert to lerp.
+ if (l < 0.001f) {
+ return false;
+ }
+
+ const real_t epsilon = 0.9995f;
+
+ real_t dot = cross.dot(p_basis.get_column(2));
+ if (dot < epsilon) {
+ return false;
+ }
+
+ cross = p_basis.get_column(1).cross(p_basis.get_column(2));
+ l = _vec3_normalize(cross);
+ // Too small numbers, revert to lerp.
+ if (l < 0.001f) {
+ return false;
+ }
+
+ dot = cross.dot(p_basis.get_column(0));
+ if (dot < epsilon) {
+ return false;
+ }
+
+ return true;
+}
+
+bool TransformInterpolator::_basis_is_orthogonal(const Basis &p_basis, real_t p_epsilon) {
+ Basis identity;
+ Basis m = p_basis * p_basis.transposed();
+
+ // Less stringent tests than the standard Godot slerp.
+ if (!_vec3_is_equal_approx(m[0], identity[0], p_epsilon) || !_vec3_is_equal_approx(m[1], identity[1], p_epsilon) || !_vec3_is_equal_approx(m[2], identity[2], p_epsilon)) {
+ return false;
+ }
+ return true;
+}
+
+real_t TransformInterpolator::checksum_transform_3d(const Transform3D &p_transform) {
+ // just a really basic checksum, this can probably be improved
+ real_t sum = _vec3_sum(p_transform.origin);
+ sum -= _vec3_sum(p_transform.basis.rows[0]);
+ sum += _vec3_sum(p_transform.basis.rows[1]);
+ sum -= _vec3_sum(p_transform.basis.rows[2]);
+ return sum;
+}
+
+TransformInterpolator::Method TransformInterpolator::find_method(const Basis &p_a, const Basis &p_b) {
+ bool needed_normalize = false;
+
+ Quaternion q0;
+ Method method = _test_basis(p_a, needed_normalize, q0);
+ if (method == INTERP_LERP) {
+ return method;
+ }
+
+ Quaternion q1;
+ method = _test_basis(p_b, needed_normalize, q1);
+ if (method == INTERP_LERP) {
+ return method;
+ }
+
+ // Are they close together?
+ // Apply the same test that will revert to lerp as is present in the slerp routine.
+ // Calculate cosine.
+ real_t cosom = Math::abs(q0.dot(q1));
+ if ((1.0f - cosom) <= (real_t)CMP_EPSILON) {
+ return INTERP_LERP;
+ }
+
+ return method;
+}
diff --git a/core/math/transform_interpolator.h b/core/math/transform_interpolator.h
index a9bce2bd7f..cc556707e4 100644
--- a/core/math/transform_interpolator.h
+++ b/core/math/transform_interpolator.h
@@ -32,15 +32,64 @@
#define TRANSFORM_INTERPOLATOR_H
#include "core/math/math_defs.h"
+#include "core/math/vector3.h"
+
+// Keep all the functions for fixed timestep interpolation together.
+// There are two stages involved:
+// Finding a method, for determining the interpolation method between two
+// keyframes (which are physics ticks).
+// And applying that pre-determined method.
+
+// Pre-determining the method makes sense because it is expensive and often
+// several frames may occur between each physics tick, which will make it cheaper
+// than performing every frame.
struct Transform2D;
+struct Transform3D;
+struct Basis;
+struct Quaternion;
class TransformInterpolator {
+public:
+ enum Method {
+ INTERP_LERP,
+ INTERP_SLERP,
+ INTERP_SCALED_SLERP,
+ };
+
private:
- static bool _sign(real_t p_val) { return p_val >= 0; }
+ _FORCE_INLINE_ static bool _sign(real_t p_val) { return p_val >= 0; }
+ static real_t _vec3_sum(const Vector3 &p_pt) { return p_pt.x + p_pt.y + p_pt.z; }
+ static real_t _vec3_normalize(Vector3 &p_vec);
+ _FORCE_INLINE_ static bool _vec3_is_equal_approx(const Vector3 &p_a, const Vector3 &p_b, real_t p_tolerance) {
+ return Math::is_equal_approx(p_a.x, p_b.x, p_tolerance) && Math::is_equal_approx(p_a.y, p_b.y, p_tolerance) && Math::is_equal_approx(p_a.z, p_b.z, p_tolerance);
+ }
+ static Vector3 _basis_orthonormalize(Basis &r_basis);
+ static Method _test_basis(Basis p_basis, bool r_needed_normalize, Quaternion &r_quat);
+ static Basis _basis_slerp_unchecked(Basis p_from, Basis p_to, real_t p_fraction);
+ static Quaternion _quat_slerp_unchecked(const Quaternion &p_from, const Quaternion &p_to, real_t p_fraction);
+ static Quaternion _basis_to_quat_unchecked(const Basis &p_basis);
+ static bool _basis_is_orthogonal(const Basis &p_basis, real_t p_epsilon = 0.01f);
+ static bool _basis_is_orthogonal_any_scale(const Basis &p_basis);
+
+ static void interpolate_basis_linear(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction);
+ static void interpolate_basis_scaled_slerp(Basis p_prev, Basis p_curr, Basis &r_result, real_t p_fraction);
public:
static void interpolate_transform_2d(const Transform2D &p_prev, const Transform2D &p_curr, Transform2D &r_result, real_t p_fraction);
+
+ // Generic functions, use when you don't know what method should be used, e.g. from GDScript.
+ // These will be slower.
+ static void interpolate_transform_3d(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction);
+ static void interpolate_basis(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction);
+
+ // Optimized function when you know ahead of time the method.
+ static void interpolate_transform_3d_via_method(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction, Method p_method);
+ static void interpolate_basis_via_method(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction, Method p_method);
+
+ static real_t checksum_transform_3d(const Transform3D &p_transform);
+
+ static Method find_method(const Basis &p_a, const Basis &p_b);
};
#endif // TRANSFORM_INTERPOLATOR_H
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index e86b97d6a8..0590ee8a37 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -203,7 +203,7 @@ bool Vector2::is_finite() const {
}
Vector2::operator String() const {
- return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ")";
+ return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ")";
}
Vector2::operator Vector2i() const {
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index 1e90002665..e18ac3b011 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -165,7 +165,7 @@ bool Vector3::is_finite() const {
}
Vector3::operator String() const {
- return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")";
+ return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ", " + String::num_real(z, true) + ")";
}
Vector3::operator Vector3i() const {
diff --git a/core/math/vector4.h b/core/math/vector4.h
index 8632f69f57..9197e3587a 100644
--- a/core/math/vector4.h
+++ b/core/math/vector4.h
@@ -55,16 +55,16 @@ struct [[nodiscard]] Vector4 {
real_t z;
real_t w;
};
- real_t components[4] = { 0, 0, 0, 0 };
+ real_t coord[4] = { 0, 0, 0, 0 };
};
_FORCE_INLINE_ real_t &operator[](int p_axis) {
DEV_ASSERT((unsigned int)p_axis < 4);
- return components[p_axis];
+ return coord[p_axis];
}
_FORCE_INLINE_ const real_t &operator[](int p_axis) const {
DEV_ASSERT((unsigned int)p_axis < 4);
- return components[p_axis];
+ return coord[p_axis];
}
Vector4::Axis min_axis_index() const;