From d4452e3a653c9539ea753cb152d82f30b7a9a126 Mon Sep 17 00:00:00 2001 From: m4nu3lf Date: Sat, 16 Sep 2017 17:50:46 +0100 Subject: [PATCH] Fix Inertia tensor update & Generic 6DOF Joint --- servers/physics/body_sw.cpp | 5 +++-- servers/physics/joints/generic_6dof_joint_sw.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 46a5192e526..6ced004118c 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 70cc549e2d5..551adc7d2ec 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -219,7 +219,7 @@ 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();