From a0cedde4b75c3ad2b1f3d51c32a41cd4e2a43f2a Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Sun, 16 Mar 2025 08:07:07 -0400 Subject: [PATCH] Unit test toe snap + fix leg tweaks & default proportions (#1350) --- .../tracking/processor/HumanPoseManager.kt | 116 +++++------------- .../tracking/processor/skeleton/LegTweaks.kt | 2 +- .../dev/slimevr/tracking/trackers/Tracker.kt | 8 ++ .../java/dev/slimevr/unit/LegTweaksTests.kt | 72 +++++++++++ 4 files changed, 113 insertions(+), 85 deletions(-) create mode 100644 server/core/src/test/java/dev/slimevr/unit/LegTweaksTests.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt index 85a6dc2c5..976032aad 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt @@ -26,7 +26,6 @@ import solarxr_protocol.datatypes.DeviceIdT import solarxr_protocol.datatypes.TrackerIdT import solarxr_protocol.rpc.StatusData import solarxr_protocol.rpc.StatusDataUnion -import solarxr_protocol.rpc.StatusUnassignedHMD import solarxr_protocol.rpc.StatusUnassignedHMDT import java.util.function.Consumer import kotlin.math.* @@ -55,7 +54,7 @@ class HumanPoseManager(val server: VRServer?) { skeleton = HumanSkeleton(this, server) // This computes all node offsets, so the defaults don't need to be // explicitly loaded into the skeleton (no need for - // `updateNodeOffsetsInSkeleton()`) + // `computeAllNodeOffsets()`) loadFromConfig(server.configManager) for (sc in onSkeletonUpdated) sc.accept(skeleton) } @@ -70,7 +69,7 @@ class HumanPoseManager(val server: VRServer?) { constructor(trackers: List?) : this(server = null) { skeleton = HumanSkeleton(this, trackers) // Set default node offsets on the new skeleton - skeletonConfigManager.updateNodeOffsetsInSkeleton() + skeletonConfigManager.computeAllNodeOffsets() skeletonConfigManager.updateSettingsInSkeleton() } @@ -87,9 +86,9 @@ class HumanPoseManager(val server: VRServer?) { offsetConfigs: Map?, ) : this(server = null) { skeleton = HumanSkeleton(this, trackers) - // Set default node offsets on the new skeleton - skeletonConfigManager.updateNodeOffsetsInSkeleton() // Set offsetConfigs from given offsetConfigs on creation + // This computes all node offsets, so the defaults don't need to be + // explicitly loaded into the skeleton (no need for `computeAllNodeOffsets()`) skeletonConfigManager.setOffsets(offsetConfigs) skeletonConfigManager.updateSettingsInSkeleton() } @@ -110,174 +109,123 @@ class HumanPoseManager(val server: VRServer?) { altOffsetConfigs: Map?, ) : this(server = null) { skeleton = HumanSkeleton(this, trackers) - // Set default node offsets on the new skeleton - skeletonConfigManager.updateNodeOffsetsInSkeleton() // Set offsetConfigs from given offsetConfigs on creation if (altOffsetConfigs != null) { // Set alts first, so if there's any overlap it doesn't affect // the values skeletonConfigManager.setOffsets(altOffsetConfigs) } + // This computes all node offsets, so the defaults don't need to be + // explicitly loaded into the skeleton (no need for `computeAllNodeOffsets()`) skeletonConfigManager.setOffsets(offsetConfigs) skeletonConfigManager.updateSettingsInSkeleton() } // #endregion // #region private methods + private fun makeComputedTracker(name: String, display: String, pos: TrackerPosition): Tracker = Tracker( + null, + getNextLocalTrackerId(), + name, + display, + pos, + hasPosition = true, + hasRotation = true, + isInternal = true, + isComputed = true, + allowFiltering = false, + // Do not track polarity, moving avg de-syncs ticks and breaks leg tweaks + trackRotDirection = false, + ) + private fun initializeComputedHumanPoseTracker() { computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://HEAD", "Computed head", TrackerPosition.HEAD, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://CHEST", "Computed chest", TrackerPosition.UPPER_CHEST, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://WAIST", "Computed hip", TrackerPosition.HIP, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://LEFT_KNEE", "Computed left knee", TrackerPosition.LEFT_UPPER_LEG, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://RIGHT_KNEE", "Computed right knee", TrackerPosition.RIGHT_UPPER_LEG, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://LEFT_FOOT", "Computed left foot", TrackerPosition.LEFT_FOOT, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://RIGHT_FOOT", "Computed right foot", TrackerPosition.RIGHT_FOOT, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://LEFT_ELBOW", "Computed left elbow", TrackerPosition.LEFT_UPPER_ARM, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://RIGHT_ELBOW", "Computed right elbow", TrackerPosition.RIGHT_UPPER_ARM, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://LEFT_HAND", "Computed left hand", TrackerPosition.LEFT_HAND, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) computedTrackers .add( - Tracker( - null, - getNextLocalTrackerId(), + makeComputedTracker( "human://RIGHT_HAND", "Computed right hand", TrackerPosition.RIGHT_HAND, - hasPosition = true, - hasRotation = true, - isInternal = true, - isComputed = true, ), ) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.kt index 7f81db3f6..46a9f9cd5 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.kt @@ -788,7 +788,7 @@ class LegTweaks(private val skeleton: HumanSkeleton) { val angle = FastMath.clamp(footPos.y - floorLevel, 0.0f, footLength) return if (angle > footLength * MAXIMUM_TOE_DOWN_ANGLE) { asin( - footLength * MAXIMUM_TOE_DOWN_ANGLE / footLength, + MAXIMUM_TOE_DOWN_ANGLE, ) } else { asin( 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 da2f7af4e..fda409ec6 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 @@ -63,6 +63,11 @@ class Tracker @JvmOverloads constructor( * Automatically set the status to DISCONNECTED */ val usesTimeout: Boolean = false, + /** + * If true, smoothing and prediction may be enabled. If either are enabled, then + * rotations will be updated with [tick]. This will not have any effect if + * [trackRotDirection] is set to false. + */ val allowFiltering: Boolean = false, val needsReset: Boolean = false, val needsMounting: Boolean = false, @@ -71,6 +76,9 @@ class Tracker @JvmOverloads constructor( * Whether to track the direction of the tracker's rotation * (positive vs negative rotation). This needs to be disabled for AutoBone and * unit tests, where the rotation is absolute and not temporal. + * + * If true, the output rotation will only be updated after [dataTick]. If false, the + * output rotation will be updated immediately with the raw rotation. */ val trackRotDirection: Boolean = true, magStatus: MagnetometerStatus = MagnetometerStatus.NOT_SUPPORTED, diff --git a/server/core/src/test/java/dev/slimevr/unit/LegTweaksTests.kt b/server/core/src/test/java/dev/slimevr/unit/LegTweaksTests.kt new file mode 100644 index 000000000..4676adc5f --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/unit/LegTweaksTests.kt @@ -0,0 +1,72 @@ +package dev.slimevr.unit + +import dev.slimevr.tracking.processor.HumanPoseManager +import dev.slimevr.tracking.processor.config.SkeletonConfigToggles +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.trackers.TrackerRole +import dev.slimevr.tracking.trackers.TrackerStatus +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.QuaternionTest +import io.github.axisangles.ktmath.Vector3 +import org.junit.jupiter.api.Test +import kotlin.test.assertFails + +class LegTweaksTests { + + @Test + fun toeSnap() { + val hmd = Tracker( + null, + 0, + "test:headset", + "Headset", + TrackerPosition.HEAD, + hasPosition = true, + hasRotation = true, + isComputed = true, + imuType = null, + needsReset = false, + needsMounting = false, + isHmd = true, + trackRotDirection = false, + ) + hmd.status = TrackerStatus.OK + + val hpm = HumanPoseManager(listOf(hmd)) + val height = hpm.userHeightFromConfig + val lFoot = hpm.getComputedTracker(TrackerRole.LEFT_FOOT) + + assert(height > 0f) { + "Skeleton was not populated with default proportions (height = $height)" + } + val lFootLen = hpm.skeleton.leftFootBone.length + assert(lFootLen > 0f) { + "Skeleton's left foot has no length (length = $lFootLen)" + } + + // Skeleton setup + hpm.skeleton.hasKneeTrackers = true + hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, true) + + // Set the floor height + hmd.position = Vector3(0f, height, 0f) + hpm.update() + + // Validate initial state + QuaternionTest.assertEquals(Quaternion.IDENTITY, lFoot.getRotation()) + + // Ensure `leftToeTouched` and `rightToeTouched` are true + hmd.position = Vector3(0f, height - 0.02f, 0f) + hpm.update() + + // Lift skeleton within toe snap range + hmd.position = Vector3(0f, height + 0.02f, 0f) + hpm.update() + + // This should fail now that the toes are snapped + assertFails { + QuaternionTest.assertEquals(Quaternion.IDENTITY, lFoot.getRotation()) + } + } +}