Compare commits

...
Author SHA1 Message Date
Butterscotch! 7a09d31b04 Reset theory tests 2026-04-19 23:56:38 -04:00
Butterscotch! 73984799f2 Rename TrackerTestUtils methods & add quat/vec 2026-04-19 23:56:37 -04:00
4 changed files with 313 additions and 42 deletions

No files matched your search

@@ -4,9 +4,10 @@ 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.TrackerTestUtils.assertAnglesApproxEqual
import dev.slimevr.unit.TrackerTestUtils.deg
import dev.slimevr.unit.TrackerTestUtils.yaw
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.degYaw
import dev.slimevr.unit.TrackerTestUtils.radToDeg
import dev.slimevr.unit.TrackerTestUtils.radYaw
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
import io.github.axisangles.ktmath.Quaternion
@@ -26,7 +27,7 @@ class MountingResetTests {
fun testResetAndMounting(): List<DynamicTest> = TrackerTestUtils.directions.flatMap { e ->
TrackerTestUtils.directions.map { m ->
DynamicTest.dynamicTest(
"Full and Mounting Reset Test of Tracker (Expected: ${deg(e)}, reference: ${deg(m)})",
"Full and Mounting Reset Test of Tracker (Expected: ${degYaw(e)}, reference: ${degYaw(m)})",
) {
checkResetMounting(e, m)
}
@@ -56,12 +57,12 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
val expectedYaw = yaw(expected)
val resultYaw = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw = radYaw(expected)
val resultYaw = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw,
resultYaw,
"Resulting mounting yaw after full reset is not equal to reference yaw (${deg(expectedYaw)} vs ${deg(resultYaw)})",
message = "Resulting mounting yaw after full reset is not equal to reference yaw (${radToDeg(expectedYaw)} vs ${radToDeg(resultYaw)})",
)
// Apply full reset and mounting plus offset
@@ -73,12 +74,12 @@ class MountingResetTests {
// it needs to be applied twice
tracker.resetsHandler.resetMounting(reference * reference)
val expectedYaw2 = yaw(expected)
val resultYaw2 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw2 = radYaw(expected)
val resultYaw2 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw2,
resultYaw2,
"Resulting mounting yaw after full reset with offset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
message = "Resulting mounting yaw after full reset with offset is not equal to reference yaw (${radToDeg(expectedYaw2)} vs ${radToDeg(resultYaw2)})",
)
// Apply yaw reset and mounting
@@ -88,12 +89,12 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
val expectedYaw3 = yaw(expected)
val resultYaw3 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw3 = radYaw(expected)
val resultYaw3 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw3,
resultYaw3,
"Resulting mounting yaw after yaw reset is not equal to reference yaw (${deg(expectedYaw3)} vs ${deg(resultYaw3)})",
message = "Resulting mounting yaw after yaw reset is not equal to reference yaw (${radToDeg(expectedYaw3)} vs ${radToDeg(resultYaw3)})",
)
// Apply yaw reset and mounting plus offset
@@ -106,12 +107,12 @@ class MountingResetTests {
// it needs to be applied twice
tracker.resetsHandler.resetMounting(reference * reference)
val expectedYaw4 = yaw(expected)
val resultYaw4 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw4 = radYaw(expected)
val resultYaw4 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw3,
resultYaw3,
"Resulting mounting yaw after yaw reset with offset is not equal to reference yaw (${deg(expectedYaw4)} vs ${deg(resultYaw4)})",
message = "Resulting mounting yaw after yaw reset with offset is not equal to reference yaw (${radToDeg(expectedYaw4)} vs ${radToDeg(resultYaw4)})",
)
}
@@ -141,23 +142,23 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
val expectedYaw = yaw(expected)
val resultYaw = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw = radYaw(expected)
val resultYaw = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw,
resultYaw,
"Resulting mounting yaw after full reset is not equal to reference yaw (${deg(expectedYaw)} vs ${deg(resultYaw)})",
message = "Resulting mounting yaw after full reset is not equal to reference yaw (${radToDeg(expectedYaw)} vs ${radToDeg(resultYaw)})",
)
tracker.setRotation(reference * reference)
tracker.resetsHandler.resetYaw(reference)
val expectedYaw2 = yaw(reference)
val resultYaw2 = yaw(tracker.getRotation())
assertAnglesApproxEqual(
val expectedYaw2 = radYaw(reference)
val resultYaw2 = radYaw(tracker.getRotation())
assertAngleEquals(
expectedYaw2,
resultYaw2,
"Resulting rotation after yaw reset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
message = "Resulting rotation after yaw reset is not equal to reference yaw (${radToDeg(expectedYaw2)} vs ${radToDeg(resultYaw2)})",
)
}
}
@@ -0,0 +1,231 @@
package dev.slimevr.unit
import com.jme3.math.FastMath
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.assertQuatEquals
import dev.slimevr.unit.TrackerTestUtils.assertQuatNotEquals
import dev.slimevr.unit.TrackerTestUtils.quatApproxEqual
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
import io.github.axisangles.ktmath.Quaternion
import org.junit.jupiter.api.DynamicTest
import org.junit.jupiter.api.TestFactory
import kotlin.math.abs
import kotlin.test.assertEquals
import kotlin.test.assertNotEquals
// These technically don't test any implemented code, but serves as a definitive proof
// and learning material for our "session calibration" math and logic.
class ResetTheoryTests {
@TestFactory
fun makeRawOrientationTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testMakeOrientation( hC: $hC, aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testMakeRawOrientation(Quaternion.IDENTITY, hC, aA, hA)
}
}
}
}
/**
* We're trying to prove stuff here using this, so let's at least prove that we can
* make a raw orientation and then bring it back to the bone frame of reference.
*/
fun testMakeRawOrientation(
boneOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val rawOrientation = makeRawOrientation(boneOrientation, headingCorrect, attitudeAlign, headingAlign)
val newBoneOrientation = headingCorrect * rawOrientation * attitudeAlign * headingAlign
// Now that we re-applied the calibrations, let's see if it matches!
assertQuatEquals(boneOrientation, newBoneOrientation)
}
@TestFactory
fun headingCorrectTimingTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testHeadingCorrectTiming( hC: $hC, aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testHeadingCorrectTiming(Quaternion.IDENTITY, hC, aA, hA)
}
}
}
}
/**
* It doesn't actually matter *when* you add heading correction, just as long as
* it's on the left side.
*/
fun testHeadingCorrectTiming(
rawOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val boneOrientationA = headingCorrect * rawOrientation * attitudeAlign * headingAlign
val boneOrientationB = headingCorrect * (rawOrientation * attitudeAlign * headingAlign)
val boneOrientationC = headingCorrect * (rawOrientation * attitudeAlign) * headingAlign
assertQuatEquals(boneOrientationA, boneOrientationB)
assertQuatEquals(boneOrientationA, boneOrientationC)
}
@TestFactory
fun headingCorrectAttitudeAlignTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.map { aA ->
DynamicTest.dynamicTest(
"testHeadingCorrectAttitudeAlign( hC: $hC, aA: $aA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testHeadingCorrectAttitudeAlign(Quaternion.IDENTITY, hC, aA)
}
}
}
/**
* Heading correction also does not affect the calculation of the attitude alignment
* using euler angles.
*/
fun testHeadingCorrectAttitudeAlign(
rawOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
) {
val boneOrientationA = (rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
val boneOrientationB = (headingCorrect * rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
assertAngleEquals(boneOrientationA.x, boneOrientationB.x, FastMath.ZERO_TOLERANCE)
assertAngleEquals(boneOrientationA.z, boneOrientationB.z, FastMath.ZERO_TOLERANCE)
// We can also show that we're calculating the right attitude alignment.
val attitudeAlignEul = attitudeAlign.toEulerAngles(EulerOrder.YZX)
assertAngleEquals(attitudeAlignEul.x, boneOrientationA.x, FastMath.ZERO_TOLERANCE)
assertAngleEquals(attitudeAlignEul.z, boneOrientationA.z, FastMath.ZERO_TOLERANCE)
}
@TestFactory
fun attitudeHeadingAlignDependenceTests(): List<DynamicTest> {
// Order doesn't matter if the attitude alignment has no attitude.
return roughAttitude.filterNot { FastMath.isApproxZero(it.x) && FastMath.isApproxZero(it.z) }.flatMap { aA ->
// Same for if heading alignment is the quaternion identity.
roughHeading.filterNot { quatApproxEqual(it, Quaternion.IDENTITY) }.map { hA ->
DynamicTest.dynamicTest(
"testAttitudeHeadingAlignDependence( aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testAttitudeHeadingAlignDependence(Quaternion.IDENTITY, aA, hA)
}
}
}
}
/**
* It *does* matter what order you apply attitude and heading alignment.
*/
fun testAttitudeHeadingAlignDependence(
rawOrientation: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val boneOrientationA = rawOrientation * attitudeAlign * headingAlign
val boneOrientationB = rawOrientation * headingAlign * attitudeAlign
assertQuatNotEquals(boneOrientationA, boneOrientationB)
}
@TestFactory
fun attitudeHeadingAlignOrderTests(): List<DynamicTest> {
// We're not proving anything if both attitude axes are of equal magnitude.
return roughAttitude.filterNot { FastMath.isApproxEqual(abs(it.x), abs(it.z)) }.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testAttitudeHeadingAlignOrder( aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testAttitudeHeadingAlignOrder(Quaternion.IDENTITY, aA, hA)
}
}
}
}
/**
* If we want to modify heading alignment but keep a constant attitude alignment,
* we need to apply heading alignment *after* attitude.
*/
fun testAttitudeHeadingAlignOrder(
rawOrientation: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
// Perpendicular heading alignment (rotated by 90 deg), makes it easy to check
// our results.
val headingAlignB = headingAlign * Quaternion.rotationAroundYAxis(FastMath.HALF_PI)
// We must also apply an inverse of the heading alignment to make our
// quaternions comparable; we just want to affect the axes, not add to the
// orientation.
val boneOrientationA = headingAlign.inv() * (rawOrientation * attitudeAlign * headingAlign)
val boneOrientationB = headingAlignB.inv() * (rawOrientation * attitudeAlign * headingAlignB)
assertEquals(abs(boneOrientationA.x), abs(boneOrientationB.z), FastMath.ZERO_TOLERANCE)
assertEquals(abs(boneOrientationA.z), abs(boneOrientationB.x), FastMath.ZERO_TOLERANCE)
// Since it's required for this test, we can also show that by applying the
// inverse of heading alignment as a heading correction, we can retain the same
// heading orientation despite changing alignment. By doing this, we remove
// dependence between correction and alignment; they can resolve to definitive
// values.
assertEquals(boneOrientationA.y, boneOrientationB.y, FastMath.ZERO_TOLERANCE)
// We can also show that this does not work when heading alignment comes before
// attitude alignment.
val boneOrientationC = headingAlign.inv() * (rawOrientation * headingAlign * attitudeAlign)
val boneOrientationD = headingAlignB.inv() * (rawOrientation * headingAlignB * attitudeAlign)
assertNotEquals(abs(boneOrientationC.x), abs(boneOrientationD.z), FastMath.ZERO_TOLERANCE)
assertNotEquals(abs(boneOrientationC.z), abs(boneOrientationD.x), FastMath.ZERO_TOLERANCE)
}
companion object {
// 5 steps
val roughStep = -180..180 step 72
// 12 steps
val fineStep = -180..180 step 30
// Will not work when we don't know the heading correction or heading alignment,
// we will need euler angles to calculate those, and it needs to sacrifice one
// axis for euler angles to work
val roughAttitude = roughStep.flatMap { x ->
roughStep.map { z ->
EulerAngles(EulerOrder.YZX, x * FastMath.DEG_TO_RAD, 0f, z * FastMath.DEG_TO_RAD).toQuaternion()
}
}
val roughHeading = roughStep.map { y ->
Quaternion.rotationAroundYAxis(y * FastMath.DEG_TO_RAD)
}
/**
* Reverse calibration to get a raw orientation from a bone orientation.
*/
// We reverse the order of headingAlign and attitudeAlign here since our
// attitude alignment is within the raw heading frame of reference, so we must
// bring the orientation back into that frame of reference first. Whatever is
// applied last must be taken off first.
fun makeRawOrientation(
boneOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
): Quaternion = headingCorrect.inv() * boneOrientation * headingAlign.inv() * attitudeAlign.inv()
}
}
@@ -3,7 +3,7 @@ package dev.slimevr.unit
import com.jme3.math.FastMath
import dev.slimevr.tracking.processor.HumanPoseManager
import dev.slimevr.tracking.trackers.TrackerPosition
import dev.slimevr.unit.TrackerTestUtils.assertAnglesApproxEqual
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.quatApproxEqual
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
@@ -57,8 +57,8 @@ class SkeletonResetTests {
hpm.resetTrackersYaw(resetSource)
for (tracker in trackers.set) {
val yaw = TrackerTestUtils.yaw(tracker.getRotation())
assertAnglesApproxEqual(0f, yaw, "\"${tracker.name}\" did not reset to the reference rotation.")
val yaw = TrackerTestUtils.radYaw(tracker.getRotation())
assertAngleEquals(0f, yaw, message = "\"${tracker.name}\" did not reset to the reference rotation.")
}
}
@@ -104,9 +104,9 @@ class SkeletonResetTests {
val actualMounting = tracker.resetsHandler.mountRotFix
// Make sure yaw matches
val expectedY = TrackerTestUtils.yaw(expectedMounting)
val actualY = TrackerTestUtils.yaw(actualMounting)
assertAnglesApproxEqual(expectedY, actualY, "\"${tracker.name}\" did not reset to the reference rotation.")
val expectedY = TrackerTestUtils.radYaw(expectedMounting)
val actualY = TrackerTestUtils.radYaw(actualMounting)
assertAngleEquals(expectedY, actualY, message = "\"${tracker.name}\" did not reset to the reference rotation.")
// X and Z components should be zero for mounting
assert(FastMath.isApproxZero(actualMounting.x)) {
@@ -56,32 +56,71 @@ object TrackerTestUtils {
/**
* Gets the yaw of a rotation in radians
*/
fun yaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
fun radYaw(rot: Quaternion): Float = posRad(rot.toEulerAngles(EulerOrder.YZX).y)
/**
* Converts radians to degrees
*/
fun deg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
fun radToDeg(rot: Float): Float = rot * FastMath.RAD_TO_DEG
fun deg(rot: Quaternion): Float = deg(yaw(rot))
fun degYaw(rot: Quaternion): Float = radToDeg(radYaw(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 angleApproxEqual(a: Float, b: Float, tolerance: Float = FastMath.ZERO_TOLERANCE): Boolean = FastMath.isApproxEqual(a, b, tolerance) ||
FastMath.isApproxEqual(a - FastMath.TWO_PI, b, tolerance) ||
FastMath.isApproxEqual(a, b - FastMath.TWO_PI, tolerance)
fun assertAnglesApproxEqual(expected: Float, actual: Float, message: String?) {
if (!anglesApproxEqual(expected, actual)) {
fun assertAngleEquals(expected: Float, actual: Float, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (!angleApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
fun assertAngleNotEquals(expected: Float, actual: Float, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (angleApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
/**
* True if the quaternions are approximately equal in quaternion space, not rotation
* space.
*/
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)
fun assertQuatEquals(expected: Quaternion, actual: Quaternion, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (!quatApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
fun assertQuatNotEquals(expected: Quaternion, actual: Quaternion, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (quatApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
fun vectorApproxEqual(v1: Vector3, v2: Vector3, tolerance: Float = FastMath.ZERO_TOLERANCE): Boolean = FastMath.isApproxEqual(v1.x, v2.x, tolerance) &&
FastMath.isApproxEqual(v1.y, v2.y, tolerance) &&
FastMath.isApproxEqual(v1.z, v2.z, tolerance)
fun assertVectorEquals(expected: Vector3, actual: Vector3, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (!vectorApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
fun assertVectorNotEquals(expected: Vector3, actual: Vector3, tolerance: Float = FastMath.ZERO_TOLERANCE, message: String? = null) {
if (vectorApproxEqual(expected, actual, tolerance)) {
AssertionFailureBuilder.assertionFailure().message(message)
.expected(expected).actual(actual).buildAndThrow()
}
}
}