diff options
Diffstat (limited to 'core/math')
38 files changed, 293 insertions, 145 deletions
diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 026f179445..fcf245d2ad 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -76,6 +76,10 @@ bool AABB::is_equal_approx(const AABB &p_aabb) const { return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size); } +bool AABB::is_finite() const { + return position.is_finite() && size.is_finite(); +} + AABB AABB::intersection(const AABB &p_aabb) const { #ifdef MATH_CHECKS if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) { diff --git a/core/math/aabb.h b/core/math/aabb.h index b9f777c6cf..9d5837ad37 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -63,6 +63,7 @@ struct _NO_DISCARD_ AABB { bool operator!=(const AABB &p_rval) const; bool is_equal_approx(const AABB &p_aabb) const; + bool is_finite() const; _FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap _FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap _FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index 1a80faaa12..d06f9bef1e 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -34,7 +34,7 @@ #include "core/math/vector2.h" #include "core/typedefs.h" -static inline float undenormalise(volatile float f) { +static inline float undenormalize(volatile float f) { union { uint32_t i; float f; @@ -101,9 +101,9 @@ struct AudioFrame { r /= p_sample; } - _ALWAYS_INLINE_ void undenormalise() { - l = ::undenormalise(l); - r = ::undenormalise(r); + _ALWAYS_INLINE_ void undenormalize() { + l = ::undenormalize(l); + r = ::undenormalize(r); } _FORCE_INLINE_ AudioFrame lerp(const AudioFrame &p_b, float p_t) const { diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 845686f339..d7bb025b69 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -453,7 +453,7 @@ void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) cons Vector3 Basis::get_euler(EulerOrder p_order) const { switch (p_order) { - case EULER_ORDER_XYZ: { + case EulerOrder::XYZ: { // Euler angles in XYZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -487,8 +487,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { euler.z = 0.0f; } return euler; - } break; - case EULER_ORDER_XZY: { + } + case EulerOrder::XZY: { // Euler angles in XZY convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -516,8 +516,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { euler.z = -Math_PI / 2.0f; } return euler; - } break; - case EULER_ORDER_YXZ: { + } + case EulerOrder::YXZ: { // Euler angles in YXZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -554,8 +554,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { } return euler; - } break; - case EULER_ORDER_YZX: { + } + case EulerOrder::YZX: { // Euler angles in YZX convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -584,7 +584,7 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { } return euler; } break; - case EULER_ORDER_ZXY: { + case EulerOrder::ZXY: { // Euler angles in ZXY convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -612,7 +612,7 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { } return euler; } break; - case EULER_ORDER_ZYX: { + case EulerOrder::ZYX: { // Euler angles in ZYX convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -639,7 +639,7 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { euler.z = -Math::atan2(rows[0][1], rows[1][1]); } return euler; - } break; + } default: { ERR_FAIL_V_MSG(Vector3(), "Invalid parameter for get_euler(order)"); } @@ -663,22 +663,22 @@ void Basis::set_euler(const Vector3 &p_euler, EulerOrder p_order) { Basis zmat(c, -s, 0, s, c, 0, 0, 0, 1); switch (p_order) { - case EULER_ORDER_XYZ: { + case EulerOrder::XYZ: { *this = xmat * (ymat * zmat); } break; - case EULER_ORDER_XZY: { + case EulerOrder::XZY: { *this = xmat * zmat * ymat; } break; - case EULER_ORDER_YXZ: { + case EulerOrder::YXZ: { *this = ymat * xmat * zmat; } break; - case EULER_ORDER_YZX: { + case EulerOrder::YZX: { *this = ymat * zmat * xmat; } break; - case EULER_ORDER_ZXY: { + case EulerOrder::ZXY: { *this = zmat * xmat * ymat; } break; - case EULER_ORDER_ZYX: { + case EulerOrder::ZYX: { *this = zmat * ymat * xmat; } break; default: { @@ -691,6 +691,10 @@ bool Basis::is_equal_approx(const Basis &p_basis) const { return rows[0].is_equal_approx(p_basis.rows[0]) && rows[1].is_equal_approx(p_basis.rows[1]) && rows[2].is_equal_approx(p_basis.rows[2]); } +bool Basis::is_finite() const { + return rows[0].is_finite() && rows[1].is_finite() && rows[2].is_finite(); +} + bool Basis::operator==(const Basis &p_matrix) const { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -811,7 +815,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { return; } // As we have reached here there are no singularities so we can handle normally. - double s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise. + double s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalize. if (Math::abs(s) < CMP_EPSILON) { // Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above. diff --git a/core/math/basis.h b/core/math/basis.h index cc2924f5ff..a1d9fccef1 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -56,15 +56,6 @@ struct _NO_DISCARD_ Basis { _FORCE_INLINE_ real_t determinant() const; - enum EulerOrder { - EULER_ORDER_XYZ, - EULER_ORDER_XZY, - EULER_ORDER_YXZ, - EULER_ORDER_YZX, - EULER_ORDER_ZXY, - EULER_ORDER_ZYX - }; - void from_z(const Vector3 &p_z); void rotate(const Vector3 &p_axis, real_t p_angle); @@ -73,13 +64,13 @@ struct _NO_DISCARD_ Basis { void rotate_local(const Vector3 &p_axis, real_t p_angle); Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const; - void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ); - Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const; + void rotate(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ); + Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ) const; void rotate(const Quaternion &p_quaternion); Basis rotated(const Quaternion &p_quaternion) const; - Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_YXZ) const; + Vector3 get_euler_normalized(EulerOrder p_order = EulerOrder::YXZ) const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; Quaternion get_rotation_quaternion() const; @@ -88,9 +79,9 @@ struct _NO_DISCARD_ Basis { Vector3 rotref_posscale_decomposition(Basis &rotref) const; - Vector3 get_euler(EulerOrder p_order = EULER_ORDER_YXZ) const; - void set_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ); - static Basis from_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) { + Vector3 get_euler(EulerOrder p_order = EulerOrder::YXZ) const; + void set_euler(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ); + static Basis from_euler(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ) { Basis b; b.set_euler(p_euler, p_order); return b; @@ -119,7 +110,7 @@ struct _NO_DISCARD_ Basis { Vector3 get_scale_local() const; void set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale); - void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EULER_ORDER_YXZ); + void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EulerOrder::YXZ); void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale); // transposed dot products @@ -134,6 +125,7 @@ struct _NO_DISCARD_ Basis { } bool is_equal_approx(const Basis &p_basis) const; + bool is_finite() const; bool operator==(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const; diff --git a/core/math/color.h b/core/math/color.h index a23a4953ce..5630539aa7 100644 --- a/core/math/color.h +++ b/core/math/color.h @@ -105,12 +105,10 @@ struct _NO_DISCARD_ Color { _FORCE_INLINE_ Color lerp(const Color &p_to, float p_weight) const { Color res = *this; - - res.r += (p_weight * (p_to.r - r)); - res.g += (p_weight * (p_to.g - g)); - res.b += (p_weight * (p_to.b - b)); - res.a += (p_weight * (p_to.a - a)); - + res.r = Math::lerp(res.r, p_to.r, p_weight); + res.g = Math::lerp(res.g, p_to.g, p_weight); + res.b = Math::lerp(res.b, p_to.b, p_weight); + res.a = Math::lerp(res.a, p_to.a, p_weight); return res; } diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp index 996f4f4d67..561970d2ee 100644 --- a/core/math/convex_hull.cpp +++ b/core/math/convex_hull.cpp @@ -62,6 +62,7 @@ subject to the following restrictions: #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> @@ -2252,19 +2253,62 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3 r_mesh.vertices = ch.vertices; + // Tag which face each edge belongs to + LocalVector<int32_t> edge_faces; + edge_faces.resize(ch.edges.size()); + + for (uint32_t i = 0; i < ch.edges.size(); i++) { + edge_faces[i] = -1; + } + + for (uint32_t i = 0; i < ch.faces.size(); i++) { + const Edge *e_start = &ch.edges[ch.faces[i]]; + const Edge *e = e_start; + do { + int64_t ofs = e - ch.edges.ptr(); + edge_faces[ofs] = i; + + e = e->get_next_edge_of_face(); + } while (e != e_start); + } + // Copy the edges over. There's two "half-edges" for every edge, so we pick only one of them. r_mesh.edges.resize(ch.edges.size() / 2); + OAHashMap<uint64_t, int32_t> edge_map; + edge_map.reserve(ch.edges.size() * 4); // The higher the capacity, the faster the insert + uint32_t edges_copied = 0; for (uint32_t i = 0; i < ch.edges.size(); i++) { + ERR_CONTINUE(edge_faces[i] == -1); // Sanity check + uint32_t a = (&ch.edges[i])->get_source_vertex(); uint32_t b = (&ch.edges[i])->get_target_vertex(); if (a < b) { // Copy only the "canonical" edge. For the reverse edge, this will be false. ERR_BREAK(edges_copied >= (uint32_t)r_mesh.edges.size()); - r_mesh.edges.write[edges_copied].a = a; - r_mesh.edges.write[edges_copied].b = b; + r_mesh.edges[edges_copied].vertex_a = a; + r_mesh.edges[edges_copied].vertex_b = b; + r_mesh.edges[edges_copied].face_a = edge_faces[i]; + r_mesh.edges[edges_copied].face_b = -1; + + uint64_t key = a; + key <<= 32; + key |= b; + edge_map.insert(key, edges_copied); + edges_copied++; + } else { + uint64_t key = b; + key <<= 32; + key |= a; + int32_t index; + if (!edge_map.lookup(key, index)) { + ERR_PRINT("Invalid edge"); + } else { + r_mesh.edges[index].face_b = edge_faces[i]; + } } } + if (edges_copied != (uint32_t)r_mesh.edges.size()) { ERR_PRINT("Invalid edge count."); } @@ -2273,7 +2317,7 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3 for (uint32_t i = 0; i < ch.faces.size(); i++) { const Edge *e_start = &ch.edges[ch.faces[i]]; const Edge *e = e_start; - Geometry3D::MeshData::Face &face = r_mesh.faces.write[i]; + Geometry3D::MeshData::Face &face = r_mesh.faces[i]; do { face.indices.push_back(e->get_target_vertex()); @@ -2284,8 +2328,8 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3 // reverse indices: Godot wants clockwise, but this is counter-clockwise if (face.indices.size() > 2) { // reverse all but the first index. - int *indices = face.indices.ptrw(); - for (int c = 0; c < (face.indices.size() - 1) / 2; c++) { + int *indices = face.indices.ptr(); + for (uint32_t c = 0; c < (face.indices.size() - 1) / 2; c++) { SWAP(indices[c + 1], indices[face.indices.size() - 1 - c]); } } diff --git a/core/math/convex_hull.h b/core/math/convex_hull.h index cc41a794bd..ab6671a7d0 100644 --- a/core/math/convex_hull.h +++ b/core/math/convex_hull.h @@ -62,6 +62,10 @@ public: friend class ConvexHullComputer; public: + int32_t get_next_relative() const { + return next; + } + int32_t get_source_vertex() const { return (this + reverse)->target_vertex; } @@ -86,7 +90,7 @@ public: }; // Vertices of the output hull - Vector<Vector3> vertices; + LocalVector<Vector3> vertices; // Edges of the output hull LocalVector<Edge> edges; diff --git a/core/math/expression.cpp b/core/math/expression.cpp index dcec3929fe..26b809e7f2 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -1420,7 +1420,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: Callable::CallError ce; Variant::call_utility_function(bifunc->func, &r_ret, (const Variant **)argp.ptr(), argp.size(), ce); if (ce.error != Callable::CallError::CALL_OK) { - r_error_str = "Builtin Call Failed. " + Variant::get_call_error_text(bifunc->func, (const Variant **)argp.ptr(), argp.size(), ce); + r_error_str = "Builtin call failed: " + Variant::get_call_error_text(bifunc->func, (const Variant **)argp.ptr(), argp.size(), ce); return true; } diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp index c5871358ed..548b9e4620 100644 --- a/core/math/geometry_3d.cpp +++ b/core/math/geometry_3d.cpp @@ -141,21 +141,21 @@ real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, co void Geometry3D::MeshData::optimize_vertices() { HashMap<int, int> vtx_remap; - for (int i = 0; i < faces.size(); i++) { - for (int j = 0; j < faces[i].indices.size(); j++) { + for (uint32_t i = 0; i < faces.size(); i++) { + for (uint32_t j = 0; j < faces[i].indices.size(); j++) { int idx = faces[i].indices[j]; if (!vtx_remap.has(idx)) { int ni = vtx_remap.size(); vtx_remap[idx] = ni; } - faces.write[i].indices.write[j] = vtx_remap[idx]; + faces[i].indices[j] = vtx_remap[idx]; } } - for (int i = 0; i < edges.size(); i++) { - int a = edges[i].a; - int b = edges[i].b; + for (uint32_t i = 0; i < edges.size(); i++) { + int a = edges[i].vertex_a; + int b = edges[i].vertex_b; if (!vtx_remap.has(a)) { int ni = vtx_remap.size(); @@ -166,16 +166,16 @@ void Geometry3D::MeshData::optimize_vertices() { vtx_remap[b] = ni; } - edges.write[i].a = vtx_remap[a]; - edges.write[i].b = vtx_remap[b]; + edges[i].vertex_a = vtx_remap[a]; + edges[i].vertex_b = vtx_remap[b]; } - Vector<Vector3> new_vertices; + LocalVector<Vector3> new_vertices; new_vertices.resize(vtx_remap.size()); - for (int i = 0; i < vertices.size(); i++) { + for (uint32_t i = 0; i < vertices.size(); i++) { if (vtx_remap.has(i)) { - new_vertices.write[vtx_remap[i]] = vertices[i]; + new_vertices[vtx_remap[i]] = vertices[i]; } } vertices = new_vertices; @@ -751,7 +751,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes Vector3 center = p.center(); // make a quad clockwise - Vector<Vector3> vertices = { + LocalVector<Vector3> vertices = { center - up * subplane_size + right * subplane_size, center - up * subplane_size - right * subplane_size, center + up * subplane_size - right * subplane_size, @@ -763,7 +763,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes continue; } - Vector<Vector3> new_vertices; + LocalVector<Vector3> new_vertices; Plane clip = p_planes[j]; if (clip.normal.dot(p.normal) > 0.95f) { @@ -774,7 +774,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes break; } - for (int k = 0; k < vertices.size(); k++) { + for (uint32_t k = 0; k < vertices.size(); k++) { int k_n = (k + 1) % vertices.size(); Vector3 edge0_A = vertices[k]; @@ -816,9 +816,9 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes MeshData::Face face; // Add face indices. - for (int j = 0; j < vertices.size(); j++) { + for (uint32_t j = 0; j < vertices.size(); j++) { int idx = -1; - for (int k = 0; k < mesh.vertices.size(); k++) { + for (uint32_t k = 0; k < mesh.vertices.size(); k++) { if (mesh.vertices[k].distance_to(vertices[j]) < 0.001f) { idx = k; break; @@ -837,28 +837,34 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes // Add edge. - for (int j = 0; j < face.indices.size(); j++) { + for (uint32_t j = 0; j < face.indices.size(); j++) { int a = face.indices[j]; int b = face.indices[(j + 1) % face.indices.size()]; bool found = false; - for (int k = 0; k < mesh.edges.size(); k++) { - if (mesh.edges[k].a == a && mesh.edges[k].b == b) { + int found_idx = -1; + for (uint32_t k = 0; k < mesh.edges.size(); k++) { + if (mesh.edges[k].vertex_a == a && mesh.edges[k].vertex_b == b) { found = true; + found_idx = k; break; } - if (mesh.edges[k].b == a && mesh.edges[k].a == b) { + if (mesh.edges[k].vertex_b == a && mesh.edges[k].vertex_a == b) { found = true; + found_idx = k; break; } } if (found) { + mesh.edges[found_idx].face_b = j; continue; } MeshData::Edge edge; - edge.a = a; - edge.b = b; + edge.vertex_a = a; + edge.vertex_b = b; + edge.face_a = j; + edge.face_b = -1; mesh.edges.push_back(edge); } } diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h index e5ace9db72..4b4c173a1e 100644 --- a/core/math/geometry_3d.h +++ b/core/math/geometry_3d.h @@ -33,6 +33,7 @@ #include "core/math/face3.h" #include "core/object/object.h" +#include "core/templates/local_vector.h" #include "core/templates/vector.h" class Geometry3D { @@ -539,18 +540,19 @@ public: struct MeshData { struct Face { Plane plane; - Vector<int> indices; + LocalVector<int> indices; }; - Vector<Face> faces; + LocalVector<Face> faces; struct Edge { - int a, b; + int vertex_a, vertex_b; + int face_a, face_b; }; - Vector<Edge> edges; + LocalVector<Edge> edges; - Vector<Vector3> vertices; + LocalVector<Vector3> vertices; void optimize_vertices(); }; diff --git a/core/math/math_defs.h b/core/math/math_defs.h index b8b82f2ff4..759667e2d5 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -116,6 +116,15 @@ enum Corner { CORNER_BOTTOM_LEFT }; +enum class EulerOrder { + XYZ, + XZY, + YXZ, + YZX, + ZXY, + ZYX +}; + /** * The "Real" type is an abstract type used for real numbers, such as 1.5, * in contrast to integer numbers. Precision can be controlled with the diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 7fa674a23d..8dff8e6e7e 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -184,6 +184,9 @@ public: #endif } + static _ALWAYS_INLINE_ bool is_finite(double p_val) { return isfinite(p_val); } + static _ALWAYS_INLINE_ bool is_finite(float p_val) { return isfinite(p_val); } + static _ALWAYS_INLINE_ double abs(double g) { return absd(g); } static _ALWAYS_INLINE_ float abs(float g) { return absf(g); } static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; } @@ -361,6 +364,26 @@ public: return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0f + p_control_2 * omt * t2 * 3.0f + p_end * t3; } + static _ALWAYS_INLINE_ double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) { + /* Formula from Wikipedia article on Bezier curves. */ + double omt = (1.0 - p_t); + double omt2 = omt * omt; + double t2 = p_t * p_t; + + double d = (p_control_1 - p_start) * 3.0 * omt2 + (p_control_2 - p_control_1) * 6.0 * omt * p_t + (p_end - p_control_2) * 3.0 * t2; + return d; + } + + static _ALWAYS_INLINE_ float bezier_derivative(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) { + /* Formula from Wikipedia article on Bezier curves. */ + float omt = (1.0f - p_t); + float omt2 = omt * omt; + float t2 = p_t * p_t; + + float d = (p_control_1 - p_start) * 3.0f * omt2 + (p_control_2 - p_control_1) * 6.0f * omt * p_t + (p_end - p_control_2) * 3.0f * t2; + return d; + } + static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) { double difference = fmod(p_to - p_from, Math_TAU); double distance = fmod(2.0 * difference, Math_TAU) - difference; diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 3b2eab4ae2..a5d2fe5628 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -176,6 +176,10 @@ bool Plane::is_equal_approx(const Plane &p_plane) const { return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d); } +bool Plane::is_finite() const { + return normal.is_finite() && Math::is_finite(d); +} + Plane::operator String() const { return "[N: " + normal.operator String() + ", D: " + String::num_real(d, false) + "]"; } diff --git a/core/math/plane.h b/core/math/plane.h index 73babfa496..77da59fb27 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -74,6 +74,7 @@ struct _NO_DISCARD_ Plane { Plane operator-() const { return Plane(-normal, -d); } bool is_equal_approx(const Plane &p_plane) const; bool is_equal_approx_any_side(const Plane &p_plane) const; + bool is_finite() const; _FORCE_INLINE_ bool operator==(const Plane &p_plane) const; _FORCE_INLINE_ bool operator!=(const Plane &p_plane) const; diff --git a/core/math/projection.cpp b/core/math/projection.cpp index 70cc9b5f7c..9af388b081 100644 --- a/core/math/projection.cpp +++ b/core/math/projection.cpp @@ -181,7 +181,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } case PLANE_FAR: { Plane new_plane = Plane(matrix[3] - matrix[2], matrix[7] - matrix[6], @@ -191,7 +191,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } case PLANE_LEFT: { Plane new_plane = Plane(matrix[3] + matrix[0], matrix[7] + matrix[4], @@ -201,7 +201,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } case PLANE_TOP: { Plane new_plane = Plane(matrix[3] - matrix[1], matrix[7] - matrix[5], @@ -211,7 +211,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } case PLANE_RIGHT: { Plane new_plane = Plane(matrix[3] - matrix[0], matrix[7] - matrix[4], @@ -221,7 +221,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } case PLANE_BOTTOM: { Plane new_plane = Plane(matrix[3] + matrix[1], matrix[7] + matrix[5], @@ -231,7 +231,7 @@ Plane Projection::get_projection_plane(Planes p_plane) const { new_plane.normal = -new_plane.normal; new_plane.normalize(); return new_plane; - } break; + } } return Plane(); diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 4a8d29e402..942a0b766e 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -38,25 +38,11 @@ real_t Quaternion::angle_to(const Quaternion &p_to) const { return Math::acos(CLAMP(d * d * 2 - 1, -1, 1)); } -// get_euler_xyz returns a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses XYZ convention (Z is the first rotation). -Vector3 Quaternion::get_euler_xyz() const { - Basis m(*this); - return m.get_euler(Basis::EULER_ORDER_XYZ); -} - -// get_euler_yxz returns a vector containing the Euler angles in the format -// (ax,ay,az), where ax is the angle of rotation around x axis, -// and similar for other axes. -// This implementation uses YXZ convention (Z is the first rotation). -Vector3 Quaternion::get_euler_yxz() const { +Vector3 Quaternion::get_euler(EulerOrder p_order) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized."); #endif - Basis m(*this); - return m.get_euler(Basis::EULER_ORDER_YXZ); + return Basis(*this).get_euler(p_order); } void Quaternion::operator*=(const Quaternion &p_q) { @@ -79,6 +65,10 @@ bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const { return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w); } +bool Quaternion::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w); +} + real_t Quaternion::length() const { return Math::sqrt(length_squared()); } @@ -326,7 +316,7 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { // (ax, ay, az), where ax is the angle of rotation around x axis, // and similar for other axes. // This implementation uses YXZ convention (Z is the first rotation). -Quaternion::Quaternion(const Vector3 &p_euler) { +Quaternion Quaternion::from_euler(const Vector3 &p_euler) { real_t half_a1 = p_euler.y * 0.5f; real_t half_a2 = p_euler.x * 0.5f; real_t half_a3 = p_euler.z * 0.5f; @@ -342,8 +332,9 @@ Quaternion::Quaternion(const Vector3 &p_euler) { real_t cos_a3 = Math::cos(half_a3); real_t sin_a3 = Math::sin(half_a3); - x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3; - y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3; - z = -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3; - w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3; + return Quaternion( + sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3, + sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3, + -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3, + sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); } diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 178cfaca70..c5af2121d9 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -55,6 +55,7 @@ struct _NO_DISCARD_ Quaternion { } _FORCE_INLINE_ real_t length_squared() const; bool is_equal_approx(const Quaternion &p_quaternion) const; + bool is_finite() const; real_t length() const; void normalize(); Quaternion normalized() const; @@ -65,9 +66,8 @@ struct _NO_DISCARD_ Quaternion { _FORCE_INLINE_ real_t dot(const Quaternion &p_q) const; real_t angle_to(const Quaternion &p_to) const; - Vector3 get_euler_xyz() const; - Vector3 get_euler_yxz() const; - Vector3 get_euler() const { return get_euler_yxz(); }; + Vector3 get_euler(EulerOrder p_order = EulerOrder::YXZ) const; + static Quaternion from_euler(const Vector3 &p_euler); Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const; Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const; @@ -127,8 +127,6 @@ struct _NO_DISCARD_ Quaternion { Quaternion(const Vector3 &p_axis, real_t p_angle); - Quaternion(const Vector3 &p_euler); - Quaternion(const Quaternion &p_q) : x(p_q.x), y(p_q.y), diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index c7727a44a1..c194e1cc21 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -369,7 +369,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { Geometry3D::MeshData::Face &f = E->get(); - for (int i = 0; i < f.indices.size(); i++) { + for (uint32_t i = 0; i < f.indices.size(); i++) { int a = E->get().indices[i]; int b = E->get().indices[(i + 1) % f.indices.size()]; Edge e(a, b); @@ -436,17 +436,24 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_ r_mesh.faces.clear(); r_mesh.faces.resize(ret_faces.size()); + HashMap<List<Geometry3D::MeshData::Face>::Element *, int> face_indices; + int idx = 0; - for (const Geometry3D::MeshData::Face &E : ret_faces) { - r_mesh.faces.write[idx++] = E; + for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { + face_indices[E] = idx; + r_mesh.faces[idx++] = E->get(); } r_mesh.edges.resize(ret_edges.size()); idx = 0; for (const KeyValue<Edge, RetFaceConnect> &E : ret_edges) { Geometry3D::MeshData::Edge e; - e.a = E.key.vertices[0]; - e.b = E.key.vertices[1]; - r_mesh.edges.write[idx++] = e; + e.vertex_a = E.key.vertices[0]; + e.vertex_b = E.key.vertices[1]; + ERR_CONTINUE(!face_indices.has(E.value.left)); + ERR_CONTINUE(!face_indices.has(E.value.right)); + e.face_a = face_indices[E.value.left]; + e.face_b = face_indices[E.value.right]; + r_mesh.edges[idx++] = e; } r_mesh.vertices = p_points; diff --git a/core/math/random_number_generator.h b/core/math/random_number_generator.h index 9352bae0a6..bf67154cd6 100644 --- a/core/math/random_number_generator.h +++ b/core/math/random_number_generator.h @@ -57,7 +57,7 @@ public: _FORCE_INLINE_ real_t randfn(real_t p_mean = 0.0, real_t p_deviation = 1.0) { return randbase.randfn(p_mean, p_deviation); } _FORCE_INLINE_ int randi_range(int p_from, int p_to) { return randbase.random(p_from, p_to); } - RandomNumberGenerator() {} + RandomNumberGenerator() { randbase.randomize(); } }; #endif // RANDOM_NUMBER_GENERATOR_H diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index 9e78ead816..facf4eb3c4 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -38,6 +38,10 @@ bool Rect2::is_equal_approx(const Rect2 &p_rect) const { return position.is_equal_approx(p_rect.position) && size.is_equal_approx(p_rect.size); } +bool Rect2::is_finite() const { + return position.is_finite() && size.is_finite(); +} + bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const { #ifdef MATH_CHECKS if (unlikely(size.x < 0 || size.y < 0)) { diff --git a/core/math/rect2.h b/core/math/rect2.h index 50dd2dc1df..9863405d8e 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -207,6 +207,7 @@ struct _NO_DISCARD_ Rect2 { } bool is_equal_approx(const Rect2 &p_rect) const; + bool is_finite() const; bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index 2bfefe979f..548a82d254 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -168,6 +168,10 @@ bool Transform2D::is_equal_approx(const Transform2D &p_transform) const { return columns[0].is_equal_approx(p_transform.columns[0]) && columns[1].is_equal_approx(p_transform.columns[1]) && columns[2].is_equal_approx(p_transform.columns[2]); } +bool Transform2D::is_finite() const { + return columns[0].is_finite() && columns[1].is_finite() && columns[2].is_finite(); +} + Transform2D Transform2D::looking_at(const Vector2 &p_target) const { Transform2D return_trans = Transform2D(get_rotation(), get_origin()); Vector2 target_position = affine_inverse().xform(p_target); diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index f23f32867a..2b11f36535 100644 --- a/core/math/transform_2d.h +++ b/core/math/transform_2d.h @@ -98,6 +98,7 @@ struct _NO_DISCARD_ Transform2D { void orthonormalize(); Transform2D orthonormalized() const; bool is_equal_approx(const Transform2D &p_transform) const; + bool is_finite() const; Transform2D looking_at(const Vector2 &p_target) const; diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index 6741ef4034..3285cbd664 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -174,6 +174,10 @@ bool Transform3D::is_equal_approx(const Transform3D &p_transform) const { return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin); } +bool Transform3D::is_finite() const { + return basis.is_finite() && origin.is_finite(); +} + bool Transform3D::operator==(const Transform3D &p_transform) const { return (basis == p_transform.basis && origin == p_transform.origin); } diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h index 44d6d826f3..cb347aa1c1 100644 --- a/core/math/transform_3d.h +++ b/core/math/transform_3d.h @@ -75,6 +75,7 @@ struct _NO_DISCARD_ Transform3D { void orthogonalize(); Transform3D orthogonalized() const; bool is_equal_approx(const Transform3D &p_transform) const; + bool is_finite() const; bool operator==(const Transform3D &p_transform) const; bool operator!=(const Transform3D &p_transform) const; diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index 56dbba393a..5366587126 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -186,6 +186,10 @@ bool Vector2::is_zero_approx() const { return Math::is_zero_approx(x) && Math::is_zero_approx(y); } +bool Vector2::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y); +} + Vector2::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ")"; } diff --git a/core/math/vector2.h b/core/math/vector2.h index 75364f72f0..835c3d1ba6 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -112,6 +112,7 @@ struct _NO_DISCARD_ Vector2 { _FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const; _FORCE_INLINE_ Vector2 cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; _FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const; + _FORCE_INLINE_ Vector2 bezier_derivative(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const; Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const; @@ -121,6 +122,7 @@ struct _NO_DISCARD_ Vector2 { bool is_equal_approx(const Vector2 &p_v) const; bool is_zero_approx() const; + bool is_finite() const; Vector2 operator+(const Vector2 &p_v) const; void operator+=(const Vector2 &p_v); @@ -241,10 +243,8 @@ _FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { Vector2 Vector2::lerp(const Vector2 &p_to, const real_t p_weight) const { Vector2 res = *this; - - res.x += (p_weight * (p_to.x - x)); - res.y += (p_weight * (p_to.y - y)); - + res.x = Math::lerp(res.x, p_to.x, p_weight); + res.y = Math::lerp(res.y, p_to.y, p_weight); return res; } @@ -277,15 +277,16 @@ Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_ Vector2 Vector2::bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const { Vector2 res = *this; + res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t); + res.y = Math::bezier_interpolate(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t); + return res; +} - /* Formula from Wikipedia article on Bezier curves. */ - real_t omt = (1.0 - p_t); - real_t omt2 = omt * omt; - real_t omt3 = omt2 * omt; - real_t t2 = p_t * p_t; - real_t t3 = t2 * p_t; - - return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; +Vector2 Vector2::bezier_derivative(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const { + Vector2 res = *this; + res.x = Math::bezier_derivative(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t); + res.y = Math::bezier_derivative(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t); + return res; } Vector2 Vector2::direction_to(const Vector2 &p_to) const { diff --git a/core/math/vector2i.cpp b/core/math/vector2i.cpp index dfed42e4d6..ff8693ee5b 100644 --- a/core/math/vector2i.cpp +++ b/core/math/vector2i.cpp @@ -39,6 +39,12 @@ Vector2i Vector2i::clamp(const Vector2i &p_min, const Vector2i &p_max) const { CLAMP(y, p_min.y, p_max.y)); } +Vector2i Vector2i::snapped(const Vector2i &p_step) const { + return Vector2i( + Math::snapped(x, p_step.x), + Math::snapped(y, p_step.y)); +} + int64_t Vector2i::length_squared() const { return x * (int64_t)x + y * (int64_t)y; } diff --git a/core/math/vector2i.h b/core/math/vector2i.h index e131bdea94..927be11030 100644 --- a/core/math/vector2i.h +++ b/core/math/vector2i.h @@ -119,6 +119,7 @@ struct _NO_DISCARD_ Vector2i { Vector2i sign() const { return Vector2i(SIGN(x), SIGN(y)); } Vector2i abs() const { return Vector2i(Math::abs(x), Math::abs(y)); } Vector2i clamp(const Vector2i &p_min, const Vector2i &p_max) const; + Vector2i snapped(const Vector2i &p_step) const; operator String() const; operator Vector2() const; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index 55ba509144..b106200c4a 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -139,6 +139,10 @@ bool Vector3::is_zero_approx() const { return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z); } +bool Vector3::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z); +} + Vector3::operator String() const { return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")"; } diff --git a/core/math/vector3.h b/core/math/vector3.h index 62e810fb4d..dc74096690 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -100,6 +100,7 @@ struct _NO_DISCARD_ Vector3 { _FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const; _FORCE_INLINE_ Vector3 cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const; _FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const; + _FORCE_INLINE_ Vector3 bezier_derivative(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const; Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; @@ -136,6 +137,7 @@ struct _NO_DISCARD_ Vector3 { bool is_equal_approx(const Vector3 &p_v) const; bool is_zero_approx() const; + bool is_finite() const; /* Operators */ @@ -207,10 +209,11 @@ Vector3 Vector3::round() const { } Vector3 Vector3::lerp(const Vector3 &p_to, const real_t p_weight) const { - return Vector3( - x + (p_weight * (p_to.x - x)), - y + (p_weight * (p_to.y - y)), - z + (p_weight * (p_to.z - z))); + Vector3 res = *this; + res.x = Math::lerp(res.x, p_to.x, p_weight); + res.y = Math::lerp(res.y, p_to.y, p_weight); + res.z = Math::lerp(res.z, p_to.z, p_weight); + return res; } Vector3 Vector3::slerp(const Vector3 &p_to, const real_t p_weight) const { @@ -253,15 +256,18 @@ Vector3 Vector3::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_ Vector3 Vector3::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const { Vector3 res = *this; + res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t); + res.y = Math::bezier_interpolate(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t); + res.z = Math::bezier_interpolate(res.z, p_control_1.z, p_control_2.z, p_end.z, p_t); + return res; +} - /* Formula from Wikipedia article on Bezier curves. */ - real_t omt = (1.0 - p_t); - real_t omt2 = omt * omt; - real_t omt3 = omt2 * omt; - real_t t2 = p_t * p_t; - real_t t3 = t2 * p_t; - - return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; +Vector3 Vector3::bezier_derivative(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const { + Vector3 res = *this; + res.x = Math::bezier_derivative(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t); + res.y = Math::bezier_derivative(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t); + res.z = Math::bezier_derivative(res.z, p_control_1.z, p_control_2.z, p_end.z, p_t); + return res; } real_t Vector3::distance_to(const Vector3 &p_to) const { diff --git a/core/math/vector3i.cpp b/core/math/vector3i.cpp index b248f35035..901f2b5a64 100644 --- a/core/math/vector3i.cpp +++ b/core/math/vector3i.cpp @@ -48,6 +48,13 @@ Vector3i Vector3i::clamp(const Vector3i &p_min, const Vector3i &p_max) const { CLAMP(z, p_min.z, p_max.z)); } +Vector3i Vector3i::snapped(const Vector3i &p_step) const { + return Vector3i( + Math::snapped(x, p_step.x), + Math::snapped(y, p_step.y), + Math::snapped(z, p_step.z)); +} + Vector3i::operator String() const { return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ")"; } diff --git a/core/math/vector3i.h b/core/math/vector3i.h index 710fd96376..36bac3d8ae 100644 --- a/core/math/vector3i.h +++ b/core/math/vector3i.h @@ -77,6 +77,7 @@ struct _NO_DISCARD_ Vector3i { _FORCE_INLINE_ Vector3i abs() const; _FORCE_INLINE_ Vector3i sign() const; Vector3i clamp(const Vector3i &p_min, const Vector3i &p_max) const; + Vector3i snapped(const Vector3i &p_step) const; /* Operators */ diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp index 9fd980aaff..5ddf2bb6f6 100644 --- a/core/math/vector4.cpp +++ b/core/math/vector4.cpp @@ -64,6 +64,10 @@ bool Vector4::is_zero_approx() const { return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z) && Math::is_zero_approx(w); } +bool Vector4::is_finite() const { + return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w); +} + real_t Vector4::length() const { return Math::sqrt(length_squared()); } @@ -126,11 +130,12 @@ Vector4 Vector4::round() const { } Vector4 Vector4::lerp(const Vector4 &p_to, const real_t p_weight) const { - return Vector4( - x + (p_weight * (p_to.x - x)), - y + (p_weight * (p_to.y - y)), - z + (p_weight * (p_to.z - z)), - w + (p_weight * (p_to.w - w))); + Vector4 res = *this; + res.x = Math::lerp(res.x, p_to.x, p_weight); + res.y = Math::lerp(res.y, p_to.y, p_weight); + res.z = Math::lerp(res.z, p_to.z, p_weight); + res.w = Math::lerp(res.w, p_to.w, p_weight); + return res; } Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const { diff --git a/core/math/vector4.h b/core/math/vector4.h index ac7b6c3aee..7c4bdc1788 100644 --- a/core/math/vector4.h +++ b/core/math/vector4.h @@ -71,6 +71,7 @@ struct _NO_DISCARD_ Vector4 { _FORCE_INLINE_ real_t length_squared() const; bool is_equal_approx(const Vector4 &p_vec4) const; bool is_zero_approx() const; + bool is_finite() const; real_t length() const; void normalize(); Vector4 normalized() const; diff --git a/core/math/vector4i.cpp b/core/math/vector4i.cpp index 77f6fbd5b7..e906ab45ad 100644 --- a/core/math/vector4i.cpp +++ b/core/math/vector4i.cpp @@ -65,6 +65,14 @@ Vector4i Vector4i::clamp(const Vector4i &p_min, const Vector4i &p_max) const { CLAMP(w, p_min.w, p_max.w)); } +Vector4i Vector4i::snapped(const Vector4i &p_step) const { + return Vector4i( + Math::snapped(x, p_step.x), + Math::snapped(y, p_step.y), + Math::snapped(z, p_step.z), + Math::snapped(w, p_step.w)); +} + Vector4i::operator String() const { return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ", " + itos(w) + ")"; } diff --git a/core/math/vector4i.h b/core/math/vector4i.h index a32414bb18..cb5a48daf9 100644 --- a/core/math/vector4i.h +++ b/core/math/vector4i.h @@ -79,6 +79,7 @@ struct _NO_DISCARD_ Vector4i { _FORCE_INLINE_ Vector4i abs() const; _FORCE_INLINE_ Vector4i sign() const; Vector4i clamp(const Vector4i &p_min, const Vector4i &p_max) const; + Vector4i snapped(const Vector4i &p_step) const; /* Operators */ |