From b13754b1e0933b4a367bb5bb679fdf2ab43cd942 Mon Sep 17 00:00:00 2001 From: Erimel Date: Mon, 26 Oct 2026 17:44:28 -0400 Subject: [PATCH] boneOffsets all the way and wip improving smoothness --- gui/app/public/i18n/en/translation.ftl | 2 +- gui/app/src/hooks/datafeed-config.ts | 1 - .../dev/slimevr/hid/behaviours/register.kt | 1 - .../inputprocessors/rotation-prediction.kt | 50 ++++++++----------- .../inputprocessors/rotation-smoothing.kt | 6 +-- .../inputprocessors/tracker-offset.kt | 6 +-- .../skeleton/inputprocessors/upsampling.kt | 4 +- .../main/java/dev/slimevr/skeleton/module.kt | 20 ++++---- .../main/java/dev/slimevr/skeleton/reducer.kt | 8 +-- .../main/java/dev/slimevr/solarxr/common.kt | 1 - .../slimevr/tracker/behaviours/to-skeleton.kt | 2 +- .../dev/slimevr/tracker/behaviours/tps.kt | 14 +++--- .../main/java/dev/slimevr/tracker/module.kt | 4 -- .../main/java/dev/slimevr/udp/behaviours.kt | 1 - .../java/dev/slimevr/vmc/InputDecoderTest.kt | 2 +- .../java/dev/slimevr/vmc/OutputEncoderTest.kt | 2 +- solarxr-protocol | 2 +- 17 files changed, 55 insertions(+), 71 deletions(-) diff --git a/gui/app/public/i18n/en/translation.ftl b/gui/app/public/i18n/en/translation.ftl index 9e5220b62..9ecbb0dc6 100644 --- a/gui/app/public/i18n/en/translation.ftl +++ b/gui/app/public/i18n/en/translation.ftl @@ -360,7 +360,7 @@ tracker-settings-use_mag-label = Allow magnetometer # The . means it's an attribute and it's related to the top key. # In this case that is the settings for the assignment section. tracker-settings-bone_offset_section = Bone offset -tracker-settings-bone_offset_section-description = Offset of the tracker from its bone, in meters. +tracker-settings-bone_offset_section-description = Offset of the bone from this tracker, in meters. tracker-settings-bone_offset_section-x = X tracker-settings-bone_offset_section-y = Y tracker-settings-bone_offset_section-z = Z diff --git a/gui/app/src/hooks/datafeed-config.ts b/gui/app/src/hooks/datafeed-config.ts index 76a666c2e..c606ef2f8 100644 --- a/gui/app/src/hooks/datafeed-config.ts +++ b/gui/app/src/hooks/datafeed-config.ts @@ -74,7 +74,6 @@ export function useBonesDataFeedConfig() { boneMask.tailPosition = false; boneMask.angularVelocity = false; boneMask.linearVelocity = false; - boneMask.trackerOffset = false; dataFeedConfig.boneMask = boneMask; dataFeedConfig.minimumTimeSinceLast = 1000 / feedMaxTps; return dataFeedConfig; diff --git a/server/core/src/main/java/dev/slimevr/hid/behaviours/register.kt b/server/core/src/main/java/dev/slimevr/hid/behaviours/register.kt index 1d2c26a60..4b4f01d13 100644 --- a/server/core/src/main/java/dev/slimevr/hid/behaviours/register.kt +++ b/server/core/src/main/java/dev/slimevr/hid/behaviours/register.kt @@ -67,7 +67,6 @@ internal fun ensureHidTracker(receiver: HIDReceiver, hidId: Int, deviceId: Int, hardwareId = deviceState.address, origin = DeviceOrigin.HID, appContext = receiver.appContext, - expectedTps = 100u, // TODO: receive from firmware ) receiver.appContext.server.context.dispatch(VRServerActions.NewTracker(trackerId, newTracker)) newTracker.context.dispatch(TrackerActions.SetStatus(TrackerStatus.OK)) diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-prediction.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-prediction.kt index b75bf842b..003f6f885 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-prediction.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-prediction.kt @@ -14,6 +14,9 @@ import solarxr_protocol.rpc.FilteringType import solarxr_protocol.rpc.ResetType import kotlin.time.Duration.Companion.milliseconds +/** How long the running average of rotation deltas used for predictions is */ +val RUNNING_AVERAGE_WINDOW = 10.milliseconds + /** How far ahead the prediction reaches at amount 1, scaled down by the configured amount */ val PREDICTION_LEAD = 12.milliseconds @@ -29,20 +32,19 @@ class RotationPredictionInputProcessor(val settings: Settings) : val lastChange: MonotonicValueTimeMark, ) - private var deltas: BodyPartMap = bodyPartMap() + // Used to minimize latency even more for arm tracking where it is more noticeable in VR. + private val PART_MULTIPLIER = BodyPartMap( + mapOf( + BodyPart.LEFT_SHOULDER to 1.4f, + BodyPart.RIGHT_SHOULDER to 1.4f, + BodyPart.LEFT_UPPER_ARM to 1.4f, + BodyPart.RIGHT_UPPER_ARM to 1.4f, + BodyPart.LEFT_LOWER_ARM to 1.4f, + BodyPart.RIGHT_LOWER_ARM to 1.4f, + ) + ) - // Used to minimize latency even more for elbow tracking in VR where it is more noticeable. - private fun getMultiplier(bodyPart: BodyPart) = when (bodyPart) { - BodyPart.LEFT_SHOULDER, - BodyPart.RIGHT_SHOULDER, - BodyPart.LEFT_UPPER_ARM, - BodyPart.RIGHT_UPPER_ARM, - BodyPart.LEFT_LOWER_ARM, - BodyPart.RIGHT_LOWER_ARM, - -> 1.4f - - else -> 1f - } + private val deltas: BodyPartMap = bodyPartMap() override fun process(mutableInputSkeleton: InputSkeleton, skeletonHeight: Float) { val config = settings.context.state.value.data.skeletonConfig.filtering @@ -54,32 +56,24 @@ class RotationPredictionInputProcessor(val settings: Settings) : } val now = timeSource.markNow() - val newVelocities = bodyPartMap() for ((bodyPart, bone) in mutableInputSkeleton) { if (!bone.isRotationActive) continue val prev = deltas[bodyPart] if (prev == null) { - newVelocities[bodyPart] = BoneDelta(bone.rotation, Quaternion.IDENTITY, now) + deltas[bodyPart] = BoneDelta(bone.rotation, Quaternion.IDENTITY, now) continue } - val bonePredictionAmount = filteringAmount * getMultiplier(bodyPart) + val leadScale = (PREDICTION_LEAD / (now - prev.lastChange)).toFloat() + val rotationDelta = (bone.rotation / prev.lastRotation).pow(leadScale) + deltas[bodyPart] = BoneDelta(bone.rotation, rotationDelta, now) - val changed = bone.rotation !== prev.lastRotation - val rotationDelta = if (changed) { - val leadScale = (PREDICTION_LEAD / (now - prev.lastChange)).toFloat() - Quaternion.IDENTITY.lerpR(bone.rotation * prev.lastRotation.inv(), leadScale).unit() - } else { - prev.rotationDelta - } - - newVelocities[bodyPart] = BoneDelta(bone.rotation, rotationDelta, if (changed) now else prev.lastChange) - val scaledDelta = Quaternion.IDENTITY.lerpR(rotationDelta, bonePredictionAmount).unit() - val predicted = (scaledDelta * bone.rotation).unit() + val bonePredictionAmount = filteringAmount * (PART_MULTIPLIER[bodyPart] ?: 1f) + val scaledDelta = rotationDelta.pow(bonePredictionAmount) + val predicted = scaledDelta * bone.rotation if (predicted != bone.rotation) mutableInputSkeleton[bodyPart] = bone.copy(rotation = predicted) } - deltas = newVelocities } override fun reset(resetType: ResetType) { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-smoothing.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-smoothing.kt index d08e94c45..1a427b7b4 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-smoothing.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/rotation-smoothing.kt @@ -22,7 +22,7 @@ private const val SMOOTH_MAX = 0.92f class RotationSmoothingInputProcessor(val settings: Settings) : SkeletonInputProcessor, ResettableSkeletonProcessor { - private var smoothed: BodyPartMap = bodyPartMap() + private val smoothed: BodyPartMap = bodyPartMap() private var lastProcessTime = timeSource.markNow() override fun process(mutableInputSkeleton: InputSkeleton, skeletonHeight: Float) { @@ -42,16 +42,14 @@ class RotationSmoothingInputProcessor(val settings: Settings) : val smoothingAmount = SMOOTH_MIN + filteringAmount * (SMOOTH_MAX - SMOOTH_MIN) val alpha = ((1 - smoothingAmount) * lastFrameTimeSeconds * SMOOTHING_MULTIPLIER).coerceIn(0f, 1f) - val newSmoothed = bodyPartMap() for ((bodyPart, bone) in mutableInputSkeleton) { if (!bone.isRotationActive) continue val prev = smoothed[bodyPart] ?: bone.rotation val rotation = prev.lerpQ(bone.rotation, alpha).unit() - newSmoothed[bodyPart] = rotation + smoothed[bodyPart] = rotation if (rotation != bone.rotation) mutableInputSkeleton[bodyPart] = bone.copy(rotation = rotation) } - smoothed = newSmoothed } override fun reset(resetType: ResetType) { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/tracker-offset.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/tracker-offset.kt index c71711e4f..7c05df406 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/tracker-offset.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/tracker-offset.kt @@ -6,11 +6,11 @@ import dev.slimevr.skeleton.SkeletonInputProcessor /** * Handles tracker offset. For example, an HMD's rotation will be about 10cm in front of the head bone. */ -class TrackerOffsetInputProcessor : SkeletonInputProcessor { +class BoneOffsetInputProcessor : SkeletonInputProcessor { override fun process(mutableInputSkeleton: InputSkeleton, skeletonHeight: Float) { for ((bodyPart, bone) in mutableInputSkeleton) { - if (bone.trackerOffset.isZero()) continue - val localOffset = bone.rotation.sandwich(bone.trackerOffset) + if (bone.boneOffset.isZero()) continue + val localOffset = bone.rotation.sandwich(bone.boneOffset) mutableInputSkeleton[bodyPart] = bone.copy(position = bone.position?.plus(localOffset)) } } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/upsampling.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/upsampling.kt index c6bc40034..0c4ad85db 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/upsampling.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/upsampling.kt @@ -42,10 +42,10 @@ class UpsamplingInputProcessor : lastProcessTime = timeSource.markNow() for ((bodyPart, bone) in mutableInputSkeleton) { - if (bone.expectedTps == null) continue + if (bone.tps == null) continue // For a 100hz tracker on 500hz skeleton, this will be 0.2f (interpolating 1/5 every frame) - val interpolationRatio = (deltaTime * bone.expectedTps.toFloat()).coerceAtMost(1f) + val interpolationRatio = (deltaTime * bone.tps.toFloat()).coerceAtMost(1f) val newBone = interpolationData[bodyPart]?.let { interpolation -> // Interpolate rotation, acceleration and position. val rotation = if (bone.isRotationActive) { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/module.kt b/server/core/src/main/java/dev/slimevr/skeleton/module.kt index a2596b9d5..8b5737d76 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -18,7 +18,7 @@ import dev.slimevr.skeleton.inputprocessors.RotationPredictionInputProcessor import dev.slimevr.skeleton.inputprocessors.RotationSmoothingInputProcessor import dev.slimevr.skeleton.inputprocessors.SpineInputProcessor import dev.slimevr.skeleton.inputprocessors.ToesInputProcessor -import dev.slimevr.skeleton.inputprocessors.TrackerOffsetInputProcessor +import dev.slimevr.skeleton.inputprocessors.BoneOffsetInputProcessor import dev.slimevr.skeleton.inputprocessors.UpperLegsRollAlignInputProcessor import dev.slimevr.skeleton.inputprocessors.UpsamplingInputProcessor import dev.slimevr.skeleton.targetprocessors.FloorClipTargetProcessor @@ -48,8 +48,8 @@ data class BoneInput( val bodyPart: BodyPart, val headOffset: Vector3, val offset: Vector3, - val trackerOffset: Vector3, - val expectedTps: UShort?, + val boneOffset: Vector3, + val tps: UShort?, val rotation: Quaternion, val acceleration: Vector3, val position: Vector3?, @@ -64,7 +64,7 @@ data class BoneState( val bodyPart: BodyPart, val headOffset: Vector3, val offset: Vector3, - val trackerOffset: Vector3, + val boneOffset: Vector3, val rotation: Quaternion, val acceleration: Vector3, val headPosition: Vector3, @@ -114,8 +114,8 @@ val DEFAULT_BONE_INPUT = BoneInput( bodyPart = BodyPart.NONE, headOffset = Vector3.ZERO, offset = Vector3.ZERO, - trackerOffset = Vector3.ZERO, - expectedTps = null, + boneOffset = Vector3.ZERO, + tps = null, rotation = Quaternion.IDENTITY, acceleration = Vector3.ZERO, position = null, @@ -155,7 +155,7 @@ fun buildBone(bone: BoneInput, parentBone: BoneState?, velocity: Velocity = ZERO bodyPart = bone.bodyPart, headOffset = bone.headOffset, offset = bone.offset, - trackerOffset = bone.trackerOffset, + boneOffset = bone.boneOffset, rotation = bone.rotation, acceleration = bone.acceleration, headPosition = headPosition, @@ -181,8 +181,8 @@ fun buildBones(mutableBones: BodyPartMap, boneInputs: InputSkeleton, sealed interface SkeletonActions { data class SetBonePose( val bodyPart: BodyPart, - val trackerOffset: Vector3?, - val expectedTps: UShort?, + val boneOffset: Vector3?, + val tps: UShort?, val rotation: Quaternion?, val acceleration: Vector3?, val position: Vector3?, @@ -277,7 +277,7 @@ class Skeleton( UpsamplingInputProcessor(), RotationPredictionInputProcessor(settings), RotationSmoothingInputProcessor(settings), - TrackerOffsetInputProcessor(), + BoneOffsetInputProcessor(), HeadPositionFallbackInputProcessor(), FallbackInputProcessor(), UpperLegsRollAlignInputProcessor(settings), diff --git a/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt b/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt index da190d2b0..8b7efec3a 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt @@ -10,8 +10,8 @@ fun reduce(state: SkeletonState, action: SkeletonActions): SkeletonState = when state.copy( boneInputs = state.boneInputs.mutateCopy { it[action.bodyPart] = bone.copy( - trackerOffset = action.trackerOffset ?: bone.trackerOffset, - expectedTps = action.expectedTps ?: bone.expectedTps, + boneOffset = action.boneOffset ?: bone.boneOffset, + tps = action.tps ?: bone.tps, rotation = action.rotation ?: bone.rotation, acceleration = action.acceleration ?: bone.acceleration, position = action.position ?: bone.position, @@ -28,8 +28,8 @@ fun reduce(state: SkeletonState, action: SkeletonActions): SkeletonState = when state.copy( boneInputs = state.boneInputs.mutateCopy { it[action.bodyPart] = bone.copy( - trackerOffset = Vector3.ZERO, - expectedTps = null, + boneOffset = Vector3.ZERO, + tps = null, rotation = Quaternion.IDENTITY, acceleration = Vector3.ZERO, position = null, diff --git a/server/core/src/main/java/dev/slimevr/solarxr/common.kt b/server/core/src/main/java/dev/slimevr/solarxr/common.kt index ab2fadf96..eb9df5864 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/common.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/common.kt @@ -17,7 +17,6 @@ fun createBone(bone: BoneState, mask: BoneMask): solarxr_protocol.datatypes.Bone tailPosition = if (mask.tailPosition) bone.tailPosition.toVec3f() else null, linearVelocity = if (mask.linearVelocity) bone.velocity.linear.toVec3f() else null, angularVelocity = if (mask.angularVelocity) bone.velocity.angular.toVec3f() else null, - trackerOffset = if (mask.trackerOffset) bone.trackerOffset.toVec3f() else null, ) fun Quat.toQuaternion() = Quaternion(w, x, y, z) diff --git a/server/core/src/main/java/dev/slimevr/tracker/behaviours/to-skeleton.kt b/server/core/src/main/java/dev/slimevr/tracker/behaviours/to-skeleton.kt index abd35325c..4a3577f9a 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/behaviours/to-skeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/behaviours/to-skeleton.kt @@ -47,7 +47,7 @@ class TrackerToSkeletonBehaviour : TrackerBehaviour { val bonePoseActions = SkeletonActions.SetBonePose( bodyPart, trackerState.boneOffsets[bodyPart], - trackerState.expectedTps ?: trackerState.tps, + trackerState.tps, trackerState.rotation, trackerState.acceleration, trackerState.position, diff --git a/server/core/src/main/java/dev/slimevr/tracker/behaviours/tps.kt b/server/core/src/main/java/dev/slimevr/tracker/behaviours/tps.kt index 033f9e282..d05ab92cc 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/behaviours/tps.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/behaviours/tps.kt @@ -21,14 +21,14 @@ class TrackerTpsBehaviour : TrackerBehaviour { val trackerState = receiver.context.state.value val tps = (trackerState.accumulatedTicks * 1000u).toLong() / elapsed.inWholeMilliseconds - // Tracker is at rest if it hasn't been updated in the last second - val updateMotionAction = if (tps == 0L && trackerState.motion != Motion.RESTING) TrackerActions.SetMotion(Motion.RESTING) else null - receiver.context.dispatchAll( - listOfNotNull( - TrackerActions.Update { copy(tps = tps.toUShort(), accumulatedTicks = 0u) }, - updateMotionAction, - ), + // Update TPS + receiver.context.dispatch( + TrackerActions.Update { copy(tps = tps.toUShort(), accumulatedTicks = 0u) }, ) + // Tracker is at rest if it has 0 TPS + if (tps == 0L && trackerState.motion != Motion.RESTING) { + receiver.context.dispatch(TrackerActions.SetMotion(Motion.RESTING)) + } mark = timeSource.markNow() } diff --git a/server/core/src/main/java/dev/slimevr/tracker/module.kt b/server/core/src/main/java/dev/slimevr/tracker/module.kt index d85a24d25..3137822b9 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/module.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/module.kt @@ -80,7 +80,6 @@ data class TrackerState( val position: Vector3?, val imuTemp: Float?, val accumulatedTicks: UShort, - val expectedTps: UShort?, val tps: UShort, val status: TrackerStatus, val completedRestCalibration: Boolean?, @@ -148,7 +147,6 @@ class Tracker( hardwareId: String, origin: DeviceOrigin, driverName: String? = null, - expectedTps: UShort? = null, appContext: AppContextProvider, ): Tracker { val settings = appContext.config.settings @@ -167,7 +165,6 @@ class Tracker( intendedBodyPart = intendedBodyPart, boneOffsets = intendedBodyPart?.let { part -> intendedBoneOffset?.let { offset -> mapOf(part to offset) } } ?: emptyMap(), stayAlignedData = DEFAULT_STATE.stayAlignedData.copy(enabled = settings.context.state.value.data.stayAlignedConfig.enabled), - expectedTps = expectedTps, ) val trackerState = if (savedConfig != null) { TrackerConfigBehaviour.restoreFromConfig(baseState, savedConfig, settings.context.state.value.data.resetsConfig.saveMountingReset) @@ -226,7 +223,6 @@ class Tracker( position = null, imuTemp = null, accumulatedTicks = 0u, - expectedTps = null, tps = 0u, status = TrackerStatus.DISCONNECTED, completedRestCalibration = null, diff --git a/server/core/src/main/java/dev/slimevr/udp/behaviours.kt b/server/core/src/main/java/dev/slimevr/udp/behaviours.kt index 136f1ab84..15c57214e 100644 --- a/server/core/src/main/java/dev/slimevr/udp/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/udp/behaviours.kt @@ -66,7 +66,6 @@ internal fun assignTracker(receiver: UDPConnection, device: Device, info: Sensor origin = DeviceOrigin.UDP, scope = receiver.appContext.server.context.scope, appContext = receiver.appContext, - expectedTps = 100u, ) receiver.appContext.server.context.dispatch(VRServerActions.NewTracker(trackerId = trackerId, context = newTracker)) receiver.context.dispatch( diff --git a/server/core/src/test/java/dev/slimevr/vmc/InputDecoderTest.kt b/server/core/src/test/java/dev/slimevr/vmc/InputDecoderTest.kt index 67bdd7ce9..208196a61 100644 --- a/server/core/src/test/java/dev/slimevr/vmc/InputDecoderTest.kt +++ b/server/core/src/test/java/dev/slimevr/vmc/InputDecoderTest.kt @@ -37,7 +37,7 @@ private fun bone(bodyPart: BodyPart, rotation: Quaternion = Quaternion.IDENTITY, bodyPart = bodyPart, headOffset = Vector3.ZERO, offset = Vector3(0f, -0.1f, 0f), - trackerOffset = Vector3.ZERO, + boneOffset = Vector3.ZERO, rotation = rotation, acceleration = Vector3.ZERO, headPosition = headPosition, diff --git a/server/core/src/test/java/dev/slimevr/vmc/OutputEncoderTest.kt b/server/core/src/test/java/dev/slimevr/vmc/OutputEncoderTest.kt index f001d99f2..7bc8a9dd9 100644 --- a/server/core/src/test/java/dev/slimevr/vmc/OutputEncoderTest.kt +++ b/server/core/src/test/java/dev/slimevr/vmc/OutputEncoderTest.kt @@ -25,7 +25,7 @@ private fun bone(bodyPart: BodyPart, rotation: Quaternion = Quaternion.IDENTITY, bodyPart = bodyPart, headOffset = Vector3.ZERO, offset = Vector3(0f, -0.1f, 0f), - trackerOffset = Vector3.ZERO, + boneOffset = Vector3.ZERO, rotation = rotation, acceleration = Vector3.ZERO, headPosition = headPosition, diff --git a/solarxr-protocol b/solarxr-protocol index 2c8d9c3f6..8242ed6d0 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit 2c8d9c3f63e8928a9560aea5504d18d0e4e64915 +Subproject commit 8242ed6d04d002c048a3eed8e1e5d37f81cf9cc6