From c742e7226d9e3fa1d2374be9bea2f012ca832bfe Mon Sep 17 00:00:00 2001 From: Erimel Date: Thu, 27 Aug 2026 18:05:20 -0400 Subject: [PATCH] Skeleton floor level --- .../dev/slimevr/android/ForegroundService.kt | 2 +- .../main/java/dev/slimevr/resets/module.kt | 13 +++++++-- .../java/dev/slimevr/skeleton/behaviours.kt | 2 +- .../main/java/dev/slimevr/skeleton/module.kt | 12 ++++++-- .../main/java/dev/slimevr/skeleton/reducer.kt | 13 +++++---- .../slimevr/skeleton/skating-correction.kt | 29 ++++++++++--------- .../src/main/java/dev/slimevr/desktop/Main.kt | 2 +- 7 files changed, 46 insertions(+), 27 deletions(-) diff --git a/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt b/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt index fcf462d5f..91d6b9efb 100644 --- a/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt +++ b/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt @@ -168,7 +168,7 @@ class ForegroundService : Service() { discoverServicesFlow = ::discoverServices, serviceFactory = ::createNetService, ) - val resetsManager = ResetsManager.create(ctx = phase1, scope = scope) + val resetsManager = ResetsManager.create(ctx = phase1, skeleton = skeleton, scope = scope) val tapDetectionManager = TapDetectionManager.create(ctx = phase1, resetsManager = resetsManager, scope = scope) val keybindManager = KeybindManager.create(scope = scope) val serverInfos = ServerInfos(::resolveAndroidLocalIpAddress) diff --git a/server/core/src/main/java/dev/slimevr/resets/module.kt b/server/core/src/main/java/dev/slimevr/resets/module.kt index 883ca2afc..3305d895a 100644 --- a/server/core/src/main/java/dev/slimevr/resets/module.kt +++ b/server/core/src/main/java/dev/slimevr/resets/module.kt @@ -9,6 +9,8 @@ import dev.slimevr.config.SettingsActions import dev.slimevr.context.Behaviour import dev.slimevr.context.Context import dev.slimevr.logging.AppLogger +import dev.slimevr.skeleton.Skeleton +import dev.slimevr.skeleton.SkeletonActions import dev.slimevr.tracker.TrackerActions import dev.slimevr.tracker.getFirstActiveFor import io.github.axisangles.ktmath.Quaternion @@ -48,7 +50,7 @@ sealed interface ResetsActions { typealias ResetsContext = Context typealias ResetsBehaviour = Behaviour -class ResetsManager(val context: ResetsContext, val server: VRServer, val settings: Settings) { +class ResetsManager(val context: ResetsContext, val server: VRServer, val settings: Settings, val skeleton: Skeleton) { fun startObserving() = context.observeAll(this) private var resetJob: Job = Job() @@ -154,6 +156,11 @@ class ResetsManager(val context: ResetsContext, val server: VRServer, val settin }, ) } + + if (resetType == ResetType.FULL) { + // Tell the skeleton to set the floor level + skeleton.context.dispatch(SkeletonActions.SetFloorLevel) + } } private fun getYawOffset(bodyPart: BodyPart?, armsResetMode: ArmsResetMode) = when (bodyPart) { @@ -179,7 +186,7 @@ class ResetsManager(val context: ResetsContext, val server: VRServer, val settin } companion object { - fun create(ctx: Phase1ContextProvider, scope: CoroutineScope): ResetsManager { + fun create(ctx: Phase1ContextProvider, skeleton: Skeleton, scope: CoroutineScope): ResetsManager { val context = Context.create( initialState = ResetsState( canDoYawReset = false, @@ -191,7 +198,7 @@ class ResetsManager(val context: ResetsContext, val server: VRServer, val settin behaviours = listOf(ResetsMountingTimeoutBehaviour()), name = "ResetsManager", ) - return ResetsManager(context, ctx.server, ctx.config.settings) + return ResetsManager(context, ctx.server, ctx.config.settings, skeleton) } } } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt index 679179aaa..55dc5077f 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt @@ -127,7 +127,7 @@ class ComputedSkeletonBehaviour( val fk = buildBones(boneInputs, rootHead) // Run IK processors - val ikTargets = targetProcessors.fold(bodyPartMap()) { targets, processor -> processor.process(fk, targets) } + val ikTargets = targetProcessors.fold(bodyPartMap()) { targets, processor -> processor.process(fk, targets, targetState.floorLevel) } // Run IK val ikOutput = ccdIk( 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 bf01482d2..0eeb2451e 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -68,7 +68,13 @@ fun ComputedSkeleton.resolveAverageRotationFor(bodyParts: Array): Quat } ?: Quaternion.IDENTITY } -data class SkeletonState(val boneInputs: InputSkeleton, val skeletonHeight: Float, val paused: Boolean, val pausedBoneInputs: InputSkeleton?) +data class SkeletonState( + val boneInputs: InputSkeleton, + val skeletonHeight: Float, + val floorLevel: Float, + val paused: Boolean, + val pausedBoneInputs: InputSkeleton?, +) val DEFAULT_SKELETON_STATE: SkeletonState = SkeletonState( boneInputs = DEFAULT_PROPORTIONS.toBoneOffsets().mapValues { bodyPart, tailOffset -> @@ -81,6 +87,7 @@ val DEFAULT_SKELETON_STATE: SkeletonState = SkeletonState( ) }, skeletonHeight = DEFAULT_HEIGHT, + floorLevel = 0f, paused = false, pausedBoneInputs = null, ) @@ -118,6 +125,7 @@ sealed interface SkeletonActions { data class SetProportions(val lengths: Map) : SkeletonActions data class PauseTracking(val pause: Boolean) : SkeletonActions data class SetPausedBoneInputs(val pausedBoneInputs: InputSkeleton) : SkeletonActions + data object SetFloorLevel : SkeletonActions } typealias SkeletonContext = Context @@ -127,7 +135,7 @@ interface SkeletonProcessor { } typealias IKTargets = BodyPartMap interface SkeletonTargetProcessor { - fun process(fk: ComputedSkeleton, ikTargets: IKTargets): IKTargets + fun process(fk: ComputedSkeleton, ikTargets: IKTargets, floorLevel: Float): IKTargets } class Skeleton( diff --git a/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt b/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt index 89e6c961d..23af3d32c 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/reducer.kt @@ -2,6 +2,7 @@ package dev.slimevr.skeleton import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 +import solarxr_protocol.datatypes.BodyPart fun reduce(state: SkeletonState, action: SkeletonActions): SkeletonState = when (action) { is SkeletonActions.SetBoneRotation -> { @@ -27,11 +28,13 @@ fun reduce(state: SkeletonState, action: SkeletonActions): SkeletonState = when state.copy(boneInputs = newBones, skeletonHeight = action.lengths.height()) } - is SkeletonActions.PauseTracking -> { - state.copy(paused = action.pause, pausedBoneInputs = null) - } + is SkeletonActions.PauseTracking -> state.copy(paused = action.pause, pausedBoneInputs = null) - is SkeletonActions.SetPausedBoneInputs -> { - state.copy(pausedBoneInputs = action.pausedBoneInputs) + is SkeletonActions.SetPausedBoneInputs -> state.copy(pausedBoneInputs = action.pausedBoneInputs) + + is SkeletonActions.SetFloorLevel -> { + val leftFootHeight = state.boneInputs[BodyPart.LEFT_FOOT]?.rawPosition?.y ?: 0f + val rightFootHeight = state.boneInputs[BodyPart.RIGHT_FOOT]?.rawPosition?.y ?: 0f + state.copy(floorLevel = (leftFootHeight + rightFootHeight) / 2f) } } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/skating-correction.kt b/server/core/src/main/java/dev/slimevr/skeleton/skating-correction.kt index d34bedb10..00b14deef 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/skating-correction.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/skating-correction.kt @@ -33,18 +33,17 @@ data class LockState( val VELOCITY_BODY_PARTS = arrayOf(BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT) -val FOOT_VELOCITY_SENSITIVITY = 1f -val FOOT_ACCELERATION_SENSITIVITY = 1f +const val FOOT_VELOCITY_SENSITIVITY = 1f +const val FOOT_ACCELERATION_SENSITIVITY = 1f -val SKATING_LOCK_ENGAGE_PERCENT = 1.1f -val SKATING_DISTANCE_THRESHOLD = 0.5f -val SKATING_VELOCITY_THRESHOLD = 2.4f -val SKATING_ROTATIONAL_VELOCITY_THRESHOLD = 4.5f -val SKATING_ACCELERATION_THRESHOLD = 0.7f +const val SKATING_LOCK_ENGAGE_PERCENT = 1.1f +const val SKATING_DISTANCE_THRESHOLD = 0.5f +const val SKATING_VELOCITY_THRESHOLD = 2.4f +const val SKATING_ROTATIONAL_VELOCITY_THRESHOLD = 4.5f +const val SKATING_ACCELERATION_THRESHOLD = 0.7f -// TODO Floor level needs to be calibrated in some way; originally done on full reset -val FLOOR_LEVEL = 0f -val FLOOR_DISTANCE_THRESHOLD = 0.065f +const val FLOOR_CALIBRATION_OFFSET = 0.0025f // TODO: This was added to floorLevel. Do we want to do this? +const val FLOOR_DISTANCE_THRESHOLD = 0.065f // TODO Where are these numbers from? val BODY_PART_MASSES = mapOf( @@ -66,10 +65,11 @@ val BODY_PART_MASSES = mapOf( fun shouldLock( velocity: VelocityState, thresholdMultiplier: Float, + floorLevel: Float, ): Boolean = (velocity.horizontalDistance <= SKATING_DISTANCE_THRESHOLD * thresholdMultiplier) && (velocity.velocity.len() <= SKATING_VELOCITY_THRESHOLD * thresholdMultiplier) && (velocity.angularVelocityMagnitude <= SKATING_ROTATIONAL_VELOCITY_THRESHOLD * thresholdMultiplier) && - (velocity.position.y - FLOOR_LEVEL <= FLOOR_DISTANCE_THRESHOLD * thresholdMultiplier) && + (velocity.position.y - floorLevel <= FLOOR_DISTANCE_THRESHOLD * thresholdMultiplier) && (velocity.acceleration.len() <= SKATING_ACCELERATION_THRESHOLD * thresholdMultiplier) // TODO Use this to calculate feet pressure @@ -156,7 +156,7 @@ fun computeLockState( // Probably not a SkeletonProcessor, maybe computed processor or smth class SkatingCorrectionProcessor : SkeletonTargetProcessor { - // Center of mass + // Centre of mass var comState: COMState? = null // Do we need to store this or do we just want velocity? We can probably just pull @@ -164,7 +164,7 @@ class SkatingCorrectionProcessor : SkeletonTargetProcessor { val velocity: BodyPartMap = bodyPartMap() val lockState: BodyPartMap = bodyPartMap() - override fun process(fk: ComputedSkeleton, ikTargets: IKTargets): IKTargets { + override fun process(fk: ComputedSkeleton, ikTargets: IKTargets, floorLevel: Float): IKTargets { val curTime = timeSource.markNow() // Update center of mass @@ -191,6 +191,7 @@ class SkatingCorrectionProcessor : SkeletonTargetProcessor { val isLocked = shouldLock( newVel, if (wasLocked) SKATING_LOCK_ENGAGE_PERCENT else 1f, + floorLevel, ) val activeState = computeLockState( @@ -214,7 +215,7 @@ class SkatingCorrectionProcessor : SkeletonTargetProcessor { class FloorClipProcessor( val bodyParts: Array = arrayOf(BodyPart.LEFT_LOWER_LEG, BodyPart.RIGHT_LOWER_LEG), ) : SkeletonTargetProcessor { - override fun process(fk: ComputedSkeleton, ikTargets: IKTargets): IKTargets { + override fun process(fk: ComputedSkeleton, ikTargets: IKTargets, floorLevel: Float): IKTargets { for (bodyPart in bodyParts) { // Get existing target or make a new one at the current bone position val target = ikTargets[bodyPart] ?: fk[bodyPart]?.tailPosition ?: continue diff --git a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt index a6c8bf276..6d20a29c8 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt +++ b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt @@ -145,7 +145,7 @@ fun main(args: Array) = runBlocking(appCoroutineExceptionHandler + discoverServicesFlow = ::discoverServices, serviceFactory = ::createNetService, ) - val resetsManager = ResetsManager.create(ctx = phase1, scope = this) + val resetsManager = ResetsManager.create(ctx = phase1, skeleton = skeleton, scope = this) val tapDetectionManager = TapDetectionManager.create(ctx = phase1, resetsManager = resetsManager, scope = this) val keybindManager = KeybindManager.create(scope = this) val serverInfos = ServerInfos(::resolveDesktopLocalIpAddress)