From ae1b60da5d61f4a61fd093d3fc36e28c6c9f081e Mon Sep 17 00:00:00 2001 From: Erimel Date: Sat, 6 Jan 2024 23:46:38 -0500 Subject: [PATCH] Prevent smoothing rollback (#921) --- .../filtering/QuaternionMovingAverage.kt | 54 ++++++++++++------- .../dev/slimevr/tracking/trackers/Tracker.kt | 2 +- .../trackers/TrackerFilteringHandler.kt | 7 +-- 3 files changed, 40 insertions(+), 23 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 5f38e285a4..b695037aab 100644 --- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt +++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt @@ -6,10 +6,10 @@ import io.github.axisangles.ktmath.Quaternion.Companion.IDENTITY // influences the range of smoothFactor. private const val SMOOTH_MULTIPLIER = 42f -private const val SMOOTH_MIN = 12f +private const val SMOOTH_MIN = 11f // influences the range of predictFactor -private const val PREDICT_MULTIPLIER = 14f +private const val PREDICT_MULTIPLIER = 15f private const val PREDICT_MIN = 10f // how many past rotations are used for prediction. @@ -27,16 +27,17 @@ class QuaternionMovingAverage( private var latestQuaternion = IDENTITY private var smoothingQuaternion = IDENTITY private val fpsTimer = VRServer.instance.fpsTimer - private var smoothingCounter = 0 + private var frameCounter = 0 + private var lastAmt = 0f init { // amount should range from 0 to 1. // GUI should clamp it from 0.01 (1%) or 0.1 (10%) // to 1 (100%). - amount = Math.max(amount, 0f) + amount = amount.coerceAtLeast(0f) if (type === TrackerFilters.SMOOTHING) { // lower smoothFactor = more smoothing - smoothFactor = SMOOTH_MULTIPLIER * (1 - Math.min(amount, 1f)) + SMOOTH_MIN + smoothFactor = SMOOTH_MULTIPLIER * (1 - amount.coerceAtMost(1f)) + SMOOTH_MIN // Totally a hack if (amount > 1) { smoothFactor /= amount @@ -53,27 +54,41 @@ class QuaternionMovingAverage( } // Runs at up to 1000hz. We use a timer to make it framerate-independent - // since it runs between 850hz to 900hz in practice. + // since it runs a bit below 1000hz in practice. @Synchronized fun update() { - if (type === TrackerFilters.PREDICTION) { + if (type == TrackerFilters.PREDICTION) { if (rotBuffer.size > 0) { var quatBuf = latestQuaternion // Applies the past rotations to the current rotation rotBuffer.forEach { quatBuf *= it } - // Slerps the target rotation to that predicted rotation by - // a certain factor. - filteredQuaternion = filteredQuaternion.interpR(quatBuf, predictFactor * (fpsTimer?.timePerFrame ?: 1f)) + // Calculate how much to slerp + val amt = predictFactor * fpsTimer.timePerFrame + + // Slerps the target rotation to that predicted rotation by amt + filteredQuaternion = filteredQuaternion.interpR(quatBuf, amt) } - } - if (type === TrackerFilters.SMOOTHING) { - // Calculate the slerp factor and limit it to 1 max - smoothingCounter++ - val amt = (smoothFactor * (fpsTimer?.timePerFrame ?: 1f) * smoothingCounter).coerceAtMost(1f) + } else { // Smoothing + // Increase every update for linear interpolation + frameCounter++ + + // Calculate the slerp factor based off the smoothFactor and smoothingCounter + var amt = smoothFactor * frameCounter + + // Make it framerate-independent + amt *= fpsTimer.timePerFrame - // Smooth towards the target rotation + // Be at least last amount to not rollback + amt = amt.coerceAtLeast(lastAmt) + + // limit to 1 to not overshoot + amt = amt.coerceAtMost(1f) + + lastAmt = amt + + // Smooth towards the target rotation by the slerp factor filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt) } } @@ -87,11 +102,12 @@ class QuaternionMovingAverage( // Gets and stores the rotation between the last 2 quaternions rotBuffer.add(latestQuaternion.inv().times(q)) - } - if (type === TrackerFilters.SMOOTHING) { - smoothingCounter = 0 + } else { // Smoothing + frameCounter = 0 + lastAmt = 0f smoothingQuaternion = filteredQuaternion } + latestQuaternion = 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 506312ec27..2e56843464 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 @@ -261,7 +261,7 @@ class Tracker @JvmOverloads constructor( status = TrackerStatus.TIMED_OUT } } - filteringHandler.tick() + filteringHandler.update() } /** 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 269b5b9f82..8a88d2df59 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 @@ -8,8 +8,9 @@ import io.github.axisangles.ktmath.Quaternion /** * Class taking care of filtering logic * (smoothing and prediction) + * See QuaternionMovingAverage.kt for the quaternion math. */ -class TrackerFilteringHandler() { +class TrackerFilteringHandler { private var movingAverage: QuaternionMovingAverage? = null var enabled = false @@ -33,9 +34,9 @@ class TrackerFilteringHandler() { } /** - * Update the moving average to make it smooth~ + * Update the moving average to make it smooth */ - fun tick() { + fun update() { movingAverage?.update() }