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) {
|
||||
if (WeldOffset) {
|
||||
MjEngineTool.SetMjTransform(
|
||||
MjEngineTool.MjTransformAtEntry(model->eq_data, MujocoId),
|
||||
MjEngineTool.MjEqualityAtEntry(model->eq_data, MujocoId)+3,
|
||||
WeldOffset.localPosition, WeldOffset.localRotation);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user