mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Fallback skeleton processor
This commit is contained in:
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)
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user