hertog/godot-module-template
Template
2
1
Fork 1
Code Issues 2 Pull requests 2 Projects Releases Packages Wiki Activity Actions

feat: updated engine version to 4.4-rc1

Browse source
This commit is contained in:
Sara 2025-02-23 14:38:14 +01:00
parent ee00efde1f
commit 21ba8e33af
5459 changed files with 1128836 additions and 198305 deletions
Download patch file Download diff file Expand all files Collapse all files

1
engine/core/math/SCsub
View file

@ -1,4 +1,5 @@
#!/usr/bin/env python
from misc.utility.scons_hints import *
Import("env")

47
engine/core/math/a_star.cpp
View file

@ -32,7 +32,6 @@
#include "a_star.compat.inc"
#include "core/math/geometry_3d.h"
#include "core/object/script_language.h"
int64_t AStar3D::get_available_point_id() const {
if (points.has(last_free_id)) {
@ -40,7 +39,7 @@ int64_t AStar3D::get_available_point_id() const {
while (points.has(cur_new_id)) {
cur_new_id++;
}
const_cast<int64_t &>(last_free_id) = cur_new_id;
last_free_id = cur_new_id;
}
return last_free_id;
@ -319,11 +318,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 +390,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 +400,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 +442,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 +496,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 +578,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 +674,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 +684,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 +725,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 +779,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 +815,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 +917,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")
}

10
engine/core/math/a_star.h
View file

@ -108,19 +108,19 @@ class AStar3D : public RefCounted {
}
};
int64_t last_free_id = 0;
mutable int64_t last_free_id = 0;
uint64_t pass = 1;
OAHashMap<int64_t, Point *> points;
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)

233
engine/core/math/a_star_grid_2d.cpp
View file

@ -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 (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 ((_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;
return _get_point_unchecked(to_x, to_y);
}
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 (_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 (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 ((_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;
return _get_point_unchecked(to_x, to_y);
}
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 (_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;
}
} 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;
}
if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) {
return p_to;
}
if (dy == 0) {
return _forced_successor(from_x, from_y, dx, 0, true);
}
return _jump(p_to, _get_point(to_x + dx, to_y + dy));
while (_is_walkable(to_x, to_y)) {
if (end->id.x == to_x && end->id.y == to_y) {
return end;
}
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 _get_point_unchecked(to_x, to_y);
}
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 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");

30
engine/core/math/a_star_grid_2d.h
View file

@ -33,7 +33,6 @@
#include "core/object/gdvirtual.gen.inc"
#include "core/object/ref_counted.h"
#include "core/templates/list.h"
#include "core/templates/local_vector.h"
class AStarGrid2D : public RefCounted {
@ -78,7 +77,6 @@ private:
struct Point {
Vector2i id;
bool solid = false;
Vector2 pos;
real_t weight_scale = 1.0;
@ -111,6 +109,7 @@ private:
}
};
LocalVector<bool> solid_mask;
LocalVector<LocalVector<Point>> points;
Point *end = nullptr;
Point *last_closest_point = nullptr;
@ -118,11 +117,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 +132,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 +158,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 +222,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);
};

23
engine/core/math/aabb.h
View file

@ -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 {

36
engine/core/math/basis.cpp
View file

@ -455,6 +455,11 @@ void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) cons
}
Vector3 Basis::get_euler(EulerOrder p_order) const {
// This epsilon value results in angles within a +/- 0.04 degree range being simplified/truncated.
// Based on testing, this is the largest the epsilon can be without the angle truncation becoming
// visually noticeable.
const real_t epsilon = 0.00000025;
switch (p_order) {
case EulerOrder::XYZ: {
// Euler angles in XYZ convention.
@ -466,8 +471,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
Vector3 euler;
real_t sy = rows[0][2];
if (sy < (1.0f - (real_t)CMP_EPSILON)) {
if (sy > -(1.0f - (real_t)CMP_EPSILON)) {
if (sy < (1.0f - epsilon)) {
if (sy > -(1.0f - epsilon)) {
// is this a pure Y rotation?
if (rows[1][0] == 0 && rows[0][1] == 0 && rows[1][2] == 0 && rows[2][1] == 0 && rows[1][1] == 1) {
// return the simplest form (human friendlier in editor and scripts)
@ -501,8 +506,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
Vector3 euler;
real_t sz = rows[0][1];
if (sz < (1.0f - (real_t)CMP_EPSILON)) {
if (sz > -(1.0f - (real_t)CMP_EPSILON)) {
if (sz < (1.0f - epsilon)) {
if (sz > -(1.0f - epsilon)) {
euler.x = Math::atan2(rows[2][1], rows[1][1]);
euler.y = Math::atan2(rows[0][2], rows[0][0]);
euler.z = Math::asin(-sz);
@ -532,8 +537,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
real_t m12 = rows[1][2];
if (m12 < (1 - (real_t)CMP_EPSILON)) {
if (m12 > -(1 - (real_t)CMP_EPSILON)) {
if (m12 < (1 - epsilon)) {
if (m12 > -(1 - epsilon)) {
// is this a pure X rotation?
if (rows[1][0] == 0 && rows[0][1] == 0 && rows[0][2] == 0 && rows[2][0] == 0 && rows[0][0] == 1) {
// return the simplest form (human friendlier in editor and scripts)
@ -568,8 +573,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
Vector3 euler;
real_t sz = rows[1][0];
if (sz < (1.0f - (real_t)CMP_EPSILON)) {
if (sz > -(1.0f - (real_t)CMP_EPSILON)) {
if (sz < (1.0f - epsilon)) {
if (sz > -(1.0f - epsilon)) {
euler.x = Math::atan2(-rows[1][2], rows[1][1]);
euler.y = Math::atan2(-rows[2][0], rows[0][0]);
euler.z = Math::asin(sz);
@ -596,8 +601,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
// -cx*sy sx cx*cy
Vector3 euler;
real_t sx = rows[2][1];
if (sx < (1.0f - (real_t)CMP_EPSILON)) {
if (sx > -(1.0f - (real_t)CMP_EPSILON)) {
if (sx < (1.0f - epsilon)) {
if (sx > -(1.0f - epsilon)) {
euler.x = Math::asin(sx);
euler.y = Math::atan2(-rows[2][0], rows[2][2]);
euler.z = Math::atan2(-rows[0][1], rows[1][1]);
@ -624,8 +629,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const {
// -sy cy*sx cy*cx
Vector3 euler;
real_t sy = rows[2][0];
if (sy < (1.0f - (real_t)CMP_EPSILON)) {
if (sy > -(1.0f - (real_t)CMP_EPSILON)) {
if (sy < (1.0f - epsilon)) {
if (sy > -(1.0f - epsilon)) {
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = Math::asin(-sy);
euler.z = Math::atan2(rows[1][0], rows[0][0]);
@ -1049,9 +1054,10 @@ Basis Basis::looking_at(const Vector3 &p_target, const Vector3 &p_up, bool p_use
v_z = -v_z;
}
Vector3 v_x = p_up.cross(v_z);
#ifdef MATH_CHECKS
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
if (v_x.is_zero_approx()) {
WARN_PRINT("Target and up vectors are colinear. This is not advised as it may cause unwanted rotation around local Z axis.");
v_x = p_up.get_any_perpendicular(); // Vectors are almost parallel.
}
v_x.normalize();
Vector3 v_y = v_z.cross(v_x);

10
engine/core/math/basis.h
View file

@ -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); }

1
engine/core/math/bvh_logic.inc
View file

@ -1,4 +1,3 @@
// for slow incremental optimization, we will periodically remove each
// item from the tree and reinsert, to give it a chance to find a better position
void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {

1
engine/core/math/bvh_misc.inc
View file

@ -1,4 +1,3 @@
int _handle_get_tree_id(BVHHandle p_handle) const {
if (USE_PAIRS) {
return _extra[p_handle.id()].tree_id;

2
engine/core/math/bvh_pair.inc
View file

@ -37,7 +37,7 @@ struct ItemPairs {
return n;
}
}
return -1;
return uint32_t(-1);
}
bool contains_pair_to(BVHHandle h) const {

1
engine/core/math/bvh_structs.inc
View file

@ -1,4 +1,3 @@
public:
struct ItemRef {
uint32_t tnode_id; // -1 is invalid

4
engine/core/math/bvh_tree.h
View file

@ -109,9 +109,7 @@ struct BVHHandle {
template <typename T>
class BVH_IterativeInfo {
public:
enum {
ALLOCA_STACK_SIZE = 128
};
constexpr static const size_t ALLOCA_STACK_SIZE = 128;
int32_t depth = 1;
int32_t threshold = ALLOCA_STACK_SIZE - 2;

4
engine/core/math/color.cpp
View file

@ -482,6 +482,10 @@ Color Color::from_rgbe9995(uint32_t p_rgbe) {
return Color(rd, gd, bd, 1.0f);
}
Color Color::from_rgba8(int64_t p_r8, int64_t p_g8, int64_t p_b8, int64_t p_a8) {
return Color(p_r8 / 255.0f, p_g8 / 255.0f, p_b8 / 255.0f, p_a8 / 255.0f);
}
Color::operator String() const {
return "(" + String::num(r, 4) + ", " + String::num(g, 4) + ", " + String::num(b, 4) + ", " + String::num(a, 4) + ")";
}

66
engine/core/math/color.h
View file

@ -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;
// 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));
float sharedexp = 65408.000f; // Result of: ((pow2to9 - 1.0f) / pow2to9) * powf(2.0f, 31.0f - 15.0f)
// 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);
float cRed = MAX(0.0f, MIN(sharedexp, r));
float cGreen = MAX(0.0f, MIN(sharedexp, g));
float cBlue = MAX(0.0f, MIN(sharedexp, b));
// Compute the maximum channel, no less than 1.0*2^-15
const float MaxChannel = MAX(MAX(_r, _g), MAX(_b, kMinVal));
float cMax = MAX(cRed, MAX(cGreen, cBlue));
// 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;
uint32_t i;
} R, G, B, E;
float expp = MAX(-B - 1.0f, floor(Math::log(cMax) / (real_t)Math_LN2)) + 1.0f + B;
E.f = MaxChannel;
E.i += 0x07804000; // Add 15 to the exponent and 0x4000 to the mantissa
E.i &= 0x7F800000; // Zero the mantissa
float sMax = (float)floor((cMax / Math::pow(2.0f, expp - B - N)) + 0.5f);
// 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;
float exps = expp + 1.0f;
// Convert the Bias to the correct exponent in the upper 5 bits.
E.i <<= 4;
E.i += 0x10000000;
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);
// 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 << 18U) | (G.i << 9U) | (R.i & 511U);
}
_FORCE_INLINE_ Color blend(const Color &p_over) const {
@ -174,16 +187,16 @@ struct [[nodiscard]] Color {
_FORCE_INLINE_ Color srgb_to_linear() const {
return Color(
r < 0.04045f ? r * (1.0f / 12.92f) : Math::pow((r + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f),
g < 0.04045f ? g * (1.0f / 12.92f) : Math::pow((g + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f),
b < 0.04045f ? b * (1.0f / 12.92f) : Math::pow((b + 0.055f) * (float)(1.0 / (1.0 + 0.055)), 2.4f),
r < 0.04045f ? r * (1.0f / 12.92f) : Math::pow(float((r + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
g < 0.04045f ? g * (1.0f / 12.92f) : Math::pow(float((g + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
b < 0.04045f ? b * (1.0f / 12.92f) : Math::pow(float((b + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
a);
}
_FORCE_INLINE_ Color linear_to_srgb() const {
return Color(
r < 0.0031308f ? 12.92f * r : (1.0f + 0.055f) * Math::pow(r, 1.0f / 2.4f) - 0.055f,
g < 0.0031308f ? 12.92f * g : (1.0f + 0.055f) * Math::pow(g, 1.0f / 2.4f) - 0.055f,
b < 0.0031308f ? 12.92f * b : (1.0f + 0.055f) * Math::pow(b, 1.0f / 2.4f) - 0.055f, a);
r < 0.0031308f ? 12.92f * r : (1.0 + 0.055) * Math::pow(r, 1.0f / 2.4f) - 0.055,
g < 0.0031308f ? 12.92f * g : (1.0 + 0.055) * Math::pow(g, 1.0f / 2.4f) - 0.055,
b < 0.0031308f ? 12.92f * b : (1.0 + 0.055) * Math::pow(b, 1.0f / 2.4f) - 0.055, a);
}
static Color hex(uint32_t p_hex);
@ -200,6 +213,7 @@ struct [[nodiscard]] Color {
static Color from_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f);
static Color from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0f);
static Color from_rgbe9995(uint32_t p_rgbe);
static Color from_rgba8(int64_t p_r8, int64_t p_g8, int64_t p_b8, int64_t p_a8 = 255);
_FORCE_INLINE_ bool operator<(const Color &p_color) const; // Used in set keys.
operator String() const;

19
engine/core/math/convex_hull.cpp
View file

@ -61,12 +61,9 @@ subject to the following restrictions:
#include "core/error/error_macros.h"
#include "core/math/aabb.h"
#include "core/math/math_defs.h"
#include "core/os/memory.h"
#include "core/templates/oa_hash_map.h"
#include "core/templates/paged_allocator.h"
#include <string.h>
//#define DEBUG_CONVEX_HULL
//#define SHOW_ITERATIONS
@ -204,7 +201,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 {
@ -2152,10 +2149,11 @@ static int32_t get_vertex_copy(ConvexHullInternal::Vertex *p_vertex, LocalVector
}
real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp) {
vertices.clear();
edges.clear();
faces.clear();
if (p_count <= 0) {
vertices.clear();
edges.clear();
faces.clear();
return 0;
}
@ -2164,16 +2162,9 @@ real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, rea
real_t shift = 0;
if ((p_shrink > 0) && ((shift = hull.shrink(p_shrink, p_shrink_clamp)) < 0)) {
vertices.clear();
edges.clear();
faces.clear();
return shift;
}
vertices.clear();
edges.clear();
faces.clear();
LocalVector<ConvexHullInternal::Vertex *> old_vertices;
get_vertex_copy(hull.vertex_list, old_vertices);
int32_t copied = 0;

12
engine/core/math/delaunay_3d.h
View file

@ -135,9 +135,9 @@ class Delaunay3D {
R128 row3_y = v3_y - v0_y;
R128 row3_z = v3_z - v0_z;
R128 sq_lenght1 = row1_x * row1_x + row1_y * row1_y + row1_z * row1_z;
R128 sq_lenght2 = row2_x * row2_x + row2_y * row2_y + row2_z * row2_z;
R128 sq_lenght3 = row3_x * row3_x + row3_y * row3_y + row3_z * row3_z;
R128 sq_length1 = row1_x * row1_x + row1_y * row1_y + row1_z * row1_z;
R128 sq_length2 = row2_x * row2_x + row2_y * row2_y + row2_z * row2_z;
R128 sq_length3 = row3_x * row3_x + row3_y * row3_y + row3_z * row3_z;
// Compute the determinant of said matrix.
R128 determinant = row1_x * (row2_y * row3_z - row3_y * row2_z) - row2_x * (row1_y * row3_z - row3_y * row1_z) + row3_x * (row1_y * row2_z - row2_y * row1_z);
@ -146,9 +146,9 @@ class Delaunay3D {
R128 volume = determinant / R128(6.f);
R128 i12volume = R128(1.f) / (volume * R128(12.f));
R128 center_x = v0_x + i12volume * ((row2_y * row3_z - row3_y * row2_z) * sq_lenght1 - (row1_y * row3_z - row3_y * row1_z) * sq_lenght2 + (row1_y * row2_z - row2_y * row1_z) * sq_lenght3);
R128 center_y = v0_y + i12volume * (-(row2_x * row3_z - row3_x * row2_z) * sq_lenght1 + (row1_x * row3_z - row3_x * row1_z) * sq_lenght2 - (row1_x * row2_z - row2_x * row1_z) * sq_lenght3);
R128 center_z = v0_z + i12volume * ((row2_x * row3_y - row3_x * row2_y) * sq_lenght1 - (row1_x * row3_y - row3_x * row1_y) * sq_lenght2 + (row1_x * row2_y - row2_x * row1_y) * sq_lenght3);
R128 center_x = v0_x + i12volume * ((row2_y * row3_z - row3_y * row2_z) * sq_length1 - (row1_y * row3_z - row3_y * row1_z) * sq_length2 + (row1_y * row2_z - row2_y * row1_z) * sq_length3);
R128 center_y = v0_y + i12volume * (-(row2_x * row3_z - row3_x * row2_z) * sq_length1 + (row1_x * row3_z - row3_x * row1_z) * sq_length2 - (row1_x * row2_z - row2_x * row1_z) * sq_length3);
R128 center_z = v0_z + i12volume * ((row2_x * row3_y - row3_x * row2_y) * sq_length1 - (row1_x * row3_y - row3_x * row1_y) * sq_length2 + (row1_x * row2_y - row2_x * row1_y) * sq_length3);
// Once we know the center, the radius is clearly the distance to any vertex.
R128 rel1_x = center_x - v0_x;

31
engine/core/math/expression.cpp
View file

@ -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) {
@ -355,16 +350,16 @@ Error Expression::_get_token(Token &r_token) {
case READING_INT: {
if (is_digit(c)) {
if (is_first_char && c == '0') {
if (next_char == 'b') {
if (next_char == 'b' || next_char == 'B') {
reading = READING_BIN;
} else if (next_char == 'x') {
} else if (next_char == 'x' || next_char == 'X') {
reading = READING_HEX;
}
}
} else if (c == '.') {
reading = READING_DEC;
is_float = true;
} else if (c == 'e') {
} else if (c == 'e' || c == 'E') {
reading = READING_EXP;
is_float = true;
} else {
@ -375,7 +370,7 @@ Error Expression::_get_token(Token &r_token) {
case READING_BIN: {
if (bin_beg && !is_binary_digit(c)) {
reading = READING_DONE;
} else if (c == 'b') {
} else if (c == 'b' || c == 'B') {
bin_beg = true;
}
@ -383,16 +378,15 @@ Error Expression::_get_token(Token &r_token) {
case READING_HEX: {
if (hex_beg && !is_hex_digit(c)) {
reading = READING_DONE;
} else if (c == 'x') {
} else if (c == 'x' || c == 'X') {
hex_beg = true;
}
} break;
case READING_DEC: {
if (is_digit(c)) {
} else if (c == 'e') {
} else if (c == 'e' || 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;
@ -477,10 +473,11 @@ Error Expression::_get_token(Token &r_token) {
} else if (id == "self") {
r_token.type = TK_SELF;
} else {
for (int i = 0; i < Variant::VARIANT_MAX; i++) {
if (id == Variant::get_type_name(Variant::Type(i))) {
{
const Variant::Type type = Variant::get_type_by_name(id);
if (type < Variant::VARIANT_MAX) {
r_token.type = TK_BASIC_TYPE;
r_token.value = i;
r_token.value = type;
return OK;
}
}
@ -1495,7 +1492,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;

14
engine/core/math/geometry_2d.cpp
View file

@ -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) {

12
engine/core/math/geometry_2d.h
View file

@ -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;

2
engine/core/math/geometry_3d.cpp
View file

@ -30,8 +30,6 @@
#include "geometry_3d.h"
#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.

15
engine/core/math/math_funcs.h
View file

@ -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);

2
engine/core/math/plane.h
View file

@ -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;

310
engine/core/math/projection.cpp
View file

@ -596,101 +596,229 @@ Projection Projection::inverse() const {
}
void Projection::invert() {
int i, j, k;
int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */
real_t pvt_val; /* Value of current pivot element */
real_t hold; /* Temporary storage */
real_t determinant = 1.0f;
for (k = 0; k < 4; k++) {
/** Locate k'th pivot element **/
pvt_val = columns[k][k]; /** Initialize for search **/
pvt_i[k] = k;
pvt_j[k] = k;
for (i = k; i < 4; i++) {
for (j = k; j < 4; j++) {
if (Math::abs(columns[i][j]) > Math::abs(pvt_val)) {
pvt_i[k] = i;
pvt_j[k] = j;
pvt_val = columns[i][j];
}
}
}
// Adapted from Mesa's `src/util/u_math.c` `util_invert_mat4x4`.
// MIT licensed. Copyright 2008 VMware, Inc. Authored by Jacques Leroy.
Projection temp;
real_t *out = (real_t *)temp.columns;
real_t *m = (real_t *)columns;
/** Product of pivots, gives determinant when finished **/
determinant *= pvt_val;
if (Math::is_zero_approx(determinant)) {
return; /** Matrix is singular (zero determinant). **/
}
real_t wtmp[4][8];
real_t m0, m1, m2, m3, s;
real_t *r0, *r1, *r2, *r3;
/** "Interchange" rows (with sign change stuff) **/
i = pvt_i[k];
if (i != k) { /** If rows are different **/
for (j = 0; j < 4; j++) {
hold = -columns[k][j];
columns[k][j] = columns[i][j];
columns[i][j] = hold;
}
}
#define MAT(m, r, c) (m)[(c) * 4 + (r)]
/** "Interchange" columns **/
j = pvt_j[k];
if (j != k) { /** If columns are different **/
for (i = 0; i < 4; i++) {
hold = -columns[i][k];
columns[i][k] = columns[i][j];
columns[i][j] = hold;
}
}
r0 = wtmp[0];
r1 = wtmp[1];
r2 = wtmp[2];
r3 = wtmp[3];
/** Divide column by minus pivot value **/
for (i = 0; i < 4; i++) {
if (i != k) {
columns[i][k] /= (-pvt_val);
}
}
r0[0] = MAT(m, 0, 0);
r0[1] = MAT(m, 0, 1);
r0[2] = MAT(m, 0, 2);
r0[3] = MAT(m, 0, 3);
r0[4] = 1.0;
r0[5] = 0.0;
r0[6] = 0.0;
r0[7] = 0.0;
/** Reduce the matrix **/
for (i = 0; i < 4; i++) {
hold = columns[i][k];
for (j = 0; j < 4; j++) {
if (i != k && j != k) {
columns[i][j] += hold * columns[k][j];
}
}
}
r1[0] = MAT(m, 1, 0);
r1[1] = MAT(m, 1, 1);
r1[2] = MAT(m, 1, 2);
r1[3] = MAT(m, 1, 3);
r1[5] = 1.0;
r1[4] = 0.0;
r1[6] = 0.0;
r1[7] = 0.0;
/** Divide row by pivot **/
for (j = 0; j < 4; j++) {
if (j != k) {
columns[k][j] /= pvt_val;
}
}
r2[0] = MAT(m, 2, 0);
r2[1] = MAT(m, 2, 1);
r2[2] = MAT(m, 2, 2);
r2[3] = MAT(m, 2, 3);
r2[6] = 1.0;
r2[4] = 0.0;
r2[5] = 0.0;
r2[7] = 0.0;
/** Replace pivot by reciprocal (at last we can touch it). **/
columns[k][k] = 1.0 / pvt_val;
r3[0] = MAT(m, 3, 0);
r3[1] = MAT(m, 3, 1);
r3[2] = MAT(m, 3, 2);
r3[3] = MAT(m, 3, 3);
r3[7] = 1.0;
r3[4] = 0.0;
r3[5] = 0.0;
r3[6] = 0.0;
/* choose pivot - or die */
if (Math::abs(r3[0]) > Math::abs(r2[0])) {
SWAP(r3, r2);
}
if (Math::abs(r2[0]) > Math::abs(r1[0])) {
SWAP(r2, r1);
}
if (Math::abs(r1[0]) > Math::abs(r0[0])) {
SWAP(r1, r0);
}
ERR_FAIL_COND(0.0 == r0[0]);
/* eliminate first variable */
m1 = r1[0] / r0[0];
m2 = r2[0] / r0[0];
m3 = r3[0] / r0[0];
s = r0[1];
r1[1] -= m1 * s;
r2[1] -= m2 * s;
r3[1] -= m3 * s;
s = r0[2];
r1[2] -= m1 * s;
r2[2] -= m2 * s;
r3[2] -= m3 * s;
s = r0[3];
r1[3] -= m1 * s;
r2[3] -= m2 * s;
r3[3] -= m3 * s;
s = r0[4];
if (s != 0.0) {
r1[4] -= m1 * s;
r2[4] -= m2 * s;
r3[4] -= m3 * s;
}
s = r0[5];
if (s != 0.0) {
r1[5] -= m1 * s;
r2[5] -= m2 * s;
r3[5] -= m3 * s;
}
s = r0[6];
if (s != 0.0) {
r1[6] -= m1 * s;
r2[6] -= m2 * s;
r3[6] -= m3 * s;
}
s = r0[7];
if (s != 0.0) {
r1[7] -= m1 * s;
r2[7] -= m2 * s;
r3[7] -= m3 * s;
}
/* That was most of the work, one final pass of row/column interchange */
/* to finish */
for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/
i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */
if (i != k) { /* If rows are different */
for (j = 0; j < 4; j++) {
hold = columns[k][j];
columns[k][j] = -columns[i][j];
columns[i][j] = hold;
}
}
j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */
if (j != k) { /* If columns are different */
for (i = 0; i < 4; i++) {
hold = columns[i][k];
columns[i][k] = -columns[i][j];
columns[i][j] = hold;
}
}
/* choose pivot - or die */
if (Math::abs(r3[1]) > Math::abs(r2[1])) {
SWAP(r3, r2);
}
if (Math::abs(r2[1]) > Math::abs(r1[1])) {
SWAP(r2, r1);
}
ERR_FAIL_COND(0.0 == r1[1]);
/* eliminate second variable */
m2 = r2[1] / r1[1];
m3 = r3[1] / r1[1];
r2[2] -= m2 * r1[2];
r3[2] -= m3 * r1[2];
r2[3] -= m2 * r1[3];
r3[3] -= m3 * r1[3];
s = r1[4];
if (0.0 != s) {
r2[4] -= m2 * s;
r3[4] -= m3 * s;
}
s = r1[5];
if (0.0 != s) {
r2[5] -= m2 * s;
r3[5] -= m3 * s;
}
s = r1[6];
if (0.0 != s) {
r2[6] -= m2 * s;
r3[6] -= m3 * s;
}
s = r1[7];
if (0.0 != s) {
r2[7] -= m2 * s;
r3[7] -= m3 * s;
}
/* choose pivot - or die */
if (Math::abs(r3[2]) > Math::abs(r2[2])) {
SWAP(r3, r2);
}
ERR_FAIL_COND(0.0 == r2[2]);
/* eliminate third variable */
m3 = r3[2] / r2[2];
r3[3] -= m3 * r2[3];
r3[4] -= m3 * r2[4];
r3[5] -= m3 * r2[5];
r3[6] -= m3 * r2[6];
r3[7] -= m3 * r2[7];
/* last check */
ERR_FAIL_COND(0.0 == r3[3]);
s = 1.0 / r3[3]; /* now back substitute row 3 */
r3[4] *= s;
r3[5] *= s;
r3[6] *= s;
r3[7] *= s;
m2 = r2[3]; /* now back substitute row 2 */
s = 1.0 / r2[2];
r2[4] = s * (r2[4] - r3[4] * m2);
r2[5] = s * (r2[5] - r3[5] * m2);
r2[6] = s * (r2[6] - r3[6] * m2);
r2[7] = s * (r2[7] - r3[7] * m2);
m1 = r1[3];
r1[4] -= r3[4] * m1;
r1[5] -= r3[5] * m1;
r1[6] -= r3[6] * m1;
r1[7] -= r3[7] * m1;
m0 = r0[3];
r0[4] -= r3[4] * m0;
r0[5] -= r3[5] * m0;
r0[6] -= r3[6] * m0;
r0[7] -= r3[7] * m0;
m1 = r1[2]; /* now back substitute row 1 */
s = 1.0 / r1[1];
r1[4] = s * (r1[4] - r2[4] * m1);
r1[5] = s * (r1[5] - r2[5] * m1),
r1[6] = s * (r1[6] - r2[6] * m1);
r1[7] = s * (r1[7] - r2[7] * m1);
m0 = r0[2];
r0[4] -= r2[4] * m0;
r0[5] -= r2[5] * m0;
r0[6] -= r2[6] * m0;
r0[7] -= r2[7] * m0;
m0 = r0[1]; /* now back substitute row 0 */
s = 1.0 / r0[0];
r0[4] = s * (r0[4] - r1[4] * m0);
r0[5] = s * (r0[5] - r1[5] * m0),
r0[6] = s * (r0[6] - r1[6] * m0);
r0[7] = s * (r0[7] - r1[7] * m0);
MAT(out, 0, 0) = r0[4];
MAT(out, 0, 1) = r0[5];
MAT(out, 0, 2) = r0[6];
MAT(out, 0, 3) = r0[7];
MAT(out, 1, 0) = r1[4];
MAT(out, 1, 1) = r1[5];
MAT(out, 1, 2) = r1[6];
MAT(out, 1, 3) = r1[7];
MAT(out, 2, 0) = r2[4];
MAT(out, 2, 1) = r2[5];
MAT(out, 2, 2) = r2[6];
MAT(out, 2, 3) = r2[7];
MAT(out, 3, 0) = r3[4];
MAT(out, 3, 1) = r3[5];
MAT(out, 3, 2) = r3[6];
MAT(out, 3, 3) = r3[7];
#undef MAT
*this = temp;
}
void Projection::flip_y() {
@ -784,14 +912,10 @@ void Projection::set_light_atlas_rect(const Rect2 &p_rect) {
}
Projection::operator String() const {
String str;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
str += String((j > 0) ? ", " : "\n") + rtos(columns[i][j]);
}
}
return str;
return "[X: " + columns[0].operator String() +
", Y: " + columns[1].operator String() +
", Z: " + columns[2].operator String() +
", W: " + columns[3].operator String() + "]";
}
real_t Projection::get_aspect() const {

23
engine/core/math/quaternion.h
View file

@ -141,15 +141,24 @@ struct [[nodiscard]] Quaternion {
}
Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { // Shortest arc.
Vector3 c = p_v0.cross(p_v1);
real_t d = p_v0.dot(p_v1);
if (d < -1.0f + (real_t)CMP_EPSILON) {
x = 0;
y = 1;
z = 0;
#ifdef MATH_CHECKS
ERR_FAIL_COND_MSG(p_v0.is_zero_approx() || p_v1.is_zero_approx(), "The vectors must not be zero.");
#endif
constexpr real_t ALMOST_ONE = 1.0f - (real_t)CMP_EPSILON;
Vector3 n0 = p_v0.normalized();
Vector3 n1 = p_v1.normalized();
real_t d = n0.dot(n1);
if (abs(d) > ALMOST_ONE) {
if (d >= 0) {
return; // Vectors are same.
}
Vector3 axis = n0.get_any_perpendicular();
x = axis.x;
y = axis.y;
z = axis.z;
w = 0;
} else {
Vector3 c = n0.cross(n1);
real_t s = Math::sqrt((1.0f + d) * 2.0f);
real_t rs = 1.0f / s;

2
engine/core/math/quick_hull.cpp
View file

@ -30,8 +30,6 @@
#include "quick_hull.h"
#include "core/templates/rb_map.h"
uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF;
Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) {

2
engine/core/math/quick_hull.h
View file

@ -31,9 +31,7 @@
#ifndef QUICK_HULL_H
#define QUICK_HULL_H
#include "core/math/aabb.h"
#include "core/math/geometry_3d.h"
#include "core/templates/hash_set.h"
#include "core/templates/list.h"
class QuickHull {

7
engine/core/math/random_pcg.cpp
View file

@ -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;
}
@ -75,5 +80,5 @@ int RandomPCG::random(int p_from, int p_to) {
if (p_from == p_to) {
return p_from;
}
return rand(abs(p_from - p_to) + 1) + MIN(p_from, p_to);
return int(rand(uint32_t(Math::abs(p_from - p_to)) + 1U)) + MIN(p_from, p_to);
}

2
engine/core/math/rect2.cpp
View file

@ -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 {

16
engine/core/math/rect2.h
View file

@ -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 {

13
engine/core/math/transform_2d.h
View file

@ -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];

1
engine/core/math/transform_3d.cpp
View file

@ -30,7 +30,6 @@
#include "transform_3d.h"
#include "core/math/math_funcs.h"
#include "core/string/ustring.h"
void Transform3D::affine_invert() {

338
engine/core/math/transform_interpolator.cpp
View file

@ -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;
}

51
engine/core/math/transform_interpolator.h
View file

@ -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

2
engine/core/math/vector2.cpp
View file

@ -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 {

2
engine/core/math/vector3.cpp
View file

@ -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 {

11
engine/core/math/vector3.h
View file

@ -130,6 +130,7 @@ struct [[nodiscard]] Vector3 {
_FORCE_INLINE_ Vector3 cross(const Vector3 &p_with) const;
_FORCE_INLINE_ real_t dot(const Vector3 &p_with) const;
Basis outer(const Vector3 &p_with) const;
_FORCE_INLINE_ Vector3 get_any_perpendicular() const;
_FORCE_INLINE_ Vector3 abs() const;
_FORCE_INLINE_ Vector3 floor() const;
@ -326,6 +327,16 @@ Vector3 Vector3::direction_to(const Vector3 &p_to) const {
return ret;
}
Vector3 Vector3::get_any_perpendicular() const {
// Return the any perpendicular vector by cross product with the Vector3.RIGHT or Vector3.UP,
// whichever has the greater angle to the current vector with the sign of each element positive.
// The only essence is "to avoid being parallel to the current vector", and there is no mathematical basis for using Vector3.RIGHT and Vector3.UP,
// since it could be a different vector depending on the prior branching code Math::abs(x) <= Math::abs(y) && Math::abs(x) <= Math::abs(z).
// However, it would be reasonable to use any of the axes of the basis, as it is simpler to calculate.
ERR_FAIL_COND_V_MSG(is_zero_approx(), Vector3(0, 0, 0), "The Vector3 must not be zero.");
return cross((Math::abs(x) <= Math::abs(y) && Math::abs(x) <= Math::abs(z)) ? Vector3(1, 0, 0) : Vector3(0, 1, 0)).normalized();
}
/* Operators */
Vector3 &Vector3::operator+=(const Vector3 &p_v) {

2
engine/core/math/vector4.cpp
View file

@ -213,7 +213,7 @@ Vector4 Vector4::clampf(real_t p_min, real_t 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) + ")";
return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ", " + String::num_real(z, true) + ", " + String::num_real(w, true) + ")";
}
static_assert(sizeof(Vector4) == 4 * sizeof(real_t));

6
engine/core/math/vector4.h
View file

@ -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;