Prevent smoothing rollback (#921)

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
 	}
 }

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()
 	}
 
 	/**

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()
 	}