github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 161 Packages Projects Releases Wiki Activity

Complete TODO of replacing quatToXyzAngles with toEulerAngles (#915)

Browse Source 
Co-authored-by: Erimel <loukalemire@gmail.com>
This commit is contained in:
VocalFan
2024-01-06 23:39:43 -05:00
committed by GitHub
parent 5ff5f81a88
commit aaa5c2ba48
1 changed files with 7 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
server/core/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java
View File

@@ -3,6 +3,8 @@ package dev.slimevr.posestreamer;
import com.jme3.math.FastMath;
import dev.slimevr.tracking.processor.Bone;
import dev.slimevr.tracking.processor.skeleton.HumanSkeleton;
import io.github.axisangles.ktmath.EulerAngles;
import io.github.axisangles.ktmath.EulerOrder;
import io.github.axisangles.ktmath.Quaternion;
import io.github.axisangles.ktmath.Vector3;
import org.apache.commons.lang3.StringUtils;
@@ -19,8 +21,6 @@ public class BVHFileStream extends PoseDataStream {
private long frameCount = 0;
private long frameCountOffset;
private float[] angleBuf = new float[3];
public BVHFileStream(OutputStream outputStream) {
super(outputStream);
writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096);
@@ -174,41 +174,6 @@ public class BVHFileStream extends PoseDataStream {
writer.write("Frame Time: " + (streamer.getFrameInterval() / 1000d) + "\n");
}
// Roughly based off code from
// https://github.com/TrackLab/ViRe/blob/50a987eff4db31036b2ebaeb5a28983cd473f267/Assets/Scripts/BVH/BVHRecorder.cs
private float[] quatToXyzAngles(Quaternion q, float[] angles) {
if (angles == null) {
angles = new float[3];
} else if (angles.length != 3) {
throw new IllegalArgumentException("Angles array must have three elements");
}
float x = q.getX();
float y = q.getY();
float z = q.getZ();
float w = q.getW();
// Roll (X)
float sinrCosp = -2f * (x * y - w * z);
float cosrCosp = w * w - x * x + y * y - z * z;
angles[0] = FastMath.atan2(sinrCosp, cosrCosp);
// Pitch (Y)
float sinp = 2f * (y * z + w * x);
// Use 90 degrees if out of range
angles[1] = FastMath.abs(sinp) >= 1f
? FastMath.copysign(FastMath.PI / 2f, sinp)
: FastMath
.asin(sinp);
// Yaw (Z)
float sinyCosp = -2f * (x * z - w * y);
float cosyCosp = w * w - x * x - y * y + z * z;
angles[2] = FastMath.atan2(sinyCosp, cosyCosp);
return angles;
}
private void writeBoneHierarchyRotation(Bone bone, Quaternion inverseRootRot)
throws IOException {
Quaternion rot = bone.getGlobalRotation();
@@ -218,19 +183,17 @@ public class BVHFileStream extends PoseDataStream {
rot = inverseRootRot.times(rot);
}
// Roll (X), pitch (Y), yaw (Z) (intrinsic)
// TODO: `quatToXyzAngles` should no longer be needed, this should be
// changed to use `Quaternion.toEulerAngles()` instead
angleBuf = quatToXyzAngles(rot.unit(), angleBuf);
// Pitch (X), Yaw (Y), Roll (Z)
EulerAngles angles = rot.toEulerAngles(EulerOrder.ZXY);
// Output in order of roll (Z), pitch (X), yaw (Y) (extrinsic)
writer
.write(
angleBuf[0] * FastMath.RAD_TO_DEG
angles.getZ() * FastMath.RAD_TO_DEG
+ " "
+ angleBuf[1] * FastMath.RAD_TO_DEG
+ angles.getX() * FastMath.RAD_TO_DEG
+ " "
+ angleBuf[2] * FastMath.RAD_TO_DEG
+ angles.getY() * FastMath.RAD_TO_DEG
);
// Get inverse rotation for child local rotations