Add Yaw Reset Smoothing (#964)

Co-authored-by: Butterscotch! <bscotchvanilla@gmail.com>

gui/public/i18n/en/translation.ftl

@@ -311,6 +311,7 @@ settings-general-tracker_mechanics-filtering-type-smoothing-description = Smooth
 settings-general-tracker_mechanics-filtering-type-prediction = Prediction
 settings-general-tracker_mechanics-filtering-type-prediction-description = Reduces latency and makes movements more snappy, but may increase jitter.
 settings-general-tracker_mechanics-filtering-amount = Amount
+settings-general-tracker_mechanics-yaw-reset-smooth-time = Yaw reset smooth time (0s disables smoothing)
 settings-general-tracker_mechanics-drift_compensation = Drift compensation
 # This cares about multilines
 settings-general-tracker_mechanics-drift_compensation-description =

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

@@ -90,6 +90,7 @@ interface SettingsForm {
   resetsSettings: {
     resetMountingFeet: boolean;
     armsMountingResetMode: number;
+    yawResetSmoothTime: number;
   };
 }
 
@@ -145,6 +146,7 @@ const defaultValues = {
   resetsSettings: {
     resetMountingFeet: false,
     armsMountingResetMode: 0,
+    yawResetSmoothTime: 0.0,
   },
 };
 
@@ -159,6 +161,12 @@ export function GeneralSettings() {
     style: 'percent',
     maximumFractionDigits: 0,
   });
+  const secondsFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'second',
+    unitDisplay: 'narrow',
+    maximumFractionDigits: 2,
+  });
 
   const { sendRPCPacket, useRPCPacket } = useWebsocketAPI();
   const { reset, control, watch, handleSubmit, getValues, setValue } =
@@ -259,6 +267,8 @@ export function GeneralSettings() {
         values.resetsSettings.resetMountingFeet;
       resetsSettings.armsMountingResetMode =
         values.resetsSettings.armsMountingResetMode;
+      resetsSettings.yawResetSmoothTime =
+        values.resetsSettings.yawResetSmoothTime;
       settings.resetsSettings = resetsSettings;
     }
 
@@ -636,6 +646,19 @@ export function GeneralSettings() {
                 step={1}
               />
             </div>
+            <div className="flex gap-5 pt-5 md:flex-row flex-col">
+              <NumberSelector
+                control={control}
+                name="resetsSettings.yawResetSmoothTime"
+                label={l10n.getString(
+                  'settings-general-tracker_mechanics-yaw-reset-smooth-time'
+                )}
+                valueLabelFormat={(value) => secondsFormat.format(value)}
+                min={0.0}
+                max={0.5}
+                step={0.05}
+              />
+            </div>
           </>
         </SettingsPagePaneLayout>
         <SettingsPagePaneLayout
@@ -1086,7 +1109,7 @@ export function GeneralSettings() {
                 label={l10n.getString(
                   'settings-general-gesture_control-yawResetDelay'
                 )}
-                valueLabelFormat={(value) => `${Math.round(value * 10) / 10} s`}
+                valueLabelFormat={(value) => secondsFormat.format(value)}
                 min={0.2}
                 max={3.0}
                 step={0.2}
@@ -1097,7 +1120,7 @@ export function GeneralSettings() {
                 label={l10n.getString(
                   'settings-general-gesture_control-fullResetDelay'
                 )}
-                valueLabelFormat={(value) => `${Math.round(value * 10) / 10} s`}
+                valueLabelFormat={(value) => secondsFormat.format(value)}
                 min={0.2}
                 max={3.0}
                 step={0.2}
@@ -1108,7 +1131,7 @@ export function GeneralSettings() {
                 label={l10n.getString(
                   'settings-general-gesture_control-mountingResetDelay'
                 )}
-                valueLabelFormat={(value) => `${Math.round(value * 10) / 10} s`}
+                valueLabelFormat={(value) => secondsFormat.format(value)}
                 min={0.2}
                 max={3.0}
                 step={0.2}

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

@@ -37,10 +37,13 @@ class ResetsConfig {
 	// Reset mode used for the arms
 	var mode = ArmsResetModes.BACK
 
+	// Yaw reset smoothing time in seconds
+	var yawResetSmoothTime = 0.0f
+
 	fun updateTrackersResetsSettings() {
 		for (t in VRServer.instance.allTrackers) {
 			if (t.needsReset) {
-				t.resetsHandler.readArmsResetModeConfig(this)
+				t.resetsHandler.readResetConfig(this)
 			}
 		}
 	}

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

@@ -101,7 +101,7 @@ class VRConfig {
 			val config = getTracker(tracker)
 			tracker.readConfig(config)
 			if (tracker.isImu()) tracker.resetsHandler.readDriftCompensationConfig(driftCompensation)
-			if (tracker.needsReset) tracker.resetsHandler.readArmsResetModeConfig(resetsConfig)
+			if (tracker.needsReset) tracker.resetsHandler.readResetConfig(resetsConfig)
 			if (tracker.allowFiltering) {
 				tracker
 					.filteringHandler

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

@@ -338,7 +338,8 @@ public class RPCSettingsBuilder {
 			.createResetsSettings(
 				fbb,
 				resetsConfig.getResetMountingFeet(),
-				resetsConfig.getMode().getId()
+				resetsConfig.getMode().getId(),
+				resetsConfig.getYawResetSmoothTime()
 			);
 	}
 

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

@@ -334,6 +334,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 				resetsConfig.mode = mode
 			}
 			resetsConfig.resetMountingFeet = req.resetsSettings().resetMountingFeet()
+			resetsConfig.yawResetSmoothTime = req.resetsSettings().yawResetSmoothTime()
 			resetsConfig.updateTrackersResetsSettings()
 		}
 

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

@@ -260,6 +260,7 @@ class Tracker @JvmOverloads constructor(
 			}
 		}
 		filteringHandler.update()
+		resetsHandler.update()
 	}
 
 	/**

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

@@ -1,11 +1,13 @@
 package dev.slimevr.tracking.trackers
 
 import com.jme3.math.FastMath
+import com.jme3.system.NanoTimer
 import dev.slimevr.VRServer
 import dev.slimevr.config.ArmsResetModes
 import dev.slimevr.config.DriftCompensationConfig
 import dev.slimevr.config.ResetsConfig
 import dev.slimevr.filtering.CircularArrayList
+import io.eiren.math.FloatMath.animateEase
 import io.github.axisangles.ktmath.EulerAngles
 import io.github.axisangles.ktmath.EulerOrder
 import io.github.axisangles.ktmath.Quaternion
@@ -38,6 +40,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	private var driftCompensationEnabled = false
 	private var resetMountingFeet = false
 	private var armsResetMode = ArmsResetModes.BACK
+	private var yawResetSmoothTime = 0.0f
+	private lateinit var fpsTimer: NanoTimer
 	var allowDriftCompensation = false
 	var lastResetQuaternion: Quaternion? = null
 
@@ -56,6 +60,11 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		private set
 	private var yawFix = Quaternion.IDENTITY
 
+	// Yaw reset smoothing vars
+	private var yawFixOld = Quaternion.IDENTITY
+	private var yawFixSmoothIncremental = Quaternion.IDENTITY
+	private var yawResetSmoothTimeRemain = 0.0f
+
 	// Zero-reference/identity adjustment quats for IMU debugging
 	private var gyroFixNoMounting = Quaternion.IDENTITY
 	private var attachmentFixNoMounting = Quaternion.IDENTITY
@@ -105,16 +114,20 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	/**
 	 * Reads/loads arms reset mode settings from given config
 	 */
-	fun readArmsResetModeConfig(config: ResetsConfig) {
+	fun readResetConfig(config: ResetsConfig) {
 		resetMountingFeet = config.resetMountingFeet
 		armsResetMode = config.mode
+		yawResetSmoothTime = config.yawResetSmoothTime
+		if (!::fpsTimer.isInitialized) {
+			fpsTimer = VRServer.instance.fpsTimer
+		}
 	}
 
 	/**
 	 * Takes a rotation and adjusts it to resets, mounting,
 	 * and drift compensation, with the HMD as the reference.
 	 */
-	fun getReferenceAdjustedDriftRotationFrom(rotation: Quaternion): Quaternion = adjustToDrift(adjustToReference(rotation))
+	fun getReferenceAdjustedDriftRotationFrom(rotation: Quaternion): Quaternion = adjustToDrift(adjustToYawResetSmoothing(adjustToReference(rotation)))
 
 	/**
 	 * Takes a rotation and adjusts it to resets and mounting,
@@ -170,6 +183,17 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		return rotation
 	}
 
+	/**
+	 * Apply yaw reset smoothing to quaternion rotated to new yaw
+	 * fix and returns smoothed quaternion
+	 */
+	private fun adjustToYawResetSmoothing(rotation: Quaternion): Quaternion {
+		if (yawResetSmoothTimeRemain > 0.0f) {
+			return yawFixSmoothIncremental * rotation
+		}
+		return rotation
+	}
+
 	/**
 	 * Reset the tracker so that its current rotation is counted as (0, HMD Yaw,
 	 * 0). This allows the tracker to be strapped to body at any pitch and roll.
@@ -206,6 +230,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		makeIdentityAdjustmentQuatsFull()
 
 		yawFix = fixYaw(mountingAdjustedRotation, reference)
+		yawResetSmoothTimeRemain = 0.0f
 
 		calculateDrift(oldRot)
 
@@ -225,12 +250,20 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		val oldRot = adjustToReference(tracker.getRawRotation())
 		lastResetQuaternion = oldRot
 
+		yawFixOld = yawFix
 		yawFix = fixYaw(tracker.getRawRotation() * mountingOrientation, reference)
+		yawResetSmoothTimeRemain = 0.0f
 
 		makeIdentityAdjustmentQuatsYaw()
 
 		calculateDrift(oldRot)
 
+		// Start at yaw before reset if smoothing enabled
+		if (yawResetSmoothTime > 0.0f) {
+			yawResetSmoothTimeRemain = yawResetSmoothTime
+			yawFixSmoothIncremental = yawFixOld / yawFix
+		}
+
 		// Remove the status if yaw reset was performed after the tracker
 		// was disconnected and connected.
 		if (this.tracker.lastResetStatus != 0u && this.tracker.statusResetRecently) {
@@ -425,6 +458,28 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		return totalMatrix.toQuaternion()
 	}
 
+	/**
+	 * Update yaw reset smoothing time
+	 */
+	@Synchronized
+	fun update() {
+		if (yawResetSmoothTimeRemain > 0.0f) {
+			var deltaTime = 0.001f
+			if (::fpsTimer.isInitialized) {
+				deltaTime = fpsTimer.timePerFrame
+			}
+			yawResetSmoothTimeRemain = yawResetSmoothTimeRemain - deltaTime
+			if (yawResetSmoothTimeRemain > 0.0f) {
+				// Remaining time decreases to 0, so the interpolation is reversed
+				yawFixSmoothIncremental = yawFix.inv() * yawFix
+					.interpR(
+						yawFixOld,
+						animateEase(yawResetSmoothTimeRemain / yawResetSmoothTime),
+					)
+			}
+		}
+	}
+
 	private fun isThighTracker(): Boolean {
 		tracker.trackerPosition?.let {
 			return it == TrackerPosition.LEFT_UPPER_LEG ||