diff --git a/gui/app/public/i18n/en/translation.ftl b/gui/app/public/i18n/en/translation.ftl index b731133207..e97cc93b9c 100644 --- a/gui/app/public/i18n/en/translation.ftl +++ b/gui/app/public/i18n/en/translation.ftl @@ -64,6 +64,7 @@ body_part-NONE = Unassigned body_part-HEAD = Head body_part-NECK = Neck body_part-RIGHT_SHOULDER = Right shoulder +body_part-RIGHT_BUST = Right bust body_part-RIGHT_UPPER_ARM = Right upper arm body_part-RIGHT_LOWER_ARM = Right lower arm body_part-RIGHT_HAND = Right hand @@ -76,6 +77,7 @@ body_part-UPPER_WAIST = Upper waist body_part-LOWER_WAIST = Lower waist body_part-HIP = Hip body_part-LEFT_SHOULDER = Left shoulder +body_part-LEFT_BUST = Left bust body_part-LEFT_UPPER_ARM = Left upper arm body_part-LEFT_LOWER_ARM = Left lower arm body_part-LEFT_HAND = Left hand diff --git a/gui/app/public/models/skeleton/breast.bin b/gui/app/public/models/skeleton/breast.bin new file mode 100644 index 0000000000..3d79c2dcd5 Binary files /dev/null and b/gui/app/public/models/skeleton/breast.bin differ diff --git a/gui/app/public/models/skeleton/bust.gltf b/gui/app/public/models/skeleton/bust.gltf new file mode 100644 index 0000000000..66ccded7ad --- /dev/null +++ b/gui/app/public/models/skeleton/bust.gltf @@ -0,0 +1,104 @@ +{ + "asset":{ + "generator":"Khronos glTF Blender I/O v4.5.48", + "version":"2.0" + }, + "scene":0, + "scenes":[ + { + "name":"Scene", + "nodes":[ + 0 + ] + } + ], + "nodes":[ + { + "mesh":0, + "name":"Sphere" + } + ], + "meshes":[ + { + "name":"Sphere", + "primitives":[ + { + "attributes":{ + "POSITION":0, + "NORMAL":1, + "TEXCOORD_0":2 + }, + "indices":3 + } + ] + } + ], + "accessors":[ + { + "bufferView":0, + "componentType":5126, + "count":559, + "max":[ + 0.9999997019767761, + 1, + 0.9999993443489075 + ], + "min":[ + -0.9999990463256836, + -1, + -1 + ], + "type":"VEC3" + }, + { + "bufferView":1, + "componentType":5126, + "count":559, + "type":"VEC3" + }, + { + "bufferView":2, + "componentType":5126, + "count":559, + "type":"VEC2" + }, + { + "bufferView":3, + "componentType":5123, + "count":2880, + "type":"SCALAR" + } + ], + "bufferViews":[ + { + "buffer":0, + "byteLength":6708, + "byteOffset":0, + "target":34962 + }, + { + "buffer":0, + "byteLength":6708, + "byteOffset":6708, + "target":34962 + }, + { + "buffer":0, + "byteLength":4472, + "byteOffset":13416, + "target":34962 + }, + { + "buffer":0, + "byteLength":5760, + "byteOffset":17888, + "target":34963 + } + ], + "buffers":[ + { + "byteLength":23648, + "uri":"breast.bin" + } + ] +} diff --git a/gui/app/src/components/commons/PersonFrontIcon.tsx b/gui/app/src/components/commons/PersonFrontIcon.tsx index f82583b832..8fc1256ec7 100644 --- a/gui/app/src/components/commons/PersonFrontIcon.tsx +++ b/gui/app/src/components/commons/PersonFrontIcon.tsx @@ -3,6 +3,7 @@ import { BodyPart } from 'solarxr-protocol'; export const SIDES = [ { shoulder: BodyPart.LEFT_SHOULDER, + bust: BodyPart.LEFT_BUST, upperArm: BodyPart.LEFT_UPPER_ARM, lowerArm: BodyPart.LEFT_LOWER_ARM, hand: BodyPart.LEFT_HAND, @@ -15,6 +16,7 @@ export const SIDES = [ }, { shoulder: BodyPart.RIGHT_SHOULDER, + bust: BodyPart.RIGHT_BUST, upperArm: BodyPart.RIGHT_UPPER_ARM, lowerArm: BodyPart.RIGHT_LOWER_ARM, hand: BodyPart.RIGHT_HAND, @@ -120,6 +122,13 @@ export function PersonFrontIcon({ r={CIRCLE_RADIUS} id={BodyPart[SIDES[right].shoulder]} /> + + [ [BodyPart.HEAD, BodyPart.NECK], - [side.shoulder, side.upperArm], + [side.shoulder], [side.bust], [side.upperArm], [side.lowerArm, side.hand], [BodyPart.HIP], [side.upperLeg, side.lowerLeg, side.foot], @@ -50,7 +50,7 @@ const LEFT_GROUPS = (side: BodySide): BodyPart[][] => [ const RIGHT_GROUPS = (side: BodySide): BodyPart[][] => [ [BodyPart.UPPER_CHEST, BodyPart.LOWER_CHEST], - [side.shoulder, side.upperArm], + [side.shoulder],[side.bust], [side.upperArm], [side.lowerArm, side.hand], [BodyPart.UPPER_WAIST, BodyPart.LOWER_WAIST], [side.upperLeg, side.lowerLeg, side.foot], diff --git a/gui/app/src/hooks/tracker-picker.ts b/gui/app/src/hooks/tracker-picker.ts index 5c427f1ca6..37204fbaf9 100644 --- a/gui/app/src/hooks/tracker-picker.ts +++ b/gui/app/src/hooks/tracker-picker.ts @@ -69,6 +69,10 @@ export const ALL_ASSIGNABLE_PARTS = [ BodyPart.NECK, BodyPart.LEFT_SHOULDER, BodyPart.RIGHT_SHOULDER, + + BodyPart.LEFT_BUST, + BodyPart.RIGHT_BUST, + BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND, BodyPart.LEFT_FOOT, diff --git a/gui/app/src/utils/skeletonParts.ts b/gui/app/src/utils/skeletonParts.ts index c724705888..55f2d75b7b 100644 --- a/gui/app/src/utils/skeletonParts.ts +++ b/gui/app/src/utils/skeletonParts.ts @@ -179,7 +179,6 @@ export interface BoneShapeConfig { rotation?: Quaternion; scale?: ShapeScale; } - export interface BonePartConfig { visible: boolean; shapes: BoneShapeConfig[]; @@ -257,6 +256,19 @@ export const SKELETON_PART_PRESETS: Record = { offset: inBoneLengths({ length: -0.2 }), }) ), + [BodyPart.LEFT_BUST]: part( + model('bust', { + scale: authoredSize({ width: 0.08, depth: 0.08, length: 0.08 }), + offset: inMetres({ width: 0.07, length: -0.12 }), + }) + ), + + [BodyPart.RIGHT_BUST]: part( + model('bust', { + scale: authoredSize({ width: 0.08, depth: 0.08, length: 0.08 }), + offset: inMetres({ width: -0.07, length: -0.12 }), + }) + ), [BodyPart.LOWER_CHEST]: part( model('chest', { scale: spanBone({ diff --git a/server/core/src/main/java/dev/slimevr/resets/bodypart-sets.kt b/server/core/src/main/java/dev/slimevr/resets/bodypart-sets.kt index f3af96b11d..d33dd092bf 100644 --- a/server/core/src/main/java/dev/slimevr/resets/bodypart-sets.kt +++ b/server/core/src/main/java/dev/slimevr/resets/bodypart-sets.kt @@ -8,11 +8,6 @@ object ResetBodyParts { BodyPart.RIGHT_UPPER_LEG, ) - val FEET = setOf( - BodyPart.LEFT_FOOT, - BodyPart.RIGHT_FOOT, - ) - val LEFT_TOES = setOf( BodyPart.LEFT_BIG_TOE, BodyPart.LEFT_INDEX_TOE, @@ -27,9 +22,18 @@ object ResetBodyParts { BodyPart.RIGHT_RING_TOE, BodyPart.RIGHT_LITTLE_TOE, ) + val BUST = setOf( + BodyPart.LEFT_BUST, + BodyPart.RIGHT_BUST + ) val TOES = LEFT_TOES + RIGHT_TOES + val FEET = setOf( + BodyPart.LEFT_FOOT, + BodyPart.RIGHT_FOOT, + ) + val LEFT_FINGERS = setOf( BodyPart.LEFT_THUMB_METACARPAL, BodyPart.LEFT_THUMB_PROXIMAL, diff --git a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-structure.kt b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-structure.kt index c6191d2436..8decc125cf 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-structure.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-structure.kt @@ -51,12 +51,19 @@ val BODY_PART_HIERARCHY_MAP: BodyPartMap> = BodyPartMap( BodyPart.RIGHT_LITTLE_PROXIMAL to arrayOf(BodyPart.RIGHT_LITTLE_INTERMEDIATE), BodyPart.RIGHT_LITTLE_INTERMEDIATE to arrayOf(BodyPart.RIGHT_LITTLE_DISTAL), - BodyPart.UPPER_CHEST to arrayOf(BodyPart.LOWER_CHEST), + BodyPart.UPPER_CHEST to arrayOf( + BodyPart.LEFT_BUST, + BodyPart.RIGHT_BUST, + BodyPart.LOWER_CHEST), + BodyPart.LOWER_CHEST to arrayOf(BodyPart.UPPER_WAIST), BodyPart.UPPER_WAIST to arrayOf(BodyPart.LOWER_WAIST), BodyPart.LOWER_WAIST to arrayOf(BodyPart.HIP), - BodyPart.HIP to arrayOf(BodyPart.LEFT_UPPER_LEG, BodyPart.RIGHT_UPPER_LEG), - + + BodyPart.HIP to arrayOf( + BodyPart.LEFT_UPPER_LEG, + BodyPart.RIGHT_UPPER_LEG, + ), BodyPart.LEFT_UPPER_LEG to arrayOf(BodyPart.LEFT_LOWER_LEG), BodyPart.LEFT_LOWER_LEG to arrayOf(BodyPart.LEFT_FOOT), diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bone-direct-link.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bone-direct-link.kt index 1554036e15..5973e0d35f 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bone-direct-link.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bone-direct-link.kt @@ -31,6 +31,21 @@ class BoneDirectLinkInputProcessor : SkeletonInputProcessor { BodyPart.LEFT_HAND to BodyPart.LEFT_LOWER_ARM, BodyPart.RIGHT_HAND to BodyPart.RIGHT_LOWER_ARM, + + BodyPart.LEFT_BIG_TOE to BodyPart.LEFT_FOOT, + BodyPart.LEFT_INDEX_TOE to BodyPart.LEFT_BIG_TOE, + BodyPart.LEFT_MIDDLE_TOE to BodyPart.LEFT_INDEX_TOE, + BodyPart.LEFT_RING_TOE to BodyPart.LEFT_MIDDLE_TOE, + BodyPart.LEFT_LITTLE_TOE to BodyPart.LEFT_RING_TOE, + + BodyPart.RIGHT_BIG_TOE to BodyPart.RIGHT_FOOT, + BodyPart.RIGHT_INDEX_TOE to BodyPart.RIGHT_BIG_TOE, + BodyPart.RIGHT_MIDDLE_TOE to BodyPart.RIGHT_INDEX_TOE, + BodyPart.RIGHT_RING_TOE to BodyPart.RIGHT_MIDDLE_TOE, + BodyPart.RIGHT_LITTLE_TOE to BodyPart.RIGHT_RING_TOE, + + BodyPart.LEFT_BUST to BodyPart.UPPER_CHEST, + BodyPart.RIGHT_BUST to BodyPart.UPPER_CHEST ) override fun process(mutableInputSkeleton: InputSkeleton, skeletonHeight: Float) { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bust-input-processor.kt b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bust-input-processor.kt new file mode 100644 index 0000000000..b8359f95ba --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/skeleton/inputprocessors/bust-input-processor.kt @@ -0,0 +1,206 @@ +package dev.slimevr.skeleton.inputprocessors + +import dev.slimevr.skeleton.InputSkeleton +import dev.slimevr.skeleton.SkeletonInputProcessor +import io.github.axisangles.ktmath.EulerAngles +import io.github.axisangles.ktmath.EulerOrder +import io.github.axisangles.ktmath.Quaternion +import solarxr_protocol.datatypes.BodyPart +import kotlin.math.abs + +/** + * Handles bust-specific rotation behavior: + * + * - Inverts the normal bust pitch. + * - Adds temporary pitch motion from vertical acceleration. + * - Uses a damped spring so acceleration-induced motion settles + * back to the actual tracked rotation. + */ +class BustInputProcessor : SkeletonInputProcessor { + + private data class BustMotionState( + var pitchOffset: Float = 0f, + var pitchVelocity: Float = 0f, + var verticalAcceleration: Float = 0f, + ) + + private val states = mutableMapOf( + BodyPart.LEFT_BUST to BustMotionState(), + BodyPart.RIGHT_BUST to BustMotionState(), + ) + + private var lastUpdateNanos = System.nanoTime() + + companion object { + /** + * How strongly vertical acceleration affects pitch. + */ + private const val ACCELERATION_SENSITIVITY = 0.6f + + /** + * How strongly the temporary pitch offset is pulled back toward zero. + * + * Higher = snaps back faster. + */ + private const val SPRING_STRENGTH = 20.0f + + /** + * Resistance to oscillation. + * + * Higher = less bouncing. + * Lower = more secondary motion / jiggle. + */ + private const val DAMPING = 6.0f + + /** + * Ignore tiny accelerometer fluctuations. + */ + private const val ACCELERATION_DEADZONE = 0.10f + + /** + * Maximum temporary pitch offset, in radians. + * + * ~15 degrees. + */ + private const val MAX_PITCH_OFFSET = 0.2617994f + + /** + * Avoid giant simulation steps after pauses/debugger stops. + */ + private const val MAX_DELTA_TIME = 0.05f + } + + /** + * Feed vertical/linear acceleration for a bust tracker into this processor. + * + * Positive/negative direction may need to be flipped depending on + * SlimeVR's acceleration coordinate system. + */ + fun setVerticalAcceleration( + bodyPart: BodyPart, + acceleration: Float, + ) { + val state = states[bodyPart] ?: return + state.verticalAcceleration = acceleration + } + + override fun process( + mutableInputSkeleton: InputSkeleton, + skeletonHeight: Float, + ) { + val now = System.nanoTime() + + var deltaTime = + (now - lastUpdateNanos).toFloat() / 1_000_000_000f + + lastUpdateNanos = now + deltaTime = deltaTime.coerceIn(0f, MAX_DELTA_TIME) + + val chest = + mutableInputSkeleton[BodyPart.UPPER_CHEST] + ?: return + + val chestRotation = chest.rotation + + for (bodyPart in arrayOf(BodyPart.LEFT_BUST, BodyPart.RIGHT_BUST)) { + val bone = mutableInputSkeleton[bodyPart] ?: continue + val state = states.getValue(bodyPart) + + updateMotion(state, deltaTime) + + // Convert absolute bust rotation into chest-local rotation. + val localRotation = + chestRotation.inv() * bone.rotation + + // Modify only the rotation relative to the chest. + val correctedLocalRotation = + applyBustRotation( + localRotation, + state.pitchOffset, + ) + + // Convert back to absolute/skeleton rotation. + val finalRotation = + chestRotation * correctedLocalRotation + + mutableInputSkeleton[bodyPart] = + bone.copy(rotation = finalRotation) + } + } + + /** + * Updates the temporary acceleration-driven pitch offset. + * + * Acceleration pushes the velocity, while a damped spring pulls + * the pitch back toward the actual tracker rotation. + */ + private fun updateMotion( + state: BustMotionState, + deltaTime: Float, + ) { + if (deltaTime <= 0f) { + return + } + + var acceleration = state.verticalAcceleration + + if (abs(acceleration) < ACCELERATION_DEADZONE) { + acceleration = 0f + } + + // Acceleration creates temporary pitch velocity. + // + // Flip this sign if upward acceleration produces motion + // in the wrong direction: + // + // state.pitchVelocity -= ... + state.pitchVelocity -= + acceleration * + ACCELERATION_SENSITIVITY * + deltaTime + + // Damped spring pulling the offset back toward zero. + val springAcceleration = + (SPRING_STRENGTH * state.pitchOffset) - + (DAMPING * state.pitchVelocity) + + state.pitchVelocity += springAcceleration * deltaTime + state.pitchOffset += state.pitchVelocity * deltaTime + + state.pitchOffset = + state.pitchOffset.coerceIn( + -MAX_PITCH_OFFSET, + MAX_PITCH_OFFSET, + ) + + // If we've basically settled, kill tiny residual motion. + if ( + abs(state.pitchOffset) < 0.0001f && + abs(state.pitchVelocity) < 0.0001f + ) { + state.pitchOffset = 0f + state.pitchVelocity = 0f + } + } + + /** + * Inverts the normal pitch and adds the temporary + * acceleration-induced pitch offset. + */ + private fun applyBustRotation( + rotation: Quaternion, + pitchOffset: Float, + ): Quaternion { + val euler = rotation.toEulerAngles(EulerOrder.XYZ) + + return EulerAngles( + EulerOrder.XYZ, + + // Inverted actual pitch + temporary inertial motion + -euler.x + pitchOffset, + + euler.y, + euler.z, + ).toQuaternion() + } +} 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 e90405e86a..8763c913b2 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -20,6 +20,7 @@ import dev.slimevr.skeleton.inputprocessors.ToeActiveLinkInputProcessor import dev.slimevr.skeleton.inputprocessors.UpperLegsRollAlignInputProcessor import dev.slimevr.skeleton.targetprocessors.FloorClipTargetProcessor import dev.slimevr.skeleton.targetprocessors.SkatingCorrectionTargetProcessor +import dev.slimevr.skeleton.inputprocessors.BustInputProcessor import dev.slimevr.util.PreciseWaiter import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 @@ -226,6 +227,7 @@ class Skeleton( RelaxedSpineInputProcessor(settings), HipYawRollAlignInputProcessor(settings), UpperLegsRollAlignInputProcessor(settings), + BustInputProcessor(), BoneDirectLinkInputProcessor(), FingerImputeInputProcessor(), ToeActiveLinkInputProcessor(), diff --git a/server/core/src/main/java/dev/slimevr/skeleton/proportions.kt b/server/core/src/main/java/dev/slimevr/skeleton/proportions.kt index f558ff6f1d..367c59ca50 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/proportions.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/proportions.kt @@ -131,6 +131,10 @@ fun toBoneOffsets(lengths: Map): BoneOffsets { tail.putAll(getToeOffsets(it)) head.putAll(getToeHeadOffsets(it)) } + lengths[SkeletonBone.UPPER_CHEST]?.let { + tail.putAll(getBustOffsets(it)) + head.putAll(getBustHeadOffsets(it)) + } return BoneOffsets(tail, head) } @@ -304,3 +308,40 @@ private fun getToeHeadOffsets(footLength: Float): Map = build put(toe.segments.second, Vector3(-k.x * footLength, k.y * footLength, k.z * footLength)) } } + + +private class Bust( + val segments: Pair, + val lengthFraction: Float, + val headOffset: Vector3, +) + +private val BUST = listOf( + Bust( + BodyPart.LEFT_BUST to BodyPart.RIGHT_BUST, + lengthFraction = 0.27f, + headOffset = Vector3(0.28f, 0f, 0f), + ), +) + + +/** + * Returns the offsets for the bust bones scaled from the chest. + */ +private fun getBustOffsets(bustLength: Float) = buildMap { + for (bust in BUST) { + val bustLength = bustLength * bust.lengthFraction + put(bust.segments.first, Vector3(0f, 0f, -bustLength * 0.2f)) + put(bust.segments.second, Vector3(0f, 0f, -bustLength * 0.2f)) + } +} + +// Head offsets spread the bust roots across the chest. X is the chest's medial-lateral axis in +// bust-local space, positive toward the left bust. Values are fractions of bustLength. +private fun getBustHeadOffsets(bustLength: Float): Map = buildMap { + for (bust in BUST) { + val k = bust.headOffset + put(bust.segments.first, Vector3(k.x * -bustLength * 0.12f, k.y * bustLength, k.z * bustLength)) + put(bust.segments.second, Vector3(-k.x * -bustLength * 0.12f, k.y * bustLength, k.z * bustLength)) + } +} diff --git a/server/core/src/main/java/dev/slimevr/vmc/mapping.kt b/server/core/src/main/java/dev/slimevr/vmc/mapping.kt index 7ae6040f7f..10f644c5b9 100644 --- a/server/core/src/main/java/dev/slimevr/vmc/mapping.kt +++ b/server/core/src/main/java/dev/slimevr/vmc/mapping.kt @@ -70,6 +70,8 @@ val BODY_PART_TO_UNITY_BONE: BodyPartMap> = BodyPartMap( BodyPart.RIGHT_MIDDLE_TOE to arrayOf("RightMiddleToe"), BodyPart.RIGHT_RING_TOE to arrayOf("RightRingToe"), BodyPart.RIGHT_LITTLE_TOE to arrayOf("RightLittleToe"), + BodyPart.LEFT_BUST to arrayOf("LeftBust"), + BodyPart.RIGHT_BUST to arrayOf("RightBust"), ), ) @@ -143,6 +145,10 @@ val VMC_HIERARCHY_MAP: BodyPartMap> = BodyPartMap( BodyPart.RIGHT_RING_TOE, BodyPart.RIGHT_LITTLE_TOE, ), + BodyPart.UPPER_CHEST to arrayOf( + BodyPart.LEFT_BUST, + BodyPart.RIGHT_BUST + ), ), ) @@ -207,6 +213,7 @@ val VMC_MIRROR_BONE_PAIRS: List> = listOf( BodyPart.LEFT_MIDDLE_TOE to BodyPart.RIGHT_MIDDLE_TOE, BodyPart.LEFT_RING_TOE to BodyPart.RIGHT_RING_TOE, BodyPart.LEFT_LITTLE_TOE to BodyPart.RIGHT_LITTLE_TOE, + BodyPart.LEFT_BUST to BodyPart.RIGHT_BUST, ) val VMC_MIRROR_BONES: BodyPartMap = BodyPartMap( diff --git a/server/core/src/main/java/dev/slimevr/vrcosc/bust-plugin-output-encoder.kt b/server/core/src/main/java/dev/slimevr/vrcosc/bust-plugin-output-encoder.kt new file mode 100644 index 0000000000..fd91a76cd3 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/vrcosc/bust-plugin-output-encoder.kt @@ -0,0 +1,326 @@ +package dev.slimevr.vrcosc + +import dev.slimevr.skeleton.BoneState +import io.github.axisangles.ktmath.EulerOrder +import solarxr_protocol.datatypes.BodyPart +import dev.slimevr.util.Side +import dev.slimevr.osc.OscArg +import dev.slimevr.osc.OscContent +import dev.slimevr.osc.OscMessage +import kotlin.math.abs +import kotlin.math.exp +private val bustOutputState = BustOutputState() + +internal fun buildBustMessages( + bones: Map, +): List = + bustOutputState.buildMessages(bones) + +private class BustOutputState { + private var verticalBaselineY = 0f + private var verticalPosition = 0f + private var verticalVelocity = 0f + private var lastUpdateNanos = 0L + private var baselineInitialized = false + + fun buildMessages( + bones: Map, + ): List { + val messages = mutableListOf() + + val chest = bones[BodyPart.UPPER_CHEST] + val leftBust = bones[BodyPart.LEFT_BUST] + val rightBust = bones[BodyPart.RIGHT_BUST] + + if (chest != null) { + if (leftBust != null) { + processBust( + messages, + chest, + leftBust, + Side.LEFT, + ) + } + + if (rightBust != null) { + processBust( + messages, + chest, + rightBust, + Side.RIGHT, + ) + } + } + + val verticalAccelerationY = + getSharedVerticalAccelerationY( + leftBust, + rightBust, + ) + + val vertical = + updateVerticalResponse( + verticalAccelerationY, + getDeltaTimeSeconds(), + ) + + addFloat( + messages, + "BustVertical", + vertical, + ) + + return messages + } + + private fun processBust( + messages: MutableList, + chest: BoneState, + bust: BoneState, + side: Side, + ) { + val currentRelative = + chest.rotation.inv() * + bust.rotation + + val euler = + currentRelative.toEulerAngles( + EulerOrder.XYZ, + ) + + val pitch = + Math.toDegrees( + euler.x.toDouble(), + ).toFloat() + + val yaw = + Math.toDegrees( + euler.z.toDouble(), + ).toFloat() + + addFloat( + messages, + "${side.oscName}BustPitch", + (pitch / 90f) + .coerceIn(-1f, 1f), + ) + + addFloat( + messages, + "${side.oscName}BustYaw", + (yaw / 90f) + .coerceIn(-1f, 1f), + ) + } + + private fun getSharedVerticalAccelerationY( + leftBust: BoneState?, + rightBust: BoneState?, + ): Float? = + when { + leftBust != null && + rightBust != null -> + ( + leftBust.acceleration.y + + rightBust.acceleration.y + ) * 0.5f + + leftBust != null -> + leftBust.acceleration.y + + rightBust != null -> + rightBust.acceleration.y + + else -> + null + } + + private fun updateVerticalResponse( + accelerationY: Float?, + dt: Float, + ): Float { + if (accelerationY != null) { + if (!baselineInitialized) { + verticalBaselineY = + accelerationY + + baselineInitialized = + true + } + + val baselineAlpha = + 1f - + exp( + -dt / + VERTICAL_BASELINE_TIME_CONSTANT, + ) + + verticalBaselineY += + (accelerationY - + verticalBaselineY) * + baselineAlpha + + var dynamicAcceleration = + accelerationY - + verticalBaselineY + + if ( + abs(dynamicAcceleration) < + VERTICAL_ACCEL_DEADZONE + ) { + dynamicAcceleration = 0f + } + + val inertialInput = + -dynamicAcceleration * + VERTICAL_ACCEL_GAIN + + stepSpring( + inertialInput, + dt, + ) + } else { + stepSpring( + 0f, + dt, + ) + } + + if ( + abs(verticalPosition) < + SNAP_POSITION_EPSILON && + abs(verticalVelocity) < + SNAP_VELOCITY_EPSILON + ) { + verticalPosition = 0f + verticalVelocity = 0f + } + + return verticalPosition + .coerceIn(-1f, 1f) + } + + private fun stepSpring( + input: Float, + dt: Float, + ) { + val restoring = + -VERTICAL_SPRING * + verticalPosition + + val damping = + -VERTICAL_DAMPING * + verticalVelocity + + verticalVelocity += + ( + input + + restoring + + damping + ) * dt + + verticalPosition += + verticalVelocity * dt + + if (verticalPosition > 1f) { + verticalPosition = 1f + + if (verticalVelocity > 0f) { + verticalVelocity = 0f + } + } else if (verticalPosition < -1f) { + verticalPosition = -1f + + if (verticalVelocity < 0f) { + verticalVelocity = 0f + } + } + } + + private fun getDeltaTimeSeconds(): Float { + val now = + System.nanoTime() + + if (lastUpdateNanos == 0L) { + lastUpdateNanos = now + return DEFAULT_FRAME_DT + } + + val dt = + ( + (now - lastUpdateNanos) + .toDouble() / + 1_000_000_000.0 + ) + .toFloat() + .coerceIn( + MIN_FRAME_DT, + MAX_FRAME_DT, + ) + + lastUpdateNanos = now + + return dt + } + + companion object { + private const val VERTICAL_ACCEL_DEADZONE = + 0.025f + + private const val VERTICAL_ACCEL_GAIN = + 18f + + private const val VERTICAL_SPRING = + 34f + + private const val VERTICAL_DAMPING = + 9f + + private const val VERTICAL_BASELINE_TIME_CONSTANT = + 2.5f + + private const val DEFAULT_FRAME_DT = + 1f / 60f + + private const val MIN_FRAME_DT = + 1f / 240f + + private const val MAX_FRAME_DT = + 0.05f + + private const val SNAP_POSITION_EPSILON = + 0.0025f + + private const val SNAP_VELOCITY_EPSILON = + 0.01f + } +} + +private fun addFloat( + messages: MutableList, + parameterName: String, + value: Float, +) { + messages.add( + OscContent.Message( + OscMessage( + "/avatar/parameters/$parameterName", + listOf( + OscArg.Float( + value.coerceIn( + -1f, + 1f, + ), + ), + ), + ), + ), + ) +} + +private val Side.oscName: String + get() = + when (this) { + Side.LEFT -> "Left" + Side.RIGHT -> "Right" + } diff --git a/server/core/src/main/java/dev/slimevr/vrcosc/output-encoder.kt b/server/core/src/main/java/dev/slimevr/vrcosc/output-encoder.kt index baa5ac49f0..c2a2f85395 100644 --- a/server/core/src/main/java/dev/slimevr/vrcosc/output-encoder.kt +++ b/server/core/src/main/java/dev/slimevr/vrcosc/output-encoder.kt @@ -54,8 +54,9 @@ internal fun buildOutgoingBundle( ), ) } - + addAll(buildToeMessages(bones)) + addAll(buildBustMessages(bones)) } return messages.takeIf { it.isNotEmpty() }?.let { OscBundle(1L, it) } diff --git a/server/core/src/main/java/dev/slimevr/vrcosc/toe-plugin-output-encoder.kt b/server/core/src/main/java/dev/slimevr/vrcosc/toe-plugin-output-encoder.kt index bae5807303..41b1c290d7 100644 --- a/server/core/src/main/java/dev/slimevr/vrcosc/toe-plugin-output-encoder.kt +++ b/server/core/src/main/java/dev/slimevr/vrcosc/toe-plugin-output-encoder.kt @@ -92,7 +92,7 @@ private fun processToe( val euler = currentRelative.toEulerAngles(EulerOrder.XYZ) val pitch = Math.toDegrees(euler.x.toDouble()).toFloat() - val yaw = Math.toDegrees(euler.z.toDouble()).toFloat() + val yaw = Math.toDegrees(euler.y.toDouble()).toFloat() val tipToe = pitch < MINIMUM_TIP_TOE_PITCH val bending = pitch > MINIMUM_BENDING_PITCH val splayed = when (splayDirection) { diff --git a/server/core/src/test/java/dev/slimevr/skeleton/BustDirectLinkInputProcessorTest.kt b/server/core/src/test/java/dev/slimevr/skeleton/BustDirectLinkInputProcessorTest.kt new file mode 100644 index 0000000000..a663bf45e9 --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/skeleton/BustDirectLinkInputProcessorTest.kt @@ -0,0 +1,36 @@ +package dev.slimevr.skeleton + +import dev.slimevr.skeleton.inputprocessors.BustInputProcessor +import io.github.axisangles.ktmath.Quaternion +import org.junit.jupiter.api.Test +import solarxr_protocol.datatypes.BodyPart +import kotlin.test.assertTrue + +class BustDirectLinkInputProcessorTest { + + @Test + fun `test all missing bust trackers`() { + val processor = BustInputProcessor() + + val inputs = DEFAULT_SKELETON_STATE.boneInputs.mutateCopy { map -> + map[BodyPart.UPPER_CHEST] = + map.getValue(BodyPart.UPPER_CHEST).copy( + rotation = Quaternion.fromRotationVector(10f, 40f, 15f), + isRotationActive = true, + ) + } + + processor.process(inputs, 1.7f) + + val leftBustIsSameRotationAsChest = + inputs[BodyPart.LEFT_BUST]?.rotation == + inputs[BodyPart.UPPER_CHEST]?.rotation + + val rightBustIsSameRotationAsChest = + inputs[BodyPart.RIGHT_BUST]?.rotation == + inputs[BodyPart.UPPER_CHEST]?.rotation + + assertTrue(leftBustIsSameRotationAsChest) + assertTrue(rightBustIsSameRotationAsChest) + } +}