From 181ba089c27e77f1c1496936d14977c5b2242eee Mon Sep 17 00:00:00 2001 From: Erimel Date: Mon, 13 Jan 2025 17:24:22 -0500 Subject: [PATCH] Attempt to fix moving average quaternions not resetting properly (#1278) --- .../filtering/QuaternionMovingAverage.kt | 19 +++++++----- .../dev/slimevr/tracking/trackers/Tracker.kt | 10 ++----- .../trackers/TrackerFilteringHandler.kt | 29 +++++++------------ 3 files changed, 23 insertions(+), 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 index f86c4525d8..09517e692b 100644 --- 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 + private var rotBuffer: CircularArrayList? = 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 index d50c091b83..16ff5193dc 100644 --- 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 index e00f06b8a4..87a6e0a805 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 @@ -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 }