Add skeleton resets unit tests

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -111,7 +111,7 @@ class Tracker @JvmOverloads constructor(
 			}
 			alreadyInitialized = true
 		}
-		if (!isInternal) {
+		if (!isInternal && VRServer.instanceInitialized) {
 			// If the status of a non-internal tracker has changed, inform
 			// the VRServer to recreate the skeleton, as it may need to
 			// assign or un-assign the tracker to a body part

server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt

@@ -4,14 +4,15 @@ import com.jme3.math.FastMath
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.udp.IMUType
+import dev.slimevr.unit.TrackerUtils.assertAnglesApproxEqual
+import dev.slimevr.unit.TrackerUtils.deg
+import dev.slimevr.unit.TrackerUtils.yaw
 import io.github.axisangles.ktmath.EulerAngles
 import io.github.axisangles.ktmath.EulerOrder
 import io.github.axisangles.ktmath.Quaternion
-import org.junit.jupiter.api.AssertionFailureBuilder
 import org.junit.jupiter.api.DynamicTest
 import org.junit.jupiter.api.Test
 import org.junit.jupiter.api.TestFactory
-import kotlin.math.*
 
 /**
  * Tests [TrackerResetsHandler.resetMounting]
@@ -22,8 +23,8 @@ import kotlin.math.*
 class MountingResetTests {
 
 	@TestFactory
-	fun testResetAndMounting(): List<DynamicTest> = directions.flatMap { e ->
-		directions.map { m ->
+	fun testResetAndMounting(): List<DynamicTest> = TrackerUtils.directions.flatMap { e ->
+		TrackerUtils.directions.map { m ->
 			DynamicTest.dynamicTest(
 				"Full and Mounting Reset Test of Tracker (Expected: ${deg(e)}, reference: ${deg(m)})",
 			) {
@@ -34,7 +35,7 @@ class MountingResetTests {
 
 	private fun checkResetMounting(expected: Quaternion, reference: Quaternion) {
 		// Compute the pitch/roll for the expected mounting
-		val trackerRot = (expected * (frontRot / expected))
+		val trackerRot = (expected * (TrackerUtils.frontRot / expected))
 
 		val tracker = Tracker(
 			null,
@@ -119,7 +120,7 @@ class MountingResetTests {
 		val expected = Quaternion.SLIMEVR.RIGHT
 		val reference = EulerAngles(EulerOrder.YZX, FastMath.PI / 8f, FastMath.HALF_PI, 0f).toQuaternion()
 		// Compute the pitch/roll for the expected mounting
-		val trackerRot = (expected * (frontRot / expected))
+		val trackerRot = (expected * (TrackerUtils.frontRot / expected))
 
 		val tracker = Tracker(
 			null,
@@ -159,51 +160,4 @@ class MountingResetTests {
 			"Resulting rotation after yaw reset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
 		)
 	}
-
-	/**
-	 * Makes a radian angle positive
-	 */
-	private fun posRad(rot: Float): Float {
-		// Reduce the rotation to the smallest form
-		val redRot = rot % FastMath.TWO_PI
-		return abs(if (rot < 0f) FastMath.TWO_PI + redRot else redRot)
-	}
-
-	/**
-	 * Gets the yaw of a rotation in radians
-	 */
-	private fun yaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
-
-	/**
-	 * Converts radians to degrees
-	 */
-	private fun deg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
-
-	private fun deg(rot: Quaternion): Float = deg(yaw(rot))
-
-	private fun anglesApproxEqual(a: Float, b: Float): Boolean = FastMath.isApproxEqual(a, b) ||
-		FastMath.isApproxEqual(a - FastMath.TWO_PI, b) ||
-		FastMath.isApproxEqual(a, b - FastMath.TWO_PI)
-
-	private fun assertAnglesApproxEqual(expected: Float, actual: Float, message: String?) {
-		if (!anglesApproxEqual(expected, actual)) {
-			AssertionFailureBuilder.assertionFailure().message(message)
-				.expected(expected).actual(actual).buildAndThrow()
-		}
-	}
-
-	companion object {
-		val directions = arrayOf(
-			Quaternion.SLIMEVR.FRONT,
-			Quaternion.SLIMEVR.FRONT_LEFT,
-			Quaternion.SLIMEVR.LEFT,
-			Quaternion.SLIMEVR.BACK_LEFT,
-			Quaternion.SLIMEVR.FRONT_RIGHT,
-			Quaternion.SLIMEVR.RIGHT,
-			Quaternion.SLIMEVR.BACK_RIGHT,
-			Quaternion.SLIMEVR.BACK,
-		)
-
-		val frontRot = EulerAngles(EulerOrder.YZX, FastMath.HALF_PI, 0f, 0f).toQuaternion()
-	}
 }

server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt

@@ -0,0 +1,180 @@
+package dev.slimevr.unit
+
+import com.jme3.math.FastMath
+import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
+import dev.slimevr.tracking.processor.HumanPoseManager
+import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.TrackerPosition
+import dev.slimevr.tracking.trackers.TrackerStatus
+import dev.slimevr.tracking.trackers.udp.IMUType
+import dev.slimevr.unit.TrackerUtils.assertAnglesApproxEqual
+import dev.slimevr.unit.TrackerUtils.quatApproxEqual
+import io.eiren.util.collections.FastList
+import io.github.axisangles.ktmath.EulerAngles
+import io.github.axisangles.ktmath.EulerOrder
+import io.github.axisangles.ktmath.Quaternion
+import org.junit.jupiter.api.Test
+import kotlin.random.Random
+
+class SkeletonResetTests {
+
+	val resetSource = "Unit Test"
+
+	@Test
+	fun testSkeletonReset() {
+		val rand = Random(42)
+
+		val hmd = mkTrack(TrackerPosition.HEAD, true, true, false)
+		val chest = mkTrack(TrackerPosition.CHEST)
+		val hip = mkTrack(TrackerPosition.HIP)
+
+		val upperLeft = mkTrack(TrackerPosition.LEFT_UPPER_LEG)
+		val lowerLeft = mkTrack(TrackerPosition.LEFT_LOWER_LEG)
+
+		val upperRight = mkTrack(TrackerPosition.RIGHT_UPPER_LEG)
+		val lowerRight = mkTrack(TrackerPosition.RIGHT_LOWER_LEG)
+
+		// Collect all our trackers
+		val tracks = arrayOf(chest, hip, upperLeft, lowerLeft, upperRight, lowerRight)
+		val tracksWithHmd = tracks.plus(hmd)
+		val trackerList = FastList(tracksWithHmd)
+
+		// Initialize skeleton and everything
+		val hpm = HumanPoseManager(trackerList)
+
+		val headRot1 = EulerAngles(EulerOrder.YZX, 0f, FastMath.HALF_PI, FastMath.QUARTER_PI).toQuaternion()
+		val expectRot1 = EulerAngles(EulerOrder.YZX, 0f, FastMath.HALF_PI, 0f).toQuaternion()
+
+		// Randomize tracker orientations, these should be zeroed and matched to the
+		// headset yaw by full reset
+		for (tracker in tracks) {
+			val init = EulerAngles(
+				EulerOrder.YZX,
+				rand.nextFloat() * FastMath.TWO_PI,
+				rand.nextFloat() * FastMath.TWO_PI,
+				rand.nextFloat() * FastMath.TWO_PI,
+			).toQuaternion()
+			tracker.setRotation(init)
+		}
+		hmd.setRotation(headRot1)
+		hpm.resetTrackersFull(resetSource)
+
+		for (tracker in tracks) {
+			val actual = tracker.getRotation()
+			assert(quatApproxEqual(expectRot1, actual)) {
+				"\"${tracker.name}\" did not reset to the reference rotation. Expected <$expectRot1>, actual <$actual>."
+			}
+		}
+
+		// Randomize full tracker orientations, these should match the headset yaw but
+		// retain orientation otherwise
+		for (tracker in tracks) {
+			val init = EulerAngles(
+				EulerOrder.YZX,
+				rand.nextFloat() * FastMath.TWO_PI,
+				rand.nextFloat() * FastMath.TWO_PI,
+				rand.nextFloat() * FastMath.TWO_PI,
+			).toQuaternion()
+			tracker.setRotation(init)
+		}
+		hmd.setRotation(Quaternion.IDENTITY)
+		hpm.resetTrackersYaw(resetSource)
+
+		for (tracker in tracks) {
+			val yaw = TrackerUtils.yaw(tracker.getRotation())
+			assertAnglesApproxEqual(0f, yaw, "\"${tracker.name}\" did not reset to the reference rotation.")
+		}
+	}
+
+	@Test
+	fun testSkeletonMount() {
+		val hmd = mkTrack(TrackerPosition.HEAD, true, true, false)
+		val chest = mkTrack(TrackerPosition.CHEST)
+		val hip = mkTrack(TrackerPosition.HIP)
+
+		val upperLeft = mkTrack(TrackerPosition.LEFT_UPPER_LEG)
+		val lowerLeft = mkTrack(TrackerPosition.LEFT_LOWER_LEG)
+
+		val upperRight = mkTrack(TrackerPosition.RIGHT_UPPER_LEG)
+		val lowerRight = mkTrack(TrackerPosition.RIGHT_LOWER_LEG)
+
+		// Collect all our trackers
+		val tracks = arrayOf(chest, hip, upperLeft, lowerLeft, upperRight, lowerRight)
+		val tracksWithHmd = tracks.plus(hmd)
+		val trackerList = FastList(tracksWithHmd)
+
+		// Initialize skeleton and everything
+		val hpm = HumanPoseManager(trackerList)
+
+		// Just a bunch of random mounting orientations
+		val expected = arrayOf(
+			Pair(chest, Quaternion.SLIMEVR.FRONT),
+			Pair(hip, Quaternion.SLIMEVR.RIGHT),
+			Pair(upperLeft, Quaternion.SLIMEVR.BACK),
+			Pair(lowerLeft, Quaternion.SLIMEVR.LEFT),
+			Pair(upperRight, Quaternion.SLIMEVR.FRONT),
+			Pair(lowerRight, Quaternion.SLIMEVR.RIGHT),
+		)
+		// Rotate the tracker to fit the expected mounting orientation
+		for ((tracker, mountRot) in expected) {
+			tracker.setRotation(mkTrackMount(mountRot))
+		}
+		// Then perform a mounting reset
+		hpm.resetTrackersMounting(resetSource)
+
+		for ((tracker, mountRot) in expected) {
+			// Some mounting needs to be inverted (when in a specific pose)
+			// TODO: Make this less hardcoded, accept alternative poses
+			val expectedMounting = when (tracker.trackerPosition) {
+				TrackerPosition.CHEST,
+				TrackerPosition.HIP,
+				TrackerPosition.LEFT_LOWER_LEG,
+				TrackerPosition.RIGHT_LOWER_LEG,
+				-> mountRot * Quaternion.SLIMEVR.FRONT
+
+				TrackerPosition.LEFT_UPPER_LEG,
+				TrackerPosition.RIGHT_UPPER_LEG,
+				-> mountRot
+
+				else -> mountRot
+			}
+			val actualMounting = tracker.resetsHandler.mountRotFix
+
+			// Make sure yaw matches
+			val expectedY = TrackerUtils.yaw(expectedMounting)
+			val actualY = TrackerUtils.yaw(actualMounting)
+			assertAnglesApproxEqual(expectedY, actualY, "\"${tracker.name}\" did not reset to the reference rotation.")
+
+			// X and Z components should be zero for mounting
+			assert(FastMath.isApproxZero(actualMounting.x)) {
+				"\"${tracker.name}\" did not reset to the reference rotation. Expected <0.0>, actual <${actualMounting.x}>."
+			}
+			assert(FastMath.isApproxZero(actualMounting.z)) {
+				"\"${tracker.name}\" did not reset to the reference rotation. Expected <0.0>, actual <${actualMounting.z}>."
+			}
+		}
+	}
+
+	fun mkTrack(pos: TrackerPosition, hmd: Boolean = false, computed: Boolean = false, reset: Boolean = true): Tracker {
+		val name = "test ${pos.designation}"
+		val tracker = Tracker(
+			null,
+			getNextLocalTrackerId(),
+			name,
+			name,
+			pos,
+			hasPosition = hmd,
+			hasRotation = true,
+			isComputed = computed,
+			imuType = IMUType.UNKNOWN,
+			needsReset = true,
+			needsMounting = reset,
+			isHmd = hmd,
+			trackRotDirection = false,
+		)
+		tracker.status = TrackerStatus.OK
+		return tracker
+	}
+
+	fun mkTrackMount(rot: Quaternion): Quaternion = rot * (TrackerUtils.frontRot / rot)
+}

server/core/src/test/java/dev/slimevr/unit/TrackerUtils.kt

@@ -0,0 +1,61 @@
+package dev.slimevr.unit
+
+import com.jme3.math.FastMath
+import io.github.axisangles.ktmath.EulerAngles
+import io.github.axisangles.ktmath.EulerOrder
+import io.github.axisangles.ktmath.Quaternion
+import org.junit.jupiter.api.AssertionFailureBuilder
+import kotlin.math.abs
+
+object TrackerUtils {
+	val directions = arrayOf(
+		Quaternion.SLIMEVR.FRONT,
+		Quaternion.SLIMEVR.FRONT_LEFT,
+		Quaternion.SLIMEVR.LEFT,
+		Quaternion.SLIMEVR.BACK_LEFT,
+		Quaternion.SLIMEVR.FRONT_RIGHT,
+		Quaternion.SLIMEVR.RIGHT,
+		Quaternion.SLIMEVR.BACK_RIGHT,
+		Quaternion.SLIMEVR.BACK,
+	)
+
+	val frontRot = EulerAngles(EulerOrder.YZX, FastMath.HALF_PI, 0f, 0f).toQuaternion()
+
+	/**
+	 * Makes a radian angle positive
+	 */
+	fun posRad(rot: Float): Float {
+		// Reduce the rotation to the smallest form
+		val redRot = rot % FastMath.TWO_PI
+		return abs(if (rot < 0f) FastMath.TWO_PI + redRot else redRot)
+	}
+
+	/**
+	 * Gets the yaw of a rotation in radians
+	 */
+	fun yaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
+
+	/**
+	 * Converts radians to degrees
+	 */
+	fun deg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
+
+	fun deg(rot: Quaternion): Float = deg(yaw(rot))
+
+	private fun anglesApproxEqual(a: Float, b: Float): Boolean = FastMath.isApproxEqual(a, b) ||
+		FastMath.isApproxEqual(a - FastMath.TWO_PI, b) ||
+		FastMath.isApproxEqual(a, b - FastMath.TWO_PI)
+
+	fun assertAnglesApproxEqual(expected: Float, actual: Float, message: String?) {
+		if (!anglesApproxEqual(expected, actual)) {
+			AssertionFailureBuilder.assertionFailure().message(message)
+				.expected(expected).actual(actual).buildAndThrow()
+		}
+	}
+
+	fun quatApproxEqual(q1: Quaternion, q2: Quaternion, tolerance: Float = FastMath.ZERO_TOLERANCE): Boolean =
+		FastMath.isApproxEqual(q1.w, q2.w, tolerance) &&
+			FastMath.isApproxEqual(q1.x, q2.x, tolerance) &&
+			FastMath.isApproxEqual(q1.y, q2.y, tolerance) &&
+			FastMath.isApproxEqual(q1.z, q2.z, tolerance)
+}