mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 01:21:25 -08:00
Add local rotation calculations to TransformNodeWrapper
This commit is contained in:
2 files changed
+30
-13
No files matched your search
@@ -8,6 +8,7 @@ import java.io.IOException;
|
||||
import java.io.OutputStream;
|
||||
import java.io.OutputStreamWriter;
|
||||
|
||||
import com.jme3.math.FastMath;
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
|
||||
@@ -159,23 +160,27 @@ public class BVHFileStream extends PoseDataStream {
|
||||
|
||||
// Adjust to local rotation
|
||||
if (inverseRootRot != null) {
|
||||
rotBuf = inverseRootRot.mult(rotBuf, rotBuf);
|
||||
rotBuf = node.calculateLocalRotationInverse(inverseRootRot, rotBuf);
|
||||
}
|
||||
|
||||
// Yaw, roll, pitch
|
||||
angleBuf = rotBuf.toAngles(angleBuf);
|
||||
writer.write(Float.toString((float)Math.toDegrees(angleBuf[2])) + " " + Float.toString((float)Math.toDegrees(angleBuf[0])) + " " + Float.toString((float)Math.toDegrees(angleBuf[1])));
|
||||
// Output in order of Z, X, Y
|
||||
writer.write(Float.toString(angleBuf[2] * FastMath.RAD_TO_DEG) + " " + Float.toString(angleBuf[0] * FastMath.RAD_TO_DEG) + " " + Float.toString(-angleBuf[1] * FastMath.RAD_TO_DEG));
|
||||
|
||||
// Get inverse rotation for child local rotations
|
||||
Quaternion inverseRot = node.localTransform.getRotation().inverse();
|
||||
for (TransformNodeWrapper childNode : node.children) {
|
||||
if (childNode.children.isEmpty()) {
|
||||
// If it's an end node, skip
|
||||
continue;
|
||||
}
|
||||
if (!node.children.isEmpty()) {
|
||||
Quaternion inverseRot = node.worldTransform.getRotation().inverse();
|
||||
for (TransformNodeWrapper childNode : node.children) {
|
||||
if (childNode.children.isEmpty()) {
|
||||
// If it's an end node, skip
|
||||
continue;
|
||||
}
|
||||
|
||||
// Add spacing
|
||||
writer.write(" ");
|
||||
writeNodeHierarchyRotation(childNode, inverseRot);
|
||||
// Add spacing
|
||||
writer.write(" ");
|
||||
writeNodeHierarchyRotation(childNode, inverseRot);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -113,8 +113,20 @@ public class TransformNodeWrapper {
|
||||
return wrappedNode.localRotation;
|
||||
}
|
||||
|
||||
public Quaternion calculateLocalRotation () {
|
||||
throw new NotImplementedException("Local rotation calculation is not yet implemented");
|
||||
public Quaternion calculateLocalRotation (Quaternion result) {
|
||||
return calculateLocalRotation(worldTransform.getRotation(), result);
|
||||
}
|
||||
|
||||
public Quaternion calculateLocalRotation (Quaternion relativeTo, Quaternion result) {
|
||||
return calculateLocalRotationInverse(relativeTo.inverse(), result);
|
||||
}
|
||||
|
||||
public Quaternion calculateLocalRotationInverse (Quaternion inverseRelativeTo, Quaternion result) {
|
||||
if (result == null) {
|
||||
result = new Quaternion();
|
||||
}
|
||||
|
||||
return inverseRelativeTo.mult(worldTransform.getRotation(result), result);
|
||||
}
|
||||
|
||||
public void attachChild(TransformNodeWrapper node) {
|
||||
|
||||
Reference in new issue
Block a user