boneOffsets all the way and wip improving smoothness

This commit is contained in:
Erimel committed 2026-10-26 17:44:28 -04:00
1 parent 8a7794545c
commit b13754b1e0
17 files changed
+55 -71

No files matched your search

+1 -1
View File
@@ -360,7 +360,7 @@ tracker-settings-use_mag-label = Allow magnetometer
# The .<name> 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
-1
View File
@@ -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;
@@ -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))
@@ -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<BoneDelta> = 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<BoneDelta> = 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<BoneDelta>()
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) {
@@ -22,7 +22,7 @@ private const val SMOOTH_MAX = 0.92f
class RotationSmoothingInputProcessor(val settings: Settings) :
SkeletonInputProcessor,
ResettableSkeletonProcessor {
private var smoothed: BodyPartMap<Quaternion> = bodyPartMap()
private val smoothed: BodyPartMap<Quaternion> = 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<Quaternion>()
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) {
@@ -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))
}
}
@@ -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) {
@@ -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<BoneState>, 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),
@@ -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,
@@ -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)
@@ -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,
@@ -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()
}
@@ -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,
@@ -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(
@@ -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,
@@ -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,