summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/quaternion.cpp52
-rw-r--r--core/math/transform_2d.cpp34
-rw-r--r--core/math/transform_2d.h5
-rw-r--r--core/math/transform_3d.cpp30
-rw-r--r--core/math/transform_3d.h3
-rw-r--r--core/math/vector3.h11
-rw-r--r--core/math/vector4.cpp94
-rw-r--r--core/math/vector4.h25
-rw-r--r--core/math/vector4i.cpp2
9 files changed, 187 insertions, 69 deletions
diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp
index 252c108166..c681c60694 100644
--- a/core/math/quaternion.cpp
+++ b/core/math/quaternion.cpp
@@ -188,45 +188,49 @@ Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif
- Quaternion ret_q = *this;
+ Quaternion from_q = *this;
Quaternion pre_q = p_pre_a;
Quaternion to_q = p_b;
Quaternion post_q = p_post_b;
// Align flip phases.
- ret_q = Basis(ret_q).get_rotation_quaternion();
+ from_q = Basis(from_q).get_rotation_quaternion();
pre_q = Basis(pre_q).get_rotation_quaternion();
to_q = Basis(to_q).get_rotation_quaternion();
post_q = Basis(post_q).get_rotation_quaternion();
// Flip quaternions to shortest path if necessary.
- bool flip1 = signbit(ret_q.dot(pre_q));
+ bool flip1 = signbit(from_q.dot(pre_q));
pre_q = flip1 ? -pre_q : pre_q;
- bool flip2 = signbit(ret_q.dot(to_q));
+ bool flip2 = signbit(from_q.dot(to_q));
to_q = flip2 ? -to_q : to_q;
bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q));
post_q = flip3 ? -post_q : post_q;
- if (flip1 || flip2 || flip3) {
- // Angle is too large, calc by Approximate.
- ret_q.x = Math::cubic_interpolate(ret_q.x, to_q.x, pre_q.x, post_q.x, p_weight);
- ret_q.y = Math::cubic_interpolate(ret_q.y, to_q.y, pre_q.y, post_q.y, p_weight);
- ret_q.z = Math::cubic_interpolate(ret_q.z, to_q.z, pre_q.z, post_q.z, p_weight);
- ret_q.w = Math::cubic_interpolate(ret_q.w, to_q.w, pre_q.w, post_q.w, p_weight);
- ret_q.normalize();
- } else {
- // Calc by Expmap.
- Quaternion ln_ret = ret_q.log();
- Quaternion ln_to = to_q.log();
- Quaternion ln_pre = pre_q.log();
- Quaternion ln_post = post_q.log();
- Quaternion ln = Quaternion(0, 0, 0, 0);
- ln.x = Math::cubic_interpolate(ln_ret.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
- ln.y = Math::cubic_interpolate(ln_ret.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
- ln.z = Math::cubic_interpolate(ln_ret.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
- ret_q = ln.exp();
- }
- return ret_q;
+ // Calc by Expmap in from_q space.
+ Quaternion ln_from = Quaternion(0, 0, 0, 0);
+ Quaternion ln_to = (from_q.inverse() * to_q).log();
+ Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+ Quaternion ln_post = (from_q.inverse() * post_q).log();
+ Quaternion ln = Quaternion(0, 0, 0, 0);
+ ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+ ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+ ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+ Quaternion q1 = from_q * ln.exp();
+
+ // Calc by Expmap in to_q space.
+ ln_from = (to_q.inverse() * from_q).log();
+ ln_to = Quaternion(0, 0, 0, 0);
+ ln_pre = (to_q.inverse() * pre_q).log();
+ ln_post = (to_q.inverse() * post_q).log();
+ ln = Quaternion(0, 0, 0, 0);
+ ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+ ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+ ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+ Quaternion q2 = to_q * ln.exp();
+
+ // To cancel error made by Expmap ambiguity, do blends.
+ return q1.slerp(q2, p_weight);
}
Quaternion::operator String() const {
diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp
index bb8bf9f569..226076029b 100644
--- a/core/math/transform_2d.cpp
+++ b/core/math/transform_2d.cpp
@@ -217,34 +217,48 @@ Transform2D Transform2D::operator*(const Transform2D &p_transform) const {
return t;
}
-Transform2D Transform2D::scaled(const Size2 &p_scale) const {
+Transform2D Transform2D::basis_scaled(const Size2 &p_scale) const {
Transform2D copy = *this;
- copy.scale(p_scale);
+ copy.scale_basis(p_scale);
return copy;
}
-Transform2D Transform2D::basis_scaled(const Size2 &p_scale) const {
+Transform2D Transform2D::scaled(const Size2 &p_scale) const {
+ // Equivalent to left multiplication
Transform2D copy = *this;
- copy.scale_basis(p_scale);
+ copy.scale(p_scale);
return copy;
}
+Transform2D Transform2D::scaled_local(const Size2 &p_scale) const {
+ // Equivalent to right multiplication
+ return Transform2D(columns[0] * p_scale.x, columns[1] * p_scale.y, columns[2]);
+}
+
Transform2D Transform2D::untranslated() const {
Transform2D copy = *this;
copy.columns[2] = Vector2();
return copy;
}
+Transform2D Transform2D::translated(const Vector2 &p_offset) const {
+ // Equivalent to left multiplication
+ return Transform2D(columns[0], columns[1], columns[2] + p_offset);
+}
+
Transform2D Transform2D::translated_local(const Vector2 &p_offset) const {
- Transform2D copy = *this;
- copy.translate_local(p_offset);
- return copy;
+ // Equivalent to right multiplication
+ return Transform2D(columns[0], columns[1], columns[2] + basis_xform(p_offset));
}
Transform2D Transform2D::rotated(const real_t p_angle) const {
- Transform2D copy = *this;
- copy.rotate(p_angle);
- return copy;
+ // Equivalent to left multiplication
+ return Transform2D(p_angle, Vector2()) * (*this);
+}
+
+Transform2D Transform2D::rotated_local(const real_t p_angle) const {
+ // Equivalent to right multiplication
+ return (*this) * Transform2D(p_angle, Vector2()); // Could be optimized, because origin transform can be skipped.
}
real_t Transform2D::basis_determinant() const {
diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h
index e64d050f0c..f23f32867a 100644
--- a/core/math/transform_2d.h
+++ b/core/math/transform_2d.h
@@ -85,10 +85,13 @@ struct _NO_DISCARD_ Transform2D {
_FORCE_INLINE_ const Vector2 &get_origin() const { return columns[2]; }
_FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { columns[2] = p_origin; }
- Transform2D scaled(const Size2 &p_scale) const;
Transform2D basis_scaled(const Size2 &p_scale) const;
+ Transform2D scaled(const Size2 &p_scale) const;
+ Transform2D scaled_local(const Size2 &p_scale) const;
+ Transform2D translated(const Vector2 &p_offset) const;
Transform2D translated_local(const Vector2 &p_offset) const;
Transform2D rotated(const real_t p_angle) const;
+ Transform2D rotated_local(const real_t p_angle) const;
Transform2D untranslated() const;
diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp
index c497a276f3..a634faca9a 100644
--- a/core/math/transform_3d.cpp
+++ b/core/math/transform_3d.cpp
@@ -62,7 +62,15 @@ void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
}
Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const {
- return Transform3D(Basis(p_axis, p_angle), Vector3()) * (*this);
+ // Equivalent to left multiplication
+ Basis p_basis(p_axis, p_angle);
+ return Transform3D(p_basis * basis, p_basis.xform(origin));
+}
+
+Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) const {
+ // Equivalent to right multiplication
+ Basis p_basis(p_axis, p_angle);
+ return Transform3D(basis * p_basis, origin);
}
void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
@@ -120,9 +128,13 @@ void Transform3D::scale(const Vector3 &p_scale) {
}
Transform3D Transform3D::scaled(const Vector3 &p_scale) const {
- Transform3D t = *this;
- t.scale(p_scale);
- return t;
+ // Equivalent to left multiplication
+ return Transform3D(basis.scaled(p_scale), origin * p_scale);
+}
+
+Transform3D Transform3D::scaled_local(const Vector3 &p_scale) const {
+ // Equivalent to right multiplication
+ return Transform3D(basis.scaled_local(p_scale), origin);
}
void Transform3D::scale_basis(const Vector3 &p_scale) {
@@ -139,10 +151,14 @@ void Transform3D::translate_local(const Vector3 &p_translation) {
}
}
+Transform3D Transform3D::translated(const Vector3 &p_translation) const {
+ // Equivalent to left multiplication
+ return Transform3D(basis, origin + p_translation);
+}
+
Transform3D Transform3D::translated_local(const Vector3 &p_translation) const {
- Transform3D t = *this;
- t.translate_local(p_translation);
- return t;
+ // Equivalent to right multiplication
+ return Transform3D(basis, origin + basis.xform(p_translation));
}
void Transform3D::orthonormalize() {
diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h
index 1f8026043f..b572e90859 100644
--- a/core/math/transform_3d.h
+++ b/core/math/transform_3d.h
@@ -46,6 +46,7 @@ struct _NO_DISCARD_ Transform3D {
Transform3D affine_inverse() const;
Transform3D rotated(const Vector3 &p_axis, real_t p_angle) const;
+ Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const;
void rotate(const Vector3 &p_axis, real_t p_angle);
void rotate_basis(const Vector3 &p_axis, real_t p_angle);
@@ -55,9 +56,11 @@ struct _NO_DISCARD_ Transform3D {
void scale(const Vector3 &p_scale);
Transform3D scaled(const Vector3 &p_scale) const;
+ Transform3D scaled_local(const Vector3 &p_scale) const;
void scale_basis(const Vector3 &p_scale);
void translate_local(real_t p_tx, real_t p_ty, real_t p_tz);
void translate_local(const Vector3 &p_translation);
+ Transform3D translated(const Vector3 &p_translation) const;
Transform3D translated_local(const Vector3 &p_translation) const;
const Basis &get_basis() const { return basis; }
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 970416234d..4ce01da60e 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -217,16 +217,25 @@ Vector3 Vector3::lerp(const Vector3 &p_to, const real_t p_weight) const {
}
Vector3 Vector3::slerp(const Vector3 &p_to, const real_t p_weight) const {
+ // This method seems more complicated than it really is, since we write out
+ // the internals of some methods for efficiency (mainly, checking length).
real_t start_length_sq = length_squared();
real_t end_length_sq = p_to.length_squared();
if (unlikely(start_length_sq == 0.0f || end_length_sq == 0.0f)) {
// Zero length vectors have no angle, so the best we can do is either lerp or throw an error.
return lerp(p_to, p_weight);
}
+ Vector3 axis = cross(p_to);
+ real_t axis_length_sq = axis.length_squared();
+ if (unlikely(axis_length_sq == 0.0f)) {
+ // Colinear vectors have no rotation axis or angle between them, so the best we can do is lerp.
+ return lerp(p_to, p_weight);
+ }
+ axis /= Math::sqrt(axis_length_sq);
real_t start_length = Math::sqrt(start_length_sq);
real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight);
real_t angle = angle_to(p_to);
- return rotated(cross(p_to).normalized(), angle * p_weight) * (result_length / start_length);
+ return rotated(axis, angle * p_weight) * (result_length / start_length);
}
Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const {
diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp
index ed42d8bfb9..4697c311b4 100644
--- a/core/math/vector4.cpp
+++ b/core/math/vector4.cpp
@@ -33,6 +33,40 @@
#include "core/math/basis.h"
#include "core/string/print_string.h"
+void Vector4::set_axis(const int p_axis, const real_t p_value) {
+ ERR_FAIL_INDEX(p_axis, 4);
+ components[p_axis] = p_value;
+}
+
+real_t Vector4::get_axis(const int p_axis) const {
+ ERR_FAIL_INDEX_V(p_axis, 4, 0);
+ return operator[](p_axis);
+}
+
+Vector4::Axis Vector4::min_axis_index() const {
+ uint32_t min_index = 0;
+ real_t min_value = x;
+ for (uint32_t i = 1; i < 4; i++) {
+ if (operator[](i) <= min_value) {
+ min_index = i;
+ min_value = operator[](i);
+ }
+ }
+ return Vector4::Axis(min_index);
+}
+
+Vector4::Axis Vector4::max_axis_index() const {
+ uint32_t max_index = 0;
+ real_t max_value = x;
+ for (uint32_t i = 1; i < 4; i++) {
+ if (operator[](i) > max_value) {
+ max_index = i;
+ max_value = operator[](i);
+ }
+ }
+ return Vector4::Axis(max_index);
+}
+
bool Vector4::is_equal_approx(const Vector4 &p_vec4) const {
return Math::is_equal_approx(x, p_vec4.x) && Math::is_equal_approx(y, p_vec4.y) && Math::is_equal_approx(z, p_vec4.z) && Math::is_equal_approx(w, p_vec4.w);
}
@@ -53,6 +87,16 @@ bool Vector4::is_normalized() const {
return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); // Use less epsilon.
}
+real_t Vector4::distance_to(const Vector4 &p_to) const {
+ return (p_to - *this).length();
+}
+
+Vector4 Vector4::direction_to(const Vector4 &p_to) const {
+ Vector4 ret(p_to.x - x, p_to.y - y, p_to.z - z, p_to.w - w);
+ ret.normalize();
+ return ret;
+}
+
Vector4 Vector4::abs() const {
return Vector4(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w));
}
@@ -81,32 +125,38 @@ Vector4 Vector4::lerp(const Vector4 &p_to, const real_t p_weight) const {
w + (p_weight * (p_to.w - w)));
}
-Vector4 Vector4::inverse() const {
- return Vector4(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w);
+Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const {
+ Vector4 res = *this;
+ res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+ res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+ res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
+ res.w = Math::cubic_interpolate(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight);
+ return res;
}
-Vector4::Axis Vector4::min_axis_index() const {
- uint32_t min_index = 0;
- real_t min_value = x;
- for (uint32_t i = 1; i < 4; i++) {
- if (operator[](i) < min_value) {
- min_index = i;
- min_value = operator[](i);
- }
- }
- return Vector4::Axis(min_index);
+Vector4 Vector4::posmod(const real_t p_mod) const {
+ return Vector4(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod), Math::fposmod(w, p_mod));
}
-Vector4::Axis Vector4::max_axis_index() const {
- uint32_t max_index = 0;
- real_t max_value = x;
- for (uint32_t i = 1; i < 4; i++) {
- if (operator[](i) > max_value) {
- max_index = i;
- max_value = operator[](i);
- }
- }
- return Vector4::Axis(max_index);
+Vector4 Vector4::posmodv(const Vector4 &p_modv) const {
+ return Vector4(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z), Math::fposmod(w, p_modv.w));
+}
+
+void Vector4::snap(const Vector4 &p_step) {
+ x = Math::snapped(x, p_step.x);
+ y = Math::snapped(y, p_step.y);
+ z = Math::snapped(z, p_step.z);
+ w = Math::snapped(w, p_step.w);
+}
+
+Vector4 Vector4::snapped(const Vector4 &p_step) const {
+ Vector4 v = *this;
+ v.snap(p_step);
+ return v;
+}
+
+Vector4 Vector4::inverse() const {
+ return Vector4(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w);
}
Vector4 Vector4::clamp(const Vector4 &p_min, const Vector4 &p_max) const {
diff --git a/core/math/vector4.h b/core/math/vector4.h
index 37ddb509d6..373a6a1218 100644
--- a/core/math/vector4.h
+++ b/core/math/vector4.h
@@ -62,6 +62,15 @@ struct _NO_DISCARD_ Vector4 {
DEV_ASSERT((unsigned int)p_axis < 4);
return components[p_axis];
}
+
+ _FORCE_INLINE_ void set_all(const real_t p_value);
+
+ void set_axis(const int p_axis, const real_t p_value);
+ real_t get_axis(const int p_axis) const;
+
+ Vector4::Axis min_axis_index() const;
+ Vector4::Axis max_axis_index() const;
+
_FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Vector4 &p_vec4) const;
real_t length() const;
@@ -69,15 +78,21 @@ struct _NO_DISCARD_ Vector4 {
Vector4 normalized() const;
bool is_normalized() const;
+ real_t distance_to(const Vector4 &p_to) const;
+ Vector4 direction_to(const Vector4 &p_to) const;
+
Vector4 abs() const;
Vector4 sign() const;
Vector4 floor() const;
Vector4 ceil() const;
Vector4 round() const;
Vector4 lerp(const Vector4 &p_to, const real_t p_weight) const;
+ Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const;
- Vector4::Axis min_axis_index() const;
- Vector4::Axis max_axis_index() const;
+ Vector4 posmod(const real_t p_mod) const;
+ Vector4 posmodv(const Vector4 &p_modv) const;
+ void snap(const Vector4 &p_step);
+ Vector4 snapped(const Vector4 &p_step) const;
Vector4 clamp(const Vector4 &p_min, const Vector4 &p_max) const;
Vector4 inverse() const;
@@ -130,6 +145,10 @@ struct _NO_DISCARD_ Vector4 {
}
};
+void Vector4::set_all(const real_t p_value) {
+ x = y = z = p_value;
+}
+
real_t Vector4::dot(const Vector4 &p_vec4) const {
return x * p_vec4.x + y * p_vec4.y + z * p_vec4.z + w * p_vec4.w;
}
@@ -193,7 +212,7 @@ Vector4 Vector4::operator/(const Vector4 &p_vec4) const {
}
Vector4 Vector4::operator-() const {
- return Vector4(x, y, z, w);
+ return Vector4(-x, -y, -z, -w);
}
Vector4 Vector4::operator*(const real_t &s) const {
diff --git a/core/math/vector4i.cpp b/core/math/vector4i.cpp
index 8c571b02e3..2dc5b74202 100644
--- a/core/math/vector4i.cpp
+++ b/core/math/vector4i.cpp
@@ -47,7 +47,7 @@ Vector4i::Axis Vector4i::min_axis_index() const {
uint32_t min_index = 0;
int32_t min_value = x;
for (uint32_t i = 1; i < 4; i++) {
- if (operator[](i) < min_value) {
+ if (operator[](i) <= min_value) {
min_index = i;
min_value = operator[](i);
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-22 11:28:16 +0000