mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 17:41:28 -08:00
Compare commits
2
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
7a09d31b04 | ||
|
|
73984799f2 |
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()
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user