Fallback skeleton processor

This commit is contained in:
Erimel committed 2026-07-17 21:54:50 -04:00
1 parent a6020ae4df
commit 5da133cffc
7 files changed
+190 -21

No files matched your search

@@ -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
}
}
@@ -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)
}
@@ -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<BodyPart, BodyPart> = 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<Pair<BodyPart?, BodyPart>>.visitBodyPart(parentBone: BodyPart?, bone: BodyPart, onlyChildren: Boolean) {
if (!onlyChildren) {
yield(Pair(parentBone, bone))
@@ -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<BodyPart, RawBone>, 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<SkeletonBone, Float>) : SkeletonActions
data class PauseTracking(val pause: Boolean) : SkeletonActions
}
@@ -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<BodyPart, List<BodyPart>>,
chain: List<BodyPart>,
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, RawBone>, 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
}
@@ -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
@@ -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)
}
}