Unit test toe snap + fix leg tweaks & default proportions (#1350)

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<Tracker>?) : 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<SkeletonConfigOffsets, Float>?,
 	) : 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<SkeletonConfigOffsets, Float>?,
 	) : 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,
 				),
 			)
 

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(

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,

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())
+		}
+	}
+}