Skip to content

Commit f0d0746

Browse files
ButterscotchVButterscotchV
authored
Add skeleton resets unit tests (#1336)
1 parent 49bcc86 commit f0d0746

File tree

4 files changed

+249
-54
lines changed

4 files changed

+249
-54
lines changed

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ class Tracker @JvmOverloads constructor(
111111
}
112112
alreadyInitialized = true
113113
}
114-
if (!isInternal) {
114+
if (!isInternal && VRServer.instanceInitialized) {
115115
// If the status of a non-internal tracker has changed, inform
116116
// the VRServer to recreate the skeleton, as it may need to
117117
// assign or un-assign the tracker to a body part

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

+7-53
Original file line numberDiff line numberDiff line change
@@ -4,14 +4,15 @@ import com.jme3.math.FastMath
44
import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
55
import dev.slimevr.tracking.trackers.Tracker
66
import dev.slimevr.tracking.trackers.udp.IMUType
7+
import dev.slimevr.unit.TrackerUtils.assertAnglesApproxEqual
8+
import dev.slimevr.unit.TrackerUtils.deg
9+
import dev.slimevr.unit.TrackerUtils.yaw
710
import io.github.axisangles.ktmath.EulerAngles
811
import io.github.axisangles.ktmath.EulerOrder
912
import io.github.axisangles.ktmath.Quaternion
10-
import org.junit.jupiter.api.AssertionFailureBuilder
1113
import org.junit.jupiter.api.DynamicTest
1214
import org.junit.jupiter.api.Test
1315
import org.junit.jupiter.api.TestFactory
14-
import kotlin.math.*
1516

1617
/**
1718
* Tests [TrackerResetsHandler.resetMounting]
@@ -22,8 +23,8 @@ import kotlin.math.*
2223
class MountingResetTests {
2324

2425
@TestFactory
25-
fun testResetAndMounting(): List<DynamicTest> = directions.flatMap { e ->
26-
directions.map { m ->
26+
fun testResetAndMounting(): List<DynamicTest> = TrackerUtils.directions.flatMap { e ->
27+
TrackerUtils.directions.map { m ->
2728
DynamicTest.dynamicTest(
2829
"Full and Mounting Reset Test of Tracker (Expected: ${deg(e)}, reference: ${deg(m)})",
2930
) {
@@ -34,7 +35,7 @@ class MountingResetTests {
3435

3536
private fun checkResetMounting(expected: Quaternion, reference: Quaternion) {
3637
// Compute the pitch/roll for the expected mounting
37-
val trackerRot = (expected * (frontRot / expected))
38+
val trackerRot = (expected * (TrackerUtils.frontRot / expected))
3839

3940
val tracker = Tracker(
4041
null,
@@ -119,7 +120,7 @@ class MountingResetTests {
119120
val expected = Quaternion.SLIMEVR.RIGHT
120121
val reference = EulerAngles(EulerOrder.YZX, FastMath.PI / 8f, FastMath.HALF_PI, 0f).toQuaternion()
121122
// Compute the pitch/roll for the expected mounting
122-
val trackerRot = (expected * (frontRot / expected))
123+
val trackerRot = (expected * (TrackerUtils.frontRot / expected))
123124

124125
val tracker = Tracker(
125126
null,
@@ -159,51 +160,4 @@ class MountingResetTests {
159160
"Resulting rotation after yaw reset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
160161
)
161162
}
162-
163-
/**
164-
* Makes a radian angle positive
165-
*/
166-
private fun posRad(rot: Float): Float {
167-
// Reduce the rotation to the smallest form
168-
val redRot = rot % FastMath.TWO_PI
169-
return abs(if (rot < 0f) FastMath.TWO_PI + redRot else redRot)
170-
}
171-
172-
/**
173-
* Gets the yaw of a rotation in radians
174-
*/
175-
private fun yaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
176-
177-
/**
178-
* Converts radians to degrees
179-
*/
180-
private fun deg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
181-
182-
private fun deg(rot: Quaternion): Float = deg(yaw(rot))
183-
184-
private fun anglesApproxEqual(a: Float, b: Float): Boolean = FastMath.isApproxEqual(a, b) ||
185-
FastMath.isApproxEqual(a - FastMath.TWO_PI, b) ||
186-
FastMath.isApproxEqual(a, b - FastMath.TWO_PI)
187-
188-
private fun assertAnglesApproxEqual(expected: Float, actual: Float, message: String?) {
189-
if (!anglesApproxEqual(expected, actual)) {
190-
AssertionFailureBuilder.assertionFailure().message(message)
191-
.expected(expected).actual(actual).buildAndThrow()
192-
}
193-
}
194-
195-
companion object {
196-
val directions = arrayOf(
197-
Quaternion.SLIMEVR.FRONT,
198-
Quaternion.SLIMEVR.FRONT_LEFT,
199-
Quaternion.SLIMEVR.LEFT,
200-
Quaternion.SLIMEVR.BACK_LEFT,
201-
Quaternion.SLIMEVR.FRONT_RIGHT,
202-
Quaternion.SLIMEVR.RIGHT,
203-
Quaternion.SLIMEVR.BACK_RIGHT,
204-
Quaternion.SLIMEVR.BACK,
205-
)
206-
207-
val frontRot = EulerAngles(EulerOrder.YZX, FastMath.HALF_PI, 0f, 0f).toQuaternion()
208-
}
209163
}

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

+180
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
package dev.slimevr.unit
2+
3+
import com.jme3.math.FastMath
4+
import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
5+
import dev.slimevr.tracking.processor.HumanPoseManager
6+
import dev.slimevr.tracking.trackers.Tracker
7+
import dev.slimevr.tracking.trackers.TrackerPosition
8+
import dev.slimevr.tracking.trackers.TrackerStatus
9+
import dev.slimevr.tracking.trackers.udp.IMUType
10+
import dev.slimevr.unit.TrackerUtils.assertAnglesApproxEqual
11+
import dev.slimevr.unit.TrackerUtils.quatApproxEqual
12+
import io.eiren.util.collections.FastList
13+
import io.github.axisangles.ktmath.EulerAngles
14+
import io.github.axisangles.ktmath.EulerOrder
15+
import io.github.axisangles.ktmath.Quaternion
16+
import org.junit.jupiter.api.Test
17+
import kotlin.random.Random
18+
19+
class SkeletonResetTests {
20+
21+
val resetSource = "Unit Test"
22+
23+
@Test
24+
fun testSkeletonReset() {
25+
val rand = Random(42)
26+
27+
val hmd = mkTrack(TrackerPosition.HEAD, true, true, false)
28+
val chest = mkTrack(TrackerPosition.CHEST)
29+
val hip = mkTrack(TrackerPosition.HIP)
30+
31+
val upperLeft = mkTrack(TrackerPosition.LEFT_UPPER_LEG)
32+
val lowerLeft = mkTrack(TrackerPosition.LEFT_LOWER_LEG)
33+
34+
val upperRight = mkTrack(TrackerPosition.RIGHT_UPPER_LEG)
35+
val lowerRight = mkTrack(TrackerPosition.RIGHT_LOWER_LEG)
36+
37+
// Collect all our trackers
38+
val tracks = arrayOf(chest, hip, upperLeft, lowerLeft, upperRight, lowerRight)
39+
val tracksWithHmd = tracks.plus(hmd)
40+
val trackerList = FastList(tracksWithHmd)
41+
42+
// Initialize skeleton and everything
43+
val hpm = HumanPoseManager(trackerList)
44+
45+
val headRot1 = EulerAngles(EulerOrder.YZX, 0f, FastMath.HALF_PI, FastMath.QUARTER_PI).toQuaternion()
46+
val expectRot1 = EulerAngles(EulerOrder.YZX, 0f, FastMath.HALF_PI, 0f).toQuaternion()
47+
48+
// Randomize tracker orientations, these should be zeroed and matched to the
49+
// headset yaw by full reset
50+
for (tracker in tracks) {
51+
val init = EulerAngles(
52+
EulerOrder.YZX,
53+
rand.nextFloat() * FastMath.TWO_PI,
54+
rand.nextFloat() * FastMath.TWO_PI,
55+
rand.nextFloat() * FastMath.TWO_PI,
56+
).toQuaternion()
57+
tracker.setRotation(init)
58+
}
59+
hmd.setRotation(headRot1)
60+
hpm.resetTrackersFull(resetSource)
61+
62+
for (tracker in tracks) {
63+
val actual = tracker.getRotation()
64+
assert(quatApproxEqual(expectRot1, actual)) {
65+
"\"${tracker.name}\" did not reset to the reference rotation. Expected <$expectRot1>, actual <$actual>."
66+
}
67+
}
68+
69+
// Randomize full tracker orientations, these should match the headset yaw but
70+
// retain orientation otherwise
71+
for (tracker in tracks) {
72+
val init = EulerAngles(
73+
EulerOrder.YZX,
74+
rand.nextFloat() * FastMath.TWO_PI,
75+
rand.nextFloat() * FastMath.TWO_PI,
76+
rand.nextFloat() * FastMath.TWO_PI,
77+
).toQuaternion()
78+
tracker.setRotation(init)
79+
}
80+
hmd.setRotation(Quaternion.IDENTITY)
81+
hpm.resetTrackersYaw(resetSource)
82+
83+
for (tracker in tracks) {
84+
val yaw = TrackerUtils.yaw(tracker.getRotation())
85+
assertAnglesApproxEqual(0f, yaw, "\"${tracker.name}\" did not reset to the reference rotation.")
86+
}
87+
}
88+
89+
@Test
90+
fun testSkeletonMount() {
91+
val hmd = mkTrack(TrackerPosition.HEAD, true, true, false)
92+
val chest = mkTrack(TrackerPosition.CHEST)
93+
val hip = mkTrack(TrackerPosition.HIP)
94+
95+
val upperLeft = mkTrack(TrackerPosition.LEFT_UPPER_LEG)
96+
val lowerLeft = mkTrack(TrackerPosition.LEFT_LOWER_LEG)
97+
98+
val upperRight = mkTrack(TrackerPosition.RIGHT_UPPER_LEG)
99+
val lowerRight = mkTrack(TrackerPosition.RIGHT_LOWER_LEG)
100+
101+
// Collect all our trackers
102+
val tracks = arrayOf(chest, hip, upperLeft, lowerLeft, upperRight, lowerRight)
103+
val tracksWithHmd = tracks.plus(hmd)
104+
val trackerList = FastList(tracksWithHmd)
105+
106+
// Initialize skeleton and everything
107+
val hpm = HumanPoseManager(trackerList)
108+
109+
// Just a bunch of random mounting orientations
110+
val expected = arrayOf(
111+
Pair(chest, Quaternion.SLIMEVR.FRONT),
112+
Pair(hip, Quaternion.SLIMEVR.RIGHT),
113+
Pair(upperLeft, Quaternion.SLIMEVR.BACK),
114+
Pair(lowerLeft, Quaternion.SLIMEVR.LEFT),
115+
Pair(upperRight, Quaternion.SLIMEVR.FRONT),
116+
Pair(lowerRight, Quaternion.SLIMEVR.RIGHT),
117+
)
118+
// Rotate the tracker to fit the expected mounting orientation
119+
for ((tracker, mountRot) in expected) {
120+
tracker.setRotation(mkTrackMount(mountRot))
121+
}
122+
// Then perform a mounting reset
123+
hpm.resetTrackersMounting(resetSource)
124+
125+
for ((tracker, mountRot) in expected) {
126+
// Some mounting needs to be inverted (when in a specific pose)
127+
// TODO: Make this less hardcoded, accept alternative poses
128+
val expectedMounting = when (tracker.trackerPosition) {
129+
TrackerPosition.CHEST,
130+
TrackerPosition.HIP,
131+
TrackerPosition.LEFT_LOWER_LEG,
132+
TrackerPosition.RIGHT_LOWER_LEG,
133+
-> mountRot * Quaternion.SLIMEVR.FRONT
134+
135+
TrackerPosition.LEFT_UPPER_LEG,
136+
TrackerPosition.RIGHT_UPPER_LEG,
137+
-> mountRot
138+
139+
else -> mountRot
140+
}
141+
val actualMounting = tracker.resetsHandler.mountRotFix
142+
143+
// Make sure yaw matches
144+
val expectedY = TrackerUtils.yaw(expectedMounting)
145+
val actualY = TrackerUtils.yaw(actualMounting)
146+
assertAnglesApproxEqual(expectedY, actualY, "\"${tracker.name}\" did not reset to the reference rotation.")
147+
148+
// X and Z components should be zero for mounting
149+
assert(FastMath.isApproxZero(actualMounting.x)) {
150+
"\"${tracker.name}\" did not reset to the reference rotation. Expected <0.0>, actual <${actualMounting.x}>."
151+
}
152+
assert(FastMath.isApproxZero(actualMounting.z)) {
153+
"\"${tracker.name}\" did not reset to the reference rotation. Expected <0.0>, actual <${actualMounting.z}>."
154+
}
155+
}
156+
}
157+
158+
fun mkTrack(pos: TrackerPosition, hmd: Boolean = false, computed: Boolean = false, reset: Boolean = true): Tracker {
159+
val name = "test ${pos.designation}"
160+
val tracker = Tracker(
161+
null,
162+
getNextLocalTrackerId(),
163+
name,
164+
name,
165+
pos,
166+
hasPosition = hmd,
167+
hasRotation = true,
168+
isComputed = computed,
169+
imuType = IMUType.UNKNOWN,
170+
needsReset = true,
171+
needsMounting = reset,
172+
isHmd = hmd,
173+
trackRotDirection = false,
174+
)
175+
tracker.status = TrackerStatus.OK
176+
return tracker
177+
}
178+
179+
fun mkTrackMount(rot: Quaternion): Quaternion = rot * (TrackerUtils.frontRot / rot)
180+
}

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

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
package dev.slimevr.unit
2+
3+
import com.jme3.math.FastMath
4+
import io.github.axisangles.ktmath.EulerAngles
5+
import io.github.axisangles.ktmath.EulerOrder
6+
import io.github.axisangles.ktmath.Quaternion
7+
import org.junit.jupiter.api.AssertionFailureBuilder
8+
import kotlin.math.abs
9+
10+
object TrackerUtils {
11+
val directions = arrayOf(
12+
Quaternion.SLIMEVR.FRONT,
13+
Quaternion.SLIMEVR.FRONT_LEFT,
14+
Quaternion.SLIMEVR.LEFT,
15+
Quaternion.SLIMEVR.BACK_LEFT,
16+
Quaternion.SLIMEVR.FRONT_RIGHT,
17+
Quaternion.SLIMEVR.RIGHT,
18+
Quaternion.SLIMEVR.BACK_RIGHT,
19+
Quaternion.SLIMEVR.BACK,
20+
)
21+
22+
val frontRot = EulerAngles(EulerOrder.YZX, FastMath.HALF_PI, 0f, 0f).toQuaternion()
23+
24+
/**
25+
* Makes a radian angle positive
26+
*/
27+
fun posRad(rot: Float): Float {
28+
// Reduce the rotation to the smallest form
29+
val redRot = rot % FastMath.TWO_PI
30+
return abs(if (rot < 0f) FastMath.TWO_PI + redRot else redRot)
31+
}
32+
33+
/**
34+
* Gets the yaw of a rotation in radians
35+
*/
36+
fun yaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
37+
38+
/**
39+
* Converts radians to degrees
40+
*/
41+
fun deg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
42+
43+
fun deg(rot: Quaternion): Float = deg(yaw(rot))
44+
45+
private fun anglesApproxEqual(a: Float, b: Float): Boolean = FastMath.isApproxEqual(a, b) ||
46+
FastMath.isApproxEqual(a - FastMath.TWO_PI, b) ||
47+
FastMath.isApproxEqual(a, b - FastMath.TWO_PI)
48+
49+
fun assertAnglesApproxEqual(expected: Float, actual: Float, message: String?) {
50+
if (!anglesApproxEqual(expected, actual)) {
51+
AssertionFailureBuilder.assertionFailure().message(message)
52+
.expected(expected).actual(actual).buildAndThrow()
53+
}
54+
}
55+
56+
fun quatApproxEqual(q1: Quaternion, q2: Quaternion, tolerance: Float = FastMath.ZERO_TOLERANCE): Boolean =
57+
FastMath.isApproxEqual(q1.w, q2.w, tolerance) &&
58+
FastMath.isApproxEqual(q1.x, q2.x, tolerance) &&
59+
FastMath.isApproxEqual(q1.y, q2.y, tolerance) &&
60+
FastMath.isApproxEqual(q1.z, q2.z, tolerance)
61+
}

0 commit comments

Comments
 (0)