Fix OSCTrackers yawAlign (#818)

SlimeVR · Aug 13, 2023 · 2d35456 · 2d35456
1 parent 25da871
commit 2d35456
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 26 deletions.
diff --git a/server/core/src/main/java/dev/slimevr/osc/VRCOSCHandler.kt b/server/core/src/main/java/dev/slimevr/osc/VRCOSCHandler.kt
@@ -428,29 +428,25 @@ class VRCOSCHandler(
 	/**
 	 * Sends the expected HMD rotation upon reset to align the trackers in VRC
 	 */
-	fun yawAlign() {
+	fun yawAlign(headRot: Quaternion) {
 		if (oscSender != null && oscSender!!.isConnected) {
-			for (shareableTracker in computedTrackers) {
-				if (shareableTracker.trackerPosition === TrackerPosition.HEAD) {
-					val (_, _, y) = shareableTracker.getRotation().toEulerAngles(EulerOrder.XYZ)
-					oscArgs.clear()
-					oscArgs.add(0f)
-					oscArgs.add(-y * FastMath.RAD_TO_DEG)
-					oscArgs.add(0f)
-					oscMessage = OSCMessage(
-						"/tracking/trackers/head/rotation",
-						oscArgs
-					)
-					try {
-						oscSender!!.send(oscMessage)
-					} catch (e: IOException) {
-						LogManager
-							.warning("[VRCOSCHandler] Error sending OSC message to VRChat: $e")
-					} catch (e: OSCSerializeException) {
-						LogManager
-							.warning("[VRCOSCHandler] Error sending OSC message to VRChat: $e")
-					}
-				}
+			val (_, _, y, _) = headRot.toEulerAngles(EulerOrder.YXZ)
+			oscArgs.clear()
+			oscArgs.add(0f)
+			oscArgs.add(-y * FastMath.RAD_TO_DEG)
+			oscArgs.add(0f)
+			oscMessage = OSCMessage(
+				"/tracking/trackers/head/rotation",
+				oscArgs
+			)
+			try {
+				oscSender!!.send(oscMessage)
+			} catch (e: IOException) {
+				LogManager
+					.warning("[VRCOSCHandler] Error sending OSC message to VRChat: $e")
+			} catch (e: OSCSerializeException) {
+				LogManager
+					.warning("[VRCOSCHandler] Error sending OSC message to VRChat: $e")
 			}
 		}
 	}

diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.java b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.java
@@ -648,7 +648,17 @@ public void resetTrackersFull(String resetSourceName) {
 		if (isSkeletonPresent()) {
 			skeleton.resetTrackersFull(resetSourceName);
 			if (server != null) {
-				server.vrcOSCHandler.yawAlign();
+				if (skeleton.headTracker == null && skeleton.neckTracker == null) {
+					server.vrcOSCHandler.yawAlign(Quaternion.Companion.getIDENTITY());
+				} else {
+					server.vrcOSCHandler
+						.yawAlign(
+							getRootNode()
+								.getLocalTransform()
+								.getRotation()
+								.project(Vector3.Companion.getPOS_Y())
+						);
+				}
 				server
 					.getVMCHandler()
 					.alignVMCTracking(getRootNode().getWorldTransform().getRotation());
@@ -661,7 +671,17 @@ public void resetTrackersYaw(String resetSourceName) {
 		if (isSkeletonPresent()) {
 			skeleton.resetTrackersYaw(resetSourceName);
 			if (server != null) {
-				server.vrcOSCHandler.yawAlign();
+				if (skeleton.headTracker == null && skeleton.neckTracker == null) {
+					server.vrcOSCHandler.yawAlign(Quaternion.Companion.getIDENTITY());
+				} else {
+					server.vrcOSCHandler
+						.yawAlign(
+							getRootNode()
+								.getLocalTransform()
+								.getRotation()
+								.project(Vector3.Companion.getPOS_Y())
+						);
+				}
 				server
 					.getVMCHandler()
 					.alignVMCTracking(getRootNode().getWorldTransform().getRotation());

diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java
@@ -87,8 +87,8 @@ public class HumanSkeleton {
 		0
 	).toQuaternion();
 	// #region Tracker Input
-	protected Tracker headTracker;
-	protected Tracker neckTracker;
+	public Tracker headTracker;
+	public Tracker neckTracker;
 	protected Tracker upperChestTracker;
 	protected Tracker chestTracker;
 	protected Tracker waistTracker;