From 4d0f435733fece5a24c9246d4c7baff6017266fe Mon Sep 17 00:00:00 2001
From: Erimel <marioluigivideo@gmail.com>
Date: Sat, 18 Jan 2025 04:03:47 -0500
Subject: [PATCH] Reinstantiate QuaternionMovingAverage on reset

---
 .../filtering/QuaternionMovingAverage.kt      | 20 ++++++++-----------
 .../trackers/TrackerFilteringHandler.kt       |  6 +++---
 2 files changed, 11 insertions(+), 15 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 09517e692b..a13b4d6323 100644
--- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
+++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
@@ -26,6 +26,7 @@ class QuaternionMovingAverage(
 	private var rotBuffer: CircularArrayList<Quaternion>? = null
 	private var latestQuaternion = IDENTITY
 	private var smoothingQuaternion = IDENTITY
+	private var temporalQuaternion = IDENTITY
 	private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer()
 	private var frameCounter = 0
 	private var lastAmt = 0f
@@ -49,7 +50,11 @@ class QuaternionMovingAverage(
 			predictFactor = PREDICT_MULTIPLIER * amount + PREDICT_MIN
 			rotBuffer = CircularArrayList(PREDICT_BUFFER)
 		}
-		resetQuats(initialRotation)
+
+		filteredQuaternion = initialRotation
+		latestQuaternion = initialRotation
+		smoothingQuaternion = initialRotation
+		temporalQuaternion = initialRotation
 	}
 
 	// Runs at up to 1000hz. We use a timer to make it framerate-independent
@@ -91,7 +96,7 @@ class QuaternionMovingAverage(
 			filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt)
 		} else {
 			// No filtering; just keep track of rotations (for going over 180 degrees)
-			filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
+			filteredQuaternion = latestQuaternion.twinNearest(temporalQuaternion)
 		}
 	}
 
@@ -109,18 +114,9 @@ class QuaternionMovingAverage(
 			lastAmt = 0f
 			smoothingQuaternion = filteredQuaternion
 		} else {
-			smoothingQuaternion = filteredQuaternion
+			temporalQuaternion = filteredQuaternion
 		}
 
 		latestQuaternion = q
 	}
-
-	fun resetQuats(q: Quaternion) {
-		if (type == TrackerFilters.PREDICTION) {
-			rotBuffer?.clear()
-			latestQuaternion = q
-		}
-		filteredQuaternion = q
-		addQuaternion(q)
-	}
 }
diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt
index 87a6e0a805..1e33f1235d 100644
--- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt
@@ -13,7 +13,7 @@ import io.github.axisangles.ktmath.Quaternion
 class TrackerFilteringHandler {
 	// Instantiated by default in case config doesn't get read (if tracker doesn't support filtering)
 	private var movingAverage = QuaternionMovingAverage(TrackerFilters.NONE)
-	var filteringEnabled = false
+	private var filteringEnabled = false
 
 	/**
 	 * Reads/loads filtering settings from given config
@@ -28,7 +28,7 @@ class TrackerFilteringHandler {
 			)
 			filteringEnabled = true
 		} else {
-			movingAverage = QuaternionMovingAverage(TrackerFilters.NONE)
+			movingAverage = QuaternionMovingAverage(TrackerFilters.NONE, initialRotation = currentRawRotation)
 			filteringEnabled = false
 		}
 	}
@@ -51,7 +51,7 @@ class TrackerFilteringHandler {
 	 * Call when doing a full reset to reset the tracking of rotations >180 degrees
 	 */
 	fun resetMovingAverage(currentRawRotation: Quaternion) {
-		movingAverage.resetQuats(currentRawRotation)
+		movingAverage = QuaternionMovingAverage(movingAverage.type, movingAverage.amount, currentRawRotation)
 	}
 
 	/**