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 a318db88d..c38a93fb6 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -1,19 +1,147 @@ package dev.slimevr.skeleton +import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart -data class Bone( +data class BoneState( val bodyPart: BodyPart, - val localRotation: Quaternion, - val parentBone: Bone?, - val childBone: List, + val length: Float, + val rotation: Quaternion, + val headPosition: Vector3, + val tailPosition: Vector3, + val parentBone: BoneState?, + val childBones: List, ) { - val globalRotation: Quaternion - get() = localRotation // FIXME: do maths LMAO + val localRotation: Quaternion + get() = parentBone?.let { it.rotation.inv() * rotation } ?: rotation + val localHeadPosition: Vector3 + get() = parentBone?.let { headPosition - it.tailPosition } ?: headPosition + val localTailPosition: Vector3 + get() = tailPosition - headPosition } data class SkeletonState( - val bones: Map, - val rootBone: Bone, + val bones: Map, + val rootBone: BoneState, ) + +fun makeBone(bodyPart: BodyPart, parent: BoneState? = null, length: Float = 0.1f): BoneState { + val head = parent?.tailPosition ?: Vector3.NULL + val offset = when (bodyPart) { + BodyPart.HEAD -> Vector3(0f, 0f, length) + BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND -> Vector3(0f, 0f, -length) + BodyPart.LEFT_SHOULDER -> Vector3(-length, -0.08f, 0f) + BodyPart.RIGHT_SHOULDER -> Vector3(length, -0.08f, 0f) + BodyPart.LEFT_HIP -> Vector3(-length, 0f, 0f) + BodyPart.RIGHT_HIP -> Vector3(length, 0f, 0f) + else -> Vector3(0f, -length, 0f) + } + val rot = when (bodyPart) { + BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT -> Quaternion.rotationAroundXAxis(FastMath.HALF_PI) + else -> Quaternion.IDENTITY + } + + val bone = BoneState( + bodyPart = bodyPart, + length = length, + rotation = rot, + headPosition = head, + tailPosition = head + offset, + parentBone = parent, + childBones = mutableListOf(), + ) + (parent?.childBones as? MutableList)?.add(bone) + return bone +} + +suspend fun SequenceScope.visitBone(bone: BoneState) { + yield(bone) + bone.childBones.forEach { visitBone(it) } +} + +fun navigateHierarchy(rootBone: BoneState): Sequence = sequence { + visitBone(rootBone) +} + +val DEFAULT_SKELETON_STATE = run { + // Head/torso + val head = makeBone(BodyPart.HEAD, length = 0.1f) + val neck = makeBone(BodyPart.NECK, head, 0.1f) + val upperChest = makeBone(BodyPart.UPPER_CHEST, neck, 0.16f) + val chest = makeBone(BodyPart.CHEST, upperChest, 0.16f) + val waist = makeBone(BodyPart.WAIST, chest, 0.2f) + val hip = makeBone(BodyPart.HIP, waist, 0.04f) + + // Left leg + val leftHip = makeBone(BodyPart.LEFT_HIP, hip, 0.13f) + val leftUpperLeg = makeBone(BodyPart.LEFT_UPPER_LEG, leftHip, 0.42f) + val leftLowerLeg = makeBone(BodyPart.LEFT_LOWER_LEG, leftUpperLeg, 0.5f) + val leftFoot = makeBone(BodyPart.LEFT_FOOT, leftLowerLeg, 0.05f) + + // Right leg + val rightHip = makeBone(BodyPart.RIGHT_HIP, hip, 0.13f) + val rightUpperLeg = makeBone(BodyPart.RIGHT_UPPER_LEG, rightHip, 0.42f) + val rightLowerLeg = makeBone(BodyPart.RIGHT_LOWER_LEG, rightUpperLeg, 0.5f) + val rightFoot = makeBone(BodyPart.RIGHT_FOOT, rightLowerLeg, 0.05f) + + // Left arm + val leftShoulder = makeBone(BodyPart.LEFT_SHOULDER, neck, 0.175f) + val leftUpperArm = makeBone(BodyPart.LEFT_UPPER_ARM, leftShoulder, 0.26f) + val leftLowerArm = makeBone(BodyPart.LEFT_LOWER_ARM, leftUpperArm, 0.26f) + val leftHand = makeBone(BodyPart.LEFT_HAND, leftLowerArm, 0.13f) + + // Left fingers + val leftThumbMetacarpal = makeBone(BodyPart.LEFT_THUMB_METACARPAL, leftHand, 0.025f) + val leftThumbProximal = makeBone(BodyPart.LEFT_THUMB_PROXIMAL, leftThumbMetacarpal, 0.025f) + val leftThumbDistal = makeBone(BodyPart.LEFT_THUMB_DISTAL, leftThumbProximal, 0.025f) + + val leftIndexProximal = makeBone(BodyPart.LEFT_INDEX_PROXIMAL, leftHand, 0.025f) + val leftIndexIntermediate = makeBone(BodyPart.LEFT_INDEX_INTERMEDIATE, leftIndexProximal, 0.025f) + val leftIndexDistal = makeBone(BodyPart.LEFT_INDEX_DISTAL, leftIndexIntermediate, 0.025f) + + val leftMiddleProximal = makeBone(BodyPart.LEFT_MIDDLE_PROXIMAL, leftHand, 0.025f) + val leftMiddleIntermediate = makeBone(BodyPart.LEFT_MIDDLE_INTERMEDIATE, leftMiddleProximal, 0.025f) + val leftMiddleDistal = makeBone(BodyPart.LEFT_MIDDLE_DISTAL, leftMiddleIntermediate, 0.025f) + + val leftRingProximal = makeBone(BodyPart.LEFT_RING_PROXIMAL, leftHand, 0.025f) + val leftRingIntermediate = makeBone(BodyPart.LEFT_RING_INTERMEDIATE, leftRingProximal, 0.025f) + val leftRingDistal = makeBone(BodyPart.LEFT_RING_DISTAL, leftRingIntermediate, 0.025f) + + val leftLittleProximal = makeBone(BodyPart.LEFT_LITTLE_PROXIMAL, leftHand, 0.025f) + val leftLittleIntermediate = makeBone(BodyPart.LEFT_LITTLE_INTERMEDIATE, leftLittleProximal, 0.025f) + val leftLittleDistal = makeBone(BodyPart.LEFT_LITTLE_DISTAL, leftLittleIntermediate, 0.025f) + + // Right arm + val rightShoulder = makeBone(BodyPart.RIGHT_SHOULDER, neck, 0.175f) + val rightUpperArm = makeBone(BodyPart.RIGHT_UPPER_ARM, rightShoulder, 0.26f) + val rightLowerArm = makeBone(BodyPart.RIGHT_LOWER_ARM, rightUpperArm, 0.26f) + val rightHand = makeBone(BodyPart.RIGHT_HAND, rightLowerArm, 0.13f) + + // Right fingers + val rightThumbMetacarpal = makeBone(BodyPart.RIGHT_THUMB_METACARPAL, rightHand, 0.025f) + val rightThumbProximal = makeBone(BodyPart.RIGHT_THUMB_PROXIMAL, rightThumbMetacarpal, 0.025f) + val rightThumbDistal = makeBone(BodyPart.RIGHT_THUMB_DISTAL, rightThumbProximal, 0.025f) + + val rightIndexProximal = makeBone(BodyPart.RIGHT_INDEX_PROXIMAL, rightHand, 0.025f) + val rightIndexIntermediate = makeBone(BodyPart.RIGHT_INDEX_INTERMEDIATE, rightIndexProximal, 0.025f) + val rightIndexDistal = makeBone(BodyPart.RIGHT_INDEX_DISTAL, rightIndexIntermediate, 0.025f) + + val rightMiddleProximal = makeBone(BodyPart.RIGHT_MIDDLE_PROXIMAL, rightHand, 0.025f) + val rightMiddleIntermediate = makeBone(BodyPart.RIGHT_MIDDLE_INTERMEDIATE, rightMiddleProximal, 0.025f) + val rightMiddleDistal = makeBone(BodyPart.RIGHT_MIDDLE_DISTAL, rightMiddleIntermediate, 0.025f) + + val rightRingProximal = makeBone(BodyPart.RIGHT_RING_PROXIMAL, rightHand, 0.025f) + val rightRingIntermediate = makeBone(BodyPart.RIGHT_RING_INTERMEDIATE, rightRingProximal, 0.025f) + val rightRingDistal = makeBone(BodyPart.RIGHT_RING_DISTAL, rightRingIntermediate, 0.025f) + + val rightLittleProximal = makeBone(BodyPart.RIGHT_LITTLE_PROXIMAL, rightHand, 0.025f) + val rightLittleIntermediate = makeBone(BodyPart.RIGHT_LITTLE_INTERMEDIATE, rightLittleProximal, 0.025f) + val rightLittleDistal = makeBone(BodyPart.RIGHT_LITTLE_DISTAL, rightLittleIntermediate, 0.025f) + + SkeletonState( + bones = navigateHierarchy(head).associateBy { it.bodyPart }, + rootBone = head, + ) +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt index f03e7f95e..0c46f8ffc 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt @@ -3,6 +3,8 @@ package dev.slimevr.solarxr import com.google.flatbuffers.FlatBufferBuilder import dev.slimevr.VRServer import dev.slimevr.device.DeviceState +import dev.slimevr.skeleton.BoneState +import dev.slimevr.skeleton.DEFAULT_SKELETON_STATE import dev.slimevr.tracker.TrackerState import io.ktor.util.moveToByteArray import kotlinx.coroutines.cancelAndJoin @@ -10,6 +12,7 @@ import kotlinx.coroutines.delay import kotlinx.coroutines.isActive import kotlinx.coroutines.launch import solarxr_protocol.MessageBundle +import solarxr_protocol.data_feed.Bone import solarxr_protocol.data_feed.DataFeedConfig import solarxr_protocol.data_feed.DataFeedMessageHeader import solarxr_protocol.data_feed.DataFeedUpdate @@ -84,6 +87,15 @@ private fun createDevice( ) } +private fun createBone( + bone: BoneState, +): Bone = Bone( + bodyPart = bone.bodyPart, + rotationG = bone.rotation.let { Quat(it.x, it.y, it.z, it.w) }, + boneLength = bone.length, + headPositionG = bone.headPosition.let { Vec3f(it.x, it.y, it.z) }, +) + fun createDatafeedFrame( serverContext: VRServer, datafeedConfig: DataFeedConfig, @@ -93,9 +105,15 @@ fun createDatafeedFrame( val trackers = serverState.trackers.values.map { it.context.state.value } val devices = serverState.devices.values.map { it.context.state.value } .map { device -> createDevice(device, trackers, datafeedConfig) } + val bones = if (datafeedConfig.boneMask) { + DEFAULT_SKELETON_STATE.bones.values.map { createBone(it) } + } else { + null + } return DataFeedMessageHeader( message = DataFeedUpdate( devices = if (datafeedConfig.dataMask?.deviceData != null) devices else null, + bones = bones, index = index.toUByte(), ), )