asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Merge pull request #11249 from m4nu3lf/bugfix/get_euler

Browse source 
Fix inertia tensor update & Generic6DOFJoint & Simplify Basis::get_euler()
This commit is contained in:
Rémi Verschelde 2017-10-31 23:03:01 +01:00 • committed by GitHub
commit cb3f594b14
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 17 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

25
core/math/matrix3.cpp
View file

@ -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. // 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. // 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 { Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(determinant() == 0, Vector3()); ERR_FAIL_COND_V(determinant() == 0, Vector3());
@ -371,31 +371,30 @@ Vector3 Basis::get_euler_xyz() const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(is_rotation() == false, euler); ERR_FAIL_COND_V(is_rotation() == false, euler);
#endif #endif
euler.y = Math::asin(elements[0][2]); real_t sy = elements[0][2];
if (euler.y < Math_PI * 0.5) { if (sy < 1.0) {
if (euler.y > -Math_PI * 0.5) { if (sy > -1.0) {
// is this a pure Y rotation? // 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) { 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.x = 0;
euler.y = atan2(elements[0][2], elements[0][0]); euler.y = atan2(elements[0][2], elements[0][0]);
euler.z = 0; euler.z = 0;
} else { } else {
euler.x = Math::atan2(-elements[1][2], elements[2][2]); 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]); euler.z = Math::atan2(-elements[0][1], elements[0][0]);
} }
} else { } 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.z = 0.0;
euler.x = euler.z - r;
} }
} else { } else {
real_t r = Math::atan2(elements[0][1], elements[1][1]); euler.x = Math::atan2(elements[0][1], elements[1][1]);
euler.z = 0; euler.y = Math_PI / 2.0;
euler.x = r - euler.z; euler.z = 0.0;
} }
return euler; return euler;
} }
@ -445,7 +444,7 @@ Vector3 Basis::get_euler_yxz() const {
if (m12 > -1) { if (m12 > -1) {
// is this a pure X rotation? // 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) { 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.x = atan2(-m12, elements[1][1]);
euler.y = 0; euler.y = 0;
euler.z = 0; euler.z = 0;

5
servers/physics/body_sw.cpp
View file

@ -45,8 +45,9 @@ void BodySW::_update_transform_dependant() {
// update inertia tensor // update inertia tensor
Basis tb = principal_inertia_axes; Basis tb = principal_inertia_axes;
Basis tbt = tb.transposed(); Basis tbt = tb.transposed();
tb.scale(_inv_inertia); Basis diag;
_inv_inertia_tensor = tb * tbt; diag.scale(_inv_inertia);
_inv_inertia_tensor = tb * diag * tbt;
} }
void BodySW::update_inertias() { void BodySW::update_inertias() {

4
servers/physics/joints/generic_6dof_joint_sw.cpp
View file

@ -219,9 +219,9 @@ Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform
} }
void Generic6DOFJointSW::calculateAngleInfo() { 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 // 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]) : // along the axes axis[0] and axis[2] (although we do use axis[1]) :