mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-20 17:11:20 -08:00
boneOffsets all the way and wip improving smoothness
This commit is contained in:
1 parent
8a7794545c
commit
b13754b1e0
17 files changed
+55
-71
No files matched your search
@@ -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
|
||||
|
||||
@@ -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))
|
||||
|
||||
+22
-28
@@ -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) {
|
||||
|
||||
+2
-4
@@ -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,
|
||||
|
||||
+1
-1
Submodule solarxr-protocol updated: 2c8d9c3f63...8242ed6d04.
Reference in new issue
Block a user