Fix MjWeld using wrong address in Unity plugin

This commit is contained in:
Bálint Hodossy
2022-09-17 17:35:09 +01:00
parent 142c0eb200
commit d0a628f464
2 changed files with 7 additions and 1 deletions
+1 -1
View File
@@ -28,7 +28,7 @@ namespace Mujoco {
protected override unsafe void OnBindToRuntime(MujocoLib.mjModel_* model, MujocoLib.mjData_* data) {
if (WeldOffset) {
MjEngineTool.SetMjTransform(
MjEngineTool.MjTransformAtEntry(model->eq_data, MujocoId),
MjEngineTool.MjEqualityAtEntry(model->eq_data, MujocoId)+3,
WeldOffset.localPosition, WeldOffset.localRotation);
}
}
+6
View File
@@ -27,6 +27,7 @@ public static class MjEngineTool {
private const int _elementsPerPosition = 3;
private const int _elementsPerRotation = 4;
private const int _elementsPerTransform = 7;
private const int _elementsPerEquality = 11;
private static double[] _mjQuat = new double[4];
private static double[] _mjMat = new double[9];
@@ -105,6 +106,11 @@ public static class MjEngineTool {
public static unsafe double* MjMatrixAtEntry(double* mjTarget, int offsetEntry) {
return mjTarget + offsetEntry * _mjMat.Length;
}
// Returns a pointer to an entry in the MuJoCo field that's 11*offsetEntry down the buffer.
public static unsafe double* MjEqualityAtEntry(double* mjTarget, int offsetEntry) {
return mjTarget + offsetEntry * _elementsPerEquality;
}
// Stores a unity transform (position+rotation) in a Mujoco target buffer.
public static unsafe void SetMjTransform(