Attempt to fix moving average quaternions not resetting properly (#1278)

SlimeVR · Jan 13, 2025 · 181ba08 · 181ba08
1 parent 394c1dd
commit 181ba08
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 35 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
@@ -23,7 +23,7 @@ class QuaternionMovingAverage(
 ) {
 	private var smoothFactor = 0f
 	private var predictFactor = 0f
-	private lateinit var rotBuffer: CircularArrayList<Quaternion>
+	private var rotBuffer: CircularArrayList<Quaternion>? = null
 	private var latestQuaternion = IDENTITY
 	private var smoothingQuaternion = IDENTITY
 	private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer()
@@ -57,11 +57,11 @@ class QuaternionMovingAverage(
 	@Synchronized
 	fun update() {
 		if (type == TrackerFilters.PREDICTION) {
-			if (rotBuffer.size > 0) {
+			if (rotBuffer!!.size > 0) {
 				var quatBuf = latestQuaternion
 
 				// Applies the past rotations to the current rotation
-				rotBuffer.forEach { quatBuf *= it }
+				rotBuffer?.forEach { quatBuf *= it }
 
 				// Calculate how much to slerp
 				val amt = predictFactor * fpsTimer.timePerFrame
@@ -98,12 +98,12 @@ class QuaternionMovingAverage(
 	@Synchronized
 	fun addQuaternion(q: Quaternion) {
 		if (type == TrackerFilters.PREDICTION) {
-			if (rotBuffer.size == rotBuffer.capacity()) {
-				rotBuffer.removeLast()
+			if (rotBuffer!!.size == rotBuffer!!.capacity()) {
+				rotBuffer?.removeLast()
 			}
 
 			// Gets and stores the rotation between the last 2 quaternions
-			rotBuffer.add(latestQuaternion.inv().times(q))
+			rotBuffer?.add(latestQuaternion.inv().times(q))
 		} else if (type == TrackerFilters.SMOOTHING) {
 			frameCounter = 0
 			lastAmt = 0f
@@ -116,8 +116,11 @@ class QuaternionMovingAverage(
 	}
 
 	fun resetQuats(q: Quaternion) {
+		if (type == TrackerFilters.PREDICTION) {
+			rotBuffer?.clear()
+			latestQuaternion = q
+		}
 		filteredQuaternion = q
-		latestQuaternion = q
-		smoothingQuaternion = q
+		addQuaternion(q)
 	}
 }
diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt
@@ -329,13 +329,7 @@ class Tracker @JvmOverloads constructor(
 	}
 
 	private fun getFilteredRotation(): Quaternion = if (trackRotDirection) {
-		if (filteringHandler.filteringEnabled) {
-			// Get filtered rotation
-			filteringHandler.getFilteredRotation()
-		} else {
-			// Get unfiltered rotation
-			filteringHandler.getTrackedRotation()
-		}
+		filteringHandler.getFilteredRotation()
 	} else {
 		// Get raw rotation
 		_rotation
@@ -433,6 +427,6 @@ class Tracker @JvmOverloads constructor(
 	 * Call when doing a full reset to reset the tracking of rotations >180 degrees
 	 */
 	fun resetFilteringQuats() {
-		filteringHandler.resetQuats(_rotation)
+		filteringHandler.resetMovingAverage(_rotation)
 	}
 }
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
@@ -11,9 +11,8 @@ import io.github.axisangles.ktmath.Quaternion
  * See QuaternionMovingAverage.kt for the quaternion math.
  */
 class TrackerFilteringHandler {
-
-	private var filteringMovingAverage: QuaternionMovingAverage? = null
-	private var trackingMovingAverage = QuaternionMovingAverage(TrackerFilters.NONE)
+	// 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
 
 	/**
@@ -22,14 +21,14 @@ class TrackerFilteringHandler {
 	fun readFilteringConfig(config: FiltersConfig, currentRawRotation: Quaternion) {
 		val type = TrackerFilters.getByConfigkey(config.type)
 		if (type == TrackerFilters.SMOOTHING || type == TrackerFilters.PREDICTION) {
-			filteringMovingAverage = QuaternionMovingAverage(
+			movingAverage = QuaternionMovingAverage(
 				type,
 				config.amount,
 				currentRawRotation,
 			)
 			filteringEnabled = true
 		} else {
-			filteringMovingAverage = null
+			movingAverage = QuaternionMovingAverage(TrackerFilters.NONE)
 			filteringEnabled = false
 		}
 	}
@@ -38,33 +37,25 @@ class TrackerFilteringHandler {
 	 * Update the moving average to make it smooth
 	 */
 	fun update() {
-		trackingMovingAverage.update()
-		filteringMovingAverage?.update()
+		movingAverage.update()
 	}
 
 	/**
 	 * Updates the latest rotation
 	 */
 	fun dataTick(currentRawRotation: Quaternion) {
-		trackingMovingAverage.addQuaternion(currentRawRotation)
-		filteringMovingAverage?.addQuaternion(currentRawRotation)
+		movingAverage.addQuaternion(currentRawRotation)
 	}
 
 	/**
 	 * Call when doing a full reset to reset the tracking of rotations >180 degrees
 	 */
-	fun resetQuats(currentRawRotation: Quaternion) {
-		trackingMovingAverage.resetQuats(currentRawRotation)
-		filteringMovingAverage?.resetQuats(currentRawRotation)
+	fun resetMovingAverage(currentRawRotation: Quaternion) {
+		movingAverage.resetQuats(currentRawRotation)
 	}
 
 	/**
-	 * Gets the tracked rotation from the moving average (allows >180 degrees)
-	 */
-	fun getTrackedRotation() = trackingMovingAverage.filteredQuaternion
-
-	/**
-	 * Get the filtered rotation from the moving average
+	 * Get the filtered rotation from the moving average (either prediction/smoothing or just >180 degs)
 	 */
-	fun getFilteredRotation() = filteringMovingAverage?.filteredQuaternion ?: Quaternion.IDENTITY
+	fun getFilteredRotation() = movingAverage.filteredQuaternion
 }