diff --git a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt index d924a06be..37fc1d88c 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt @@ -5,157 +5,154 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart -// For fingers and toes -// TODO: should constrain splay separately from swing. -private val DIGIT_CONSTRAINT = TwistSwingConstraint( - 10f * FastMath.DEG_TO_RAD, - 115f * FastMath.DEG_TO_RAD, +private val FINGER_CONSTRAINT = LooseAxisConstraint( + 25f * FastMath.DEG_TO_RAD, + Vector3.POS_Z, +) +private val THUMB_CONSTRAINT = LooseAxisConstraint( + 35f * FastMath.DEG_TO_RAD, + Vector3.POS_X, +) +private val TOE_CONSTRAINT = LooseAxisConstraint( + 25f * FastMath.DEG_TO_RAD, + Vector3.POS_X, ) -// TODO Actually figure out good values for these constraints, maybe a source would be -// good? val BODY_PART_CONSTRAINT_MAP: BodyPartMap = BodyPartMap( mapOf( // Left arm BodyPart.LEFT_SHOULDER to TwistSwingConstraint( 105f * FastMath.DEG_TO_RAD, + 50f * FastMath.DEG_TO_RAD, + ), + BodyPart.LEFT_UPPER_ARM to TwistConstraint( 45f * FastMath.DEG_TO_RAD, ), - BodyPart.LEFT_UPPER_ARM to TwistSwingConstraint( - 120f * FastMath.DEG_TO_RAD, - 180f * FastMath.DEG_TO_RAD, - ), BodyPart.LEFT_LOWER_ARM to LooseHingeConstraint( - -180f * FastMath.DEG_TO_RAD, + -165f * FastMath.DEG_TO_RAD, 0f * FastMath.DEG_TO_RAD, - 40f * FastMath.DEG_TO_RAD, + 55f * FastMath.DEG_TO_RAD, ), BodyPart.LEFT_HAND to TwistSwingConstraint( - 120f * FastMath.DEG_TO_RAD, - 120f * FastMath.DEG_TO_RAD, + 70f * FastMath.DEG_TO_RAD, + 95f * FastMath.DEG_TO_RAD, ), // Right arm BodyPart.RIGHT_SHOULDER to TwistSwingConstraint( 105f * FastMath.DEG_TO_RAD, + 50f * FastMath.DEG_TO_RAD, + ), + BodyPart.RIGHT_UPPER_ARM to TwistConstraint( 45f * FastMath.DEG_TO_RAD, ), - BodyPart.RIGHT_UPPER_ARM to TwistSwingConstraint( - 120f * FastMath.DEG_TO_RAD, - 180f * FastMath.DEG_TO_RAD, - ), BodyPart.RIGHT_LOWER_ARM to LooseHingeConstraint( - -180f * FastMath.DEG_TO_RAD, + -165f * FastMath.DEG_TO_RAD, 0f * FastMath.DEG_TO_RAD, - 40f * FastMath.DEG_TO_RAD, + 55f * FastMath.DEG_TO_RAD, ), BodyPart.RIGHT_HAND to TwistSwingConstraint( - 120f * FastMath.DEG_TO_RAD, - 120f * FastMath.DEG_TO_RAD, + 70f * FastMath.DEG_TO_RAD, + 95f * FastMath.DEG_TO_RAD, ), // Spine BodyPart.UPPER_CHEST to TwistSwingConstraint( 95f * FastMath.DEG_TO_RAD, - 120f * FastMath.DEG_TO_RAD, + 115f * FastMath.DEG_TO_RAD, ), BodyPart.LOWER_CHEST to TwistSwingConstraint( 40f * FastMath.DEG_TO_RAD, - 110f * FastMath.DEG_TO_RAD, + 90f * FastMath.DEG_TO_RAD, ), BodyPart.UPPER_WAIST to TwistSwingConstraint( 40f * FastMath.DEG_TO_RAD, - 110f * FastMath.DEG_TO_RAD, + 90f * FastMath.DEG_TO_RAD, ), BodyPart.LOWER_WAIST to TwistSwingConstraint( 40f * FastMath.DEG_TO_RAD, - 110f * FastMath.DEG_TO_RAD, + 90f * FastMath.DEG_TO_RAD, ), BodyPart.HIP to TwistSwingConstraint( - 65f * FastMath.DEG_TO_RAD, - 120f * FastMath.DEG_TO_RAD, + 60f * FastMath.DEG_TO_RAD, + 100f * FastMath.DEG_TO_RAD, ), // Left leg BodyPart.LEFT_UPPER_LEG to TwistSwingConstraint( 120f * FastMath.DEG_TO_RAD, - 170f * FastMath.DEG_TO_RAD, + 165f * FastMath.DEG_TO_RAD, ), BodyPart.LEFT_LOWER_LEG to LooseHingeConstraint( - -5f * FastMath.DEG_TO_RAD, - 180f * FastMath.DEG_TO_RAD, - 40f * FastMath.DEG_TO_RAD, + -6f * FastMath.DEG_TO_RAD, + 165f * FastMath.DEG_TO_RAD, + 50f * FastMath.DEG_TO_RAD, + ), + BodyPart.LEFT_FOOT to TwistSwingConstraint( + 35f * FastMath.DEG_TO_RAD, + 55f * FastMath.DEG_TO_RAD, ), - // TODO enforcing constraint looks bad -// BodyPart.LEFT_FOOT to LooseHingeConstraint( -// -60f * FastMath.DEG_TO_RAD, -// 90f * FastMath.DEG_TO_RAD, -// 80f * FastMath.DEG_TO_RAD, -// ), // Right leg BodyPart.RIGHT_UPPER_LEG to TwistSwingConstraint( 120f * FastMath.DEG_TO_RAD, - 170f * FastMath.DEG_TO_RAD, + 165f * FastMath.DEG_TO_RAD, ), BodyPart.RIGHT_LOWER_LEG to LooseHingeConstraint( - -5f * FastMath.DEG_TO_RAD, - 180f * FastMath.DEG_TO_RAD, - 40f * FastMath.DEG_TO_RAD, + -6f * FastMath.DEG_TO_RAD, + 165f * FastMath.DEG_TO_RAD, + 50f * FastMath.DEG_TO_RAD, + ), + BodyPart.RIGHT_FOOT to TwistSwingConstraint( + 35f * FastMath.DEG_TO_RAD, + 55f * FastMath.DEG_TO_RAD, ), -// BodyPart.RIGHT_FOOT to LooseHingeConstraint( -// -60f * FastMath.DEG_TO_RAD, -// 90f * FastMath.DEG_TO_RAD, -// 80f * FastMath.DEG_TO_RAD, -// ), // Left fingers - BodyPart.LEFT_THUMB_METACARPAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_THUMB_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_THUMB_DISTAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_INDEX_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_INDEX_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.LEFT_INDEX_DISTAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_MIDDLE_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_MIDDLE_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.LEFT_MIDDLE_DISTAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_RING_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_RING_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.LEFT_RING_DISTAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_LITTLE_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.LEFT_LITTLE_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.LEFT_LITTLE_DISTAL to DIGIT_CONSTRAINT, + BodyPart.LEFT_THUMB_METACARPAL to THUMB_CONSTRAINT, + BodyPart.LEFT_THUMB_PROXIMAL to THUMB_CONSTRAINT, + BodyPart.LEFT_THUMB_DISTAL to THUMB_CONSTRAINT, + BodyPart.LEFT_INDEX_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.LEFT_INDEX_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.LEFT_INDEX_DISTAL to FINGER_CONSTRAINT, + BodyPart.LEFT_MIDDLE_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.LEFT_MIDDLE_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.LEFT_MIDDLE_DISTAL to FINGER_CONSTRAINT, + BodyPart.LEFT_RING_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.LEFT_RING_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.LEFT_RING_DISTAL to FINGER_CONSTRAINT, + BodyPart.LEFT_LITTLE_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.LEFT_LITTLE_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.LEFT_LITTLE_DISTAL to FINGER_CONSTRAINT, // Right fingers - BodyPart.RIGHT_THUMB_METACARPAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_THUMB_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_THUMB_DISTAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_INDEX_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_INDEX_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_INDEX_DISTAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_MIDDLE_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_MIDDLE_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_MIDDLE_DISTAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_RING_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_RING_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_RING_DISTAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_LITTLE_PROXIMAL to DIGIT_CONSTRAINT, - BodyPart.RIGHT_LITTLE_INTERMEDIATE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_LITTLE_DISTAL to DIGIT_CONSTRAINT, + BodyPart.RIGHT_THUMB_METACARPAL to THUMB_CONSTRAINT, + BodyPart.RIGHT_THUMB_PROXIMAL to THUMB_CONSTRAINT, + BodyPart.RIGHT_THUMB_DISTAL to THUMB_CONSTRAINT, + BodyPart.RIGHT_INDEX_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_INDEX_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.RIGHT_INDEX_DISTAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_MIDDLE_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_MIDDLE_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.RIGHT_MIDDLE_DISTAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_RING_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_RING_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.RIGHT_RING_DISTAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_LITTLE_PROXIMAL to FINGER_CONSTRAINT, + BodyPart.RIGHT_LITTLE_INTERMEDIATE to FINGER_CONSTRAINT, + BodyPart.RIGHT_LITTLE_DISTAL to FINGER_CONSTRAINT, - // Left toes - BodyPart.LEFT_BIG_TOE to DIGIT_CONSTRAINT, - BodyPart.LEFT_INDEX_TOE to DIGIT_CONSTRAINT, - BodyPart.LEFT_MIDDLE_TOE to DIGIT_CONSTRAINT, - BodyPart.LEFT_RING_TOE to DIGIT_CONSTRAINT, - BodyPart.LEFT_LITTLE_TOE to DIGIT_CONSTRAINT, - - // Right toes - BodyPart.RIGHT_BIG_TOE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_INDEX_TOE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_MIDDLE_TOE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_RING_TOE to DIGIT_CONSTRAINT, - BodyPart.RIGHT_LITTLE_TOE to DIGIT_CONSTRAINT, + // Toes + BodyPart.LEFT_BIG_TOE to TOE_CONSTRAINT, + BodyPart.LEFT_INDEX_TOE to TOE_CONSTRAINT, + BodyPart.LEFT_MIDDLE_TOE to TOE_CONSTRAINT, + BodyPart.LEFT_RING_TOE to TOE_CONSTRAINT, + BodyPart.LEFT_LITTLE_TOE to TOE_CONSTRAINT, + BodyPart.RIGHT_BIG_TOE to TOE_CONSTRAINT, + BodyPart.RIGHT_INDEX_TOE to TOE_CONSTRAINT, + BodyPart.RIGHT_MIDDLE_TOE to TOE_CONSTRAINT, + BodyPart.RIGHT_RING_TOE to TOE_CONSTRAINT, + BodyPart.RIGHT_LITTLE_TOE to TOE_CONSTRAINT, ), ) diff --git a/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt b/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt index cc4f39da1..18fd1f0b0 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt @@ -51,6 +51,20 @@ data class TwistSwingConstraint( } } +data class TwistConstraint( + /** + * Twist range in radians. + */ + val twist: Float = 0f, + val twistAxis: Vector3 = Vector3.NEG_Y, +) : Constraint { + override fun apply(localRotation: Quaternion): Quaternion { + var (swingQ, twistQ) = decomposeToSwingTwist(localRotation, twistAxis) + twistQ = constrainAngle(twistQ, twist) + return swingQ * twistQ + } +} + data class HingeConstraint( /** * Minimum rotation in radians. diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/arms.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/arms.kt new file mode 100644 index 000000000..fd824b630 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/arms.kt @@ -0,0 +1,107 @@ +package dev.slimevr.skeleton.inputprocessors + +import dev.slimevr.skeleton.BodyPartMap +import dev.slimevr.skeleton.InputSkeleton +import dev.slimevr.skeleton.SkeletonInputProcessor +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import solarxr_protocol.datatypes.BodyPart + +data class ArmSource( + val bodyPart: BodyPart, + val needsActive: Boolean = true, + // null means it uses all remaining. + val ratio: Float? = null, + // For body-part-prerequisites.kt + val reliable: Boolean = false, +) + +val ARM_TO_SOURCE = arrayOf( + BodyPart.LEFT_SHOULDER to arrayOf( + ArmSource(BodyPart.LEFT_UPPER_ARM, true, 0.04f), + ArmSource(BodyPart.UPPER_CHEST, false, null, true), + ), + BodyPart.RIGHT_SHOULDER to arrayOf( + ArmSource(BodyPart.RIGHT_UPPER_ARM, true, 0.04f), + ArmSource(BodyPart.UPPER_CHEST, false, null, true), + ), + + BodyPart.LEFT_UPPER_ARM to arrayOf( + ArmSource(BodyPart.LEFT_LOWER_ARM, true, 0.5f), + ArmSource(BodyPart.LEFT_SHOULDER, true, 0.5f), + ArmSource(BodyPart.UPPER_CHEST, false, 0.75f), + ArmSource(BodyPart.LEFT_HAND), + ), + BodyPart.RIGHT_UPPER_ARM to arrayOf( + ArmSource(BodyPart.RIGHT_LOWER_ARM, true, 0.5f), + ArmSource(BodyPart.RIGHT_SHOULDER, true, 0.5f), + ArmSource(BodyPart.UPPER_CHEST, false, 0.75f), + ArmSource(BodyPart.RIGHT_HAND), + ), + + BodyPart.LEFT_LOWER_ARM to arrayOf( + ArmSource(BodyPart.LEFT_HAND, true, 0.8f), + ArmSource(BodyPart.LEFT_UPPER_ARM, true), + ArmSource(BodyPart.LEFT_SHOULDER, true, 0.2f), + ArmSource(BodyPart.UPPER_CHEST, false), + ), + BodyPart.RIGHT_LOWER_ARM to arrayOf( + ArmSource(BodyPart.RIGHT_HAND, true, 0.8f), + ArmSource(BodyPart.RIGHT_UPPER_ARM, true), + ArmSource(BodyPart.RIGHT_SHOULDER, true, 0.2f), + ArmSource(BodyPart.UPPER_CHEST, false), + ), + + BodyPart.LEFT_HAND to arrayOf( + ArmSource(BodyPart.LEFT_LOWER_ARM), + ArmSource(BodyPart.LEFT_UPPER_ARM), + ArmSource(BodyPart.LEFT_SHOULDER, true, 0.25f), + ArmSource(BodyPart.UPPER_CHEST, false), + ), + BodyPart.RIGHT_HAND to arrayOf( + ArmSource(BodyPart.RIGHT_LOWER_ARM), + ArmSource(BodyPart.RIGHT_UPPER_ARM), + ArmSource(BodyPart.RIGHT_SHOULDER, true, 0.25f), + ArmSource(BodyPart.UPPER_CHEST, false), + ), +) + +/** + * Handles imputing missing arm bones from other arm bones and the upper chest. + * + * This processor must run after the spine processor. + */ +class ArmsInputProcessor : SkeletonInputProcessor { + override fun process(mutableInputSkeleton: InputSkeleton, skeletonHeight: Float) { + val boneInputs = BodyPartMap(mutableInputSkeleton) + for ((bodyPart, sources) in ARM_TO_SOURCE) { + val bone = boneInputs[bodyPart] ?: continue + if (bone.isRotationActive && bone.isAccelerationActive) continue + + var ratioSum = 0f + var rotation: Quaternion? = null + var acceleration: Vector3? = null + for ((index, source) in sources.withIndex()) { + val sourceBone = boneInputs[source.bodyPart] ?: continue + if (source.needsActive && !sourceBone.isRotationActive) continue + + val isLastSource = index == sources.count() - 1 + val remainingRatio = 1f - ratioSum + val ratio = (if (!isLastSource) source.ratio?.coerceAtMost(remainingRatio) else null) ?: remainingRatio + if (!bone.isRotationActive) { + rotation = (rotation ?: Quaternion.ZERO) + (sourceBone.rotation * ratio) + } + if (!bone.isAccelerationActive) { + acceleration = (acceleration ?: Vector3.ZERO) + (sourceBone.acceleration * ratio) + } + + ratioSum += ratio + if (ratioSum >= 1f) break + } + + if ((rotation == null || rotation == bone.rotation) && (acceleration == null || acceleration == bone.acceleration)) continue + + mutableInputSkeleton[bodyPart] = bone.copy(rotation = rotation?.unit() ?: bone.rotation, acceleration = acceleration ?: bone.acceleration) + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/direct-link.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/direct-link.kt index b7e52e441..6d2c9c499 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/direct-link.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/direct-link.kt @@ -5,27 +5,12 @@ import dev.slimevr.skeleton.SkeletonInputProcessor import solarxr_protocol.datatypes.BodyPart /** - * Linked BodyPart to the BodyPart it takes its rotation from. - * - * Order matters: an untracked arm cascades down from the upper chest in one pass. + * Linked BodyPart to the BodyPart it takes its rotation from. Order matters. */ val BONE_DIRECT_LINK_SOURCES = arrayOf( BodyPart.HEAD to BodyPart.NECK, - BodyPart.LEFT_FOOT to BodyPart.LEFT_LOWER_LEG, BodyPart.RIGHT_FOOT to BodyPart.RIGHT_LOWER_LEG, - - BodyPart.LEFT_SHOULDER to BodyPart.UPPER_CHEST, - BodyPart.RIGHT_SHOULDER to BodyPart.UPPER_CHEST, - - BodyPart.LEFT_UPPER_ARM to BodyPart.LEFT_SHOULDER, - BodyPart.RIGHT_UPPER_ARM to BodyPart.RIGHT_SHOULDER, - - BodyPart.LEFT_LOWER_ARM to BodyPart.LEFT_UPPER_ARM, - BodyPart.RIGHT_LOWER_ARM to BodyPart.RIGHT_UPPER_ARM, - - BodyPart.LEFT_HAND to BodyPart.LEFT_LOWER_ARM, - BodyPart.RIGHT_HAND to BodyPart.RIGHT_LOWER_ARM, ) /** @@ -37,8 +22,9 @@ class DirectLinkInputProcessor : SkeletonInputProcessor { val bone = mutableInputSkeleton[bodyPart] ?: continue if (bone.isRotationActive && bone.isAccelerationActive) continue - val rotation = if (!bone.isRotationActive) mutableInputSkeleton[source]?.rotation ?: bone.rotation else bone.rotation - val acceleration = if (!bone.isAccelerationActive) mutableInputSkeleton[source]?.acceleration ?: bone.acceleration else bone.acceleration + val source = mutableInputSkeleton[source] + val rotation = if (!bone.isRotationActive) source?.rotation ?: bone.rotation else bone.rotation + val acceleration = if (!bone.isAccelerationActive) source?.acceleration ?: bone.acceleration else bone.acceleration if (rotation == bone.rotation && acceleration == bone.acceleration) continue 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 22fba43af..a525913ed 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -8,6 +8,7 @@ import dev.slimevr.skeleton.computedprocessors.VelocityComputedProcessor import dev.slimevr.skeleton.fkprocessors.FootPlantFkProcessor import dev.slimevr.skeleton.fkprocessors.LocalizerFkProcessor import dev.slimevr.skeleton.fkprocessors.ToeSnapFkProcessor +import dev.slimevr.skeleton.inputprocessors.ArmsInputProcessor import dev.slimevr.skeleton.inputprocessors.BoneOffsetInputProcessor import dev.slimevr.skeleton.inputprocessors.ConstraintInputProcessor import dev.slimevr.skeleton.inputprocessors.DirectLinkInputProcessor @@ -280,6 +281,7 @@ class Skeleton( NeckInputProcessor(), UpperLegsRollAlignInputProcessor(settings), DirectLinkInputProcessor(), + ArmsInputProcessor(), FingersInputProcessor(), ToesInputProcessor(), ConstraintInputProcessor(settings), diff --git a/server/core/src/main/java/dev/slimevr/solarxr/rpc/body-part-prerequisites.kt b/server/core/src/main/java/dev/slimevr/solarxr/rpc/body-part-prerequisites.kt index d8dd01897..12853981c 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/rpc/body-part-prerequisites.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/rpc/body-part-prerequisites.kt @@ -4,6 +4,7 @@ import dev.slimevr.AppContextProvider import dev.slimevr.VRServer import dev.slimevr.skeleton.BODY_PART_IK_CHAIN_MAP import dev.slimevr.skeleton.BodyPartMap +import dev.slimevr.skeleton.inputprocessors.ARM_TO_SOURCE import dev.slimevr.skeleton.inputprocessors.BONE_DIRECT_LINK_SOURCES import dev.slimevr.skeleton.inputprocessors.SPINE_IMPUTED_PARTS import dev.slimevr.skeleton.iterateBodyPartHierarchy @@ -27,6 +28,11 @@ import solarxr_protocol.rpc.BodyPartPrerequisitesResponse import solarxr_protocol.rpc.BodyPartRequirement private val DIRECT_LINK_SOURCE_OF: BodyPartMap = BodyPartMap(BONE_DIRECT_LINK_SOURCES.toMap()) +private val ARM_SOURCE_OF: BodyPartMap> = BodyPartMap( + ARM_TO_SOURCE.associate { armSource -> + armSource.first to armSource.second.mapNotNull { if (it.reliable) it.bodyPart else null } + }, +) // Finger and toe bones. Their processors impute each one from its siblings, so one is never a // prerequisite for another: only the hand or foot they hang off is. @@ -53,8 +59,8 @@ private fun trackingPointsFlow(server: VRServer): Flow> = al * List the bones that are solved by IK * * TODO: Ahead of the solver. [dev.slimevr.skeleton.targetprocessors.PositionalTargetProcessor] still - * drops the IK once any arm bone has a tracker, so a controller plus an upper arm leaves the - * forearm copying the upper arm. + * drops the IK once any arm bone has a tracker, so a controller plus an upper arm leaves the + * forearm copying the upper arm. */ internal fun ikSolvedParts(trackers: List, useTrackerPositions: Boolean): Set { if (!useTrackerPositions) return emptySet() @@ -96,8 +102,9 @@ private fun limbPrerequisites(bodyPart: BodyPart): List> { var ancestor = parentOf(bodyPart) while (ancestor != null && ancestor !in SPINE_IMPUTED_PARTS && ancestor != BodyPart.NECK) { - val source = DIRECT_LINK_SOURCE_OF[ancestor] - val followsWithoutTracker = (source != null && source in SPINE_IMPUTED_PARTS) || ancestor in DIGIT_PARTS + val directLink = DIRECT_LINK_SOURCE_OF[ancestor] + val armSources = ARM_SOURCE_OF[ancestor] + val followsWithoutTracker = directLink in SPINE_IMPUTED_PARTS || armSources?.any { it in SPINE_IMPUTED_PARTS } == true || ancestor in DIGIT_PARTS if (!followsWithoutTracker) required.add(listOf(ancestor)) ancestor = parentOf(ancestor) }