1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
|
// Copyright 2019 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
using System;
using UnityEngine;
namespace Mujoco {
// Utility class that represents a translation and rotation.
// Used for converting between MuJoCo and Unity spaces.
public struct MjTransformation {
public Vector3 Translation;
public Quaternion Rotation;
public MjTransformation(Vector3 translation, Quaternion rotation) {
Translation = translation;
Rotation = rotation;
}
public void Set(Vector3 translation, Quaternion rotation) {
Translation = translation;
Rotation = rotation;
}
// Initializes MjTransformation from the specified Transform's local coordinates.
public static MjTransformation LoadGlobal(Transform transform) {
if (transform != null) {
return new MjTransformation(transform.position, transform.rotation);
} else {
return new MjTransformation(Vector3.zero, Quaternion.identity);
}
}
// Initializes MjTransformation from the specified Transform's global coordinates.
public static MjTransformation LoadLocal(Transform transform) {
if (transform != null) {
return new MjTransformation(transform.localPosition, transform.localRotation);
} else {
return new MjTransformation(Vector3.zero, Quaternion.identity);
}
}
// Stores the transform coordinates in the specified Transform's global coordinates.
public void StoreGlobal(Transform transform) {
transform.position = Translation;
transform.rotation = Rotation;
}
// Stores the transform coordinates in the specified Transform's local coordinates.
public void StoreLocal(Transform transform) {
transform.localPosition = Translation;
transform.localRotation = Rotation;
}
// Concatenates two transforms.
// Equivalent of a matrix multiplication. This operation is not transitive.
public static MjTransformation operator *(
MjTransformation lhs, MjTransformation rhs) {
return new MjTransformation(
lhs.Translation + lhs.Rotation * rhs.Translation, lhs.Rotation * rhs.Rotation);
}
// Returns an inverse of this transform.
public MjTransformation Inverse() {
var inverseRotation = Quaternion.Inverse(Rotation);
return new MjTransformation(inverseRotation * Translation * -1.0f, inverseRotation);
}
public override string ToString() {
return $"[{Translation}, {Rotation}]";
}
}
}
|
