diff --git a/unity/Runtime/Components/Equality/MjWeld.cs b/unity/Runtime/Components/Equality/MjWeld.cs index 6eeb3479..2ea700ee 100644 --- a/unity/Runtime/Components/Equality/MjWeld.cs +++ b/unity/Runtime/Components/Equality/MjWeld.cs @@ -27,8 +27,9 @@ namespace Mujoco { protected override unsafe void OnBindToRuntime(MujocoLib.mjModel_* model, MujocoLib.mjData_* data) { if (WeldOffset) { - MjEngineTool.SetMjTransform(model->eq_data, WeldOffset.localPosition, - WeldOffset.localRotation, MujocoId); + MjEngineTool.SetMjTransform( + MjEngineTool.MjTransformAtEntry(model->eq_data, MujocoId), + WeldOffset.localPosition, WeldOffset.localRotation); } } diff --git a/unity/Runtime/Tools/MjEngineTool.cs b/unity/Runtime/Tools/MjEngineTool.cs index 1f5cd473..01e42538 100644 --- a/unity/Runtime/Tools/MjEngineTool.cs +++ b/unity/Runtime/Tools/MjEngineTool.cs @@ -92,20 +92,23 @@ public static class MjEngineTool { mjTarget[startOffset + 3] = mjQuat.z; } + // Returns a pointer to an entry in the MuJoCo field that's 7*offsetEntry down the buffer. + public static unsafe double* MjTransformAtEntry(double* mjTarget, int offsetEntry) { + return mjTarget + offsetEntry * _elementsPerTransform; + } + // Stores a unity transform (position+rotation) in a Mujoco target buffer. - // The vector coordinates will be stored at the 'entryIndex * 7' of the 'mjTarget' array. public static unsafe void SetMjTransform( - double* mjTarget, Vector3 unityVec, Quaternion unityQuat, int entryIndex) { - var startOffset = entryIndex * _elementsPerTransform; + double* mjTarget, Vector3 unityVec, Quaternion unityQuat) { var mjVec = MjVector3(unityVec); var mjQuat = MjQuaternion(unityQuat); - mjTarget[startOffset] = mjVec[0]; - mjTarget[startOffset + 1] = mjVec[1]; - mjTarget[startOffset + 2] = mjVec[2]; - mjTarget[startOffset + 3] = mjQuat.w; - mjTarget[startOffset + 4] = mjQuat.x; - mjTarget[startOffset + 5] = mjQuat.y; - mjTarget[startOffset + 6] = mjQuat.z; + mjTarget[0] = mjVec[0]; + mjTarget[1] = mjVec[1]; + mjTarget[2] = mjVec[2]; + mjTarget[3] = mjQuat.w; + mjTarget[4] = mjQuat.x; + mjTarget[5] = mjQuat.y; + mjTarget[6] = mjQuat.z; } // Converts a Unity Vector3 to a Mujoco vector.