From 708cab9ea887c2a520c8a3cabd515b453b510801 Mon Sep 17 00:00:00 2001 From: Erimel Date: Wed, 26 Jun 2024 22:45:34 -0400 Subject: [PATCH] Properly track rotations over 180 degrees --- .../filtering/QuaternionMovingAverage.kt | 31 ++++--- .../dev/slimevr/tracking/trackers/Tracker.kt | 17 ++-- .../trackers/TrackerFilteringHandler.kt | 34 +++++-- .../tracking/trackers/TrackerResetsHandler.kt | 90 +++++++++++-------- 4 files changed, 110 insertions(+), 62 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 b695037aa..f069619ff 100644 --- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt +++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt @@ -17,10 +17,9 @@ private const val PREDICT_BUFFER = 6 class QuaternionMovingAverage( val type: TrackerFilters, - var amount: Float, - initialRotation: Quaternion, + var amount: Float = 0f, + initialRotation: Quaternion = IDENTITY, ) { - var filteredQuaternion = IDENTITY private var smoothFactor = 0f private var predictFactor = 0f private lateinit var rotBuffer: CircularArrayList @@ -29,13 +28,14 @@ class QuaternionMovingAverage( private val fpsTimer = VRServer.instance.fpsTimer private var frameCounter = 0 private var lastAmt = 0f + var filteredQuaternion = IDENTITY init { // amount should range from 0 to 1. // GUI should clamp it from 0.01 (1%) or 0.1 (10%) // to 1 (100%). amount = amount.coerceAtLeast(0f) - if (type === TrackerFilters.SMOOTHING) { + if (type == TrackerFilters.SMOOTHING) { // lower smoothFactor = more smoothing smoothFactor = SMOOTH_MULTIPLIER * (1 - amount.coerceAtMost(1f)) + SMOOTH_MIN // Totally a hack @@ -43,14 +43,12 @@ class QuaternionMovingAverage( smoothFactor /= amount } } - if (type === TrackerFilters.PREDICTION) { + if (type == TrackerFilters.PREDICTION) { // higher predictFactor = more prediction predictFactor = PREDICT_MULTIPLIER * amount + PREDICT_MIN rotBuffer = CircularArrayList(PREDICT_BUFFER) } - filteredQuaternion = initialRotation - latestQuaternion = initialRotation - smoothingQuaternion = initialRotation + resetQuats(initialRotation) } // Runs at up to 1000hz. We use a timer to make it framerate-independent @@ -70,7 +68,7 @@ class QuaternionMovingAverage( // Slerps the target rotation to that predicted rotation by amt filteredQuaternion = filteredQuaternion.interpR(quatBuf, amt) } - } else { // Smoothing + } else if (type == TrackerFilters.SMOOTHING) { // Increase every update for linear interpolation frameCounter++ @@ -90,24 +88,35 @@ class QuaternionMovingAverage( // Smooth towards the target rotation by the slerp factor filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt) + } else { + // No filtering; just keep track of rotations (for going over 180 degrees) + filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion) } } @Synchronized fun addQuaternion(q: Quaternion) { - if (type === TrackerFilters.PREDICTION) { + if (type == TrackerFilters.PREDICTION) { if (rotBuffer.size == rotBuffer.capacity()) { rotBuffer.removeLast() } // Gets and stores the rotation between the last 2 quaternions rotBuffer.add(latestQuaternion.inv().times(q)) - } else { // Smoothing + } else if (type == TrackerFilters.SMOOTHING) { frameCounter = 0 lastAmt = 0f smoothingQuaternion = filteredQuaternion + } else { + smoothingQuaternion = filteredQuaternion } latestQuaternion = q } + + fun resetQuats(q: Quaternion) { + filteredQuaternion = q + latestQuaternion = q + smoothingQuaternion = 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 99addabdd..4a09bd832 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 @@ -315,12 +315,12 @@ class Tracker @JvmOverloads constructor( * it too much should be avoided for performance reasons. */ fun getRotation(): Quaternion { - var rot = if (allowFiltering && filteringHandler.enabled) { + var rot = if (allowFiltering && filteringHandler.filteringEnabled) { // Get filtered rotation filteringHandler.getFilteredRotation() } else { // Get unfiltered rotation - _rotation + filteringHandler.getTrackedRotation() } // Reset if needed and is not computed and internal @@ -347,12 +347,12 @@ class Tracker @JvmOverloads constructor( * This is used for debugging/visualizing tracker data */ fun getIdentityAdjustedRotation(): Quaternion { - var rot = if (filteringHandler.enabled) { + var rot = if (filteringHandler.filteringEnabled) { // Get filtered rotation filteringHandler.getFilteredRotation() } else { // Get unfiltered rotation - _rotation + filteringHandler.getTrackedRotation() } // Reset if needed or is a computed tracker besides head @@ -368,7 +368,7 @@ class Tracker @JvmOverloads constructor( * Gets the raw (unadjusted) rotation of the tracker. * If this is an IMU, this will be the raw sensor rotation. */ - fun getRawRotation(): Quaternion = _rotation + fun getRawRotation() = _rotation /** * Sets the raw (unadjusted) rotation of the tracker. @@ -391,4 +391,11 @@ class Tracker @JvmOverloads constructor( */ val tps: Float get() = timer.averageFPS + + /** + * Call when doing a full reset to reset the tracking of rotations >180 degrees + */ + fun resetFilteringQuats() { + filteringHandler.resetQuats(_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 5e7a8ca12..e00f06b8a 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 @@ -12,8 +12,9 @@ import io.github.axisangles.ktmath.Quaternion */ class TrackerFilteringHandler { - private var movingAverage: QuaternionMovingAverage? = null - var enabled = false + private var filteringMovingAverage: QuaternionMovingAverage? = null + private var trackingMovingAverage = QuaternionMovingAverage(TrackerFilters.NONE) + var filteringEnabled = false /** * Reads/loads filtering settings from given config @@ -21,15 +22,15 @@ class TrackerFilteringHandler { fun readFilteringConfig(config: FiltersConfig, currentRawRotation: Quaternion) { val type = TrackerFilters.getByConfigkey(config.type) if (type == TrackerFilters.SMOOTHING || type == TrackerFilters.PREDICTION) { - movingAverage = QuaternionMovingAverage( + filteringMovingAverage = QuaternionMovingAverage( type, config.amount, currentRawRotation, ) - enabled = true + filteringEnabled = true } else { - movingAverage = null - enabled = false + filteringMovingAverage = null + filteringEnabled = false } } @@ -37,18 +38,33 @@ class TrackerFilteringHandler { * Update the moving average to make it smooth */ fun update() { - movingAverage?.update() + trackingMovingAverage.update() + filteringMovingAverage?.update() } /** * Updates the latest rotation */ fun dataTick(currentRawRotation: Quaternion) { - movingAverage?.addQuaternion(currentRawRotation) + trackingMovingAverage.addQuaternion(currentRawRotation) + filteringMovingAverage?.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) + } + + /** + * Gets the tracked rotation from the moving average (allows >180 degrees) + */ + fun getTrackedRotation() = trackingMovingAverage.filteredQuaternion + /** * Get the filtered rotation from the moving average */ - fun getFilteredRotation(): Quaternion = movingAverage?.filteredQuaternion ?: Quaternion.IDENTITY + fun getFilteredRotation() = filteringMovingAverage?.filteredQuaternion ?: Quaternion.IDENTITY } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index f0f99f8ae..8e8ac9da9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -214,9 +214,9 @@ class TrackerResetsHandler(val tracker: Tracker) { */ fun resetFull(reference: Quaternion) { // Adjust for T-Pose (down) - tposeDownFix = if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { + tposeDownFix = if (((isLeftArmTracker() || isLeftFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { EulerAngles(EulerOrder.YZX, 0f, 0f, -FastMath.HALF_PI).toQuaternion() - } else if ((isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { + } else if (((isRightArmTracker() || isRightFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { EulerAngles(EulerOrder.YZX, 0f, 0f, FastMath.HALF_PI).toQuaternion() } else { Quaternion.IDENTITY @@ -280,6 +280,8 @@ class TrackerResetsHandler(val tracker: Tracker) { VRServer.instance.statusSystem.removeStatus(this.tracker.lastResetStatus) this.tracker.lastResetStatus = 0u } + + tracker.resetFilteringQuats() } /** @@ -338,15 +340,17 @@ class TrackerResetsHandler(val tracker: Tracker) { // Calculate the yaw angle using tan var yawAngle = atan2(rotVector.x, rotVector.z) - // Adjust for T-Pose + // Adjust for T-Pose and fingers if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) || - (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) + (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) || + isLeftFingerTracker() ) { // Tracker goes right yawAngle -= FastMath.HALF_PI } if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) || - (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) + (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) || + isRightFingerTracker() ) { // Tracker goes left yawAngle += FastMath.HALF_PI @@ -535,22 +539,7 @@ class TrackerResetsHandler(val tracker: Tracker) { return it == TrackerPosition.LEFT_SHOULDER || it == TrackerPosition.LEFT_UPPER_ARM || it == TrackerPosition.LEFT_LOWER_ARM || - it == TrackerPosition.LEFT_HAND || - it == TrackerPosition.LEFT_THUMB_PROXIMAL || - it == TrackerPosition.LEFT_THUMB_INTERMEDIATE || - it == TrackerPosition.LEFT_THUMB_DISTAL || - it == TrackerPosition.LEFT_INDEX_PROXIMAL || - it == TrackerPosition.LEFT_INDEX_INTERMEDIATE || - it == TrackerPosition.LEFT_INDEX_DISTAL || - it == TrackerPosition.LEFT_MIDDLE_PROXIMAL || - it == TrackerPosition.LEFT_MIDDLE_INTERMEDIATE || - it == TrackerPosition.LEFT_MIDDLE_DISTAL || - it == TrackerPosition.LEFT_RING_PROXIMAL || - it == TrackerPosition.LEFT_RING_INTERMEDIATE || - it == TrackerPosition.LEFT_RING_DISTAL || - it == TrackerPosition.LEFT_LITTLE_PROXIMAL || - it == TrackerPosition.LEFT_LITTLE_INTERMEDIATE || - it == TrackerPosition.LEFT_LITTLE_DISTAL + it == TrackerPosition.LEFT_HAND } return false } @@ -560,22 +549,7 @@ class TrackerResetsHandler(val tracker: Tracker) { return it == TrackerPosition.RIGHT_SHOULDER || it == TrackerPosition.RIGHT_UPPER_ARM || it == TrackerPosition.RIGHT_LOWER_ARM || - it == TrackerPosition.RIGHT_HAND || - it == TrackerPosition.RIGHT_THUMB_PROXIMAL || - it == TrackerPosition.RIGHT_THUMB_INTERMEDIATE || - it == TrackerPosition.RIGHT_THUMB_DISTAL || - it == TrackerPosition.RIGHT_INDEX_PROXIMAL || - it == TrackerPosition.RIGHT_INDEX_INTERMEDIATE || - it == TrackerPosition.RIGHT_INDEX_DISTAL || - it == TrackerPosition.RIGHT_MIDDLE_PROXIMAL || - it == TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE || - it == TrackerPosition.RIGHT_MIDDLE_DISTAL || - it == TrackerPosition.RIGHT_RING_PROXIMAL || - it == TrackerPosition.RIGHT_RING_INTERMEDIATE || - it == TrackerPosition.RIGHT_RING_DISTAL || - it == TrackerPosition.RIGHT_LITTLE_PROXIMAL || - it == TrackerPosition.RIGHT_LITTLE_INTERMEDIATE || - it == TrackerPosition.RIGHT_LITTLE_DISTAL + it == TrackerPosition.RIGHT_HAND } return false } @@ -603,4 +577,46 @@ class TrackerResetsHandler(val tracker: Tracker) { } return false } + + private fun isLeftFingerTracker(): Boolean { + tracker.trackerPosition?.let { + return it == TrackerPosition.LEFT_THUMB_PROXIMAL || + it == TrackerPosition.LEFT_THUMB_INTERMEDIATE || + it == TrackerPosition.LEFT_THUMB_DISTAL || + it == TrackerPosition.LEFT_INDEX_PROXIMAL || + it == TrackerPosition.LEFT_INDEX_INTERMEDIATE || + it == TrackerPosition.LEFT_INDEX_DISTAL || + it == TrackerPosition.LEFT_MIDDLE_PROXIMAL || + it == TrackerPosition.LEFT_MIDDLE_INTERMEDIATE || + it == TrackerPosition.LEFT_MIDDLE_DISTAL || + it == TrackerPosition.LEFT_RING_PROXIMAL || + it == TrackerPosition.LEFT_RING_INTERMEDIATE || + it == TrackerPosition.LEFT_RING_DISTAL || + it == TrackerPosition.LEFT_LITTLE_PROXIMAL || + it == TrackerPosition.LEFT_LITTLE_INTERMEDIATE || + it == TrackerPosition.LEFT_LITTLE_DISTAL + } + return false + } + + private fun isRightFingerTracker(): Boolean { + tracker.trackerPosition?.let { + return it == TrackerPosition.RIGHT_THUMB_PROXIMAL || + it == TrackerPosition.RIGHT_THUMB_INTERMEDIATE || + it == TrackerPosition.RIGHT_THUMB_DISTAL || + it == TrackerPosition.RIGHT_INDEX_PROXIMAL || + it == TrackerPosition.RIGHT_INDEX_INTERMEDIATE || + it == TrackerPosition.RIGHT_INDEX_DISTAL || + it == TrackerPosition.RIGHT_MIDDLE_PROXIMAL || + it == TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE || + it == TrackerPosition.RIGHT_MIDDLE_DISTAL || + it == TrackerPosition.RIGHT_RING_PROXIMAL || + it == TrackerPosition.RIGHT_RING_INTERMEDIATE || + it == TrackerPosition.RIGHT_RING_DISTAL || + it == TrackerPosition.RIGHT_LITTLE_PROXIMAL || + it == TrackerPosition.RIGHT_LITTLE_INTERMEDIATE || + it == TrackerPosition.RIGHT_LITTLE_DISTAL + } + return false + } }