diff options
| -rw-r--r-- | core/math/matrix3.cpp | 25 | ||||
| -rw-r--r-- | servers/physics/body_sw.cpp | 5 | ||||
| -rw-r--r-- | servers/physics/joints/generic_6dof_joint_sw.cpp | 4 |
3 files changed, 17 insertions, 17 deletions
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index 85421c074b..ab3bca79ae 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -279,7 +279,7 @@ Vector3 Basis::get_signed_scale() const { // Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S. // Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3. -// This (internal) function is too specıfıc and named too ugly to expose to users, and probably there's no need to do so. +// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so. Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const { #ifdef MATH_CHECKS ERR_FAIL_COND_V(determinant() == 0, Vector3()); @@ -371,31 +371,30 @@ Vector3 Basis::get_euler_xyz() const { #ifdef MATH_CHECKS ERR_FAIL_COND_V(is_rotation() == false, euler); #endif - euler.y = Math::asin(elements[0][2]); - if (euler.y < Math_PI * 0.5) { - if (euler.y > -Math_PI * 0.5) { + real_t sy = elements[0][2]; + if (sy < 1.0) { + if (sy > -1.0) { // is this a pure Y rotation? if (elements[1][0] == 0.0 && elements[0][1] == 0.0 && elements[1][2] == 0 && elements[2][1] == 0 && elements[1][1] == 1) { - // return the simplest form + // return the simplest form (human friendlier in editor and scripts) euler.x = 0; euler.y = atan2(elements[0][2], elements[0][0]); euler.z = 0; } else { euler.x = Math::atan2(-elements[1][2], elements[2][2]); + euler.y = Math::asin(sy); euler.z = Math::atan2(-elements[0][1], elements[0][0]); } - } else { - real_t r = Math::atan2(elements[1][0], elements[1][1]); + euler.x = -Math::atan2(elements[0][1], elements[1][1]); + euler.y = -Math_PI / 2.0; euler.z = 0.0; - euler.x = euler.z - r; } } else { - real_t r = Math::atan2(elements[0][1], elements[1][1]); - euler.z = 0; - euler.x = r - euler.z; + euler.x = Math::atan2(elements[0][1], elements[1][1]); + euler.y = Math_PI / 2.0; + euler.z = 0.0; } - return euler; } @@ -445,7 +444,7 @@ Vector3 Basis::get_euler_yxz() const { if (m12 > -1) { // is this a pure X rotation? if (elements[1][0] == 0 && elements[0][1] == 0 && elements[0][2] == 0 && elements[2][0] == 0 && elements[0][0] == 1) { - // return the simplest form + // return the simplest form (human friendlier in editor and scripts) euler.x = atan2(-m12, elements[1][1]); euler.y = 0; euler.z = 0; diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 46a5192e52..6ced004118 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -45,8 +45,9 @@ void BodySW::_update_transform_dependant() { // update inertia tensor Basis tb = principal_inertia_axes; Basis tbt = tb.transposed(); - tb.scale(_inv_inertia); - _inv_inertia_tensor = tb * tbt; + Basis diag; + diag.scale(_inv_inertia); + _inv_inertia_tensor = tb * diag * tbt; } void BodySW::update_inertias() { diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp index 70cc549e2d..1e323be36c 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -219,9 +219,9 @@ Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform } void Generic6DOFJointSW::calculateAngleInfo() { - Basis relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis; + Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; - m_calculatedAxisAngleDiff = relative_frame.get_euler(); + m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : |