diff --git a/server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt b/server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt index 79f0718a9a..86172412ba 100644 --- a/server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt +++ b/server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt @@ -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 = 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)})", ) } } diff --git a/server/core/src/test/java/dev/slimevr/unit/ResetTheoryTests.kt b/server/core/src/test/java/dev/slimevr/unit/ResetTheoryTests.kt new file mode 100644 index 0000000000..cd1599c666 --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/unit/ResetTheoryTests.kt @@ -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 = 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 = 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 = 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 { + // 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 { + // 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() + } +} diff --git a/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt b/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt index cbbc3f734f..0c02f4300a 100644 --- a/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt +++ b/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt @@ -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)) { diff --git a/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt b/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt index 8668e2d225..4e46c0a602 100644 --- a/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt +++ b/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt @@ -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() + } + } }