From eb23778bec5fcb4e7fb1252333a8cdd2c17cb467 Mon Sep 17 00:00:00 2001
From: Butterscotch! <bscotchvanilla@gmail.com>
Date: Sun, 16 Mar 2025 08:04:10 -0400
Subject: [PATCH] Fix moving average quaternion desync & smoothing (#1351)

---
 .../filtering/QuaternionMovingAverage.kt      | 35 ++++++-------------
 1 file changed, 10 insertions(+), 25 deletions(-)

diff --git a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
index 479263ba1..c92668986 100644
--- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
+++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
@@ -29,8 +29,7 @@ class QuaternionMovingAverage(
 	private var latestQuaternion = IDENTITY
 	private var smoothingQuaternion = IDENTITY
 	private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer()
-	private var frameCounter = 0
-	private var lastAmt = 0f
+	private var timeSinceUpdate = 0f
 
 	init {
 		// amount should range from 0 to 1.
@@ -71,28 +70,15 @@ class QuaternionMovingAverage(
 				filteredQuaternion = filteredQuaternion.interpR(quatBuf, amt)
 			}
 		} else if (type == TrackerFilters.SMOOTHING) {
-			// Increase every update for linear interpolation
-			frameCounter++
+			// Make it framerate-independent
+			timeSinceUpdate += fpsTimer.timePerFrame
 
 			// Calculate the slerp factor based off the smoothFactor and smoothingCounter
-			var amt = smoothFactor * frameCounter
-
-			// Make it framerate-independent
-			amt *= fpsTimer.timePerFrame
-
-			// Be at least last amount to not rollback
-			amt = amt.coerceAtLeast(lastAmt)
-
 			// limit to 1 to not overshoot
-			amt = amt.coerceAtMost(1f)
-
-			lastAmt = amt
+			val amt = (smoothFactor * timeSinceUpdate).coerceAtMost(1f)
 
 			// Smooth towards the target rotation by the slerp factor
 			filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt)
-		} else {
-			// No filtering; just keep track of rotations (for going over 180 degrees)
-			filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
 		}
 
 		filteringImpact = latestQuaternion.angleToR(filteredQuaternion)
@@ -108,21 +94,20 @@ class QuaternionMovingAverage(
 			// Gets and stores the rotation between the last 2 quaternions
 			rotBuffer?.add(latestQuaternion.inv().times(q))
 		} else if (type == TrackerFilters.SMOOTHING) {
-			frameCounter = 0
-			lastAmt = 0f
+			timeSinceUpdate = 0f
 			smoothingQuaternion = filteredQuaternion
 		} else {
-			smoothingQuaternion = filteredQuaternion
+			// No filtering; just keep track of rotations (for going over 180 degrees)
+			filteredQuaternion = q.twinNearest(filteredQuaternion)
 		}
 
 		latestQuaternion = q
 	}
 
+	@Synchronized
 	fun resetQuats(q: Quaternion) {
-		if (type == TrackerFilters.PREDICTION) {
-			rotBuffer?.clear()
-			latestQuaternion = q
-		}
+		rotBuffer?.clear()
+		latestQuaternion = q
 		filteredQuaternion = q
 		addQuaternion(q)
 	}