diff --git a/server/core/src/main/java/dev/slimevr/config/settings.kt b/server/core/src/main/java/dev/slimevr/config/settings.kt index ddea58dc5..611d78024 100644 --- a/server/core/src/main/java/dev/slimevr/config/settings.kt +++ b/server/core/src/main/java/dev/slimevr/config/settings.kt @@ -139,7 +139,7 @@ data class SkeletonRatiosConfig( val imputeSpineFromUpperToLower: Float = 0.5f, val imputeSpineCurvature: Float = 0.5f, val interpolateHipWithUpperLegs: Float = 0.25f, - val interpolateUpperLegsWithLowerLegs: Float = 0.2f, // TODO make this only applied after FK somehow + val interpolateUpperLegsWithLowerLegs: Float = 1f, val skatingCorrectionStrength: Float = 0.3f, ) 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 857435720..d3a8a883a 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -8,9 +8,10 @@ import dev.slimevr.skeleton.processors.BoneDirectLinkProcessor import dev.slimevr.skeleton.processors.BonePredictionProcessor import dev.slimevr.skeleton.processors.BoneSmoothingProcessor import dev.slimevr.skeleton.processors.BoneYawFallbackProcessor -import dev.slimevr.skeleton.processors.BoneYawRollAlignProcessor import dev.slimevr.skeleton.processors.FingerImputeProcessor +import dev.slimevr.skeleton.processors.HipYawRollAlignProcessor import dev.slimevr.skeleton.processors.SpineImputeProcessor +import dev.slimevr.skeleton.processors.UpperLegsRollAlignProcessor import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import kotlinx.coroutines.CoroutineScope @@ -158,7 +159,8 @@ class Skeleton( BoneYawFallbackProcessor(), BoneActiveLinkProcessor(), SpineImputeProcessor(ctx.config.settings), - BoneYawRollAlignProcessor(ctx.config.settings), + HipYawRollAlignProcessor(ctx.config.settings), + UpperLegsRollAlignProcessor(ctx.config.settings), BoneDirectLinkProcessor(), FingerImputeProcessor(), BonePredictionProcessor(ctx.config.settings), diff --git a/server/core/src/main/java/dev/slimevr/skeleton/processors/bone-yaw-roll-align.kt b/server/core/src/main/java/dev/slimevr/skeleton/processors/bone-yaw-roll-align.kt deleted file mode 100644 index 7a034aac5..000000000 --- a/server/core/src/main/java/dev/slimevr/skeleton/processors/bone-yaw-roll-align.kt +++ /dev/null @@ -1,64 +0,0 @@ -package dev.slimevr.skeleton.processors - -import dev.slimevr.config.Settings -import dev.slimevr.skeleton.SkeletonProcessor -import dev.slimevr.skeleton.SkeletonState -import dev.slimevr.skeleton.mutate -import dev.slimevr.skeleton.resolveAverageRotationFor -import io.github.axisangles.ktmath.Quaternion -import solarxr_protocol.datatypes.BodyPart - -/** - * Handles rotating bones' yaw and roll to match other bones' yaw and roll. - */ -class BoneYawRollAlignProcessor(val settings: Settings) : SkeletonProcessor { - private class SourceLink(val bodyPart: BodyPart, val sources: Array, val mustBeActive: Boolean) - - /** - * First value is the BodyPart to be aligned. - * - * Second value is a SourceLink containing a list of BodyParts to align with as well as a boolean specifying - * whether the BodyPart to be aligned must be inactive. - */ - private val bodyPartToSources = arrayOf( - SourceLink(BodyPart.HIP, arrayOf(BodyPart.LEFT_UPPER_LEG, BodyPart.RIGHT_UPPER_LEG), mustBeActive = false), - SourceLink(BodyPart.LEFT_UPPER_LEG, arrayOf(BodyPart.LEFT_LOWER_LEG), mustBeActive = true), - SourceLink(BodyPart.RIGHT_UPPER_LEG, arrayOf(BodyPart.RIGHT_LOWER_LEG), mustBeActive = true), - ) - - override fun process(state: SkeletonState): SkeletonState { - val boneInputs = state.boneInputs - val ratios = settings.context.state.value.data.skeletonConfig.ratios - - val updatedAlignedBones = boneInputs.mutate { updated -> - for (link in bodyPartToSources) { - val bone = boneInputs.getValue(link.bodyPart) - if (bone.isActive != link.mustBeActive) continue - - val mixFactor = when (link.bodyPart) { - BodyPart.HIP -> ratios.interpolateHipWithUpperLegs - else -> ratios.interpolateUpperLegsWithLowerLegs - } - val sourceRotation = boneInputs.resolveAverageRotationFor(link.sources) - val aligned = alignYawRoll(bone.rawRotation, sourceRotation) - updated[link.bodyPart] = bone.copy(rawRotation = bone.rawRotation.interpR(aligned, mixFactor)) - } - } - - return state.copy(boneInputs = updatedAlignedBones) - } - - /** - * Rotates the first Quaternion to match its yaw and roll to the rotation of - * the second Quaternion - * - * @param from the first Quaternion - * @param to the second Quaternion - * @return the rotated Quaternion - */ - private fun alignYawRoll(from: Quaternion, to: Quaternion): Quaternion { - val r = from.inv() * to - val c = Quaternion(r.w, -r.x, 0f, 0f) - return (from * r * c).unit() - } -} diff --git a/server/core/src/main/java/dev/slimevr/skeleton/processors/hip-yaw-roll-align.kt b/server/core/src/main/java/dev/slimevr/skeleton/processors/hip-yaw-roll-align.kt new file mode 100644 index 000000000..7323f69e4 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/skeleton/processors/hip-yaw-roll-align.kt @@ -0,0 +1,45 @@ +package dev.slimevr.skeleton.processors + +import dev.slimevr.config.Settings +import dev.slimevr.skeleton.SkeletonProcessor +import dev.slimevr.skeleton.SkeletonState +import dev.slimevr.skeleton.mutate +import dev.slimevr.skeleton.resolveAverageRotationFor +import io.github.axisangles.ktmath.Quaternion +import solarxr_protocol.datatypes.BodyPart + +/** + * Handles rotating the hip' yaw and roll to match the upper legs' yaw and roll. + */ +class HipYawRollAlignProcessor(val settings: Settings) : SkeletonProcessor { + + val source = arrayOf(BodyPart.LEFT_UPPER_LEG, BodyPart.RIGHT_UPPER_LEG) + + override fun process(state: SkeletonState): SkeletonState { + val boneInputs = state.boneInputs + + val hipBone = boneInputs.getValue(BodyPart.HIP) + if (!hipBone.isActive) { + val ratio = settings.context.state.value.data.skeletonConfig.ratios.interpolateHipWithUpperLegs + val sourceRotation = boneInputs.resolveAverageRotationFor(source) + val alignedRotation = alignYawRoll(hipBone.rawRotation, sourceRotation) + return state.copy(boneInputs = boneInputs.mutate { it[BodyPart.HIP] = hipBone.copy(rawRotation = hipBone.rawRotation.interpR(alignedRotation, ratio)) }) + } + + return state + } + + /** + * Rotates the first Quaternion to match its yaw and roll to the rotation of + * the second Quaternion + * + * @param from the first Quaternion + * @param to the second Quaternion + * @return the rotated Quaternion + */ + private fun alignYawRoll(from: Quaternion, to: Quaternion): Quaternion { + val r = from.inv() * to + val c = Quaternion(r.w, -r.x, 0f, 0f) + return (from * r * c).unit() + } +} diff --git a/server/core/src/main/java/dev/slimevr/skeleton/processors/upper-legs-roll-align.kt b/server/core/src/main/java/dev/slimevr/skeleton/processors/upper-legs-roll-align.kt new file mode 100644 index 000000000..f9c2024fc --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/skeleton/processors/upper-legs-roll-align.kt @@ -0,0 +1,55 @@ +package dev.slimevr.skeleton.processors + +import dev.slimevr.config.Settings +import dev.slimevr.skeleton.SkeletonProcessor +import dev.slimevr.skeleton.SkeletonState +import dev.slimevr.skeleton.mutate +import io.github.axisangles.ktmath.Quaternion +import solarxr_protocol.datatypes.BodyPart + +/** + * Handles rotating the upper legs' roll to match the lower legs' roll. + */ +class UpperLegsRollAlignProcessor(val settings: Settings) : SkeletonProcessor { + /** + * First value is the BodyPart to be aligned. + * + * Second value is a source BodyPart + */ + private val bodyPartToSources = arrayOf( + BodyPart.LEFT_UPPER_LEG to BodyPart.LEFT_LOWER_LEG, + BodyPart.RIGHT_UPPER_LEG to BodyPart.RIGHT_LOWER_LEG, + ) + + override fun process(state: SkeletonState): SkeletonState { + val boneInputs = state.boneInputs + val ratio = settings.context.state.value.data.skeletonConfig.ratios.interpolateUpperLegsWithLowerLegs + + val updatedAlignedBones = boneInputs.mutate { updated -> + for (bodyPartToSource in bodyPartToSources) { + val bone = boneInputs.getValue(bodyPartToSource.first) + if (!bone.isActive) continue + + val sourceRotation = boneInputs.getValue(bodyPartToSource.second).rawRotation + val alignedRotation = alignRoll(bone.rawRotation, sourceRotation) + updated[bodyPartToSource.first] = bone.copy(rawRotation = bone.rawRotation.interpR(alignedRotation, ratio)) + } + } + + return state.copy(boneInputs = updatedAlignedBones) + } + + /** + * Rotates the first Quaternion to match its roll to the rotation of + * the second Quaternion + * + * @param from the first Quaternion + * @param to the second Quaternion + * @return the rotated Quaternion + */ + private fun alignRoll(from: Quaternion, to: Quaternion): Quaternion { + val r = to.inv() * from + val c = Quaternion(r.w, 0f, -r.y, 0f) + return (to * r * c).unit() + } +} diff --git a/server/core/src/main/java/dev/slimevr/vrcosc/input.kt b/server/core/src/main/java/dev/slimevr/vrcosc/input.kt index 96fff6420..1f0772883 100644 --- a/server/core/src/main/java/dev/slimevr/vrcosc/input.kt +++ b/server/core/src/main/java/dev/slimevr/vrcosc/input.kt @@ -71,7 +71,7 @@ private class VRSystemTrackerRegistry( deviceId?.let { id -> appContext.server.getDevice(id) } ?.context?.dispatch(DeviceActions.Update { copy(status = status) }) for ((_, trackerId) in trackerIds) { - appContext.server.getTracker(trackerId)?.context?.dispatch(TrackerActions.Update { copy(status = status) }) + appContext.server.getTracker(trackerId)?.context?.dispatch(TrackerActions.SetStatus(status)) } }