[Spine Yaw Compensation] Add small yaw rotations to keep spine tracke…

…rs aligned

gui/public/i18n/en/translation.ftl

@@ -403,6 +403,11 @@ settings-general-tracker_mechanics-use_mag_on_all_trackers-description =
     Uses magnetometer on all trackers that have a compatible firmware for it, reducing drift in stable magnetic environments.
     Can be disabled per tracker in the tracker's settings. <b>Please don't shutdown any of the trackers while toggling this!</b>
 settings-general-tracker_mechanics-use_mag_on_all_trackers-label = Use magnetometer on trackers
+settings-general-tracker_mechanics-spine_yaw_compensation = Keep Spine Aligned
+settings-general-tracker_mechanics-spine_yaw_compensation-description =
+    Prevents the spine trackers from drifting, by nudging them to stay aligned to the headset.
+    Players are usually standing facing forwards or walking forwards, where the spine trackers are aligned to the yaw of the headset. However due to yaw drift, the spine trackers will eventually rotate to the side, requiring a reset. This feature adds a small centering rotation to compensate for the yaw drift. (For now, this compensation only activates when your trackers are pointing "up", e.g. when you're standing or sitting straight up.)
+    The correction amount should be approximately the maximum yaw drift of your gyroscope (0.3 degrees/sec is a good default).
 
 ## FK/Tracking settings
 settings-general-fk_settings = Tracking settings

gui/src/components/settings/pages/GeneralSettings.tsx

@@ -16,6 +16,7 @@ import {
   SettingsResponseT,
   SteamVRTrackersSettingT,
   TapDetectionSettingsT,
+  YawCorrectionSettingsT,
 } from 'solarxr-protocol';
 import { useConfig } from '@/hooks/config';
 import { useWebsocketAPI } from '@/hooks/websocket-api';
@@ -101,6 +102,9 @@ interface SettingsForm {
     saveMountingReset: boolean;
     resetHmdPitch: boolean;
   };
+  yawCorrectionSettings: {
+    amountInDegPerSec: number;
+  };
 }
 
 const defaultValues: SettingsForm = {
@@ -165,6 +169,9 @@ const defaultValues: SettingsForm = {
     saveMountingReset: false,
     resetHmdPitch: false,
   },
+  yawCorrectionSettings: {
+    amountInDegPerSec: 0.0,
+  },
 };
 
 export function GeneralSettings() {
@@ -184,6 +191,10 @@ export function GeneralSettings() {
     unitDisplay: 'narrow',
     maximumFractionDigits: 2,
   });
+  const degreeFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'degree',
+  })
 
   const { sendRPCPacket, useRPCPacket } = useWebsocketAPI();
   const { reset, control, watch, handleSubmit, getValues, setValue } =
@@ -291,6 +302,10 @@ export function GeneralSettings() {
     driftCompensation.maxResets = values.driftCompensation.maxResets;
     settings.driftCompensation = driftCompensation;
 
+    const yawCorrectionSettings = new YawCorrectionSettingsT()
+    yawCorrectionSettings.amountInDegPerSec = values.yawCorrectionSettings.amountInDegPerSec;
+    settings.yawCorrectionSettings = yawCorrectionSettings
+
     if (values.resetsSettings) {
       const resetsSettings = new ResetsSettingsT();
       resetsSettings.resetMountingFeet =
@@ -414,6 +429,10 @@ export function GeneralSettings() {
       formData.resetsSettings = settings.resetsSettings;
     }
 
+    if (settings.yawCorrectionSettings) {
+      formData.yawCorrectionSettings = settings.yawCorrectionSettings;
+    }
+
     reset({ ...getValues(), ...formData });
   });
 
@@ -813,6 +832,33 @@ export function GeneralSettings() {
               settingType="general"
               id="mechanics-magnetometer"
             />
+            <div className="flex flex-col pt-4 pb-4"></div>
+            <Typography bold>{l10n.getString(
+                'settings-general-tracker_mechanics-spine_yaw_compensation'
+              )}
+            </Typography>
+            <div className="flex flex-col pt-2 pb-4">
+              {l10n
+                .getString(
+                  'settings-general-tracker_mechanics-spine_yaw_compensation-description'
+                )
+                .split('\n')
+                .map((line, i) => (
+                  <Typography color="secondary" key={i}>
+                    {line}
+                  </Typography>
+                ))}
+            </div>
+            <div className="flex md:flex-row flex-col">
+              <NumberSelector
+                control={control}
+                name="yawCorrectionSettings.amountInDegPerSec"
+                valueLabelFormat={(value) => degreeFormat.format(value)}
+                min={0.0}
+                max={2.0}
+                step={0.1}
+              />
+            </div>
           </>
         </SettingsPagePaneLayout>
         <SettingsPagePaneLayout

server/core/src/main/java/dev/slimevr/config/VRConfig.kt

@@ -40,6 +40,8 @@ class VRConfig {
 
 	val resetsConfig: ResetsConfig = ResetsConfig()
 
+	val yawCorrectionConfig = YawCorrectionConfig()
+
 	@JsonDeserialize(using = TrackerConfigMapDeserializer::class)
 	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer::class)
 	private val trackers: MutableMap<String, TrackerConfig> = HashMap()

server/core/src/main/java/dev/slimevr/config/YawCorrectionConfig.kt

@@ -0,0 +1,9 @@
+package dev.slimevr.config
+
+class YawCorrectionConfig {
+	private var _amountInDegPerSec = 0.0f
+
+	var amountInDegPerSec: Float
+		get() = _amountInDegPerSec
+		set(value) { _amountInDegPerSec = if (value > 0.0f) value else 0.0f }
+}

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java

@@ -349,6 +349,17 @@ public static int createArmsResetModeSettings(
 			);
 	}
 
+	public static int createYawCorrectionSettings(
+		FlatBufferBuilder fbb,
+		YawCorrectionConfig yawCorrectionConfig
+	) {
+		return YawCorrectionSettings
+			.createYawCorrectionSettings(
+				fbb,
+				yawCorrectionConfig.getAmountInDegPerSec()
+			);
+	}
+
 	public static int createSettingsResponse(FlatBufferBuilder fbb, VRServer server) {
 		ISteamVRBridge bridge = server.getVRBridge(ISteamVRBridge.class);
 
@@ -401,6 +412,11 @@ public static int createSettingsResponse(FlatBufferBuilder fbb, VRServer server)
 					.createArmsResetModeSettings(
 						fbb,
 						server.configManager.getVrConfig().getResetsConfig()
+					),
+				RPCSettingsBuilder
+					.createYawCorrectionSettings(
+						fbb,
+						server.configManager.getVrConfig().getYawCorrectionConfig()
 					)
 			);
 	}

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt

@@ -345,6 +345,14 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			resetsConfig.updateTrackersResetsSettings()
 		}
 
+		if (req.yawCorrectionSettings() != null) {
+			val yawCorrectionConfig = api.server.configManager
+				.vrConfig
+				.yawCorrectionConfig
+			yawCorrectionConfig.amountInDegPerSec = req.yawCorrectionSettings().amountInDegPerSec()
+			api.server.humanPoseManager.setYawCorrectionInDegPerSec(yawCorrectionConfig.amountInDegPerSec)
+		}
+
 		api.server.configManager.saveConfig()
 	}
 
@@ -361,7 +369,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			val settings = SettingsResponse
 				.createSettingsResponse(
 					fbb,
-					RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0,
+					RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 				)
 			val outbound =
 				rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)

server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt

@@ -286,6 +286,7 @@ class HumanPoseManager(val server: VRServer?) {
 
 	fun loadFromConfig(configManager: ConfigManager) {
 		skeletonConfigManager.loadFromConfig(configManager)
+		skeleton.yawCorrectionInDegPerSec = configManager.vrConfig.yawCorrectionConfig.amountInDegPerSec
 	}
 
 	@VRServerThread
@@ -663,6 +664,10 @@ class HumanPoseManager(val server: VRServer?) {
 		}
 	}
 
+	fun setYawCorrectionInDegPerSec(value: Float) {
+		skeleton.yawCorrectionInDegPerSec = value
+	}
+
 	fun setLegTweaksStateTemp(
 		skatingCorrection: Boolean,
 		floorClip: Boolean,

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt

@@ -13,6 +13,8 @@ import dev.slimevr.tracking.trackers.TrackerRole
 import dev.slimevr.tracking.trackers.TrackerUtils.getFirstAvailableTracker
 import dev.slimevr.tracking.trackers.TrackerUtils.getTrackerForSkeleton
 import dev.slimevr.util.ann.VRServerThread
+import io.eiren.math.FloatMath.toDegrees
+import io.eiren.math.FloatMath.toRad
 import io.eiren.util.ann.ThreadSafe
 import io.eiren.util.collections.FastList
 import io.eiren.util.logging.LogManager
@@ -28,6 +30,7 @@ import io.github.axisangles.ktmath.Vector3.Companion.NULL
 import io.github.axisangles.ktmath.Vector3.Companion.POS_Y
 import solarxr_protocol.rpc.StatusData
 import java.lang.IllegalArgumentException
+import kotlin.math.*
 import kotlin.properties.Delegates
 
 class HumanSkeleton(
@@ -201,6 +204,7 @@ class HumanSkeleton(
 
 	// Others
 	private var pauseTracking = false // Pauses skeleton tracking if true, resumes skeleton tracking if false
+	var yawCorrectionInDegPerSec = 0.0f
 
 	// Modules
 	var legTweaks = LegTweaks(this)
@@ -497,6 +501,7 @@ class HumanSkeleton(
 	fun updatePose() {
 		tapDetectionManager.update()
 
+		updateTrackerYawCorrections()
 		updateTransforms()
 		updateBones()
 		updateComputedTrackers()
@@ -519,6 +524,72 @@ class HumanSkeleton(
 		if (isTrackingRightArmFromController) rightHandTrackerBone.update()
 	}
 
+	private fun updateTrackerYawCorrections() {
+		val trackersToAlign = listOf(
+			headTracker,
+			upperChestTracker,
+			chestTracker,
+			waistTracker,
+			hipTracker
+		);
+
+		var parentTracker: Tracker? = null;
+		for (tracker in trackersToAlign) {
+			if (tracker == null) {
+				continue;
+			}
+
+			if (parentTracker == null) {
+				parentTracker = tracker;
+				continue;
+			}
+
+			updateTrackerYawCorrection(tracker, parentTracker);
+			parentTracker = tracker;
+		}
+	}
+
+	private fun isTrackerPointingUp(trackerRot: Quaternion, maxDeviationInRad: Float): Boolean {
+		val trackerUp = trackerRot.sandwich(POS_Y)
+		return trackerUp.angleTo(POS_Y) < maxDeviationInRad;
+	}
+
+	private fun updateTrackerYawCorrection(tracker: Tracker, parentTracker: Tracker) {
+		val trackerRot = tracker.getRotation()
+		val parentTrackerRot = parentTracker.getRotation()
+
+		// For now, we only handle trackers which are relatively "upright", i.e. in the
+		// standing position, or sitting position for trackers that are high up on the
+		// spine. When someone is lying down, they could be curled up such that the yaws
+		// don't necessarily align.
+		val maxDeviationInRad = toRad(30.0f)
+		if (!isTrackerPointingUp(trackerRot, maxDeviationInRad) ||
+			!isTrackerPointingUp(parentTrackerRot, maxDeviationInRad)) {
+			return;
+		}
+
+		val deltaRot = trackerRot * parentTrackerRot.inv()
+		val deltaYawInRad = deltaRot.toEulerAngles(EulerOrder.YZX).y
+
+		// Amount of yaw should be roughly the maximum yaw bias of the gyroscope. If it
+		// is too small, the gyroscope will overpower the correction and the skeleton
+		// will drift. If it is too big, the player will notice that the skeleton is
+		// rotating when the player doesn't face forward for a long time.
+		val serverUpdateFreqInHz = 1000.0f; // FIXME: Use actual server update rate
+		val adjustYawInRad = -sign(deltaYawInRad) * toRad(yawCorrectionInDegPerSec) / serverUpdateFreqInHz
+
+		// Adjust the tracker's yaw towards the parent tracker's yaw
+		tracker.yawCorrectionInRad += adjustYawInRad
+	}
+
+	private fun resetTrackerYawCorrections() {
+		for (tracker in trackersToReset) {
+			if (tracker != null) {
+				tracker.yawCorrectionInRad = 0.0f
+			}
+		}
+	}
+
 	/**
 	 * Update all the bones' transforms from trackers
 	 */
@@ -1488,6 +1559,7 @@ class HumanSkeleton(
 		}
 		legTweaks.resetBuffer()
 		localizer.reset()
+		resetTrackerYawCorrections();
 		LogManager.info("[HumanSkeleton] Reset: full ($resetSourceName)")
 	}
 
@@ -1509,6 +1581,7 @@ class HumanSkeleton(
 			}
 		}
 		legTweaks.resetBuffer()
+		resetTrackerYawCorrections();
 		LogManager.info("[HumanSkeleton] Reset: yaw ($resetSourceName)")
 	}
 
@@ -1537,6 +1610,7 @@ class HumanSkeleton(
 		}
 		legTweaks.resetBuffer()
 		localizer.reset()
+		resetTrackerYawCorrections();
 		LogManager.info("[HumanSkeleton] Reset: mounting ($resetSourceName)")
 	}
 
@@ -1553,6 +1627,7 @@ class HumanSkeleton(
 			}
 		}
 		legTweaks.resetBuffer()
+		resetTrackerYawCorrections();
 		LogManager.info("[HumanSkeleton] Clear: mounting ($resetSourceName)")
 	}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -6,6 +6,8 @@ import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
 import io.eiren.util.BufferedTimer
+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 solarxr_protocol.datatypes.DeviceIdT
@@ -83,6 +85,7 @@ class Tracker @JvmOverloads constructor(
 	var customName: String? = null
 	var magStatus: MagnetometerStatus = magStatus
 		private set
+	var yawCorrectionInRad: Float = 0.0f
 
 	/**
 	 * If the tracker has gotten disconnected after it was initialized first time
@@ -327,6 +330,8 @@ class Tracker @JvmOverloads constructor(
 			filteringHandler.getTrackedRotation()
 		}
 
+		rot = EulerAngles(EulerOrder.YZX, 0.0f, yawCorrectionInRad, 0.0f).toQuaternion() * rot
+
 		// Reset if needed and is not computed and internal
 		if (needsReset && !(isComputed && isInternal)) {
 			// Adjust to reset, mounting and drift compensation
@@ -359,6 +364,8 @@ class Tracker @JvmOverloads constructor(
 			filteringHandler.getTrackedRotation()
 		}
 
+		rot = EulerAngles(EulerOrder.YZX, 0.0f, yawCorrectionInRad, 0.0f).toQuaternion() * rot
+
 		// Reset if needed or is a computed tracker besides head
 		if (needsReset && !(isComputed && trackerPosition != TrackerPosition.HEAD)) {
 			// Adjust to reset and mounting