Fix MjWeld using wrong address in Unity plugin
This commit is contained in:
@@ -28,7 +28,7 @@ namespace Mujoco {
|
|||||||
protected override unsafe void OnBindToRuntime(MujocoLib.mjModel_* model, MujocoLib.mjData_* data) {
|
protected override unsafe void OnBindToRuntime(MujocoLib.mjModel_* model, MujocoLib.mjData_* data) {
|
||||||
if (WeldOffset) {
|
if (WeldOffset) {
|
||||||
MjEngineTool.SetMjTransform(
|
MjEngineTool.SetMjTransform(
|
||||||
MjEngineTool.MjTransformAtEntry(model->eq_data, MujocoId),
|
MjEngineTool.MjEqualityAtEntry(model->eq_data, MujocoId)+3,
|
||||||
WeldOffset.localPosition, WeldOffset.localRotation);
|
WeldOffset.localPosition, WeldOffset.localRotation);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -27,6 +27,7 @@ public static class MjEngineTool {
|
|||||||
private const int _elementsPerPosition = 3;
|
private const int _elementsPerPosition = 3;
|
||||||
private const int _elementsPerRotation = 4;
|
private const int _elementsPerRotation = 4;
|
||||||
private const int _elementsPerTransform = 7;
|
private const int _elementsPerTransform = 7;
|
||||||
|
private const int _elementsPerEquality = 11;
|
||||||
private static double[] _mjQuat = new double[4];
|
private static double[] _mjQuat = new double[4];
|
||||||
private static double[] _mjMat = new double[9];
|
private static double[] _mjMat = new double[9];
|
||||||
|
|
||||||
@@ -105,6 +106,11 @@ public static class MjEngineTool {
|
|||||||
public static unsafe double* MjMatrixAtEntry(double* mjTarget, int offsetEntry) {
|
public static unsafe double* MjMatrixAtEntry(double* mjTarget, int offsetEntry) {
|
||||||
return mjTarget + offsetEntry * _mjMat.Length;
|
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.
|
// Stores a unity transform (position+rotation) in a Mujoco target buffer.
|
||||||
public static unsafe void SetMjTransform(
|
public static unsafe void SetMjTransform(
|
||||||
|
|||||||
Reference in New Issue
Block a user