diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index 8ee8ccb457c..f8c7f436a54 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -356,8 +356,7 @@ void Basis::rotate(const Quat &p_quat) { *this = rotated(p_quat); } -// TODO: rename this to get_rotation_euler -Vector3 Basis::get_rotation() const { +Vector3 Basis::get_rotation_euler() const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). // See the comment in get_scale() for further information. @@ -371,6 +370,20 @@ Vector3 Basis::get_rotation() const { return m.get_euler(); } +Quat Basis::get_rotation_quat() const { + // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, + // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). + // See the comment in get_scale() for further information. + Basis m = orthonormalized(); + real_t det = m.determinant(); + if (det < 0) { + // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles. + m.scale(Vector3(-1, -1, -1)); + } + + return m.get_quat(); +} + void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). @@ -591,10 +604,9 @@ Basis::operator String() const { } Quat Basis::get_quat() const { - //commenting this check because precision issues cause it to fail when it shouldn't - //#ifdef MATH_CHECKS - //ERR_FAIL_COND_V(is_rotation() == false, Quat()); - //#endif +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_rotation() == false, Quat()); +#endif real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; real_t temp[4]; diff --git a/core/math/matrix3.h b/core/math/matrix3.h index 63d4f5d79dd..cd1b51baa61 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -84,9 +84,11 @@ public: void rotate(const Quat &p_quat); Basis rotated(const Quat &p_quat) const; - Vector3 get_rotation() const; + Vector3 get_rotation_euler() 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; + Quat get_rotation_quat() const; + Vector3 get_rotation() const { return get_rotation_euler(); }; Vector3 rotref_posscale_decomposition(Basis &rotref) const; diff --git a/core/math/quat.cpp b/core/math/quat.cpp index b938fc3cfd3..67c9048a414 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -139,15 +139,15 @@ bool Quat::is_normalized() const { Quat Quat::inverse() const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0)); + ERR_FAIL_COND_V(is_normalized() == false, Quat()); #endif return Quat(-x, -y, -z, w); } Quat Quat::slerp(const Quat &q, const real_t &t) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0)); - ERR_FAIL_COND_V(q.is_normalized() == false, Quat(0, 0, 0, 0)); + ERR_FAIL_COND_V(is_normalized() == false, Quat()); + ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); #endif Quat to1; real_t omega, cosom, sinom, scale0, scale1; @@ -192,7 +192,10 @@ Quat Quat::slerp(const Quat &q, const real_t &t) const { } Quat Quat::slerpni(const Quat &q, const real_t &t) const { - +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, Quat()); + ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); +#endif const Quat &from = *this; real_t dot = from.dot(q); @@ -211,7 +214,10 @@ Quat Quat::slerpni(const Quat &q, const real_t &t) const { } Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const { - +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, Quat()); + ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); +#endif //the only way to do slerp :| real_t t2 = (1.0 - t) * t * 2; Quat sp = this->slerp(q, t); diff --git a/core/math/quat.h b/core/math/quat.h index 3e1344a913b..6dc8d66f60c 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -84,7 +84,9 @@ public: } _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { - +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, v); +#endif Vector3 u(x, y, z); Vector3 uv = u.cross(v); return v + ((uv * w) + u.cross(uv)) * ((real_t)2); diff --git a/core/math/transform.cpp b/core/math/transform.cpp index 7cd186ca607..d1e190f4b96 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -120,11 +120,11 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) /* not sure if very "efficient" but good enough? */ Vector3 src_scale = basis.get_scale(); - Quat src_rot = basis.orthonormalized(); + Quat src_rot = basis.get_rotation_quat(); Vector3 src_loc = origin; Vector3 dst_scale = p_transform.basis.get_scale(); - Quat dst_rot = p_transform.basis; + Quat dst_rot = p_transform.basis.get_rotation_quat(); Vector3 dst_loc = p_transform.origin; Transform dst; //this could be made faster by using a single function in Basis.. diff --git a/editor/import/editor_import_collada.cpp b/editor/import/editor_import_collada.cpp index 2fb3bf7b1e6..a13f741ee70 100644 --- a/editor/import/editor_import_collada.cpp +++ b/editor/import/editor_import_collada.cpp @@ -1785,8 +1785,7 @@ void ColladaImport::create_animation(int p_clip, bool p_make_tracks_in_all_bones } } - Quat q = xform.basis; - q.normalize(); + Quat q = xform.basis.get_rotation_quat(); Vector3 s = xform.basis.get_scale(); Vector3 l = xform.origin; @@ -1838,8 +1837,7 @@ void ColladaImport::create_animation(int p_clip, bool p_make_tracks_in_all_bones xform = sk->get_bone_rest(nm.bone).affine_inverse() * xform; - Quat q = xform.basis; - q.normalize(); + Quat q = xform.basis.get_rotation_quat(); Vector3 s = xform.basis.get_scale(); Vector3 l = xform.origin; diff --git a/editor/import/editor_scene_importer_gltf.cpp b/editor/import/editor_scene_importer_gltf.cpp index f4be6e8d59f..2b6a453d8fb 100644 --- a/editor/import/editor_scene_importer_gltf.cpp +++ b/editor/import/editor_scene_importer_gltf.cpp @@ -1983,8 +1983,7 @@ void EditorSceneImporterGLTF::_import_animation(GLTFState &state, AnimationPlaye int bone = node->joints[i].godot_bone_index; xform = skeleton->get_bone_rest(bone).affine_inverse() * xform; - rot = xform.basis; - rot.normalize(); + rot = xform.basis.get_rotation_quat(); scale = xform.basis.get_scale(); pos = xform.origin; }