diff --git a/.gitignore b/.gitignore index aaa77a2856..9264517ff2 100644 --- a/.gitignore +++ b/.gitignore @@ -41,3 +41,6 @@ build/ # Ignore Android local properties local.properties + +# Ignore temporary config +vrconfig.yml.tmp diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index 783ab650a4..bd241036b1 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -44,6 +44,36 @@ body_part-LEFT_HAND = Left hand body_part-LEFT_UPPER_LEG = Left thigh body_part-LEFT_LOWER_LEG = Left ankle body_part-LEFT_FOOT = Left foot +body_part-LEFT_THUMB_PROXIMAL = Left thumb proximal +body_part-LEFT_THUMB_INTERMEDIATE = Left thumb intermediate +body_part-LEFT_THUMB_DISTAL = Left thumb distal +body_part-LEFT_INDEX_PROXIMAL = Left index proximal +body_part-LEFT_INDEX_INTERMEDIATE = Left index intermediate +body_part-LEFT_INDEX_DISTAL = Left index distal +body_part-LEFT_MIDDLE_PROXIMAL = Left middle proximal +body_part-LEFT_MIDDLE_INTERMEDIATE = Left middle intermediate +body_part-LEFT_MIDDLE_DISTAL = Left middle distal +body_part-LEFT_RING_PROXIMAL = Left ring proximal +body_part-LEFT_RING_INTERMEDIATE = Left ring intermediate +body_part-LEFT_RING_DISTAL = Left ring distal +body_part-LEFT_LITTLE_PROXIMAL = Left little proximal +body_part-LEFT_LITTLE_INTERMEDIATE = Left little intermediate +body_part-LEFT_LITTLE_DISTAL = Left little distal +body_part-RIGHT_THUMB_PROXIMAL = Right thumb proximal +body_part-RIGHT_THUMB_INTERMEDIATE = Right thumb intermediate +body_part-RIGHT_THUMB_DISTAL = Right thumb distal +body_part-RIGHT_INDEX_PROXIMAL = Right index proximal +body_part-RIGHT_INDEX_INTERMEDIATE = Right index intermediate +body_part-RIGHT_INDEX_DISTAL = Right index distal +body_part-RIGHT_MIDDLE_PROXIMAL = Right middle proximal +body_part-RIGHT_MIDDLE_INTERMEDIATE = Right middle intermediate +body_part-RIGHT_MIDDLE_DISTAL = Right middle distal +body_part-RIGHT_RING_PROXIMAL = Right ring proximal +body_part-RIGHT_RING_INTERMEDIATE = Right ring intermediate +body_part-RIGHT_RING_DISTAL = Right ring distal +body_part-RIGHT_LITTLE_PROXIMAL = Right little proximal +body_part-RIGHT_LITTLE_INTERMEDIATE = Right little intermediate +body_part-RIGHT_LITTLE_DISTAL = Right little distal ## Proportions skeleton_bone-NONE = None diff --git a/gui/src/components/commons/BodyPartIcon.tsx b/gui/src/components/commons/BodyPartIcon.tsx index 8bfbbde485..87a01ea47e 100644 --- a/gui/src/components/commons/BodyPartIcon.tsx +++ b/gui/src/components/commons/BodyPartIcon.tsx @@ -15,6 +15,7 @@ import { UpperArmIcon } from './icon/UpperArmIcon'; import { UpperLegIcon } from './icon/UpperLegIcon'; import { WaistIcon } from './icon/WaistIcon'; import { UpperChestIcon } from './icon/UpperChestIcon'; +import { FingersIcon } from './icon/FingersIcon'; // All body parts that are right or left, are by default left! export const mapPart: Record< @@ -86,6 +87,96 @@ export const mapPart: Record< ), [BodyPart.WAIST]: ({ width }) => , + [BodyPart.LEFT_THUMB_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_THUMB_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_THUMB_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_DISTAL]: ({ width }) => ( + + ), }; export function BodyPartIcon({ diff --git a/gui/src/components/commons/icon/FingersIcon.tsx b/gui/src/components/commons/icon/FingersIcon.tsx new file mode 100644 index 0000000000..bd25c88ee2 --- /dev/null +++ b/gui/src/components/commons/icon/FingersIcon.tsx @@ -0,0 +1,15 @@ +export function FingersIcon({ width = 28 }: { width?: number }) { + return ( + + + + + + ); +} diff --git a/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx b/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx index 11609f6186..292b275b3e 100644 --- a/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx +++ b/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx @@ -10,6 +10,7 @@ import { SlimeUpIcon } from '@/components/commons/icon/SlimeUpIcon'; import { BodyPart } from 'solarxr-protocol'; import { PawIcon } from '@/components/commons/icon/PawIcon'; import { useLocaleConfig } from '@/i18n/config'; +import { FingersIcon } from '@/components/commons/icon/FingersIcon'; // All body parts that are right or left, are by default left! export const mapPart: Record< @@ -97,6 +98,96 @@ export const mapPart: Record< ), [BodyPart.WAIST]: ({ width }) => , + [BodyPart.LEFT_THUMB_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_THUMB_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_THUMB_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_INDEX_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_MIDDLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_RING_DISTAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.LEFT_LITTLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_THUMB_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_INDEX_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_MIDDLE_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_RING_DISTAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_PROXIMAL]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_INTERMEDIATE]: ({ width }) => ( + + ), + [BodyPart.RIGHT_LITTLE_DISTAL]: ({ width }) => ( + + ), }; export function MountingBodyPartIcon({ diff --git a/gui/src/utils/skeletonHelper.ts b/gui/src/utils/skeletonHelper.ts index 578174b758..4f93a82035 100644 --- a/gui/src/utils/skeletonHelper.ts +++ b/gui/src/utils/skeletonHelper.ts @@ -223,6 +223,37 @@ export class BoneKind extends Bone { case BodyPart.LEFT_HIP: case BodyPart.RIGHT_HIP: return new Color('pink'); + case BodyPart.LEFT_THUMB_PROXIMAL: + case BodyPart.LEFT_THUMB_INTERMEDIATE: + case BodyPart.LEFT_THUMB_DISTAL: + case BodyPart.LEFT_INDEX_PROXIMAL: + case BodyPart.LEFT_INDEX_INTERMEDIATE: + case BodyPart.LEFT_INDEX_DISTAL: + case BodyPart.LEFT_MIDDLE_PROXIMAL: + case BodyPart.LEFT_MIDDLE_INTERMEDIATE: + case BodyPart.LEFT_MIDDLE_DISTAL: + case BodyPart.LEFT_RING_PROXIMAL: + case BodyPart.LEFT_RING_INTERMEDIATE: + case BodyPart.LEFT_RING_DISTAL: + case BodyPart.LEFT_LITTLE_PROXIMAL: + case BodyPart.LEFT_LITTLE_INTERMEDIATE: + case BodyPart.LEFT_LITTLE_DISTAL: + case BodyPart.RIGHT_THUMB_PROXIMAL: + case BodyPart.RIGHT_THUMB_INTERMEDIATE: + case BodyPart.RIGHT_THUMB_DISTAL: + case BodyPart.RIGHT_INDEX_PROXIMAL: + case BodyPart.RIGHT_INDEX_INTERMEDIATE: + case BodyPart.RIGHT_INDEX_DISTAL: + case BodyPart.RIGHT_MIDDLE_PROXIMAL: + case BodyPart.RIGHT_MIDDLE_INTERMEDIATE: + case BodyPart.RIGHT_MIDDLE_DISTAL: + case BodyPart.RIGHT_RING_PROXIMAL: + case BodyPart.RIGHT_RING_INTERMEDIATE: + case BodyPart.RIGHT_RING_DISTAL: + case BodyPart.RIGHT_LITTLE_PROXIMAL: + case BodyPart.RIGHT_LITTLE_INTERMEDIATE: + case BodyPart.RIGHT_LITTLE_DISTAL: + return new Color('pink'); } } @@ -275,8 +306,81 @@ export class BoneKind extends Bone { case BodyPart.RIGHT_LOWER_ARM: return [BodyPart.RIGHT_HAND]; case BodyPart.LEFT_HAND: - return []; + return [ + BodyPart.LEFT_THUMB_PROXIMAL, + BodyPart.LEFT_INDEX_PROXIMAL, + BodyPart.LEFT_MIDDLE_PROXIMAL, + BodyPart.LEFT_RING_PROXIMAL, + BodyPart.LEFT_LITTLE_PROXIMAL, + ]; case BodyPart.RIGHT_HAND: + return [ + BodyPart.RIGHT_THUMB_PROXIMAL, + BodyPart.RIGHT_INDEX_PROXIMAL, + BodyPart.RIGHT_MIDDLE_PROXIMAL, + BodyPart.RIGHT_RING_PROXIMAL, + BodyPart.RIGHT_LITTLE_PROXIMAL, + ]; + + case BodyPart.LEFT_THUMB_PROXIMAL: + return [BodyPart.LEFT_THUMB_INTERMEDIATE]; + case BodyPart.LEFT_THUMB_INTERMEDIATE: + return [BodyPart.LEFT_THUMB_DISTAL]; + case BodyPart.LEFT_THUMB_DISTAL: + return []; + case BodyPart.LEFT_INDEX_PROXIMAL: + return [BodyPart.LEFT_INDEX_INTERMEDIATE]; + case BodyPart.LEFT_INDEX_INTERMEDIATE: + return [BodyPart.LEFT_INDEX_DISTAL]; + case BodyPart.LEFT_INDEX_DISTAL: + return []; + case BodyPart.LEFT_MIDDLE_PROXIMAL: + return [BodyPart.LEFT_MIDDLE_INTERMEDIATE]; + case BodyPart.LEFT_MIDDLE_INTERMEDIATE: + return [BodyPart.LEFT_MIDDLE_DISTAL]; + case BodyPart.LEFT_MIDDLE_DISTAL: + return []; + case BodyPart.LEFT_RING_PROXIMAL: + return [BodyPart.LEFT_RING_INTERMEDIATE]; + case BodyPart.LEFT_RING_INTERMEDIATE: + return [BodyPart.LEFT_RING_DISTAL]; + case BodyPart.LEFT_RING_DISTAL: + return []; + case BodyPart.LEFT_LITTLE_PROXIMAL: + return [BodyPart.LEFT_LITTLE_INTERMEDIATE]; + case BodyPart.LEFT_LITTLE_INTERMEDIATE: + return [BodyPart.LEFT_LITTLE_DISTAL]; + case BodyPart.LEFT_LITTLE_DISTAL: + return []; + case BodyPart.RIGHT_THUMB_PROXIMAL: + return [BodyPart.RIGHT_THUMB_INTERMEDIATE]; + case BodyPart.RIGHT_THUMB_INTERMEDIATE: + return [BodyPart.RIGHT_THUMB_DISTAL]; + case BodyPart.RIGHT_THUMB_DISTAL: + return []; + case BodyPart.RIGHT_INDEX_PROXIMAL: + return [BodyPart.RIGHT_INDEX_INTERMEDIATE]; + case BodyPart.RIGHT_INDEX_INTERMEDIATE: + return [BodyPart.RIGHT_INDEX_DISTAL]; + case BodyPart.RIGHT_INDEX_DISTAL: + return []; + case BodyPart.RIGHT_MIDDLE_PROXIMAL: + return [BodyPart.RIGHT_MIDDLE_INTERMEDIATE]; + case BodyPart.RIGHT_MIDDLE_INTERMEDIATE: + return [BodyPart.RIGHT_MIDDLE_DISTAL]; + case BodyPart.RIGHT_MIDDLE_DISTAL: + return []; + case BodyPart.RIGHT_RING_PROXIMAL: + return [BodyPart.RIGHT_RING_INTERMEDIATE]; + case BodyPart.RIGHT_RING_INTERMEDIATE: + return [BodyPart.RIGHT_RING_DISTAL]; + case BodyPart.RIGHT_RING_DISTAL: + return []; + case BodyPart.RIGHT_LITTLE_PROXIMAL: + return [BodyPart.RIGHT_LITTLE_INTERMEDIATE]; + case BodyPart.RIGHT_LITTLE_INTERMEDIATE: + return [BodyPart.RIGHT_LITTLE_DISTAL]; + case BodyPart.RIGHT_LITTLE_DISTAL: return []; } } @@ -329,6 +433,67 @@ export class BoneKind extends Bone { return BodyPart.LEFT_LOWER_ARM; case BodyPart.RIGHT_HAND: return BodyPart.RIGHT_LOWER_ARM; + + case BodyPart.LEFT_THUMB_PROXIMAL: + return BodyPart.LEFT_HAND; + case BodyPart.LEFT_THUMB_INTERMEDIATE: + return BodyPart.LEFT_THUMB_PROXIMAL; + case BodyPart.LEFT_THUMB_DISTAL: + return BodyPart.LEFT_THUMB_INTERMEDIATE; + case BodyPart.LEFT_INDEX_PROXIMAL: + return BodyPart.LEFT_HAND; + case BodyPart.LEFT_INDEX_INTERMEDIATE: + return BodyPart.LEFT_INDEX_PROXIMAL; + case BodyPart.LEFT_INDEX_DISTAL: + return BodyPart.LEFT_INDEX_INTERMEDIATE; + case BodyPart.LEFT_MIDDLE_PROXIMAL: + return BodyPart.LEFT_HAND; + case BodyPart.LEFT_MIDDLE_INTERMEDIATE: + return BodyPart.LEFT_MIDDLE_PROXIMAL; + case BodyPart.LEFT_MIDDLE_DISTAL: + return BodyPart.LEFT_MIDDLE_INTERMEDIATE; + case BodyPart.LEFT_RING_PROXIMAL: + return BodyPart.LEFT_HAND; + case BodyPart.LEFT_RING_INTERMEDIATE: + return BodyPart.LEFT_RING_PROXIMAL; + case BodyPart.LEFT_RING_DISTAL: + return BodyPart.LEFT_RING_INTERMEDIATE; + case BodyPart.LEFT_LITTLE_PROXIMAL: + return BodyPart.LEFT_HAND; + case BodyPart.LEFT_LITTLE_INTERMEDIATE: + return BodyPart.LEFT_LITTLE_PROXIMAL; + case BodyPart.LEFT_LITTLE_DISTAL: + return BodyPart.LEFT_LITTLE_INTERMEDIATE; + case BodyPart.RIGHT_THUMB_PROXIMAL: + return BodyPart.RIGHT_HAND; + case BodyPart.RIGHT_THUMB_INTERMEDIATE: + return BodyPart.RIGHT_THUMB_PROXIMAL; + case BodyPart.RIGHT_THUMB_DISTAL: + return BodyPart.RIGHT_THUMB_INTERMEDIATE; + case BodyPart.RIGHT_INDEX_PROXIMAL: + return BodyPart.RIGHT_HAND; + case BodyPart.RIGHT_INDEX_INTERMEDIATE: + return BodyPart.RIGHT_INDEX_PROXIMAL; + case BodyPart.RIGHT_INDEX_DISTAL: + return BodyPart.RIGHT_INDEX_INTERMEDIATE; + case BodyPart.RIGHT_MIDDLE_PROXIMAL: + return BodyPart.RIGHT_HAND; + case BodyPart.RIGHT_MIDDLE_INTERMEDIATE: + return BodyPart.RIGHT_MIDDLE_PROXIMAL; + case BodyPart.RIGHT_MIDDLE_DISTAL: + return BodyPart.RIGHT_MIDDLE_INTERMEDIATE; + case BodyPart.RIGHT_RING_PROXIMAL: + return BodyPart.RIGHT_HAND; + case BodyPart.RIGHT_RING_INTERMEDIATE: + return BodyPart.RIGHT_RING_PROXIMAL; + case BodyPart.RIGHT_RING_DISTAL: + return BodyPart.RIGHT_RING_INTERMEDIATE; + case BodyPart.RIGHT_LITTLE_PROXIMAL: + return BodyPart.RIGHT_HAND; + case BodyPart.RIGHT_LITTLE_INTERMEDIATE: + return BodyPart.RIGHT_LITTLE_PROXIMAL; + case BodyPart.RIGHT_LITTLE_DISTAL: + return BodyPart.RIGHT_LITTLE_INTERMEDIATE; } } } diff --git a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt index b695037aab..f86c4525d8 100644 --- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt +++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt @@ -1,5 +1,6 @@ package dev.slimevr.filtering +import com.jme3.system.NanoTimer import dev.slimevr.VRServer import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Quaternion.Companion.IDENTITY @@ -17,25 +18,25 @@ private const val PREDICT_BUFFER = 6 class QuaternionMovingAverage( val type: TrackerFilters, - var amount: Float, - initialRotation: Quaternion, + var amount: Float = 0f, + initialRotation: Quaternion = IDENTITY, ) { - var filteredQuaternion = IDENTITY private var smoothFactor = 0f private var predictFactor = 0f private lateinit var rotBuffer: CircularArrayList private var latestQuaternion = IDENTITY private var smoothingQuaternion = IDENTITY - private val fpsTimer = VRServer.instance.fpsTimer + private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer() private var frameCounter = 0 private var lastAmt = 0f + var filteredQuaternion = IDENTITY init { // amount should range from 0 to 1. // GUI should clamp it from 0.01 (1%) or 0.1 (10%) // to 1 (100%). amount = amount.coerceAtLeast(0f) - if (type === TrackerFilters.SMOOTHING) { + if (type == TrackerFilters.SMOOTHING) { // lower smoothFactor = more smoothing smoothFactor = SMOOTH_MULTIPLIER * (1 - amount.coerceAtMost(1f)) + SMOOTH_MIN // Totally a hack @@ -43,14 +44,12 @@ class QuaternionMovingAverage( smoothFactor /= amount } } - if (type === TrackerFilters.PREDICTION) { + if (type == TrackerFilters.PREDICTION) { // higher predictFactor = more prediction predictFactor = PREDICT_MULTIPLIER * amount + PREDICT_MIN rotBuffer = CircularArrayList(PREDICT_BUFFER) } - filteredQuaternion = initialRotation - latestQuaternion = initialRotation - smoothingQuaternion = initialRotation + resetQuats(initialRotation) } // Runs at up to 1000hz. We use a timer to make it framerate-independent @@ -70,7 +69,7 @@ class QuaternionMovingAverage( // Slerps the target rotation to that predicted rotation by amt filteredQuaternion = filteredQuaternion.interpR(quatBuf, amt) } - } else { // Smoothing + } else if (type == TrackerFilters.SMOOTHING) { // Increase every update for linear interpolation frameCounter++ @@ -90,24 +89,35 @@ class QuaternionMovingAverage( // Smooth towards the target rotation by the slerp factor filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt) + } else { + // No filtering; just keep track of rotations (for going over 180 degrees) + filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion) } } @Synchronized fun addQuaternion(q: Quaternion) { - if (type === TrackerFilters.PREDICTION) { + if (type == TrackerFilters.PREDICTION) { if (rotBuffer.size == rotBuffer.capacity()) { rotBuffer.removeLast() } // Gets and stores the rotation between the last 2 quaternions rotBuffer.add(latestQuaternion.inv().times(q)) - } else { // Smoothing + } else if (type == TrackerFilters.SMOOTHING) { frameCounter = 0 lastAmt = 0f smoothingQuaternion = filteredQuaternion + } else { + smoothingQuaternion = filteredQuaternion } latestQuaternion = q } + + fun resetQuats(q: Quaternion) { + filteredQuaternion = q + latestQuaternion = q + smoothingQuaternion = q + } } diff --git a/server/core/src/main/java/dev/slimevr/osc/UnityArmature.kt b/server/core/src/main/java/dev/slimevr/osc/UnityArmature.kt index 6d10111925..79b9dc0da6 100644 --- a/server/core/src/main/java/dev/slimevr/osc/UnityArmature.kt +++ b/server/core/src/main/java/dev/slimevr/osc/UnityArmature.kt @@ -12,9 +12,12 @@ import io.github.axisangles.ktmath.Vector3 */ class UnityArmature(localRot: Boolean) { + // Head private val headNode = TransformNode(localRotation = localRot) private val neckTailNode = TransformNode(localRotation = localRot) private val neckHeadNode = TransformNode(localRotation = localRot) + + // Spine private val upperChestNode = TransformNode(localRotation = localRot) private val chestNode = TransformNode(localRotation = localRot) private val spineTailNode = TransformNode(localRotation = localRot) @@ -22,12 +25,16 @@ class UnityArmature(localRot: Boolean) { private val hipsNode = TransformNode(localRotation = localRot) private val leftHipNode = TransformNode(localRotation = localRot) private val rightHipNode = TransformNode(localRotation = localRot) + + // Legs private val leftKneeNode = TransformNode(localRotation = localRot) private val leftAnkleNode = TransformNode(localRotation = localRot) private val leftFootNode = TransformNode(localRotation = localRot) private val rightKneeNode = TransformNode(localRotation = localRot) private val rightAnkleNode = TransformNode(localRotation = localRot) private val rightFootNode = TransformNode(localRotation = localRot) + + // Arms private val leftShoulderHeadNode = TransformNode(localRotation = localRot) private val rightShoulderHeadNode = TransformNode(localRotation = localRot) private val leftShoulderTailNode = TransformNode(localRotation = localRot) @@ -36,8 +43,50 @@ class UnityArmature(localRot: Boolean) { private val rightElbowNode = TransformNode(localRotation = localRot) private val leftWristNode = TransformNode(localRotation = localRot) private val rightWristNode = TransformNode(localRotation = localRot) - private val leftHandNode = TransformNode(localRotation = localRot) - private val rightHandNode = TransformNode(localRotation = localRot) + private val leftHandNode = TransformNode(localRotation = !localRot) + private val rightHandNode = TransformNode(localRotation = !localRot) + + // Fingers + val leftThumbProximalHeadNode = TransformNode(localRotation = localRot) + val leftThumbProximalTailNode = TransformNode(localRotation = localRot) + val leftThumbIntermediateNode = TransformNode(localRotation = localRot) + val leftThumbDistalNode = TransformNode(localRotation = localRot) + val leftIndexProximalHeadNode = TransformNode(localRotation = localRot) + val leftIndexProximalTailNode = TransformNode(localRotation = localRot) + val leftIndexIntermediateNode = TransformNode(localRotation = localRot) + val leftIndexDistalNode = TransformNode(localRotation = localRot) + val leftMiddleProximalHeadNode = TransformNode(localRotation = localRot) + val leftMiddleProximalTailNode = TransformNode(localRotation = localRot) + val leftMiddleIntermediateNode = TransformNode(localRotation = localRot) + val leftMiddleDistalNode = TransformNode(localRotation = localRot) + val leftRingProximalHeadNode = TransformNode(localRotation = localRot) + val leftRingProximalTailNode = TransformNode(localRotation = localRot) + val leftRingIntermediateNode = TransformNode(localRotation = localRot) + val leftRingDistalNode = TransformNode(localRotation = localRot) + val leftLittleProximalHeadNode = TransformNode(localRotation = localRot) + val leftLittleProximalTailNode = TransformNode(localRotation = localRot) + val leftLittleIntermediateNode = TransformNode(localRotation = localRot) + val leftLittleDistalNode = TransformNode(localRotation = localRot) + val rightThumbProximalHeadNode = TransformNode(localRotation = localRot) + val rightThumbProximalTailNode = TransformNode(localRotation = localRot) + val rightThumbIntermediateNode = TransformNode(localRotation = localRot) + val rightThumbDistalNode = TransformNode(localRotation = localRot) + val rightIndexProximalHeadNode = TransformNode(localRotation = localRot) + val rightIndexProximalTailNode = TransformNode(localRotation = localRot) + val rightIndexIntermediateNode = TransformNode(localRotation = localRot) + val rightIndexDistalNode = TransformNode(localRotation = localRot) + val rightMiddleProximalHeadNode = TransformNode(localRotation = localRot) + val rightMiddleProximalTailNode = TransformNode(localRotation = localRot) + val rightMiddleIntermediateNode = TransformNode(localRotation = localRot) + val rightMiddleDistalNode = TransformNode(localRotation = localRot) + val rightRingProximalHeadNode = TransformNode(localRotation = localRot) + val rightRingProximalTailNode = TransformNode(localRotation = localRot) + val rightRingIntermediateNode = TransformNode(localRotation = localRot) + val rightRingDistalNode = TransformNode(localRotation = localRot) + val rightLittleProximalHeadNode = TransformNode(localRotation = localRot) + val rightLittleProximalTailNode = TransformNode(localRotation = localRot) + val rightLittleIntermediateNode = TransformNode(localRotation = localRot) + val rightLittleDistalNode = TransformNode(localRotation = localRot) private var rootPosition = Vector3.NULL private var rootRotation = Quaternion.IDENTITY @@ -74,6 +123,48 @@ class UnityArmature(localRot: Boolean) { rightElbowNode.attachChild(rightWristNode) leftWristNode.attachChild(leftHandNode) rightWristNode.attachChild(rightHandNode) + + // Fingers + leftHandNode.attachChild(leftThumbProximalHeadNode) + leftThumbProximalHeadNode.attachChild(leftThumbProximalTailNode) + leftThumbProximalTailNode.attachChild(leftThumbIntermediateNode) + leftThumbIntermediateNode.attachChild(leftThumbDistalNode) + leftHandNode.attachChild(leftIndexProximalHeadNode) + leftIndexProximalHeadNode.attachChild(leftIndexProximalTailNode) + leftIndexProximalTailNode.attachChild(leftIndexIntermediateNode) + leftIndexIntermediateNode.attachChild(leftIndexDistalNode) + leftHandNode.attachChild(leftMiddleProximalHeadNode) + leftMiddleProximalHeadNode.attachChild(leftMiddleProximalTailNode) + leftMiddleProximalTailNode.attachChild(leftMiddleIntermediateNode) + leftMiddleIntermediateNode.attachChild(leftMiddleDistalNode) + leftHandNode.attachChild(leftRingProximalHeadNode) + leftRingProximalHeadNode.attachChild(leftRingProximalTailNode) + leftRingProximalTailNode.attachChild(leftRingIntermediateNode) + leftRingIntermediateNode.attachChild(leftRingDistalNode) + leftHandNode.attachChild(leftLittleProximalHeadNode) + leftLittleProximalHeadNode.attachChild(leftLittleProximalTailNode) + leftLittleProximalTailNode.attachChild(leftLittleIntermediateNode) + leftLittleIntermediateNode.attachChild(leftLittleDistalNode) + rightHandNode.attachChild(rightThumbProximalHeadNode) + rightThumbProximalHeadNode.attachChild(rightThumbProximalTailNode) + rightThumbProximalTailNode.attachChild(rightThumbIntermediateNode) + rightThumbIntermediateNode.attachChild(rightThumbDistalNode) + rightHandNode.attachChild(rightIndexProximalHeadNode) + rightIndexProximalHeadNode.attachChild(rightIndexProximalTailNode) + rightIndexProximalTailNode.attachChild(rightIndexIntermediateNode) + rightIndexIntermediateNode.attachChild(rightIndexDistalNode) + rightHandNode.attachChild(rightMiddleProximalHeadNode) + rightMiddleProximalHeadNode.attachChild(rightMiddleProximalTailNode) + rightMiddleProximalTailNode.attachChild(rightMiddleIntermediateNode) + rightMiddleIntermediateNode.attachChild(rightMiddleDistalNode) + rightHandNode.attachChild(rightRingProximalHeadNode) + rightRingProximalHeadNode.attachChild(rightRingProximalTailNode) + rightRingProximalTailNode.attachChild(rightRingIntermediateNode) + rightRingIntermediateNode.attachChild(rightRingDistalNode) + rightHandNode.attachChild(rightLittleProximalHeadNode) + rightLittleProximalHeadNode.attachChild(rightLittleProximalTailNode) + rightLittleProximalTailNode.attachChild(rightLittleIntermediateNode) + rightLittleIntermediateNode.attachChild(rightLittleDistalNode) } fun update() { @@ -91,10 +182,12 @@ class UnityArmature(localRot: Boolean) { fun setGlobalRotationForBone(unityBone: UnityBone, globalRot: Quaternion) { val node = getHeadNodeOfBone(unityBone) if (node != null) { - node.localTransform.rotation = when (unityBone) { - UnityBone.LEFT_UPPER_ARM, UnityBone.LEFT_LOWER_ARM, UnityBone.LEFT_HAND -> globalRot * LEFT_SHOULDER_OFFSET - UnityBone.RIGHT_UPPER_ARM, UnityBone.RIGHT_LOWER_ARM, UnityBone.RIGHT_HAND -> globalRot * RIGHT_SHOULDER_OFFSET - else -> globalRot + node.localTransform.rotation = if (UnityBone.isLeftArmBone(unityBone)) { + globalRot * LEFT_SHOULDER_OFFSET + } else if (UnityBone.isRightArmBone(unityBone)) { + globalRot * RIGHT_SHOULDER_OFFSET + } else { + globalRot } } } @@ -105,10 +198,12 @@ class UnityArmature(localRot: Boolean) { if (unityBone === UnityBone.HIPS) { node.worldTransform.rotation = localRot } else { - node.localTransform.rotation = when (unityBone) { - UnityBone.LEFT_UPPER_ARM -> localRot * RIGHT_SHOULDER_OFFSET - UnityBone.RIGHT_UPPER_ARM -> localRot * LEFT_SHOULDER_OFFSET - else -> localRot + node.localTransform.rotation = if (UnityBone.isLeftStartOfArmOrFingerBone(unityBone)) { + localRot * RIGHT_SHOULDER_OFFSET + } else if (UnityBone.isRightStartOfArmOrFingerBone(unityBone)) { + localRot * LEFT_SHOULDER_OFFSET + } else { + localRot } } } @@ -194,6 +289,36 @@ class UnityArmature(localRot: Boolean) { UnityBone.RIGHT_LOWER_ARM -> rightElbowNode UnityBone.LEFT_HAND -> leftWristNode UnityBone.RIGHT_HAND -> rightWristNode + UnityBone.LEFT_THUMB_PROXIMAL -> leftThumbProximalHeadNode + UnityBone.LEFT_THUMB_INTERMEDIATE -> leftThumbProximalTailNode + UnityBone.LEFT_THUMB_DISTAL -> leftThumbIntermediateNode + UnityBone.LEFT_INDEX_PROXIMAL -> leftIndexProximalHeadNode + UnityBone.LEFT_INDEX_INTERMEDIATE -> leftIndexProximalTailNode + UnityBone.LEFT_INDEX_DISTAL -> leftIndexIntermediateNode + UnityBone.LEFT_MIDDLE_PROXIMAL -> leftMiddleProximalHeadNode + UnityBone.LEFT_MIDDLE_INTERMEDIATE -> leftMiddleProximalTailNode + UnityBone.LEFT_MIDDLE_DISTAL -> leftMiddleIntermediateNode + UnityBone.LEFT_RING_PROXIMAL -> leftRingProximalHeadNode + UnityBone.LEFT_RING_INTERMEDIATE -> leftRingProximalTailNode + UnityBone.LEFT_RING_DISTAL -> leftRingIntermediateNode + UnityBone.LEFT_LITTLE_PROXIMAL -> leftLittleProximalHeadNode + UnityBone.LEFT_LITTLE_INTERMEDIATE -> leftLittleProximalTailNode + UnityBone.LEFT_LITTLE_DISTAL -> leftLittleIntermediateNode + UnityBone.RIGHT_THUMB_PROXIMAL -> rightThumbProximalHeadNode + UnityBone.RIGHT_THUMB_INTERMEDIATE -> rightThumbProximalTailNode + UnityBone.RIGHT_THUMB_DISTAL -> rightThumbIntermediateNode + UnityBone.RIGHT_INDEX_PROXIMAL -> rightIndexProximalHeadNode + UnityBone.RIGHT_INDEX_INTERMEDIATE -> rightIndexProximalTailNode + UnityBone.RIGHT_INDEX_DISTAL -> rightIndexIntermediateNode + UnityBone.RIGHT_MIDDLE_PROXIMAL -> rightMiddleProximalHeadNode + UnityBone.RIGHT_MIDDLE_INTERMEDIATE -> rightMiddleProximalTailNode + UnityBone.RIGHT_MIDDLE_DISTAL -> rightMiddleIntermediateNode + UnityBone.RIGHT_RING_PROXIMAL -> rightRingProximalHeadNode + UnityBone.RIGHT_RING_INTERMEDIATE -> rightRingProximalTailNode + UnityBone.RIGHT_RING_DISTAL -> rightRingIntermediateNode + UnityBone.RIGHT_LITTLE_PROXIMAL -> rightLittleProximalHeadNode + UnityBone.RIGHT_LITTLE_INTERMEDIATE -> rightLittleProximalTailNode + UnityBone.RIGHT_LITTLE_DISTAL -> rightLittleIntermediateNode else -> null } } diff --git a/server/core/src/main/java/dev/slimevr/osc/UnityBone.kt b/server/core/src/main/java/dev/slimevr/osc/UnityBone.kt index fe549bd4a5..f84e882bd5 100644 --- a/server/core/src/main/java/dev/slimevr/osc/UnityBone.kt +++ b/server/core/src/main/java/dev/slimevr/osc/UnityBone.kt @@ -38,36 +38,36 @@ enum class UnityBone( LEFT_EYE("LeftEye", null, null), RIGHT_EYE("RightEye", null, null), JAW("Jaw", null, null), - LEFT_THUMB_PROXIMAL("LeftThumbProximal", null, null), - LEFT_THUMB_INTERMEDIATE("LeftThumbIntermediate", null, null), - LEFT_THUMB_DISTAL("LeftThumbDistal", null, null), - LEFT_INDEX_PROXIMAL("LeftIndexProximal", null, null), - LEFT_INDEX_INTERMEDIATE("LeftIndexIntermediate", null, null), - LEFT_INDEX_DISTAL("LeftIndexDistal", null, null), - LEFT_MIDDLE_PROXIMAL("LeftMiddleProximal", null, null), - LEFT_MIDDLE_INTERMEDIATE("LeftMiddleIntermediate", null, null), - LEFT_MIDDLE_DISTAL("LeftMiddleDistal", null, null), - LEFT_RING_PROXIMAL("LeftRingProximal", null, null), - LEFT_RING_INTERMEDIATE("LeftRingIntermediate", null, null), - LEFT_RING_DISTAL("LeftRingDistal", null, null), - LEFT_LITTLE_PROXIMAL("LeftLittleProximal", null, null), - LEFT_LITTLE_INTERMEDIATE("LeftLittleIntermediate", null, null), - LEFT_LITTLE_DISTAL("LeftLittleDistal", null, null), - RIGHT_THUMB_PROXIMAL("RightThumbProximal", null, null), - RIGHT_THUMB_INTERMEDIATE("RightThumbIntermediate", null, null), - RIGHT_THUMB_DISTAL("RightThumbDistal", null, null), - RIGHT_INDEX_PROXIMAL("RightIndexProximal", null, null), - RIGHT_INDEX_INTERMEDIATE("RightIndexIntermediate", null, null), - RIGHT_INDEX_DISTAL("RightIndexDistal", null, null), - RIGHT_MIDDLE_PROXIMAL("RightMiddleProximal", null, null), - RIGHT_MIDDLE_INTERMEDIATE("RightMiddleIntermediate", null, null), - RIGHT_MIDDLE_DISTAL("RightMiddleDistal", null, null), - RIGHT_RING_PROXIMAL("RightRingProximal", null, null), - RIGHT_RING_INTERMEDIATE("RightRingIntermediate", null, null), - RIGHT_RING_DISTAL("RightRingDistal", null, null), - RIGHT_LITTLE_PROXIMAL("RightLittleProximal", null, null), - RIGHT_LITTLE_INTERMEDIATE("RightLittleIntermediate", null, null), - RIGHT_LITTLE_DISTAL("RightLittleDistal", null, null), + LEFT_THUMB_PROXIMAL("LeftThumbProximal", BoneType.LEFT_THUMB_PROXIMAL, TrackerPosition.LEFT_THUMB_PROXIMAL), + LEFT_THUMB_INTERMEDIATE("LeftThumbIntermediate", BoneType.LEFT_THUMB_INTERMEDIATE, TrackerPosition.LEFT_THUMB_INTERMEDIATE), + LEFT_THUMB_DISTAL("LeftThumbDistal", BoneType.LEFT_THUMB_DISTAL, TrackerPosition.LEFT_THUMB_DISTAL), + LEFT_INDEX_PROXIMAL("LeftIndexProximal", BoneType.LEFT_INDEX_PROXIMAL, TrackerPosition.LEFT_INDEX_PROXIMAL), + LEFT_INDEX_INTERMEDIATE("LeftIndexIntermediate", BoneType.LEFT_INDEX_INTERMEDIATE, TrackerPosition.LEFT_INDEX_INTERMEDIATE), + LEFT_INDEX_DISTAL("LeftIndexDistal", BoneType.LEFT_INDEX_DISTAL, TrackerPosition.LEFT_INDEX_DISTAL), + LEFT_MIDDLE_PROXIMAL("LeftMiddleProximal", BoneType.LEFT_MIDDLE_PROXIMAL, TrackerPosition.LEFT_MIDDLE_PROXIMAL), + LEFT_MIDDLE_INTERMEDIATE("LeftMiddleIntermediate", BoneType.LEFT_MIDDLE_INTERMEDIATE, TrackerPosition.LEFT_MIDDLE_INTERMEDIATE), + LEFT_MIDDLE_DISTAL("LeftMiddleDistal", BoneType.LEFT_MIDDLE_DISTAL, TrackerPosition.LEFT_MIDDLE_DISTAL), + LEFT_RING_PROXIMAL("LeftRingProximal", BoneType.LEFT_RING_PROXIMAL, TrackerPosition.LEFT_RING_PROXIMAL), + LEFT_RING_INTERMEDIATE("LeftRingIntermediate", BoneType.LEFT_RING_INTERMEDIATE, TrackerPosition.LEFT_RING_INTERMEDIATE), + LEFT_RING_DISTAL("LeftRingDistal", BoneType.LEFT_RING_DISTAL, TrackerPosition.LEFT_RING_DISTAL), + LEFT_LITTLE_PROXIMAL("LeftLittleProximal", BoneType.LEFT_LITTLE_PROXIMAL, TrackerPosition.LEFT_LITTLE_PROXIMAL), + LEFT_LITTLE_INTERMEDIATE("LeftLittleIntermediate", BoneType.LEFT_LITTLE_INTERMEDIATE, TrackerPosition.LEFT_LITTLE_INTERMEDIATE), + LEFT_LITTLE_DISTAL("LeftLittleDistal", BoneType.LEFT_LITTLE_DISTAL, TrackerPosition.LEFT_LITTLE_DISTAL), + RIGHT_THUMB_PROXIMAL("RightThumbProximal", BoneType.RIGHT_THUMB_PROXIMAL, TrackerPosition.RIGHT_THUMB_PROXIMAL), + RIGHT_THUMB_INTERMEDIATE("RightThumbIntermediate", BoneType.RIGHT_THUMB_INTERMEDIATE, TrackerPosition.RIGHT_THUMB_INTERMEDIATE), + RIGHT_THUMB_DISTAL("RightThumbDistal", BoneType.RIGHT_THUMB_DISTAL, TrackerPosition.RIGHT_THUMB_DISTAL), + RIGHT_INDEX_PROXIMAL("RightIndexProximal", BoneType.RIGHT_INDEX_PROXIMAL, TrackerPosition.RIGHT_INDEX_PROXIMAL), + RIGHT_INDEX_INTERMEDIATE("RightIndexIntermediate", BoneType.RIGHT_INDEX_INTERMEDIATE, TrackerPosition.RIGHT_INDEX_INTERMEDIATE), + RIGHT_INDEX_DISTAL("RightIndexDistal", BoneType.RIGHT_INDEX_DISTAL, TrackerPosition.RIGHT_INDEX_DISTAL), + RIGHT_MIDDLE_PROXIMAL("RightMiddleProximal", BoneType.RIGHT_MIDDLE_PROXIMAL, TrackerPosition.RIGHT_MIDDLE_PROXIMAL), + RIGHT_MIDDLE_INTERMEDIATE("RightMiddleIntermediate", BoneType.RIGHT_MIDDLE_INTERMEDIATE, TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE), + RIGHT_MIDDLE_DISTAL("RightMiddleDistal", BoneType.RIGHT_MIDDLE_DISTAL, TrackerPosition.RIGHT_MIDDLE_DISTAL), + RIGHT_RING_PROXIMAL("RightRingProximal", BoneType.RIGHT_RING_PROXIMAL, TrackerPosition.RIGHT_RING_PROXIMAL), + RIGHT_RING_INTERMEDIATE("RightRingIntermediate", BoneType.RIGHT_RING_INTERMEDIATE, TrackerPosition.RIGHT_RING_INTERMEDIATE), + RIGHT_RING_DISTAL("RightRingDistal", BoneType.RIGHT_RING_DISTAL, TrackerPosition.RIGHT_RING_DISTAL), + RIGHT_LITTLE_PROXIMAL("RightLittleProximal", BoneType.RIGHT_LITTLE_PROXIMAL, TrackerPosition.RIGHT_LITTLE_PROXIMAL), + RIGHT_LITTLE_INTERMEDIATE("RightLittleIntermediate", BoneType.RIGHT_LITTLE_INTERMEDIATE, TrackerPosition.RIGHT_LITTLE_INTERMEDIATE), + RIGHT_LITTLE_DISTAL("RightLittleDistal", BoneType.RIGHT_LITTLE_DISTAL, TrackerPosition.RIGHT_LITTLE_DISTAL), LAST_BONE("LastBone", null, null), ; @@ -76,5 +76,107 @@ enum class UnityBone( @JvmStatic fun getByStringVal(stringVal: String): UnityBone? = byStringVal[stringVal.lowercase()] + + /** + * Returns the bone on the opposite limb, or the original bone if + * it not a limb bone. + */ + fun tryGetOppositeArmBone(bone: UnityBone): UnityBone = when (bone) { + LEFT_SHOULDER -> RIGHT_SHOULDER + LEFT_UPPER_ARM -> RIGHT_UPPER_ARM + LEFT_LOWER_ARM -> RIGHT_LOWER_ARM + LEFT_HAND -> RIGHT_HAND + RIGHT_SHOULDER -> LEFT_SHOULDER + RIGHT_UPPER_ARM -> LEFT_UPPER_ARM + RIGHT_LOWER_ARM -> LEFT_LOWER_ARM + RIGHT_HAND -> LEFT_HAND + LEFT_UPPER_LEG -> RIGHT_UPPER_LEG + LEFT_LOWER_LEG -> RIGHT_LOWER_LEG + LEFT_FOOT -> RIGHT_FOOT + RIGHT_UPPER_LEG -> LEFT_UPPER_LEG + RIGHT_LOWER_LEG -> LEFT_LOWER_LEG + RIGHT_FOOT -> LEFT_FOOT + LEFT_THUMB_PROXIMAL -> RIGHT_THUMB_PROXIMAL + LEFT_THUMB_INTERMEDIATE -> RIGHT_THUMB_INTERMEDIATE + LEFT_THUMB_DISTAL -> RIGHT_THUMB_DISTAL + LEFT_INDEX_PROXIMAL -> RIGHT_INDEX_PROXIMAL + LEFT_INDEX_INTERMEDIATE -> RIGHT_INDEX_INTERMEDIATE + LEFT_INDEX_DISTAL -> RIGHT_INDEX_DISTAL + LEFT_MIDDLE_PROXIMAL -> RIGHT_MIDDLE_PROXIMAL + LEFT_MIDDLE_INTERMEDIATE -> RIGHT_MIDDLE_INTERMEDIATE + LEFT_MIDDLE_DISTAL -> RIGHT_MIDDLE_DISTAL + LEFT_RING_PROXIMAL -> RIGHT_RING_PROXIMAL + LEFT_RING_INTERMEDIATE -> RIGHT_RING_INTERMEDIATE + LEFT_RING_DISTAL -> RIGHT_RING_DISTAL + LEFT_LITTLE_PROXIMAL -> RIGHT_LITTLE_PROXIMAL + LEFT_LITTLE_INTERMEDIATE -> RIGHT_LITTLE_INTERMEDIATE + LEFT_LITTLE_DISTAL -> RIGHT_LITTLE_DISTAL + RIGHT_THUMB_PROXIMAL -> LEFT_THUMB_PROXIMAL + RIGHT_THUMB_INTERMEDIATE -> LEFT_THUMB_INTERMEDIATE + RIGHT_THUMB_DISTAL -> LEFT_THUMB_DISTAL + RIGHT_INDEX_PROXIMAL -> LEFT_INDEX_PROXIMAL + RIGHT_INDEX_INTERMEDIATE -> LEFT_INDEX_INTERMEDIATE + RIGHT_INDEX_DISTAL -> LEFT_INDEX_DISTAL + RIGHT_MIDDLE_PROXIMAL -> LEFT_MIDDLE_PROXIMAL + RIGHT_MIDDLE_INTERMEDIATE -> LEFT_MIDDLE_INTERMEDIATE + RIGHT_MIDDLE_DISTAL -> LEFT_MIDDLE_DISTAL + RIGHT_RING_PROXIMAL -> LEFT_RING_PROXIMAL + RIGHT_RING_INTERMEDIATE -> LEFT_RING_INTERMEDIATE + RIGHT_RING_DISTAL -> LEFT_RING_DISTAL + RIGHT_LITTLE_PROXIMAL -> LEFT_LITTLE_PROXIMAL + RIGHT_LITTLE_INTERMEDIATE -> LEFT_LITTLE_INTERMEDIATE + RIGHT_LITTLE_DISTAL -> LEFT_LITTLE_DISTAL + else -> bone + } + + /** + * Returns true if the bone is part of the left arm (incl. fingers, excl. shoulder) + */ + fun isLeftArmBone(bone: UnityBone): Boolean = bone == LEFT_UPPER_ARM || bone == LEFT_LOWER_ARM || bone == LEFT_HAND || + bone == LEFT_THUMB_PROXIMAL || bone == LEFT_THUMB_INTERMEDIATE || bone == LEFT_THUMB_DISTAL || + bone == LEFT_INDEX_PROXIMAL || bone == LEFT_INDEX_INTERMEDIATE || bone == LEFT_INDEX_DISTAL || + bone == LEFT_MIDDLE_PROXIMAL || bone == LEFT_MIDDLE_INTERMEDIATE || bone == LEFT_MIDDLE_DISTAL || + bone == LEFT_RING_PROXIMAL || bone == LEFT_RING_INTERMEDIATE || bone == LEFT_RING_DISTAL || + bone == LEFT_LITTLE_PROXIMAL || bone == LEFT_LITTLE_INTERMEDIATE || bone == LEFT_LITTLE_DISTAL + + /** + * Returns true if the bone is part of the right arm (incl. fingers, excl. shoulder) + */ + fun isRightArmBone(bone: UnityBone): Boolean = bone == RIGHT_UPPER_ARM || bone == RIGHT_LOWER_ARM || bone == RIGHT_HAND || + bone == RIGHT_THUMB_PROXIMAL || bone == RIGHT_THUMB_INTERMEDIATE || bone == RIGHT_THUMB_DISTAL || + bone == RIGHT_INDEX_PROXIMAL || bone == RIGHT_INDEX_INTERMEDIATE || bone == RIGHT_INDEX_DISTAL || + bone == RIGHT_MIDDLE_PROXIMAL || bone == RIGHT_MIDDLE_INTERMEDIATE || bone == RIGHT_MIDDLE_DISTAL || + bone == RIGHT_RING_PROXIMAL || bone == RIGHT_RING_INTERMEDIATE || bone == RIGHT_RING_DISTAL || + bone == RIGHT_LITTLE_PROXIMAL || bone == RIGHT_LITTLE_INTERMEDIATE || bone == RIGHT_LITTLE_DISTAL + + /** + * Returns true if the bone is the left upper arm or proximal left finger bone + */ + fun isLeftStartOfArmOrFingerBone(bone: UnityBone): Boolean = bone == LEFT_UPPER_ARM || bone == LEFT_THUMB_PROXIMAL || + bone == LEFT_INDEX_PROXIMAL || bone == LEFT_MIDDLE_PROXIMAL || bone == LEFT_RING_PROXIMAL || bone == LEFT_LITTLE_PROXIMAL + + /** + * Returns true if the bone is the right upper arm or proximal right finger bone + */ + fun isRightStartOfArmOrFingerBone(bone: UnityBone): Boolean = bone == RIGHT_UPPER_ARM || bone == RIGHT_THUMB_PROXIMAL || + bone == RIGHT_INDEX_PROXIMAL || bone == RIGHT_MIDDLE_PROXIMAL || bone == RIGHT_RING_PROXIMAL || bone == RIGHT_LITTLE_PROXIMAL + + /** + * Returns true if the bone is part of the left fingers + */ + fun isLeftFingerBone(bone: UnityBone): Boolean = bone == LEFT_THUMB_PROXIMAL || bone == LEFT_THUMB_INTERMEDIATE || bone == LEFT_THUMB_DISTAL || + bone == LEFT_INDEX_PROXIMAL || bone == LEFT_INDEX_INTERMEDIATE || bone == LEFT_INDEX_DISTAL || + bone == LEFT_MIDDLE_PROXIMAL || bone == LEFT_MIDDLE_INTERMEDIATE || bone == LEFT_MIDDLE_DISTAL || + bone == LEFT_RING_PROXIMAL || bone == LEFT_RING_INTERMEDIATE || bone == LEFT_RING_DISTAL || + bone == LEFT_LITTLE_PROXIMAL || bone == LEFT_LITTLE_INTERMEDIATE || bone == LEFT_LITTLE_DISTAL + + /** + * Returns true if the bone part of the right fingers + */ + fun isRightFingerBone(bone: UnityBone): Boolean = bone == RIGHT_THUMB_PROXIMAL || bone == RIGHT_THUMB_INTERMEDIATE || bone == RIGHT_THUMB_DISTAL || + bone == RIGHT_INDEX_PROXIMAL || bone == RIGHT_INDEX_INTERMEDIATE || bone == RIGHT_INDEX_DISTAL || + bone == RIGHT_MIDDLE_PROXIMAL || bone == RIGHT_MIDDLE_INTERMEDIATE || bone == RIGHT_MIDDLE_DISTAL || + bone == RIGHT_RING_PROXIMAL || bone == RIGHT_RING_INTERMEDIATE || bone == RIGHT_RING_DISTAL || + bone == RIGHT_LITTLE_PROXIMAL || bone == RIGHT_LITTLE_INTERMEDIATE || bone == RIGHT_LITTLE_DISTAL } } diff --git a/server/core/src/main/java/dev/slimevr/osc/VMCHandler.kt b/server/core/src/main/java/dev/slimevr/osc/VMCHandler.kt index c9047d60cc..e3cd29f3c7 100644 --- a/server/core/src/main/java/dev/slimevr/osc/VMCHandler.kt +++ b/server/core/src/main/java/dev/slimevr/osc/VMCHandler.kt @@ -345,13 +345,13 @@ class VMCHandler( oscBundle.addPacket(OSCMessage("/VMC/Ext/Root/Pos", oscArgs.clone())) for (unityBone in UnityBone.entries) { - if (unityBone.boneType == null) continue - // Get opposite bone if tracking must be mirrored - val boneType = (if (mirrorTracking) tryGetOppositeArmBone(unityBone) else unityBone).boneType + val boneType = (if (mirrorTracking) UnityBone.tryGetOppositeArmBone(unityBone) else unityBone).boneType + + if (boneType == null) continue // Get SlimeVR bone - val bone = humanPoseManager.getBone(boneType!!) + val bone = humanPoseManager.getBone(boneType) // Update unity hierarchy from bone's global rotation val boneRotation = if (mirrorTracking) { @@ -384,20 +384,28 @@ class VMCHandler( } // Update Unity skeleton - outputUnityArmature!!.update() + outputUnityArmature?.update() // Add Unity humanoid bones transforms - for (bone in UnityBone.entries) { - if (bone.boneType != null && - !(humanPoseManager.isTrackingLeftArmFromController && isLeftArmUnityBone(bone)) && - !(humanPoseManager.isTrackingRightArmFromController && isRightArmUnityBone(bone)) + for (unityBone in UnityBone.entries) { + // Don't send bones for which we don't have an equivalent + // Don't send fingers if we don't have any tracker for them + // Don't send arm bones if we're tracking from the controller + if (unityBone.boneType != null && + (!UnityBone.isLeftFingerBone(unityBone) || humanPoseManager.skeleton.hasLeftFingerTracker || (mirrorTracking && humanPoseManager.skeleton.hasRightFingerTracker)) && + (!UnityBone.isRightFingerBone(unityBone) || humanPoseManager.skeleton.hasRightFingerTracker || (mirrorTracking && humanPoseManager.skeleton.hasLeftFingerTracker)) && + !(humanPoseManager.isTrackingLeftArmFromController && (UnityBone.isLeftArmBone(unityBone) || unityBone == UnityBone.LEFT_SHOULDER)) && + !(humanPoseManager.isTrackingRightArmFromController && (UnityBone.isRightArmBone(unityBone) || unityBone == UnityBone.RIGHT_SHOULDER)) ) { oscArgs.clear() - oscArgs.add(bone.stringVal) - addTransformToArgs( - outputUnityArmature!!.getLocalTranslationForBone(bone), - outputUnityArmature!!.getLocalRotationForBone(bone), - ) + oscArgs.add(unityBone.stringVal) + outputUnityArmature?.let { + addTransformToArgs( + it.getLocalTranslationForBone(unityBone), + it.getLocalRotationForBone(unityBone), + ) + } + oscBundle.addPacket(OSCMessage("/VMC/Ext/Bone/Pos", oscArgs.clone())) } } @@ -491,32 +499,6 @@ class VMCHandler( oscArgs.add(-rot.w) } - /** - * Returns the bone on the opposite limb, or the original bone if - * it not a limb bone. - */ - private fun tryGetOppositeArmBone(bone: UnityBone): UnityBone = when (bone) { - UnityBone.LEFT_SHOULDER -> UnityBone.RIGHT_SHOULDER - UnityBone.LEFT_UPPER_ARM -> UnityBone.RIGHT_UPPER_ARM - UnityBone.LEFT_LOWER_ARM -> UnityBone.RIGHT_LOWER_ARM - UnityBone.LEFT_HAND -> UnityBone.RIGHT_HAND - UnityBone.RIGHT_SHOULDER -> UnityBone.LEFT_SHOULDER - UnityBone.RIGHT_UPPER_ARM -> UnityBone.LEFT_UPPER_ARM - UnityBone.RIGHT_LOWER_ARM -> UnityBone.LEFT_LOWER_ARM - UnityBone.RIGHT_HAND -> UnityBone.LEFT_HAND - UnityBone.LEFT_UPPER_LEG -> UnityBone.RIGHT_UPPER_LEG - UnityBone.LEFT_LOWER_LEG -> UnityBone.RIGHT_LOWER_LEG - UnityBone.LEFT_FOOT -> UnityBone.RIGHT_FOOT - UnityBone.RIGHT_UPPER_LEG -> UnityBone.LEFT_UPPER_LEG - UnityBone.RIGHT_LOWER_LEG -> UnityBone.LEFT_LOWER_LEG - UnityBone.RIGHT_FOOT -> UnityBone.LEFT_FOOT - else -> bone - } - - private fun isLeftArmUnityBone(bone: UnityBone): Boolean = bone == UnityBone.LEFT_SHOULDER || bone == UnityBone.LEFT_UPPER_ARM || bone == UnityBone.LEFT_LOWER_ARM || bone == UnityBone.LEFT_HAND - - private fun isRightArmUnityBone(bone: UnityBone): Boolean = bone == UnityBone.RIGHT_SHOULDER || bone == UnityBone.RIGHT_UPPER_ARM || bone == UnityBone.RIGHT_LOWER_ARM || bone == UnityBone.RIGHT_HAND - override fun getOscSender(): OSCPortOut = oscSender!! override fun getPortOut(): Int = lastPortOut diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java b/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java index 146311747d..0794cf7a97 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java @@ -39,7 +39,37 @@ public enum BoneType { LEFT_HAND(BodyPart.LEFT_HAND), RIGHT_HAND(BodyPart.RIGHT_HAND), LEFT_HAND_TRACKER, - RIGHT_HAND_TRACKER; + RIGHT_HAND_TRACKER, + LEFT_THUMB_PROXIMAL(BodyPart.LEFT_THUMB_PROXIMAL), + LEFT_THUMB_INTERMEDIATE(BodyPart.LEFT_THUMB_INTERMEDIATE), + LEFT_THUMB_DISTAL(BodyPart.LEFT_THUMB_DISTAL), + LEFT_INDEX_PROXIMAL(BodyPart.LEFT_INDEX_PROXIMAL), + LEFT_INDEX_INTERMEDIATE(BodyPart.LEFT_INDEX_INTERMEDIATE), + LEFT_INDEX_DISTAL(BodyPart.LEFT_INDEX_DISTAL), + LEFT_MIDDLE_PROXIMAL(BodyPart.LEFT_MIDDLE_PROXIMAL), + LEFT_MIDDLE_INTERMEDIATE(BodyPart.LEFT_MIDDLE_INTERMEDIATE), + LEFT_MIDDLE_DISTAL(BodyPart.LEFT_MIDDLE_DISTAL), + LEFT_RING_PROXIMAL(BodyPart.LEFT_RING_PROXIMAL), + LEFT_RING_INTERMEDIATE(BodyPart.LEFT_RING_INTERMEDIATE), + LEFT_RING_DISTAL(BodyPart.LEFT_RING_DISTAL), + LEFT_LITTLE_PROXIMAL(BodyPart.LEFT_LITTLE_PROXIMAL), + LEFT_LITTLE_INTERMEDIATE(BodyPart.LEFT_LITTLE_INTERMEDIATE), + LEFT_LITTLE_DISTAL(BodyPart.LEFT_LITTLE_DISTAL), + RIGHT_THUMB_PROXIMAL(BodyPart.RIGHT_THUMB_PROXIMAL), + RIGHT_THUMB_INTERMEDIATE(BodyPart.RIGHT_THUMB_INTERMEDIATE), + RIGHT_THUMB_DISTAL(BodyPart.RIGHT_THUMB_DISTAL), + RIGHT_INDEX_PROXIMAL(BodyPart.RIGHT_INDEX_PROXIMAL), + RIGHT_INDEX_INTERMEDIATE(BodyPart.RIGHT_INDEX_INTERMEDIATE), + RIGHT_INDEX_DISTAL(BodyPart.RIGHT_INDEX_DISTAL), + RIGHT_MIDDLE_PROXIMAL(BodyPart.RIGHT_MIDDLE_PROXIMAL), + RIGHT_MIDDLE_INTERMEDIATE(BodyPart.RIGHT_MIDDLE_INTERMEDIATE), + RIGHT_MIDDLE_DISTAL(BodyPart.RIGHT_MIDDLE_DISTAL), + RIGHT_RING_PROXIMAL(BodyPart.RIGHT_RING_PROXIMAL), + RIGHT_RING_INTERMEDIATE(BodyPart.RIGHT_RING_INTERMEDIATE), + RIGHT_RING_DISTAL(BodyPart.RIGHT_RING_DISTAL), + RIGHT_LITTLE_PROXIMAL(BodyPart.RIGHT_LITTLE_PROXIMAL), + RIGHT_LITTLE_INTERMEDIATE(BodyPart.RIGHT_LITTLE_INTERMEDIATE), + RIGHT_LITTLE_DISTAL(BodyPart.RIGHT_LITTLE_DISTAL); public static final BoneType[] values = values(); diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt index 8d43d07ce4..5ce231bbe2 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt @@ -287,6 +287,51 @@ class SkeletonConfigManager( 0f, ) + BoneType.LEFT_THUMB_PROXIMAL, BoneType.LEFT_THUMB_INTERMEDIATE, BoneType.LEFT_THUMB_DISTAL, + BoneType.RIGHT_THUMB_PROXIMAL, BoneType.RIGHT_THUMB_INTERMEDIATE, BoneType.RIGHT_THUMB_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.2f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.1f, + ) + + BoneType.LEFT_INDEX_PROXIMAL, BoneType.LEFT_INDEX_INTERMEDIATE, BoneType.LEFT_INDEX_DISTAL, + BoneType.RIGHT_INDEX_PROXIMAL, BoneType.RIGHT_INDEX_INTERMEDIATE, BoneType.RIGHT_INDEX_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.25f, + 0f, + ) + + BoneType.LEFT_MIDDLE_PROXIMAL, BoneType.LEFT_MIDDLE_INTERMEDIATE, BoneType.LEFT_MIDDLE_DISTAL, + BoneType.RIGHT_MIDDLE_PROXIMAL, BoneType.RIGHT_MIDDLE_INTERMEDIATE, BoneType.RIGHT_MIDDLE_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.3f, + 0f, + ) + + BoneType.LEFT_RING_PROXIMAL, BoneType.LEFT_RING_INTERMEDIATE, BoneType.LEFT_RING_DISTAL, + BoneType.RIGHT_RING_PROXIMAL, BoneType.RIGHT_RING_INTERMEDIATE, BoneType.RIGHT_RING_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.28f, + 0f, + ) + + BoneType.LEFT_LITTLE_PROXIMAL, BoneType.LEFT_LITTLE_INTERMEDIATE, BoneType.LEFT_LITTLE_DISTAL, + BoneType.RIGHT_LITTLE_PROXIMAL, BoneType.RIGHT_LITTLE_INTERMEDIATE, BoneType.RIGHT_LITTLE_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.2f, + 0f, + ) + else -> {} } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 3c2c0b5ca9..2865f221c3 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -60,6 +60,38 @@ class HumanSkeleton( val leftHandBone = Bone(BoneType.LEFT_HAND) val rightHandBone = Bone(BoneType.RIGHT_HAND) + // Finger bones + val leftThumbProximalBone = Bone(BoneType.LEFT_THUMB_PROXIMAL) + val leftThumbIntermediateBone = Bone(BoneType.LEFT_THUMB_INTERMEDIATE) + val leftThumbDistalBone = Bone(BoneType.LEFT_THUMB_DISTAL) + val leftIndexProximalBone = Bone(BoneType.LEFT_INDEX_PROXIMAL) + val leftIndexIntermediateBone = Bone(BoneType.LEFT_INDEX_INTERMEDIATE) + val leftIndexDistalBone = Bone(BoneType.LEFT_INDEX_DISTAL) + val leftMiddleProximalBone = Bone(BoneType.LEFT_MIDDLE_PROXIMAL) + val leftMiddleIntermediateBone = Bone(BoneType.LEFT_MIDDLE_INTERMEDIATE) + val leftMiddleDistalBone = Bone(BoneType.LEFT_MIDDLE_DISTAL) + val leftRingProximalBone = Bone(BoneType.LEFT_RING_PROXIMAL) + val leftRingIntermediateBone = Bone(BoneType.LEFT_RING_INTERMEDIATE) + val leftRingDistalBone = Bone(BoneType.LEFT_RING_DISTAL) + val leftLittleProximalBone = Bone(BoneType.LEFT_LITTLE_PROXIMAL) + val leftLittleIntermediateBone = Bone(BoneType.LEFT_LITTLE_INTERMEDIATE) + val leftLittleDistalBone = Bone(BoneType.LEFT_LITTLE_DISTAL) + val rightThumbProximalBone = Bone(BoneType.RIGHT_THUMB_PROXIMAL) + val rightThumbIntermediateBone = Bone(BoneType.RIGHT_THUMB_INTERMEDIATE) + val rightThumbDistalBone = Bone(BoneType.RIGHT_THUMB_DISTAL) + val rightIndexProximalBone = Bone(BoneType.RIGHT_INDEX_PROXIMAL) + val rightIndexIntermediateBone = Bone(BoneType.RIGHT_INDEX_INTERMEDIATE) + val rightIndexDistalBone = Bone(BoneType.RIGHT_INDEX_DISTAL) + val rightMiddleProximalBone = Bone(BoneType.RIGHT_MIDDLE_PROXIMAL) + val rightMiddleIntermediateBone = Bone(BoneType.RIGHT_MIDDLE_INTERMEDIATE) + val rightMiddleDistalBone = Bone(BoneType.RIGHT_MIDDLE_DISTAL) + val rightRingProximalBone = Bone(BoneType.RIGHT_RING_PROXIMAL) + val rightRingIntermediateBone = Bone(BoneType.RIGHT_RING_INTERMEDIATE) + val rightRingDistalBone = Bone(BoneType.RIGHT_RING_DISTAL) + val rightLittleProximalBone = Bone(BoneType.RIGHT_LITTLE_PROXIMAL) + val rightLittleIntermediateBone = Bone(BoneType.RIGHT_LITTLE_INTERMEDIATE) + val rightLittleDistalBone = Bone(BoneType.RIGHT_LITTLE_DISTAL) + // Tracker bones val headTrackerBone = Bone(BoneType.HEAD_TRACKER) val chestTrackerBone = Bone(BoneType.CHEST_TRACKER) @@ -76,12 +108,10 @@ class HumanSkeleton( // Buffers var hasSpineTracker = false var hasKneeTrackers = false - var hasLeftLegTracker = false - var hasRightLegTracker = false - var hasLeftFootTracker = false - var hasRightFootTracker = false var hasLeftArmTracker = false var hasRightArmTracker = false + var hasLeftFingerTracker = false + var hasRightFingerTracker = false // Input trackers var headTracker: Tracker? by Delegates.observable(null) { _, old, new -> @@ -109,6 +139,36 @@ class HumanSkeleton( var rightHandTracker: Tracker? = null var leftShoulderTracker: Tracker? = null var rightShoulderTracker: Tracker? = null + var leftThumbProximalTracker: Tracker? = null + var leftThumbIntermediateTracker: Tracker? = null + var leftThumbDistalTracker: Tracker? = null + var leftIndexProximalTracker: Tracker? = null + var leftIndexIntermediateTracker: Tracker? = null + var leftIndexDistalTracker: Tracker? = null + var leftMiddleProximalTracker: Tracker? = null + var leftMiddleIntermediateTracker: Tracker? = null + var leftMiddleDistalTracker: Tracker? = null + var leftRingProximalTracker: Tracker? = null + var leftRingIntermediateTracker: Tracker? = null + var leftRingDistalTracker: Tracker? = null + var leftLittleProximalTracker: Tracker? = null + var leftLittleIntermediateTracker: Tracker? = null + var leftLittleDistalTracker: Tracker? = null + var rightThumbProximalTracker: Tracker? = null + var rightThumbIntermediateTracker: Tracker? = null + var rightThumbDistalTracker: Tracker? = null + var rightIndexProximalTracker: Tracker? = null + var rightIndexIntermediateTracker: Tracker? = null + var rightIndexDistalTracker: Tracker? = null + var rightMiddleProximalTracker: Tracker? = null + var rightMiddleIntermediateTracker: Tracker? = null + var rightMiddleDistalTracker: Tracker? = null + var rightRingProximalTracker: Tracker? = null + var rightRingIntermediateTracker: Tracker? = null + var rightRingDistalTracker: Tracker? = null + var rightLittleProximalTracker: Tracker? = null + var rightLittleIntermediateTracker: Tracker? = null + var rightLittleDistalTracker: Tracker? = null // Output trackers var computedHeadTracker: Tracker? = null @@ -258,24 +318,63 @@ class HumanSkeleton( rightLowerArmBone.attachChild(rightHandBone) rightHandBone.attachChild(rightHandTrackerBone) } + + // Fingers + leftHandBone.attachChild(leftThumbProximalBone) + leftThumbProximalBone.attachChild(leftThumbIntermediateBone) + leftThumbIntermediateBone.attachChild(leftThumbDistalBone) + leftHandBone.attachChild(leftIndexProximalBone) + leftIndexProximalBone.attachChild(leftIndexIntermediateBone) + leftIndexIntermediateBone.attachChild(leftIndexDistalBone) + leftHandBone.attachChild(leftMiddleProximalBone) + leftMiddleProximalBone.attachChild(leftMiddleIntermediateBone) + leftMiddleIntermediateBone.attachChild(leftMiddleDistalBone) + leftHandBone.attachChild(leftRingProximalBone) + leftRingProximalBone.attachChild(leftRingIntermediateBone) + leftRingIntermediateBone.attachChild(leftRingDistalBone) + leftHandBone.attachChild(leftLittleProximalBone) + leftLittleProximalBone.attachChild(leftLittleIntermediateBone) + leftLittleIntermediateBone.attachChild(leftLittleDistalBone) + rightHandBone.attachChild(rightThumbProximalBone) + rightThumbProximalBone.attachChild(rightThumbIntermediateBone) + rightThumbIntermediateBone.attachChild(rightThumbDistalBone) + rightHandBone.attachChild(rightIndexProximalBone) + rightIndexProximalBone.attachChild(rightIndexIntermediateBone) + rightIndexIntermediateBone.attachChild(rightIndexDistalBone) + rightHandBone.attachChild(rightMiddleProximalBone) + rightMiddleProximalBone.attachChild(rightMiddleIntermediateBone) + rightMiddleIntermediateBone.attachChild(rightMiddleDistalBone) + rightHandBone.attachChild(rightRingProximalBone) + rightRingProximalBone.attachChild(rightRingIntermediateBone) + rightRingIntermediateBone.attachChild(rightRingDistalBone) + rightHandBone.attachChild(rightLittleProximalBone) + rightLittleProximalBone.attachChild(rightLittleIntermediateBone) + rightLittleIntermediateBone.attachChild(rightLittleDistalBone) } /** * Set input trackers from a list */ fun setTrackersFromList(trackers: List) { + // Head headTracker = getTrackerForSkeleton(trackers, TrackerPosition.HEAD) neckTracker = getTrackerForSkeleton(trackers, TrackerPosition.NECK) + + // Spine upperChestTracker = getTrackerForSkeleton(trackers, TrackerPosition.UPPER_CHEST) chestTracker = getTrackerForSkeleton(trackers, TrackerPosition.CHEST) waistTracker = getTrackerForSkeleton(trackers, TrackerPosition.WAIST) hipTracker = getTrackerForSkeleton(trackers, TrackerPosition.HIP) + + // Legs leftUpperLegTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_UPPER_LEG) leftLowerLegTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_LOWER_LEG) leftFootTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_FOOT) rightUpperLegTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_UPPER_LEG) rightLowerLegTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_LOWER_LEG) rightFootTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_FOOT) + + // Arms leftLowerArmTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_LOWER_ARM) rightLowerArmTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_LOWER_ARM) leftUpperArmTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_UPPER_ARM) @@ -285,15 +384,53 @@ class HumanSkeleton( leftShoulderTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_SHOULDER) rightShoulderTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_SHOULDER) - // Check for specific conditions and store them in booleans. + // Fingers + leftThumbProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_THUMB_PROXIMAL) + leftThumbIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_THUMB_INTERMEDIATE) + leftThumbDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_THUMB_DISTAL) + leftIndexProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_INDEX_PROXIMAL) + leftIndexIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_INDEX_INTERMEDIATE) + leftIndexDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_INDEX_DISTAL) + leftMiddleProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_MIDDLE_PROXIMAL) + leftMiddleIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_MIDDLE_INTERMEDIATE) + leftMiddleDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_MIDDLE_DISTAL) + leftRingProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_RING_PROXIMAL) + leftRingIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_RING_INTERMEDIATE) + leftRingDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_RING_DISTAL) + leftLittleProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_LITTLE_PROXIMAL) + leftLittleIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_LITTLE_INTERMEDIATE) + leftLittleDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_LITTLE_DISTAL) + rightThumbProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_THUMB_PROXIMAL) + rightThumbIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_THUMB_INTERMEDIATE) + rightThumbDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_THUMB_DISTAL) + rightIndexProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_INDEX_PROXIMAL) + rightIndexIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_INDEX_INTERMEDIATE) + rightIndexDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_INDEX_DISTAL) + rightMiddleProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_MIDDLE_PROXIMAL) + rightMiddleIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE) + rightMiddleDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_MIDDLE_DISTAL) + rightRingProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_RING_PROXIMAL) + rightRingIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_RING_INTERMEDIATE) + rightRingDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_RING_DISTAL) + rightLittleProximalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_LITTLE_PROXIMAL) + rightLittleIntermediateTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_LITTLE_INTERMEDIATE) + rightLittleDistalTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_LITTLE_DISTAL) + + // Check for specific conditions and cache them hasSpineTracker = upperChestTracker != null || chestTracker != null || waistTracker != null || hipTracker != null hasKneeTrackers = leftUpperLegTracker != null && rightUpperLegTracker != null - hasLeftLegTracker = leftUpperLegTracker != null || leftLowerLegTracker != null || leftFootTracker != null - hasRightLegTracker = rightUpperLegTracker != null || rightLowerLegTracker != null || rightFootTracker != null - hasLeftFootTracker = leftFootTracker != null - hasRightFootTracker = rightFootTracker != null hasLeftArmTracker = leftLowerArmTracker != null || leftUpperArmTracker != null hasRightArmTracker = rightLowerArmTracker != null || rightUpperArmTracker != null + hasLeftFingerTracker = leftThumbProximalTracker != null || leftThumbIntermediateTracker != null || leftThumbDistalTracker != null || + leftIndexProximalTracker != null || leftIndexIntermediateTracker != null || leftIndexDistalTracker != null || + leftMiddleProximalTracker != null || leftMiddleIntermediateTracker != null || leftMiddleDistalTracker != null || + leftRingProximalTracker != null || leftRingIntermediateTracker != null || leftRingDistalTracker != null || + leftLittleProximalTracker != null || leftLittleIntermediateTracker != null || leftLittleDistalTracker != null + hasRightFingerTracker = rightThumbProximalTracker != null || rightThumbIntermediateTracker != null || rightThumbDistalTracker != null || + rightIndexProximalTracker != null || rightIndexIntermediateTracker != null || rightIndexDistalTracker != null || + rightMiddleProximalTracker != null || rightMiddleIntermediateTracker != null || rightMiddleDistalTracker != null || + rightRingProximalTracker != null || rightRingIntermediateTracker != null || rightRingDistalTracker != null || + rightLittleProximalTracker != null || rightLittleIntermediateTracker != null || rightLittleDistalTracker != null // Rebuilds the arm skeleton nodes attachments assembleSkeletonArms(true) @@ -393,6 +530,7 @@ class HumanSkeleton( // Spine updateSpineTransforms() + // Left leg updateLegTransforms( leftUpperLegBone, @@ -404,6 +542,7 @@ class HumanSkeleton( leftLowerLegTracker, leftFootTracker, ) + // Right leg updateLegTransforms( rightUpperLegBone, @@ -415,6 +554,7 @@ class HumanSkeleton( rightLowerLegTracker, rightFootTracker, ) + // Left arm updateArmTransforms( isTrackingLeftArmFromController, @@ -429,6 +569,7 @@ class HumanSkeleton( leftLowerArmTracker, leftHandTracker, ) + // Right arm updateArmTransforms( isTrackingRightArmFromController, @@ -443,6 +584,116 @@ class HumanSkeleton( rightLowerArmTracker, rightHandTracker, ) + + // Left thumb + updateFingerTransforms( + leftHandBone.getGlobalRotation(), + leftThumbProximalBone, + leftThumbIntermediateBone, + leftThumbDistalBone, + leftThumbProximalTracker, + leftThumbIntermediateTracker, + leftThumbDistalTracker, + ) + + // Left index + updateFingerTransforms( + leftHandBone.getGlobalRotation(), + leftIndexProximalBone, + leftIndexIntermediateBone, + leftIndexDistalBone, + leftIndexProximalTracker, + leftIndexIntermediateTracker, + leftIndexDistalTracker, + ) + + // Left middle + updateFingerTransforms( + leftHandBone.getGlobalRotation(), + leftMiddleProximalBone, + leftMiddleIntermediateBone, + leftMiddleDistalBone, + leftMiddleProximalTracker, + leftMiddleIntermediateTracker, + leftMiddleDistalTracker, + ) + + // Left ring + updateFingerTransforms( + leftHandBone.getGlobalRotation(), + leftRingProximalBone, + leftRingIntermediateBone, + leftRingDistalBone, + leftRingProximalTracker, + leftRingIntermediateTracker, + leftRingDistalTracker, + ) + + // Left little + updateFingerTransforms( + leftHandBone.getGlobalRotation(), + leftLittleProximalBone, + leftLittleIntermediateBone, + leftLittleDistalBone, + leftLittleProximalTracker, + leftLittleIntermediateTracker, + leftLittleDistalTracker, + ) + + // Right thumb + updateFingerTransforms( + rightHandBone.getGlobalRotation(), + rightThumbProximalBone, + rightThumbIntermediateBone, + rightThumbDistalBone, + rightThumbProximalTracker, + rightThumbIntermediateTracker, + rightThumbDistalTracker, + ) + + // Right index + updateFingerTransforms( + rightHandBone.getGlobalRotation(), + rightIndexProximalBone, + rightIndexIntermediateBone, + rightIndexDistalBone, + rightIndexProximalTracker, + rightIndexIntermediateTracker, + rightIndexDistalTracker, + ) + + // Right middle + updateFingerTransforms( + rightHandBone.getGlobalRotation(), + rightMiddleProximalBone, + rightMiddleIntermediateBone, + rightMiddleDistalBone, + rightMiddleProximalTracker, + rightMiddleIntermediateTracker, + rightMiddleDistalTracker, + ) + + // Right ring + updateFingerTransforms( + rightHandBone.getGlobalRotation(), + rightRingProximalBone, + rightRingIntermediateBone, + rightRingDistalBone, + rightRingProximalTracker, + rightRingIntermediateTracker, + rightRingDistalTracker, + ) + + // Right little + updateFingerTransforms( + rightHandBone.getGlobalRotation(), + rightLittleProximalBone, + rightLittleIntermediateBone, + rightLittleDistalBone, + rightLittleProximalTracker, + rightLittleIntermediateTracker, + rightLittleDistalTracker, + ) } /** @@ -766,6 +1017,46 @@ class HumanSkeleton( } } + /** + * Update a finger's 3 bones' transforms + */ + private fun updateFingerTransforms( + handRotation: Quaternion, + proximalBone: Bone, + intermediateBone: Bone, + distalBone: Bone, + proximalTracker: Tracker?, + intermediateTracker: Tracker?, + distalTracker: Tracker?, + ) { + if (distalTracker == null && intermediateTracker == null && proximalTracker == null) { + // Set fingers' rotations to the hand's if no finger tracker + proximalBone.setRotation(handRotation) + intermediateBone.setRotation(handRotation) + distalBone.setRotation(handRotation) + } + + // Note: we use interpQ instead of interpR in order to slerp over 180 degrees. + // Start of finger + proximalTracker?.let { + proximalBone.setRotation(it.getRotation()) + if (intermediateTracker == null) intermediateBone.setRotation(handRotation.interpQ(it.getRotation(), 2.12f)) + if (distalTracker == null) distalBone.setRotation(handRotation.interpQ(it.getRotation(), 3.03f)) + } + // Middle of finger + intermediateTracker?.let { + if (proximalTracker == null) proximalBone.setRotation(handRotation.interpQ(it.getRotation(), 0.47f)) + intermediateBone.setRotation(it.getRotation()) + if (distalTracker == null) distalBone.setRotation(handRotation.interpQ(it.getRotation(), 1.43f)) + } + // Tip of finger + distalTracker?.let { + if (proximalTracker == null && intermediateTracker == null) proximalBone.setRotation(handRotation.interpQ(it.getRotation(), 0.33f)) + if (intermediateTracker == null) intermediateBone.setRotation(handRotation.interpQ(it.getRotation(), 0.7f)) + distalBone.setRotation(it.getRotation()) + } + } + /** * Rotates the first Quaternion to match its yaw and roll to the rotation of * the second Quaternion @@ -958,6 +1249,36 @@ class HumanSkeleton( BoneType.RIGHT_HAND -> rightHandBone BoneType.LEFT_HAND_TRACKER -> leftHandTrackerBone BoneType.RIGHT_HAND_TRACKER -> rightHandTrackerBone + BoneType.LEFT_THUMB_PROXIMAL -> leftThumbProximalBone + BoneType.LEFT_THUMB_INTERMEDIATE -> leftThumbIntermediateBone + BoneType.LEFT_THUMB_DISTAL -> leftThumbDistalBone + BoneType.LEFT_INDEX_PROXIMAL -> leftIndexProximalBone + BoneType.LEFT_INDEX_INTERMEDIATE -> leftIndexIntermediateBone + BoneType.LEFT_INDEX_DISTAL -> leftIndexDistalBone + BoneType.LEFT_MIDDLE_PROXIMAL -> leftMiddleProximalBone + BoneType.LEFT_MIDDLE_INTERMEDIATE -> leftMiddleIntermediateBone + BoneType.LEFT_MIDDLE_DISTAL -> leftMiddleDistalBone + BoneType.LEFT_RING_PROXIMAL -> leftRingProximalBone + BoneType.LEFT_RING_INTERMEDIATE -> leftRingIntermediateBone + BoneType.LEFT_RING_DISTAL -> leftRingDistalBone + BoneType.LEFT_LITTLE_PROXIMAL -> leftLittleProximalBone + BoneType.LEFT_LITTLE_INTERMEDIATE -> leftLittleIntermediateBone + BoneType.LEFT_LITTLE_DISTAL -> leftLittleDistalBone + BoneType.RIGHT_THUMB_PROXIMAL -> rightThumbProximalBone + BoneType.RIGHT_THUMB_INTERMEDIATE -> rightThumbIntermediateBone + BoneType.RIGHT_THUMB_DISTAL -> rightThumbDistalBone + BoneType.RIGHT_INDEX_PROXIMAL -> rightIndexProximalBone + BoneType.RIGHT_INDEX_INTERMEDIATE -> rightIndexIntermediateBone + BoneType.RIGHT_INDEX_DISTAL -> rightIndexDistalBone + BoneType.RIGHT_MIDDLE_PROXIMAL -> rightMiddleProximalBone + BoneType.RIGHT_MIDDLE_INTERMEDIATE -> rightMiddleIntermediateBone + BoneType.RIGHT_MIDDLE_DISTAL -> rightMiddleDistalBone + BoneType.RIGHT_RING_PROXIMAL -> rightRingProximalBone + BoneType.RIGHT_RING_INTERMEDIATE -> rightRingIntermediateBone + BoneType.RIGHT_RING_DISTAL -> rightRingDistalBone + BoneType.RIGHT_LITTLE_PROXIMAL -> rightLittleProximalBone + BoneType.RIGHT_LITTLE_INTERMEDIATE -> rightLittleIntermediateBone + BoneType.RIGHT_LITTLE_DISTAL -> rightLittleDistalBone } /** @@ -987,6 +1308,36 @@ class HumanSkeleton( rightLowerArmBone, leftHandBone, rightHandBone, + leftThumbProximalBone, + leftThumbIntermediateBone, + leftThumbDistalBone, + leftIndexProximalBone, + leftIndexIntermediateBone, + leftIndexDistalBone, + leftMiddleProximalBone, + leftMiddleIntermediateBone, + leftMiddleDistalBone, + leftRingProximalBone, + leftRingIntermediateBone, + leftRingDistalBone, + leftLittleProximalBone, + leftLittleIntermediateBone, + leftLittleDistalBone, + rightThumbProximalBone, + rightThumbIntermediateBone, + rightThumbDistalBone, + rightIndexProximalBone, + rightIndexIntermediateBone, + rightIndexDistalBone, + rightMiddleProximalBone, + rightMiddleIntermediateBone, + rightMiddleDistalBone, + rightRingProximalBone, + rightRingIntermediateBone, + rightRingDistalBone, + rightLittleProximalBone, + rightLittleIntermediateBone, + rightLittleDistalBone, ) /** @@ -1006,6 +1357,36 @@ class HumanSkeleton( rightHandBone, leftHandTrackerBone, rightHandTrackerBone, + leftThumbProximalBone, + leftThumbIntermediateBone, + leftThumbDistalBone, + leftIndexProximalBone, + leftIndexIntermediateBone, + leftIndexDistalBone, + leftMiddleProximalBone, + leftMiddleIntermediateBone, + leftMiddleDistalBone, + leftRingProximalBone, + leftRingIntermediateBone, + leftRingDistalBone, + leftLittleProximalBone, + leftLittleIntermediateBone, + leftLittleDistalBone, + rightThumbProximalBone, + rightThumbIntermediateBone, + rightThumbDistalBone, + rightIndexProximalBone, + rightIndexIntermediateBone, + rightIndexDistalBone, + rightMiddleProximalBone, + rightMiddleIntermediateBone, + rightMiddleDistalBone, + rightRingProximalBone, + rightRingIntermediateBone, + rightRingDistalBone, + rightLittleProximalBone, + rightLittleIntermediateBone, + rightLittleDistalBone, ) val hmdHeight: Float @@ -1052,6 +1433,36 @@ class HumanSkeleton( rightHandTracker, leftShoulderTracker, rightShoulderTracker, + leftThumbProximalTracker, + leftThumbIntermediateTracker, + leftThumbDistalTracker, + leftIndexProximalTracker, + leftIndexIntermediateTracker, + leftIndexDistalTracker, + leftMiddleProximalTracker, + leftMiddleIntermediateTracker, + leftMiddleDistalTracker, + leftRingProximalTracker, + leftRingIntermediateTracker, + leftRingDistalTracker, + leftLittleProximalTracker, + leftLittleIntermediateTracker, + leftLittleDistalTracker, + rightThumbProximalTracker, + rightThumbIntermediateTracker, + rightThumbDistalTracker, + rightIndexProximalTracker, + rightIndexIntermediateTracker, + rightIndexDistalTracker, + rightMiddleProximalTracker, + rightMiddleIntermediateTracker, + rightMiddleDistalTracker, + rightRingProximalTracker, + rightRingIntermediateTracker, + rightRingDistalTracker, + rightLittleProximalTracker, + rightLittleIntermediateTracker, + rightLittleDistalTracker, ) fun resetTrackersFull(resetSourceName: String?) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index 564dd4b85f..4871f52ae4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt @@ -315,12 +315,12 @@ class Tracker @JvmOverloads constructor( * it too much should be avoided for performance reasons. */ fun getRotation(): Quaternion { - var rot = if (allowFiltering && filteringHandler.enabled) { + var rot = if (allowFiltering && filteringHandler.filteringEnabled) { // Get filtered rotation filteringHandler.getFilteredRotation() } else { // Get unfiltered rotation - _rotation + filteringHandler.getTrackedRotation() } if (needsReset || (isComputed && !isInternal)) { @@ -346,12 +346,12 @@ class Tracker @JvmOverloads constructor( * This is used for debugging/visualizing tracker data */ fun getIdentityAdjustedRotation(): Quaternion { - var rot = if (filteringHandler.enabled) { + var rot = if (filteringHandler.filteringEnabled) { // Get filtered rotation filteringHandler.getFilteredRotation() } else { // Get unfiltered rotation - _rotation + filteringHandler.getTrackedRotation() } if (needsReset || (isComputed && trackerPosition == TrackerPosition.HEAD)) { @@ -366,7 +366,7 @@ class Tracker @JvmOverloads constructor( * Gets the raw (unadjusted) rotation of the tracker. * If this is an IMU, this will be the raw sensor rotation. */ - fun getRawRotation(): Quaternion = _rotation + fun getRawRotation() = _rotation /** * Sets the raw (unadjusted) rotation of the tracker. @@ -389,4 +389,11 @@ class Tracker @JvmOverloads constructor( */ val tps: Float get() = timer.averageFPS + + /** + * Call when doing a full reset to reset the tracking of rotations >180 degrees + */ + fun resetFilteringQuats() { + filteringHandler.resetQuats(_rotation) + } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt index 5e7a8ca129..e00f06b8a4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt @@ -12,8 +12,9 @@ import io.github.axisangles.ktmath.Quaternion */ class TrackerFilteringHandler { - private var movingAverage: QuaternionMovingAverage? = null - var enabled = false + private var filteringMovingAverage: QuaternionMovingAverage? = null + private var trackingMovingAverage = QuaternionMovingAverage(TrackerFilters.NONE) + var filteringEnabled = false /** * Reads/loads filtering settings from given config @@ -21,15 +22,15 @@ class TrackerFilteringHandler { fun readFilteringConfig(config: FiltersConfig, currentRawRotation: Quaternion) { val type = TrackerFilters.getByConfigkey(config.type) if (type == TrackerFilters.SMOOTHING || type == TrackerFilters.PREDICTION) { - movingAverage = QuaternionMovingAverage( + filteringMovingAverage = QuaternionMovingAverage( type, config.amount, currentRawRotation, ) - enabled = true + filteringEnabled = true } else { - movingAverage = null - enabled = false + filteringMovingAverage = null + filteringEnabled = false } } @@ -37,18 +38,33 @@ class TrackerFilteringHandler { * Update the moving average to make it smooth */ fun update() { - movingAverage?.update() + trackingMovingAverage.update() + filteringMovingAverage?.update() } /** * Updates the latest rotation */ fun dataTick(currentRawRotation: Quaternion) { - movingAverage?.addQuaternion(currentRawRotation) + trackingMovingAverage.addQuaternion(currentRawRotation) + filteringMovingAverage?.addQuaternion(currentRawRotation) } + /** + * Call when doing a full reset to reset the tracking of rotations >180 degrees + */ + fun resetQuats(currentRawRotation: Quaternion) { + trackingMovingAverage.resetQuats(currentRawRotation) + filteringMovingAverage?.resetQuats(currentRawRotation) + } + + /** + * Gets the tracked rotation from the moving average (allows >180 degrees) + */ + fun getTrackedRotation() = trackingMovingAverage.filteredQuaternion + /** * Get the filtered rotation from the moving average */ - fun getFilteredRotation(): Quaternion = movingAverage?.filteredQuaternion ?: Quaternion.IDENTITY + fun getFilteredRotation() = filteringMovingAverage?.filteredQuaternion ?: Quaternion.IDENTITY } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt index 559aea8995..bc4d1f5544 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt @@ -36,16 +36,64 @@ enum class TrackerPosition( RIGHT_HAND("body:right_hand", TrackerRole.RIGHT_HAND, BodyPart.RIGHT_HAND), LEFT_SHOULDER("body:left_shoulder", TrackerRole.LEFT_SHOULDER, BodyPart.LEFT_SHOULDER), RIGHT_SHOULDER("body:right_shoulder", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_SHOULDER), + LEFT_THUMB_PROXIMAL("body:left_thumb_proximal", null, BodyPart.LEFT_THUMB_PROXIMAL), + LEFT_THUMB_INTERMEDIATE("body:left_thumb_intermediate", null, BodyPart.LEFT_THUMB_INTERMEDIATE), + LEFT_THUMB_DISTAL("body:left_thumb_distal", null, BodyPart.LEFT_THUMB_DISTAL), + LEFT_INDEX_PROXIMAL("body:left_index_proximal", null, BodyPart.LEFT_INDEX_PROXIMAL), + LEFT_INDEX_INTERMEDIATE("body:left_index_intermediate", null, BodyPart.LEFT_INDEX_INTERMEDIATE), + LEFT_INDEX_DISTAL("body:left_index_distal", null, BodyPart.LEFT_INDEX_DISTAL), + LEFT_MIDDLE_PROXIMAL("body:left_middle_proximal", null, BodyPart.LEFT_MIDDLE_PROXIMAL), + LEFT_MIDDLE_INTERMEDIATE("body:left_middle_intermediate", null, BodyPart.LEFT_MIDDLE_INTERMEDIATE), + LEFT_MIDDLE_DISTAL("body:left_middle_distal", null, BodyPart.LEFT_MIDDLE_DISTAL), + LEFT_RING_PROXIMAL("body:left_ring_proximal", null, BodyPart.LEFT_RING_PROXIMAL), + LEFT_RING_INTERMEDIATE("body:left_ring_intermediate", null, BodyPart.LEFT_RING_INTERMEDIATE), + LEFT_RING_DISTAL("body:left_ring_distal", null, BodyPart.LEFT_RING_DISTAL), + LEFT_LITTLE_PROXIMAL("body:left_little_proximal", null, BodyPart.LEFT_LITTLE_PROXIMAL), + LEFT_LITTLE_INTERMEDIATE("body:left_little_intermediate", null, BodyPart.LEFT_LITTLE_INTERMEDIATE), + LEFT_LITTLE_DISTAL("body:left_little_distal", null, BodyPart.LEFT_LITTLE_DISTAL), + RIGHT_THUMB_PROXIMAL("body:right_thumb_proximal", null, BodyPart.RIGHT_THUMB_PROXIMAL), + RIGHT_THUMB_INTERMEDIATE("body:right_thumb_intermediate", null, BodyPart.RIGHT_THUMB_INTERMEDIATE), + RIGHT_THUMB_DISTAL("body:right_thumb_distal", null, BodyPart.RIGHT_THUMB_DISTAL), + RIGHT_INDEX_PROXIMAL("body:right_index_proximal", null, BodyPart.RIGHT_INDEX_PROXIMAL), + RIGHT_INDEX_INTERMEDIATE("body:right_index_intermediate", null, BodyPart.RIGHT_INDEX_INTERMEDIATE), + RIGHT_INDEX_DISTAL("body:right_index_distal", null, BodyPart.RIGHT_INDEX_DISTAL), + RIGHT_MIDDLE_PROXIMAL("body:right_middle_proximal", null, BodyPart.RIGHT_MIDDLE_PROXIMAL), + RIGHT_MIDDLE_INTERMEDIATE("body:right_middle_intermediate", null, BodyPart.RIGHT_MIDDLE_INTERMEDIATE), + RIGHT_MIDDLE_DISTAL("body:right_middle_distal", null, BodyPart.RIGHT_MIDDLE_DISTAL), + RIGHT_RING_PROXIMAL("body:right_ring_proximal", null, BodyPart.RIGHT_RING_PROXIMAL), + RIGHT_RING_INTERMEDIATE("body:right_ring_intermediate", null, BodyPart.RIGHT_RING_INTERMEDIATE), + RIGHT_RING_DISTAL("body:right_ring_distal", null, BodyPart.RIGHT_RING_DISTAL), + RIGHT_LITTLE_PROXIMAL("body:right_little_proximal", null, BodyPart.RIGHT_LITTLE_PROXIMAL), + RIGHT_LITTLE_INTERMEDIATE("body:right_little_intermediate", null, BodyPart.RIGHT_LITTLE_INTERMEDIATE), + RIGHT_LITTLE_DISTAL("body:right_little_distal", null, BodyPart.RIGHT_LITTLE_DISTAL), ; /** * Returns the default mounting orientation for the body part */ fun defaultMounting(): Quaternion = when (this) { - LEFT_LOWER_ARM, LEFT_HAND -> Quaternion.SLIMEVR.LEFT - RIGHT_LOWER_ARM, RIGHT_HAND -> Quaternion.SLIMEVR.RIGHT + LEFT_LOWER_ARM, LEFT_HAND, + LEFT_INDEX_PROXIMAL, LEFT_INDEX_INTERMEDIATE, + LEFT_INDEX_DISTAL, LEFT_MIDDLE_PROXIMAL, + LEFT_MIDDLE_INTERMEDIATE, LEFT_MIDDLE_DISTAL, + LEFT_RING_PROXIMAL, LEFT_RING_INTERMEDIATE, + LEFT_RING_DISTAL, LEFT_LITTLE_PROXIMAL, + LEFT_LITTLE_INTERMEDIATE, LEFT_LITTLE_DISTAL, + -> Quaternion.SLIMEVR.LEFT + + RIGHT_LOWER_ARM, RIGHT_HAND, + RIGHT_INDEX_PROXIMAL, RIGHT_INDEX_INTERMEDIATE, + RIGHT_INDEX_DISTAL, RIGHT_MIDDLE_PROXIMAL, + RIGHT_MIDDLE_INTERMEDIATE, RIGHT_MIDDLE_DISTAL, + RIGHT_RING_PROXIMAL, RIGHT_RING_INTERMEDIATE, + RIGHT_RING_DISTAL, RIGHT_LITTLE_PROXIMAL, + RIGHT_LITTLE_INTERMEDIATE, RIGHT_LITTLE_DISTAL, + -> Quaternion.SLIMEVR.RIGHT + LEFT_UPPER_ARM, LEFT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_LEFT + RIGHT_UPPER_ARM, RIGHT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_RIGHT + else -> Quaternion.SLIMEVR.FRONT } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index a47dbd0a6e..2fa2c2ae3c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -212,9 +212,9 @@ class TrackerResetsHandler(val tracker: Tracker) { */ fun resetFull(reference: Quaternion) { // Adjust for T-Pose (down) - tposeDownFix = if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { + tposeDownFix = if (((isLeftArmTracker() || isLeftFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { EulerAngles(EulerOrder.YZX, 0f, 0f, -FastMath.HALF_PI).toQuaternion() - } else if ((isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { + } else if (((isRightArmTracker() || isRightFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { EulerAngles(EulerOrder.YZX, 0f, 0f, FastMath.HALF_PI).toQuaternion() } else { Quaternion.IDENTITY @@ -278,6 +278,8 @@ class TrackerResetsHandler(val tracker: Tracker) { VRServer.instance.statusSystem.removeStatus(this.tracker.lastResetStatus) this.tracker.lastResetStatus = 0u } + + tracker.resetFilteringQuats() } /** @@ -312,6 +314,8 @@ class TrackerResetsHandler(val tracker: Tracker) { this.tracker.statusResetRecently = false this.tracker.lastResetStatus = 0u } + + tracker.resetFilteringQuats() } /** @@ -336,15 +340,17 @@ class TrackerResetsHandler(val tracker: Tracker) { // Calculate the yaw angle using tan var yawAngle = atan2(rotVector.x, rotVector.z) - // Adjust for T-Pose + // Adjust for T-Pose and fingers if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) || - (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) + (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) || + isLeftFingerTracker() ) { // Tracker goes right yawAngle -= FastMath.HALF_PI } if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) || - (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) + (isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) || + isRightFingerTracker() ) { // Tracker goes left yawAngle += FastMath.HALF_PI @@ -363,6 +369,8 @@ class TrackerResetsHandler(val tracker: Tracker) { // save mounting reset if (saveMountingReset) tracker.saveMountingResetOrientation(mountRotFix) + + tracker.resetFilteringQuats() } fun clearMounting() { @@ -571,4 +579,46 @@ class TrackerResetsHandler(val tracker: Tracker) { } return false } + + private fun isLeftFingerTracker(): Boolean { + tracker.trackerPosition?.let { + return it == TrackerPosition.LEFT_THUMB_PROXIMAL || + it == TrackerPosition.LEFT_THUMB_INTERMEDIATE || + it == TrackerPosition.LEFT_THUMB_DISTAL || + it == TrackerPosition.LEFT_INDEX_PROXIMAL || + it == TrackerPosition.LEFT_INDEX_INTERMEDIATE || + it == TrackerPosition.LEFT_INDEX_DISTAL || + it == TrackerPosition.LEFT_MIDDLE_PROXIMAL || + it == TrackerPosition.LEFT_MIDDLE_INTERMEDIATE || + it == TrackerPosition.LEFT_MIDDLE_DISTAL || + it == TrackerPosition.LEFT_RING_PROXIMAL || + it == TrackerPosition.LEFT_RING_INTERMEDIATE || + it == TrackerPosition.LEFT_RING_DISTAL || + it == TrackerPosition.LEFT_LITTLE_PROXIMAL || + it == TrackerPosition.LEFT_LITTLE_INTERMEDIATE || + it == TrackerPosition.LEFT_LITTLE_DISTAL + } + return false + } + + private fun isRightFingerTracker(): Boolean { + tracker.trackerPosition?.let { + return it == TrackerPosition.RIGHT_THUMB_PROXIMAL || + it == TrackerPosition.RIGHT_THUMB_INTERMEDIATE || + it == TrackerPosition.RIGHT_THUMB_DISTAL || + it == TrackerPosition.RIGHT_INDEX_PROXIMAL || + it == TrackerPosition.RIGHT_INDEX_INTERMEDIATE || + it == TrackerPosition.RIGHT_INDEX_DISTAL || + it == TrackerPosition.RIGHT_MIDDLE_PROXIMAL || + it == TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE || + it == TrackerPosition.RIGHT_MIDDLE_DISTAL || + it == TrackerPosition.RIGHT_RING_PROXIMAL || + it == TrackerPosition.RIGHT_RING_INTERMEDIATE || + it == TrackerPosition.RIGHT_RING_DISTAL || + it == TrackerPosition.RIGHT_LITTLE_PROXIMAL || + it == TrackerPosition.RIGHT_LITTLE_INTERMEDIATE || + it == TrackerPosition.RIGHT_LITTLE_DISTAL + } + return false + } } diff --git a/solarxr-protocol b/solarxr-protocol index b182bec2ca..285eb06354 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit b182bec2cad042fdbfdeb27236a310cb0bd40617 +Subproject commit 285eb063547802ecaa1fb5890adb1a3f49adfb80