Quaternion to Euler correction

bulletphysics · Feb 27, 2024 · 2c79184 · 2c79184
1 parent 6bb8d11
commit 2c79184
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -12360,9 +12360,10 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
 					q[4] = pos[1];
 					q[5] = pos[2];
 				}
+				int quatToEulerCorrection =  baseDofQ ? -1 : 0; // The conversion to Euler angles drops the DOF by one
 				for (int i = 0; i < num_dofs; i++)
 				{
-					q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
+					q[i + baseDofQ + quatToEulerCorrection] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
 				}
 				for (int i = 0; i < num_dofs + baseDofQdot; i++)
 				{