diff --git a/server/core/src/main/java/dev/slimevr/resets/behaviours.kt b/server/core/src/main/java/dev/slimevr/resets/behaviours.kt index 6ea245a39..a79ae0d75 100644 --- a/server/core/src/main/java/dev/slimevr/resets/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/resets/behaviours.kt @@ -1,6 +1,5 @@ package dev.slimevr.resets -import dev.slimevr.config.MountingMethods import kotlinx.coroutines.ExperimentalCoroutinesApi import kotlinx.coroutines.delay import kotlinx.coroutines.flow.distinctUntilChangedBy @@ -8,8 +7,6 @@ import kotlinx.coroutines.flow.launchIn import kotlinx.coroutines.flow.mapLatest import solarxr_protocol.rpc.ResetType -const val MOUNTING_RESET_TIMEOUT = 120 * 1000L // 120 seconds - class ResetsBasicBehaviour : ResetsBehaviour { override fun reduce(state: ResetsState, action: ResetsActions) = when (action) { // Clear the states of the `canDoXReset`s to false @@ -42,4 +39,8 @@ class ResetsMountingTimeoutBehaviour : ResetsBehaviour { } .launchIn(receiver.context.scope) } + + companion object { + const val MOUNTING_RESET_TIMEOUT = 120 * 1000L // 120 seconds + } } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt index 8d09ddb48..58810072a 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt @@ -19,14 +19,21 @@ class BoneTransformBehaviour : SkeletonBehaviour { is SkeletonActions.SetBoneRotation -> { val bones = state.rawBones.toMutableMap() val bone = bones[action.bodyPart] ?: return state - bones[action.bodyPart] = bone.copy(rawRotation = action.rotation) + bones[action.bodyPart] = bone.copy(rawRotation = action.rotation, isActive = true) state.copy(rawBones = bones) } is SkeletonActions.SetBonePosition -> { val bones = state.rawBones.toMutableMap() val bone = bones[action.bodyPart] ?: return state - bones[action.bodyPart] = bone.copy(rawPosition = action.position) + bones[action.bodyPart] = bone.copy(rawPosition = action.position, isActive = true) + state.copy(rawBones = bones) + } + + is SkeletonActions.DisableBone -> { + val bones = state.rawBones.toMutableMap() + val bone = bones[action.bodyPart] ?: return state + bones[action.bodyPart] = bone.copy(isActive = false) state.copy(rawBones = bones) } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/bodypartstructure.kt b/server/core/src/main/java/dev/slimevr/skeleton/bodypartstructure.kt index 8399e399e..ae6e4aecf 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/bodypartstructure.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/bodypartstructure.kt @@ -64,6 +64,19 @@ val BODY_PART_HIERARCHY_MAP = mapOf( BodyPart.RIGHT_LOWER_LEG to arrayOf(BodyPart.RIGHT_FOOT), ) +val BODY_PART_PARENT_MAP: Map = BODY_PART_HIERARCHY_MAP + .flatMap { (parent, children) -> children.map { child -> child to parent } } + .toMap() + +inline fun findBodyPartParent(rootChild: BodyPart, predicate: (BodyPart) -> Boolean): BodyPart? { + var current = BODY_PART_PARENT_MAP[rootChild] + while (current != null) { + if (predicate(current)) return current + current = BODY_PART_PARENT_MAP[current] + } + return null +} + private suspend fun SequenceScope>.visitBodyPart(parentBone: BodyPart?, bone: BodyPart, onlyChildren: Boolean) { if (!onlyChildren) { yield(Pair(parentBone, bone)) 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 825c512ec..99c7afe2f 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -16,7 +16,7 @@ data class RawBone( val offset: Vector3, val rawRotation: Quaternion, val rawPosition: Vector3, - val isReceivingData: Boolean, // TODO set it with an action or whatever + val isActive: Boolean, ) data class BoneState( @@ -39,14 +39,10 @@ data class SkeletonState(val rawBones: Map, val skeletonHeigh val DEFAULT_SKELETON_STATE: SkeletonState = run { val bones = DEFAULT_BONE_OFFSETS.entries.associate { (bodyPart, tailOffset) -> - val restRotation = when (bodyPart) { - BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT -> Quaternion.rotationAroundXAxis(FastMath.HALF_PI) - else -> Quaternion.IDENTITY - } - bodyPart to BoneState(bodyPart = bodyPart, offset = tailOffset, rotation = restRotation) + bodyPart to BoneState(bodyPart = bodyPart, offset = tailOffset) } SkeletonState( - rawBones = bones.mapValues { (_, bone) -> RawBone(rawRotation = bone.rotation, bodyPart = bone.bodyPart, offset = bone.offset, rawPosition = Vector3.NULL, isReceivingData = false) }, + rawBones = bones.mapValues { (_, bone) -> RawBone(rawRotation = bone.rotation, bodyPart = bone.bodyPart, offset = bone.offset, rawPosition = Vector3.NULL, isActive = false) }, skeletonHeight = DEFAULT_HEIGHT, paused = false, ) @@ -77,6 +73,7 @@ fun buildBones( sealed interface SkeletonActions { data class SetBoneRotation(val bodyPart: BodyPart, val rotation: Quaternion) : SkeletonActions data class SetBonePosition(val bodyPart: BodyPart, val position: Vector3) : SkeletonActions + data class DisableBone(val bodyPart: BodyPart) : SkeletonActions data class SetProportions(val lengths: Map) : SkeletonActions data class PauseTracking(val pause: Boolean) : SkeletonActions } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/processors.kt b/server/core/src/main/java/dev/slimevr/skeleton/processors.kt index 2a9ca9a43..9f197327c 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/processors.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/processors.kt @@ -10,10 +10,134 @@ interface SkeletonProcessor { fun process(state: SkeletonState): SkeletonState } +/** + * Handles replacing rotations of rawBones that are not actively receiving data by falling back + * to either another rawBone's rotation, their parent's yaw or the identity rotation. + */ class FallbackProcessor : SkeletonProcessor { - override fun process(state: SkeletonState): SkeletonState = state + private val leftFingerChains = listOf( + listOf(BodyPart.LEFT_THUMB_DISTAL, BodyPart.LEFT_THUMB_PROXIMAL, BodyPart.LEFT_THUMB_METACARPAL), + listOf(BodyPart.LEFT_INDEX_DISTAL, BodyPart.LEFT_INDEX_INTERMEDIATE, BodyPart.LEFT_INDEX_PROXIMAL), + listOf(BodyPart.LEFT_MIDDLE_DISTAL, BodyPart.LEFT_MIDDLE_INTERMEDIATE, BodyPart.LEFT_MIDDLE_PROXIMAL), + listOf(BodyPart.LEFT_RING_DISTAL, BodyPart.LEFT_RING_INTERMEDIATE, BodyPart.LEFT_RING_PROXIMAL), + listOf(BodyPart.LEFT_LITTLE_DISTAL, BodyPart.LEFT_LITTLE_INTERMEDIATE, BodyPart.LEFT_LITTLE_PROXIMAL), + ) + private val rightFingerChains = listOf( + listOf(BodyPart.RIGHT_THUMB_DISTAL, BodyPart.RIGHT_THUMB_PROXIMAL, BodyPart.RIGHT_THUMB_METACARPAL), + listOf(BodyPart.RIGHT_INDEX_DISTAL, BodyPart.RIGHT_INDEX_INTERMEDIATE, BodyPart.RIGHT_INDEX_PROXIMAL), + listOf(BodyPart.RIGHT_MIDDLE_DISTAL, BodyPart.RIGHT_MIDDLE_INTERMEDIATE, BodyPart.RIGHT_MIDDLE_PROXIMAL), + listOf(BodyPart.RIGHT_RING_DISTAL, BodyPart.RIGHT_RING_INTERMEDIATE, BodyPart.RIGHT_RING_PROXIMAL), + listOf(BodyPart.RIGHT_LITTLE_DISTAL, BodyPart.RIGHT_LITTLE_INTERMEDIATE, BodyPart.RIGHT_LITTLE_PROXIMAL), + ) + + private fun putFingerFallbacks( + map: MutableMap>, + chain: List, + hand: BodyPart, + ) { + chain.forEachIndexed { i, segment -> + map[segment] = chain.drop(i + 1) + hand + } + } + + /** + * First element is the BodyPart whose rawBone is not actively receiving data. + * Second element contains a set of BodyParts whose rotation should be used as a fallback prioritized from first to last. + */ + private val missingToFallbacks = mapOf( + BodyPart.HEAD to setOf(BodyPart.NECK), + BodyPart.NECK to setOf(BodyPart.HEAD), + + // TODO could be removed once ImputeSpineProcessor is done vvv + BodyPart.UPPER_CHEST to setOf(BodyPart.CHEST, BodyPart.WAIST, BodyPart.HIP), + BodyPart.CHEST to setOf(BodyPart.UPPER_CHEST, BodyPart.WAIST, BodyPart.HIP), + BodyPart.WAIST to setOf(BodyPart.CHEST, BodyPart.HIP, BodyPart.UPPER_CHEST), + BodyPart.HIP to setOf(BodyPart.WAIST, BodyPart.CHEST, BodyPart.UPPER_CHEST), + + BodyPart.LEFT_HIP to setOf(BodyPart.HIP, BodyPart.WAIST, BodyPart.CHEST, BodyPart.UPPER_CHEST), + BodyPart.RIGHT_HIP to setOf(BodyPart.HIP, BodyPart.WAIST, BodyPart.CHEST, BodyPart.UPPER_CHEST), + + BodyPart.LEFT_SHOULDER to setOf(BodyPart.UPPER_CHEST, BodyPart.CHEST, BodyPart.WAIST, BodyPart.HIP), + BodyPart.RIGHT_SHOULDER to setOf(BodyPart.UPPER_CHEST, BodyPart.CHEST, BodyPart.WAIST, BodyPart.HIP), + + BodyPart.LEFT_UPPER_ARM to setOf(BodyPart.LEFT_LOWER_ARM), + BodyPart.RIGHT_UPPER_ARM to setOf(BodyPart.RIGHT_LOWER_ARM), + BodyPart.LEFT_LOWER_ARM to setOf(BodyPart.LEFT_UPPER_ARM), + BodyPart.RIGHT_LOWER_ARM to setOf(BodyPart.RIGHT_UPPER_ARM), + BodyPart.LEFT_HAND to setOf(BodyPart.LEFT_LOWER_ARM, BodyPart.LEFT_UPPER_ARM), + BodyPart.RIGHT_HAND to setOf(BodyPart.RIGHT_LOWER_ARM, BodyPart.RIGHT_UPPER_ARM), + + // TODO could be removed once ImputeFingersProcessor is done vvv + BodyPart.LEFT_THUMB_METACARPAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_THUMB_PROXIMAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_THUMB_DISTAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_INDEX_PROXIMAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_INDEX_INTERMEDIATE to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_INDEX_DISTAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_MIDDLE_PROXIMAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_MIDDLE_INTERMEDIATE to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_MIDDLE_DISTAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_RING_PROXIMAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_RING_INTERMEDIATE to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_RING_DISTAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_LITTLE_PROXIMAL to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_LITTLE_INTERMEDIATE to setOf(BodyPart.LEFT_HAND), + BodyPart.LEFT_LITTLE_DISTAL to setOf(BodyPart.LEFT_HAND), + BodyPart.RIGHT_THUMB_METACARPAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_THUMB_PROXIMAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_THUMB_DISTAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_INDEX_PROXIMAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_INDEX_INTERMEDIATE to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_INDEX_DISTAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_MIDDLE_PROXIMAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_MIDDLE_INTERMEDIATE to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_MIDDLE_DISTAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_RING_PROXIMAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_RING_INTERMEDIATE to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_RING_DISTAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_LITTLE_PROXIMAL to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_LITTLE_INTERMEDIATE to setOf(BodyPart.RIGHT_HAND), + BodyPart.RIGHT_LITTLE_DISTAL to setOf(BodyPart.RIGHT_HAND), + ) + + override fun process(state: SkeletonState): SkeletonState { + val rawBones = state.rawBones + + return state.copy( + rawBones = rawBones.mapValues { (bodyPart, bone) -> + if (bone.isActive) return@mapValues bone + + bone.copy(rawRotation = computeFallbackRotation(rawBones, bodyPart)) + }, + ) + } + + private fun computeFallbackRotation(rawBones: Map, bodyPart: BodyPart): Quaternion { + // Fallback bone's rotation + missingToFallbacks[bodyPart] + ?.firstNotNullOfOrNull { part -> + rawBones[part]?.takeIf { it.isActive } + } + ?.let { return it.rawRotation } + + // First active parent bone's yaw + rawBones[ + findBodyPartParent(bodyPart) { part -> + rawBones[part]?.isActive == true + }, + ]?.let { return it.rawRotation.project(Vector3.POS_Y).unit() } + + // Fallback to identity + return Quaternion.IDENTITY + } } +/** + * Handles imputing the rotation of spine bones that are not actively receiving data from the rotations + * of nearby bones. + * + * Similar to FallbackProcessor but specialized for the spine, using more data and math for better results. + */ class ImputeSpineProcessor(val settings: Settings) : SkeletonProcessor { override fun process(state: SkeletonState): SkeletonState = state } diff --git a/server/core/src/main/java/dev/slimevr/tapdetection/behaviours.kt b/server/core/src/main/java/dev/slimevr/tapdetection/behaviours.kt index 26f5182e3..2daa81184 100644 --- a/server/core/src/main/java/dev/slimevr/tapdetection/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/tapdetection/behaviours.kt @@ -70,7 +70,14 @@ class TapDetectionBasicBehaviour : TapDetectionBehaviour { .firstOrNull { it in trackersBodyParts } ?: BodyPart.RIGHT_UPPER_LEG trackers.map { tracker -> - val trackerTapDetectionState = createTrackerTapDetectionState(tapDetectionConfig, setupMode, tracker, yawResetBodyPart, fullResetBodyPart, mountingResetBodyPart) + val trackerTapDetectionState = createTrackerTapDetectionState( + tapDetectionConfig, + setupMode, + tracker, + yawResetBodyPart, + fullResetBodyPart, + mountingResetBodyPart, + ) // Inner flow emits whenever a tracker's rawAcceleration is updated tracker.context.state diff --git a/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt b/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt index a9e8c2a05..59db381d2 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt @@ -3,15 +3,18 @@ package dev.slimevr.tracker import dev.slimevr.skeleton.SkeletonActions import dev.slimevr.util.safeLaunch import io.github.axisangles.ktmath.Quaternion +import kotlinx.coroutines.ExperimentalCoroutinesApi import kotlinx.coroutines.delay import kotlinx.coroutines.flow.distinctUntilChanged import kotlinx.coroutines.flow.distinctUntilChangedBy import kotlinx.coroutines.flow.filter +import kotlinx.coroutines.flow.flatMapLatest import kotlinx.coroutines.flow.launchIn import kotlinx.coroutines.flow.map import kotlinx.coroutines.flow.onEach import kotlinx.coroutines.isActive import solarxr_protocol.datatypes.BodyPart +import solarxr_protocol.datatypes.TrackerStatus import kotlin.concurrent.atomics.AtomicInt import kotlin.concurrent.atomics.ExperimentalAtomicApi import kotlin.concurrent.atomics.incrementAndFetch @@ -201,17 +204,34 @@ class TrackerTPSBehaviour : TrackerBehaviour { } class TrackerToSkeletonBehaviour : TrackerBehaviour { + var lastBodyPartSent: BodyPart? = null + + @OptIn(ExperimentalCoroutinesApi::class) override fun observe(receiver: Tracker) { receiver.context.state - .filter { it.bodyPart != null } - .map { Pair(it.bodyPart, it.rotation) } - .distinctUntilChanged() - .onEach { (bodyPart, rotation) -> - bodyPart?.let { + .distinctUntilChangedBy { it.status to it.bodyPart } + .onEach { _ -> + // Tell the skeleton the tracker has stopped sending data to the last bone it was sending data to. + lastBodyPartSent?.let { receiver.appContext.skeleton.context.dispatch( - SkeletonActions.SetBoneRotation(it, rotation), + SkeletonActions.DisableBone(it), ) + lastBodyPartSent = null } - }.launchIn(receiver.context.scope) + } + .flatMapLatest { _ -> + receiver.context.state + .distinctUntilChangedBy { it.rotation } + .onEach { trackerState -> + if (trackerState.bodyPart != null && trackerState.status == TrackerStatus.OK) { + // Send data to the skeleton + receiver.appContext.skeleton.context.dispatch( + SkeletonActions.SetBoneRotation(trackerState.bodyPart, trackerState.rotation), + ) + lastBodyPartSent = trackerState.bodyPart + } + } + } + .launchIn(receiver.context.scope) } }