Fix Inertia tensor update & Generic 6DOF Joint

2 files changed, 4 insertions, 3 deletions

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..551adc7d2e 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();