Skip to content

Commit 7a09d31

Browse files
ButterscotchVButterscotchV
committed
Reset theory tests
1 parent 7398479 commit 7a09d31

1 file changed

Lines changed: 231 additions & 0 deletions

File tree

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

Lines changed: 231 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,231 @@
1+
package dev.slimevr.unit
2+
3+
import com.jme3.math.FastMath
4+
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
5+
import dev.slimevr.unit.TrackerTestUtils.assertQuatEquals
6+
import dev.slimevr.unit.TrackerTestUtils.assertQuatNotEquals
7+
import dev.slimevr.unit.TrackerTestUtils.quatApproxEqual
8+
import io.github.axisangles.ktmath.EulerAngles
9+
import io.github.axisangles.ktmath.EulerOrder
10+
import io.github.axisangles.ktmath.Quaternion
11+
import org.junit.jupiter.api.DynamicTest
12+
import org.junit.jupiter.api.TestFactory
13+
import kotlin.math.abs
14+
import kotlin.test.assertEquals
15+
import kotlin.test.assertNotEquals
16+
17+
// These technically don't test any implemented code, but serves as a definitive proof
18+
// and learning material for our "session calibration" math and logic.
19+
class ResetTheoryTests {
20+
@TestFactory
21+
fun makeRawOrientationTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
22+
roughAttitude.flatMap { aA ->
23+
roughHeading.map { hA ->
24+
DynamicTest.dynamicTest(
25+
"testMakeOrientation( hC: $hC, aA: $aA, hA: $hA )",
26+
) {
27+
// We can just use identity for the target orientation as only the
28+
// calibration quaternions themselves matter.
29+
testMakeRawOrientation(Quaternion.IDENTITY, hC, aA, hA)
30+
}
31+
}
32+
}
33+
}
34+
35+
/**
36+
* We're trying to prove stuff here using this, so let's at least prove that we can
37+
* make a raw orientation and then bring it back to the bone frame of reference.
38+
*/
39+
fun testMakeRawOrientation(
40+
boneOrientation: Quaternion,
41+
headingCorrect: Quaternion,
42+
attitudeAlign: Quaternion,
43+
headingAlign: Quaternion,
44+
) {
45+
val rawOrientation = makeRawOrientation(boneOrientation, headingCorrect, attitudeAlign, headingAlign)
46+
val newBoneOrientation = headingCorrect * rawOrientation * attitudeAlign * headingAlign
47+
// Now that we re-applied the calibrations, let's see if it matches!
48+
assertQuatEquals(boneOrientation, newBoneOrientation)
49+
}
50+
51+
@TestFactory
52+
fun headingCorrectTimingTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
53+
roughAttitude.flatMap { aA ->
54+
roughHeading.map { hA ->
55+
DynamicTest.dynamicTest(
56+
"testHeadingCorrectTiming( hC: $hC, aA: $aA, hA: $hA )",
57+
) {
58+
// We can just use identity for the target orientation as only the
59+
// calibration quaternions themselves matter.
60+
testHeadingCorrectTiming(Quaternion.IDENTITY, hC, aA, hA)
61+
}
62+
}
63+
}
64+
}
65+
66+
/**
67+
* It doesn't actually matter *when* you add heading correction, just as long as
68+
* it's on the left side.
69+
*/
70+
fun testHeadingCorrectTiming(
71+
rawOrientation: Quaternion,
72+
headingCorrect: Quaternion,
73+
attitudeAlign: Quaternion,
74+
headingAlign: Quaternion,
75+
) {
76+
val boneOrientationA = headingCorrect * rawOrientation * attitudeAlign * headingAlign
77+
val boneOrientationB = headingCorrect * (rawOrientation * attitudeAlign * headingAlign)
78+
val boneOrientationC = headingCorrect * (rawOrientation * attitudeAlign) * headingAlign
79+
assertQuatEquals(boneOrientationA, boneOrientationB)
80+
assertQuatEquals(boneOrientationA, boneOrientationC)
81+
}
82+
83+
@TestFactory
84+
fun headingCorrectAttitudeAlignTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
85+
roughAttitude.map { aA ->
86+
DynamicTest.dynamicTest(
87+
"testHeadingCorrectAttitudeAlign( hC: $hC, aA: $aA )",
88+
) {
89+
// We can just use identity for the target orientation as only the
90+
// calibration quaternions themselves matter.
91+
testHeadingCorrectAttitudeAlign(Quaternion.IDENTITY, hC, aA)
92+
}
93+
}
94+
}
95+
96+
/**
97+
* Heading correction also does not affect the calculation of the attitude alignment
98+
* using euler angles.
99+
*/
100+
fun testHeadingCorrectAttitudeAlign(
101+
rawOrientation: Quaternion,
102+
headingCorrect: Quaternion,
103+
attitudeAlign: Quaternion,
104+
) {
105+
val boneOrientationA = (rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
106+
val boneOrientationB = (headingCorrect * rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
107+
assertAngleEquals(boneOrientationA.x, boneOrientationB.x, FastMath.ZERO_TOLERANCE)
108+
assertAngleEquals(boneOrientationA.z, boneOrientationB.z, FastMath.ZERO_TOLERANCE)
109+
// We can also show that we're calculating the right attitude alignment.
110+
val attitudeAlignEul = attitudeAlign.toEulerAngles(EulerOrder.YZX)
111+
assertAngleEquals(attitudeAlignEul.x, boneOrientationA.x, FastMath.ZERO_TOLERANCE)
112+
assertAngleEquals(attitudeAlignEul.z, boneOrientationA.z, FastMath.ZERO_TOLERANCE)
113+
}
114+
115+
@TestFactory
116+
fun attitudeHeadingAlignDependenceTests(): List<DynamicTest> {
117+
// Order doesn't matter if the attitude alignment has no attitude.
118+
return roughAttitude.filterNot { FastMath.isApproxZero(it.x) && FastMath.isApproxZero(it.z) }.flatMap { aA ->
119+
// Same for if heading alignment is the quaternion identity.
120+
roughHeading.filterNot { quatApproxEqual(it, Quaternion.IDENTITY) }.map { hA ->
121+
DynamicTest.dynamicTest(
122+
"testAttitudeHeadingAlignDependence( aA: $aA, hA: $hA )",
123+
) {
124+
// We can just use identity for the target orientation as only the
125+
// calibration quaternions themselves matter.
126+
testAttitudeHeadingAlignDependence(Quaternion.IDENTITY, aA, hA)
127+
}
128+
}
129+
}
130+
}
131+
132+
/**
133+
* It *does* matter what order you apply attitude and heading alignment.
134+
*/
135+
fun testAttitudeHeadingAlignDependence(
136+
rawOrientation: Quaternion,
137+
attitudeAlign: Quaternion,
138+
headingAlign: Quaternion,
139+
) {
140+
val boneOrientationA = rawOrientation * attitudeAlign * headingAlign
141+
val boneOrientationB = rawOrientation * headingAlign * attitudeAlign
142+
assertQuatNotEquals(boneOrientationA, boneOrientationB)
143+
}
144+
145+
@TestFactory
146+
fun attitudeHeadingAlignOrderTests(): List<DynamicTest> {
147+
// We're not proving anything if both attitude axes are of equal magnitude.
148+
return roughAttitude.filterNot { FastMath.isApproxEqual(abs(it.x), abs(it.z)) }.flatMap { aA ->
149+
roughHeading.map { hA ->
150+
DynamicTest.dynamicTest(
151+
"testAttitudeHeadingAlignOrder( aA: $aA, hA: $hA )",
152+
) {
153+
// We can just use identity for the target orientation as only the
154+
// calibration quaternions themselves matter.
155+
testAttitudeHeadingAlignOrder(Quaternion.IDENTITY, aA, hA)
156+
}
157+
}
158+
}
159+
}
160+
161+
/**
162+
* If we want to modify heading alignment but keep a constant attitude alignment,
163+
* we need to apply heading alignment *after* attitude.
164+
*/
165+
fun testAttitudeHeadingAlignOrder(
166+
rawOrientation: Quaternion,
167+
attitudeAlign: Quaternion,
168+
headingAlign: Quaternion,
169+
) {
170+
// Perpendicular heading alignment (rotated by 90 deg), makes it easy to check
171+
// our results.
172+
val headingAlignB = headingAlign * Quaternion.rotationAroundYAxis(FastMath.HALF_PI)
173+
174+
// We must also apply an inverse of the heading alignment to make our
175+
// quaternions comparable; we just want to affect the axes, not add to the
176+
// orientation.
177+
val boneOrientationA = headingAlign.inv() * (rawOrientation * attitudeAlign * headingAlign)
178+
val boneOrientationB = headingAlignB.inv() * (rawOrientation * attitudeAlign * headingAlignB)
179+
assertEquals(abs(boneOrientationA.x), abs(boneOrientationB.z), FastMath.ZERO_TOLERANCE)
180+
assertEquals(abs(boneOrientationA.z), abs(boneOrientationB.x), FastMath.ZERO_TOLERANCE)
181+
182+
// Since it's required for this test, we can also show that by applying the
183+
// inverse of heading alignment as a heading correction, we can retain the same
184+
// heading orientation despite changing alignment. By doing this, we remove
185+
// dependence between correction and alignment; they can resolve to definitive
186+
// values.
187+
assertEquals(boneOrientationA.y, boneOrientationB.y, FastMath.ZERO_TOLERANCE)
188+
189+
// We can also show that this does not work when heading alignment comes before
190+
// attitude alignment.
191+
val boneOrientationC = headingAlign.inv() * (rawOrientation * headingAlign * attitudeAlign)
192+
val boneOrientationD = headingAlignB.inv() * (rawOrientation * headingAlignB * attitudeAlign)
193+
assertNotEquals(abs(boneOrientationC.x), abs(boneOrientationD.z), FastMath.ZERO_TOLERANCE)
194+
assertNotEquals(abs(boneOrientationC.z), abs(boneOrientationD.x), FastMath.ZERO_TOLERANCE)
195+
}
196+
197+
companion object {
198+
// 5 steps
199+
val roughStep = -180..180 step 72
200+
201+
// 12 steps
202+
val fineStep = -180..180 step 30
203+
204+
// Will not work when we don't know the heading correction or heading alignment,
205+
// we will need euler angles to calculate those, and it needs to sacrifice one
206+
// axis for euler angles to work
207+
val roughAttitude = roughStep.flatMap { x ->
208+
roughStep.map { z ->
209+
EulerAngles(EulerOrder.YZX, x * FastMath.DEG_TO_RAD, 0f, z * FastMath.DEG_TO_RAD).toQuaternion()
210+
}
211+
}
212+
213+
val roughHeading = roughStep.map { y ->
214+
Quaternion.rotationAroundYAxis(y * FastMath.DEG_TO_RAD)
215+
}
216+
217+
/**
218+
* Reverse calibration to get a raw orientation from a bone orientation.
219+
*/
220+
// We reverse the order of headingAlign and attitudeAlign here since our
221+
// attitude alignment is within the raw heading frame of reference, so we must
222+
// bring the orientation back into that frame of reference first. Whatever is
223+
// applied last must be taken off first.
224+
fun makeRawOrientation(
225+
boneOrientation: Quaternion,
226+
headingCorrect: Quaternion,
227+
attitudeAlign: Quaternion,
228+
headingAlign: Quaternion,
229+
): Quaternion = headingCorrect.inv() * boneOrientation * headingAlign.inv() * attitudeAlign.inv()
230+
}
231+
}

0 commit comments

Comments
 (0)