summaryrefslogtreecommitdiff
path: root/core
diff options
context:
space:
mode:
Diffstat (limited to 'core')
-rw-r--r--core/math/basis.cpp51
-rw-r--r--core/variant/variant.cpp30
-rw-r--r--core/variant/variant.h17
3 files changed, 42 insertions, 56 deletions
diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 0eb6320ac6..4b163409ce 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -754,29 +754,28 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND(!is_rotation());
#endif
-*/
- real_t angle, x, y, z; // variables for result
- real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees
-
- if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[1][2]) < CMP_EPSILON)) {
- // singularity found
- // first check for identity matrix which must have +1 for all terms
- // in leading diagonal and zero in other terms
- if ((Math::abs(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_epsilon)) {
- // this singularity is identity matrix so angle = 0
+ */
+
+ // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
+ real_t x, y, z; // Variables for result.
+ if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[2][1])) {
+ // Singularity found.
+ // First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms.
+ if (is_diagonal() && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) {
+ // This singularity is identity matrix so angle = 0.
r_axis = Vector3(0, 1, 0);
r_angle = 0;
return;
}
- // otherwise this singularity is angle = 180
- angle = Math_PI;
+ // Otherwise this singularity is angle = 180.
real_t xx = (rows[0][0] + 1) / 2;
real_t yy = (rows[1][1] + 1) / 2;
real_t zz = (rows[2][2] + 1) / 2;
- real_t xy = (rows[1][0] + rows[0][1]) / 4;
- real_t xz = (rows[2][0] + rows[0][2]) / 4;
- real_t yz = (rows[2][1] + rows[1][2]) / 4;
- if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term
+ real_t xy = (rows[0][1] + rows[1][0]) / 4;
+ real_t xz = (rows[0][2] + rows[2][0]) / 4;
+ real_t yz = (rows[1][2] + rows[2][1]) / 4;
+
+ if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term.
if (xx < CMP_EPSILON) {
x = 0;
y = Math_SQRT12;
@@ -786,7 +785,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
y = xy / x;
z = xz / x;
}
- } else if (yy > zz) { // rows[1][1] is the largest diagonal term
+ } else if (yy > zz) { // rows[1][1] is the largest diagonal term.
if (yy < CMP_EPSILON) {
x = Math_SQRT12;
y = 0;
@@ -796,7 +795,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
x = xy / y;
z = yz / y;
}
- } else { // rows[2][2] is the largest diagonal term so base result on this
+ } else { // rows[2][2] is the largest diagonal term so base result on this.
if (zz < CMP_EPSILON) {
x = Math_SQRT12;
y = Math_SQRT12;
@@ -808,22 +807,24 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
}
}
r_axis = Vector3(x, y, z);
- r_angle = angle;
+ r_angle = Math_PI;
return;
}
- // as we have reached here there are no singularities so we can handle normally
- real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise
+ // 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.
- angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2);
- if (angle < 0) {
- s = -s;
+ 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.
+ s = 1;
}
+
x = (rows[2][1] - rows[1][2]) / s;
y = (rows[0][2] - rows[2][0]) / s;
z = (rows[1][0] - rows[0][1]) / s;
r_axis = Vector3(x, y, z);
- r_angle = angle;
+ // CLAMP to avoid NaN if the value passed to acos is not in [0,1].
+ r_angle = Math::acos(CLAMP((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2, (real_t)0.0, (real_t)1.0));
}
void Basis::set_quaternion(const Quaternion &p_quaternion) {
diff --git a/core/variant/variant.cpp b/core/variant/variant.cpp
index b280fc9fe3..f24ffeb1a9 100644
--- a/core/variant/variant.cpp
+++ b/core/variant/variant.cpp
@@ -3727,36 +3727,6 @@ String Variant::get_callable_error_text(const Callable &p_callable, const Varian
return get_call_error_text(p_callable.get_object(), p_callable.get_method(), p_argptrs, p_argcount, ce);
}
-String vformat(const String &p_text, const Variant &p1, const Variant &p2, const Variant &p3, const Variant &p4, const Variant &p5) {
- Array args;
- if (p1.get_type() != Variant::NIL) {
- args.push_back(p1);
-
- if (p2.get_type() != Variant::NIL) {
- args.push_back(p2);
-
- if (p3.get_type() != Variant::NIL) {
- args.push_back(p3);
-
- if (p4.get_type() != Variant::NIL) {
- args.push_back(p4);
-
- if (p5.get_type() != Variant::NIL) {
- args.push_back(p5);
- }
- }
- }
- }
- }
-
- bool error = false;
- String fmt = p_text.sprintf(args, &error);
-
- ERR_FAIL_COND_V_MSG(error, String(), fmt);
-
- return fmt;
-}
-
void Variant::register_types() {
_register_variant_operators();
_register_variant_methods();
diff --git a/core/variant/variant.h b/core/variant/variant.h
index 212f94a9a8..9b213a7682 100644
--- a/core/variant/variant.h
+++ b/core/variant/variant.h
@@ -807,7 +807,22 @@ const Variant::ObjData &Variant::_get_obj() const {
return *reinterpret_cast<const ObjData *>(&_data._mem[0]);
}
-String vformat(const String &p_text, const Variant &p1 = Variant(), const Variant &p2 = Variant(), const Variant &p3 = Variant(), const Variant &p4 = Variant(), const Variant &p5 = Variant());
+template <typename... VarArgs>
+String vformat(const String &p_text, const VarArgs... p_args) {
+ Variant args[sizeof...(p_args) + 1] = { p_args..., Variant() }; // +1 makes sure zero sized arrays are also supported.
+ Array args_array;
+ args_array.resize(sizeof...(p_args));
+ for (uint32_t i = 0; i < sizeof...(p_args); i++) {
+ args_array[i] = args[i];
+ }
+
+ bool error = false;
+ String fmt = p_text.sprintf(args_array, &error);
+
+ ERR_FAIL_COND_V_MSG(error, String(), fmt);
+
+ return fmt;
+}
template <typename... VarArgs>
Callable Callable::bind(VarArgs... p_args) {
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-22 09:21:46 +0000