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 479263ba1f..606210e8e7 100644 --- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt +++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt @@ -28,6 +28,7 @@ class QuaternionMovingAverage( private var rotBuffer: CircularArrayList? = 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 @@ -50,7 +51,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 @@ -92,7 +97,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) } filteringImpact = latestQuaternion.angleToR(filteredQuaternion) @@ -112,18 +117,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 1ab9c2ecbb..2fb1f348a8 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) } /**