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 afa855716..255e3e356 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -93,7 +93,7 @@ class Skeleton( ProportionsBehaviour(), ScaledProportionsBehaviour(ctx.config.userConfig), HeightLogBehaviour(), - YouSpinMeRightRoundBehaviour(inputHz = 10f), + YouSpinMeRightRoundBehaviour(inputHz = 50f), ComputedSkeletonBehaviour( processors = listOf( // PredictionProcessor(predictionAmount = 0.3f), diff --git a/server/core/src/main/java/dev/slimevr/vmc/behaviours.kt b/server/core/src/main/java/dev/slimevr/vmc/behaviours.kt index ae5bf81fb..60058d992 100644 --- a/server/core/src/main/java/dev/slimevr/vmc/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/vmc/behaviours.kt @@ -91,11 +91,9 @@ class VMCOutputBehaviour( }.launchIn(receiver.context.scope) } - private fun buildBundle(bones: Map, config: VMCConfig, currentTime: Long, vrm: VrmGeometry?): OscBundle = - OscBundle(1L, buildMessages(bones, config, currentTime, vrm).map { msg -> OscContent.Message(msg) }.toList()) + private fun buildBundle(bones: Map, config: VMCConfig, currentTime: Long, vrm: VrmGeometry?): OscBundle = OscBundle(1L, buildMessages(bones, config, currentTime, vrm).map { msg -> OscContent.Message(msg) }.toList()) - // Z-axis handedness flip for SlimeVR (RH) to Unity/VMC (LH). - private fun boneMessage(address: String, name: String, pos: Vector3, rot: Quaternion): OscMessage = OscMessage( + private fun transformMessage(address: String, name: String, pos: Vector3, rot: Quaternion): OscMessage = OscMessage( address, listOf( OscArg.String(name), @@ -113,46 +111,42 @@ class VMCOutputBehaviour( val time = (currentTime - initTime) / 1000f yield(OscMessage("/VMC/Ext/T", listOf(OscArg.Float(time)))) yield(OscMessage("/VMC/Ext/OK", listOf(OscArg.Int(1)))) - yield( - OscMessage( - "/VMC/Ext/Root/Pos", - listOf( - OscArg.String("root"), - OscArg.Float(0f), - OscArg.Float(0f), - OscArg.Float(0f), - OscArg.Float(0f), - OscArg.Float(0f), - OscArg.Float(0f), - OscArg.Float(1f), - ), - ), - ) - // Hip world position. - // With VRM data: anchor at HEAD: scale our skeleton head into VRM - // units and walk back to the hip via the avatar's own bind pose so the model stays - // grounded at its native proportions. - // Without VRM data: fall back to absolute world hip. - val hipPos = if (vrm != null) { - val userHeight = skeleton.context.state.value.userHeight - val scale = if (userHeight > 0f) vrm.height / userHeight else 1f - val ourHead = bones[BodyPart.HEAD]?.headPosition ?: Vector3.NULL - ourHead * scale - vrm.headOffsetFromHip - } else { - bones[BodyPart.HIP]?.headPosition ?: Vector3.NULL - } + val rootPos = vmcRootPosition(bones, config, vrm) + yield(transformMessage("/VMC/Ext/Root/Pos", "root", rootPos, Quaternion.IDENTITY)) - // TODO: implement mirrorTracking and anchor at hip - for ((bodyPart, unityName) in BODY_PART_TO_UNITY_BONE) { - val bone = bones[bodyPart] ?: continue - val parent = VMC_BONE_PARENTS[bodyPart]?.let { bones[it] } - val pos = when { - parent == null -> hipPos - vrm != null -> vrm.bindOffsets[bodyPart] ?: Vector3.NULL - else -> vmcLocalPosition(bone, parent) + for ((targetBodyPart, unityName) in BODY_PART_TO_UNITY_BONE) { + val targetParentBodyPart = VMC_BONE_PARENTS[targetBodyPart] + val trackingBodyPart = if (config.mirrorTracking) vmcMirrorSource(targetBodyPart) else targetBodyPart + val trackingBone = bones[trackingBodyPart] ?: continue + + if (targetParentBodyPart == null) { + val pos = vrm?.hipLocalPosition ?: Vector3.NULL + val rot = vmcLocalRotation(trackingBone, null, targetBodyPart, null, config.mirrorTracking) + yield(transformMessage("/VMC/Ext/Bone/Pos", unityName, pos, rot)) + continue } - yield(boneMessage("/VMC/Ext/Bone/Pos", unityName, pos, vmcLocalRotation(bone, parent))) + + val trackingParentBodyPart = if (config.mirrorTracking) { + vmcMirrorSource(targetParentBodyPart) + } else { + targetParentBodyPart + } + val trackingParent = bones[trackingParentBodyPart] ?: continue + + val pos = if (vrm != null) { + vrm.bindOffsets[targetBodyPart] ?: Vector3.NULL + } else { + vmcLocalPosition(trackingBone, trackingParent, targetParentBodyPart, config.mirrorTracking) + } + val rot = vmcLocalRotation( + trackingBone, + trackingParent, + targetBodyPart, + targetParentBodyPart, + config.mirrorTracking, + ) + yield(transformMessage("/VMC/Ext/Bone/Pos", unityName, pos, rot)) } } } diff --git a/server/core/src/main/java/dev/slimevr/vmc/mapping.kt b/server/core/src/main/java/dev/slimevr/vmc/mapping.kt index cd64d676e..b414dba0e 100644 --- a/server/core/src/main/java/dev/slimevr/vmc/mapping.kt +++ b/server/core/src/main/java/dev/slimevr/vmc/mapping.kt @@ -6,7 +6,6 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart - val BODY_PART_TO_UNITY_BONE: Map = mapOf( BodyPart.HEAD to "Head", BodyPart.NECK to "Neck", @@ -123,6 +122,37 @@ fun iterateVMCHierarchy() = sequence { visitVMC(null, BodyPart.HIP) } val VMC_BONE_PARENTS: Map = iterateVMCHierarchy().associate { (parent, child) -> child to parent } +val VMC_MIRROR_BONE_PAIRS: List> = listOf( + BodyPart.LEFT_SHOULDER to BodyPart.RIGHT_SHOULDER, + BodyPart.LEFT_UPPER_ARM to BodyPart.RIGHT_UPPER_ARM, + BodyPart.LEFT_LOWER_ARM to BodyPart.RIGHT_LOWER_ARM, + BodyPart.LEFT_HAND to BodyPart.RIGHT_HAND, + BodyPart.LEFT_UPPER_LEG to BodyPart.RIGHT_UPPER_LEG, + BodyPart.LEFT_LOWER_LEG to BodyPart.RIGHT_LOWER_LEG, + BodyPart.LEFT_FOOT to BodyPart.RIGHT_FOOT, + BodyPart.LEFT_THUMB_METACARPAL to BodyPart.RIGHT_THUMB_METACARPAL, + BodyPart.LEFT_THUMB_PROXIMAL to BodyPart.RIGHT_THUMB_PROXIMAL, + BodyPart.LEFT_THUMB_DISTAL to BodyPart.RIGHT_THUMB_DISTAL, + BodyPart.LEFT_INDEX_PROXIMAL to BodyPart.RIGHT_INDEX_PROXIMAL, + BodyPart.LEFT_INDEX_INTERMEDIATE to BodyPart.RIGHT_INDEX_INTERMEDIATE, + BodyPart.LEFT_INDEX_DISTAL to BodyPart.RIGHT_INDEX_DISTAL, + BodyPart.LEFT_MIDDLE_PROXIMAL to BodyPart.RIGHT_MIDDLE_PROXIMAL, + BodyPart.LEFT_MIDDLE_INTERMEDIATE to BodyPart.RIGHT_MIDDLE_INTERMEDIATE, + BodyPart.LEFT_MIDDLE_DISTAL to BodyPart.RIGHT_MIDDLE_DISTAL, + BodyPart.LEFT_RING_PROXIMAL to BodyPart.RIGHT_RING_PROXIMAL, + BodyPart.LEFT_RING_INTERMEDIATE to BodyPart.RIGHT_RING_INTERMEDIATE, + BodyPart.LEFT_RING_DISTAL to BodyPart.RIGHT_RING_DISTAL, + BodyPart.LEFT_LITTLE_PROXIMAL to BodyPart.RIGHT_LITTLE_PROXIMAL, + BodyPart.LEFT_LITTLE_INTERMEDIATE to BodyPart.RIGHT_LITTLE_INTERMEDIATE, + BodyPart.LEFT_LITTLE_DISTAL to BodyPart.RIGHT_LITTLE_DISTAL, +) + +val VMC_MIRROR_BONES: Map = VMC_MIRROR_BONE_PAIRS + .flatMap { (left, right) -> listOf(left to right, right to left) } + .toMap() + +fun vmcMirrorSource(bodyPart: BodyPart): BodyPart = VMC_MIRROR_BONES[bodyPart] ?: bodyPart + // Per-bone rest offset, subtracted from the live world rotation before computing the VMC local. // Foot cancels R(90deg,X) baked into DEFAULT_SKELETON_STATE. Arms remap our hanging rest (NEG_Y) // to the VRM rig's T-pose rest direction so the avatar isn't stuck at T regardless of our pose. @@ -153,28 +183,51 @@ val VMC_REST_ROTATIONS: Map = run { BodyPart.RIGHT_UPPER_ARM to rightArm, BodyPart.RIGHT_LOWER_ARM to rightArm, BodyPart.RIGHT_HAND to rightArm, - ) + leftFingers.associateWith { leftArm } + rightFingers.associateWith { rightArm } + ) + + leftFingers.associateWith { leftArm } + + rightFingers.associateWith { rightArm } } -private fun restAdjustedWorld(bone: BoneState): Quaternion { - val rest = VMC_REST_ROTATIONS[bone.bodyPart] ?: return bone.rotation - return bone.rotation * rest.inv() +fun vmcMirrorPosition(pos: Vector3): Vector3 = Vector3(-pos.x, pos.y, pos.z) + +fun vmcMirrorRotation(rot: Quaternion): Quaternion = Quaternion(rot.w, rot.x, -rot.y, -rot.z) + +private fun restAdjustedWorld( + bone: BoneState, + restBodyPart: BodyPart = bone.bodyPart, + mirror: Boolean = false, +): Quaternion { + val world = if (mirror) vmcMirrorRotation(bone.rotation) else bone.rotation + val rest = VMC_REST_ROTATIONS[restBodyPart] ?: return world + return world * rest.inv() } -// Local rotation relative to the VMC parent. Rest offset is applied to both bone and parent -// before taking the relative rotation. Parent is explicit because VMC hierarchy differs from -// the skeleton's head-rooted parentBone. -fun vmcLocalRotation(bone: BoneState, parent: BoneState?): Quaternion { - val adjusted = restAdjustedWorld(bone) +fun vmcLocalRotation( + bone: BoneState, + parent: BoneState?, + restBodyPart: BodyPart, + restParentBodyPart: BodyPart?, + mirror: Boolean, +): Quaternion { + val adjusted = restAdjustedWorld(bone, restBodyPart, mirror) if (parent == null) return adjusted - return restAdjustedWorld(parent).inv() * adjusted + return restAdjustedWorld(parent, restParentBodyPart ?: parent.bodyPart, mirror).inv() * adjusted } -// Local position relative to the VMC parent. Overrides the VRM bind-pose offset so each bone -// lands at our skeleton's world position when the receiver honors it. HIP root is handled by -// the caller since the root has no parent in VMC. -fun vmcLocalPosition(bone: BoneState, parent: BoneState): Vector3 { - val parentAdjusted = restAdjustedWorld(parent) - return parentAdjusted.inv().sandwich(bone.headPosition - parent.headPosition) +fun vmcLocalPosition( + bone: BoneState, + parent: BoneState, + restParentBodyPart: BodyPart, + mirror: Boolean, +): Vector3 { + val parentAdjusted = restAdjustedWorld(parent, restParentBodyPart, mirror) + val localPosition = bone.headPosition - parent.headPosition + return parentAdjusted.inv().sandwich(if (mirror) vmcMirrorPosition(localPosition) else localPosition) } +fun vmcRootPosition(bones: Map, config: VMCConfig, vrm: VrmGeometry?): Vector3 { + val userHip = bones[BodyPart.HIP]?.headPosition ?: Vector3.NULL + val hipLocalPosition = vrm?.hipLocalPosition ?: Vector3.NULL + val targetHip = if (config.anchorAtHips) Vector3.NULL else userHip + return (if (config.mirrorTracking) vmcMirrorPosition(targetHip) else targetHip) - hipLocalPosition +} diff --git a/server/core/src/main/java/dev/slimevr/vmc/vrm.kt b/server/core/src/main/java/dev/slimevr/vmc/vrm.kt index cd0fa99c7..93bd13fdb 100644 --- a/server/core/src/main/java/dev/slimevr/vmc/vrm.kt +++ b/server/core/src/main/java/dev/slimevr/vmc/vrm.kt @@ -81,13 +81,11 @@ data class Node( val children: List = emptyList(), ) -// VRM bind-pose geometry derived from a parsed VRM JSON. Used to anchor the avatar at HEAD, -// scale our skeleton positions into VRM units, and override per-bone positions with the -// model's own bind pose so the mesh isn't stretched. +// VRM bind-pose geometry derived from a parsed VRM JSON. Used to keep the avatar's +// local bone offsets aligned with the model's own proportions. data class VrmGeometry( val bindOffsets: Map, - val height: Float, - val headOffsetFromHip: Vector3, + val hipLocalPosition: Vector3, ) fun buildVrmGeometry(reader: VrmReader): VrmGeometry { @@ -96,12 +94,10 @@ fun buildVrmGeometry(reader: VrmReader): VrmGeometry { } fun offset(bodyPart: BodyPart) = bindOffsets[bodyPart] ?: Vector3.NULL - val headOffsetFromHip = offset(BodyPart.WAIST) + offset(BodyPart.CHEST) + offset(BodyPart.UPPER_CHEST) + offset(BodyPart.NECK) + offset(BodyPart.HEAD) - val height = offset(BodyPart.HIP).y + headOffsetFromHip.y + val hipLocalPosition = offset(BodyPart.HIP) return VrmGeometry( bindOffsets = bindOffsets, - height = height, - headOffsetFromHip = headOffsetFromHip, + hipLocalPosition = hipLocalPosition, ) -} \ No newline at end of file +}