github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-05 18:01:56 +02:00
Code Issues 157 Packages Projects Releases Wiki Activity

add tests and spotlessfix

Browse Source
This commit is contained in:
ImUrX
2023-01-25 21:21:29 -03:00
parent 5cecb3ed17
commit c5733069c1
8 changed files with 1082 additions and 874 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.editorconfig
View File

@@ -21,3 +21,4 @@ max_line_length = 88
indent_size = 4
indent_style = tab
max_line_length = 88
ij_kotlin_packages_to_use_import_on_demand = java.util.*,kotlin.math.*

3
server/build.gradle.kts
View File

@@ -139,7 +139,8 @@ configure<com.diffplug.gradle.spotless.SpotlessExtension> {
"indent_size" to 4,
"indent_style" to "tab",
// "max_line_length" to 88,
"ktlint_experimental" to "enabled"
"ktlint_experimental" to "enabled",
"ij_kotlin_packages_to_use_import_on_demand" to "java.util.*,kotlin.math.*"
)
val ktlintVersion = "0.47.1"
kotlinGradle {

37
server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt
View File

@@ -1,4 +1,5 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.cos
@@ -29,32 +30,38 @@ data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z:
cX * cY * cZ - sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY - cY * sX * sZ,
cZ*sX*sY + cX*cY*sZ)
cZ * sX * sY + cX * cY * sZ
)
EulerOrder.YZX -> Quaternion(
cX * cY * cZ - sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY + cY * sX * sZ,
cX*cY*sZ - cZ*sX*sY)
cX * cY * sZ - cZ * sX * sY
)
EulerOrder.ZXY -> Quaternion(
cX * cY * cZ - sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY + cY * sX * sZ,
cZ*sX*sY + cX*cY*sZ)
cZ * sX * sY + cX * cY * sZ
)
EulerOrder.ZYX -> Quaternion(
cX * cY * cZ + sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY + cY * sX * sZ,
cX*cY*sZ - cZ*sX*sY)
cX * cY * sZ - cZ * sX * sY
)
EulerOrder.YXZ -> Quaternion(
cX * cY * cZ + sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY - cY * sX * sZ,
cX*cY*sZ - cZ*sX*sY)
cX * cY * sZ - cZ * sX * sY
)
EulerOrder.XZY -> Quaternion(
cX * cY * cZ + sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY - cY * sX * sZ,
cZ*sX*sY + cX*cY*sZ)
cZ * sX * sY + cX * cY * sZ
)
}
}
@@ -76,27 +83,33 @@ data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z:
EulerOrder.XYZ -> Matrix3(
cY * cZ, -cY * sZ, sY,
cZ * sX * sY + cX * sZ, cX * cZ - sX * sY * sZ, -cY * sX,
sX*sZ - cX*cZ*sY, cZ*sX + cX*sY*sZ, cX*cY)
sX * sZ - cX * cZ * sY, cZ * sX + cX * sY * sZ, cX * cY
)
EulerOrder.YZX -> Matrix3(
cY * cZ, sX * sY - cX * cY * sZ, cX * sY + cY * sX * sZ,
sZ, cX * cZ, -cZ * sX,
-cZ*sY, cY*sX + cX*sY*sZ, cX*cY - sX*sY*sZ)
-cZ * sY, cY * sX + cX * sY * sZ, cX * cY - sX * sY * sZ
)
EulerOrder.ZXY -> Matrix3(
cY * cZ - sX * sY * sZ, -cX * sZ, cZ * sY + cY * sX * sZ,
cZ * sX * sY + cY * sZ, cX * cZ, sY * sZ - cY * cZ * sX,
-cX*sY, sX, cX*cY)
-cX * sY, sX, cX * cY
)
EulerOrder.ZYX -> Matrix3(
cY * cZ, cZ * sX * sY - cX * sZ, cX * cZ * sY + sX * sZ,
cY * sZ, cX * cZ + sX * sY * sZ, cX * sY * sZ - cZ * sX,
-sY, cY*sX, cX*cY)
-sY, cY * sX, cX * cY
)
EulerOrder.YXZ -> Matrix3(
cY * cZ + sX * sY * sZ, cZ * sX * sY - cY * sZ, cX * sY,
cX * sZ, cX * cZ, -sX,
cY*sX*sZ - cZ*sY, cY*cZ*sX + sY*sZ, cX*cY)
cY * sX * sZ - cZ * sY, cY * cZ * sX + sY * sZ, cX * cY
)
EulerOrder.XZY -> Matrix3(
cY * cZ, -sZ, cZ * sY,
sX * sY + cX * cY * sZ, cX * cZ, cX * sY * sZ - cY * sX,
cY*sX*sZ - cX*sY, cZ*sX, cX*cY + sX*sY*sZ)
cY * sX * sZ - cX * sY, cZ * sX, cX * cY + sX * sY * sZ
)
}
}
}

32
server/src/main/java/io/github/axisangles/ktmath/Main.kt
View File

@@ -1,7 +1,6 @@
package io.github.axisangles.ktmath
import kotlin.math.*
import kotlin.system.measureTimeMillis
var randSeed = 0
fun randInt(): Int {
@@ -122,49 +121,58 @@ fun testQuaternionInv() {
for (i in 1..1000) {
val Q = randQuaternion()
if (relError(Q*Q.inv(), Quaternion.ONE) > 1e-6f)
if (relError(Q * Q.inv(), Quaternion.ONE) > 1e-6f) {
throw Exception("Quaternion inv accuracy test failed")
}
}
}
fun testQuaternionDiv() {
for (i in 1..1000) {
val Q = randQuaternion()
if (!checkError(1e-6f, Q/Q, Quaternion.ONE))
if (!checkError(1e-6f, Q/Q, Quaternion.ONE)) {
throw Exception("Quaternion div accuracy test failed")
if (!checkError(1e-6f, 2f/Q, 2f*Q.inv()))
}
if (!checkError(1e-6f, 2f/Q, 2f*Q.inv())) {
throw Exception("Float/Quaternion accuracy test failed")
if (!checkError(1e-6f, Q/2f, 0.5f*Q))
}
if (!checkError(1e-6f, Q/2f, 0.5f*Q)) {
throw Exception("Quaternion/Float accuracy test failed")
}
}
}
// 19 binary digits of accuracy
fun testQuaternionPow() {
for (i in 1..1000) {
val Q = randQuaternion()
if (!checkError(2e-6f, Q.pow(-1f), Q.inv()))
if (!checkError(2e-6f, Q.pow(-1f), Q.inv())) {
throw Exception("Quaternion pow -1 accuracy test failed")
if (!checkError(2e-6f, Q.pow(0f), Quaternion.ONE))
}
if (!checkError(2e-6f, Q.pow(0f), Quaternion.ONE)) {
throw Exception("Quaternion pow 0 accuracy test failed")
if (!checkError(2e-6f, Q.pow(1f), Q))
}
if (!checkError(2e-6f, Q.pow(1f), Q)) {
throw Exception("Quaternion pow 1 accuracy test failed")
if (!checkError(2e-6f, Q.pow(2f), Q*Q))
}
if (!checkError(2e-6f, Q.pow(2f), Q*Q)) {
throw Exception("Quaternion pow 2 accuracy test failed")
}
}
}
fun testQuaternionSandwich() {
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
if (!checkError(5e-7f, Q.toMatrix()*v, Q.sandwich(v)))
if (!checkError(5e-7f, Q.toMatrix()*v, Q.sandwich(v))) {
throw Exception("Quaternion sandwich accuracy test failed")
}
}
}
// projection and alignment are expected to be less accurate in some extreme cases
// so we expect to see some cases in which half the bits are lost
@@ -230,7 +238,6 @@ fun testEulerConversions(order: EulerOrder, exception: String) {
}
}
fun main() {
val X90 = Matrix3(
1f, 0f, 0f,
@@ -289,8 +296,6 @@ fun main() {
testEulerSingularity(EulerOrder.YXZ, X90, "toEulerAnglesYXZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.XZY, Z90, "toEulerAnglesXZY singularity accuracy test failed")
// speed test a linear (align) method against some standard math functions
// var x = Quaternion(1f, 2f, 3f, 4f)
//
@@ -352,7 +357,6 @@ fun main() {
// println(dtAtan2Total) // 610
// println(dtAsinTotal) // 3558
// var x = Quaternion(2f, 1f, 4f, 3f)
// val dtPow = measureTimeMillis {
// for (i in 1..10_000_000) {

87
server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt
View File

@@ -1,12 +1,19 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.*
data class Matrix3(
val xx: Float, val yx: Float, val zx: Float,
val xy: Float, val yy: Float, val zy: Float,
val xz: Float, val yz: Float, val zz: Float
val xx: Float,
val yx: Float,
val zx: Float,
val xy: Float,
val yy: Float,
val zy: Float,
val xz: Float,
val yz: Float,
val zz: Float
) {
companion object {
val ZERO = Matrix3(0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f)
@@ -19,7 +26,8 @@ data class Matrix3 (
constructor(x: Vector3, y: Vector3, z: Vector3) : this(
x.x, y.x, z.x,
x.y, y.y, z.y,
x.z, y.z, z.z)
x.z, y.z, z.z
)
// column getters
val x get() = Vector3(xx, xy, xz)
@@ -34,27 +42,32 @@ data class Matrix3 (
operator fun unaryMinus(): Matrix3 = Matrix3(
-xx, -yx, -zx,
-xy, -yy, -zy,
-xz, -yz, -zz)
-xz, -yz, -zz
)
operator fun plus(that: Matrix3): Matrix3 = Matrix3(
this.xx + that.xx, this.yx + that.yx, this.zx + that.zx,
this.xy + that.xy, this.yy + that.yy, this.zy + that.zy,
this.xz + that.xz, this.yz + that.yz, this.zz + that.zz)
this.xz + that.xz, this.yz + that.yz, this.zz + that.zz
)
operator fun minus(that: Matrix3): Matrix3 = Matrix3(
this.xx - that.xx, this.yx - that.yx, this.zx - that.zx,
this.xy - that.xy, this.yy - that.yy, this.zy - that.zy,
this.xz - that.xz, this.yz - that.yz, this.zz - that.zz)
this.xz - that.xz, this.yz - that.yz, this.zz - that.zz
)
operator fun times(that: Float): Matrix3 = Matrix3(
this.xx * that, this.yx * that, this.zx * that,
this.xy * that, this.yy * that, this.zy * that,
this.xz*that, this.yz*that, this.zz*that)
this.xz * that, this.yz * that, this.zz * that
)
operator fun times(that: Vector3): Vector3 = Vector3(
this.xx * that.x + this.yx * that.y + this.zx * that.z,
this.xy * that.x + this.yy * that.y + this.zy * that.z,
this.xz*that.x + this.yz*that.y + this.zz*that.z)
this.xz * that.x + this.yz * that.y + this.zz * that.z
)
operator fun times(that: Matrix3): Matrix3 = Matrix3(
this.xx * that.xx + this.yx * that.xy + this.zx * that.xz,
@@ -65,7 +78,8 @@ data class Matrix3 (
this.xy * that.zx + this.yy * that.zy + this.zy * that.zz,
this.xz * that.xx + this.yz * that.xy + this.zz * that.xz,
this.xz * that.yx + this.yz * that.yy + this.zz * that.yz,
this.xz*that.zx + this.yz*that.zy + this.zz*that.zz)
this.xz * that.zx + this.yz * that.zy + this.zz * that.zz
)
/**
* computes the square of the frobenius norm of this matrix
@@ -98,7 +112,8 @@ data class Matrix3 (
fun transpose(): Matrix3 = Matrix3(
xx, xy, xz,
yx, yy, yz,
zx, zy, zz)
zx, zy, zz
)
/**
* computes the inverse of this matrix
@@ -109,7 +124,8 @@ data class Matrix3 (
return Matrix3(
(yy * zz - yz * zy) / det, (yz * zx - yx * zz) / det, (yx * zy - yy * zx) / det,
(xz * zy - xy * zz) / det, (xx * zz - xz * zx) / det, (xy * zx - xx * zy) / det,
(xy*yz - xz*yy)/det, (xz*yx - xx*yz)/det, (xx*yy - xy*yx)/det)
(xy * yz - xz * yy) / det, (xz * yx - xx * yz) / det, (xx * yy - xy * yx) / det
)
}
operator fun div(that: Float): Matrix3 = this * (1f / that)
@@ -128,7 +144,8 @@ data class Matrix3 (
return Matrix3(
(yy * zz - yz * zy) / det, (xz * zy - xy * zz) / det, (xy * yz - xz * yy) / det,
(yz * zx - yx * zz) / det, (xx * zz - xz * zx) / det, (xz * yx - xx * yz) / det,
(yx*zy - yy*zx)/det, (xy*zx - xx*zy)/det, (xx*yy - xy*yx)/det)
(yx * zy - yy * zx) / det, (xy * zx - xx * zy) / det, (xx * yy - xy * yx) / det
)
}
/**
@@ -183,8 +200,9 @@ data class Matrix3 (
* @return the quaternion
*/
fun toQuaternionAssumingOrthonormal(): Quaternion {
if (this.det() <= 0f)
if (this.det() <= 0f) {
throw Exception("Attempt to convert negative determinant matrix to quaternion")
}
return if (yy > -zz && zz > -xx && xx > -yy) {
Quaternion(1 + xx + yy + zz, yz - zy, zx - xz, xy - yx).unit()
@@ -204,7 +222,6 @@ data class Matrix3 (
*/
fun toQuaternion(): Quaternion = orthonormalize().toQuaternionAssumingOrthonormal()
/*
the standard algorithm:
@@ -271,7 +288,6 @@ data class Matrix3 (
built into the prerequisites for this function
*/
// fun toEulerAnglesXYZFaulty(): EulerAngles {
// return if (abs(zx) < 0.9999999f)
// EulerAngles(EulerOrder.XYZ,
@@ -290,8 +306,9 @@ data class Matrix3 (
* @return the eulerAngles
*/
fun toEulerAnglesAssumingOrthonormal(order: EulerOrder): EulerAngles {
if (this.det() <= 0f)
if (this.det() <= 0f) {
throw Exception("Attempt to convert negative determinant matrix to euler angles")
}
val ETA = 1.57079632f
when (order) {
@@ -299,55 +316,67 @@ data class Matrix3 (
val kc = zy * zy + zz * zz
if (kc == 0f) return EulerAngles(EulerOrder.XYZ, atan2(yz, yy), ETA.withSign(zx), 0f)
return EulerAngles(EulerOrder.XYZ,
return EulerAngles(
EulerOrder.XYZ,
atan2(-zy, zz),
atan2(zx, sqrt(kc)),
atan2(xy*zz - xz*zy, yy*zz - yz*zy))
atan2(xy * zz - xz * zy, yy * zz - yz * zy)
)
}
EulerOrder.YZX -> {
val kc = xx * xx + xz * xz
if (kc == 0f) return EulerAngles(EulerOrder.YZX, 0f, atan2(zx, zz), ETA.withSign(xy))
return EulerAngles(EulerOrder.YZX,
return EulerAngles(
EulerOrder.YZX,
atan2(xx * yz - xz * yx, xx * zz - xz * zx),
atan2(-xz, xx),
atan2( xy, sqrt(kc)))
atan2(xy, sqrt(kc))
)
}
EulerOrder.ZXY -> {
val kc = yy * yy + yx * yx
if (kc == 0f) return EulerAngles(EulerOrder.ZXY, ETA.withSign(yz), 0f, atan2(xy, xx))
return EulerAngles(EulerOrder.ZXY,
return EulerAngles(
EulerOrder.ZXY,
atan2(yz, sqrt(kc)),
atan2(yy * zx - yx * zy, yy * xx - yx * xy),
atan2( -yx, yy))
atan2(-yx, yy)
)
}
EulerOrder.ZYX -> {
val kc = xy * xy + xx * xx
if (kc == 0f) return EulerAngles(EulerOrder.ZYX, 0f, ETA.withSign(-xz), atan2(-yx, yy))
return EulerAngles(EulerOrder.ZYX,
return EulerAngles(
EulerOrder.ZYX,
atan2(zx * xy - zy * xx, yy * xx - yx * xy),
atan2(-xz, sqrt(kc)),
atan2( xy, xx))
atan2(xy, xx)
)
}
EulerOrder.YXZ -> {
val kc = zx * zx + zz * zz
if (kc == 0f) return EulerAngles(EulerOrder.YXZ, ETA.withSign(-zy), atan2(-xz, xx), 0f)
return EulerAngles(EulerOrder.YXZ,
return EulerAngles(
EulerOrder.YXZ,
atan2(-zy, sqrt(kc)),
atan2(zx, zz),
atan2(yz*zx - yx*zz, xx*zz - xz*zx))
atan2(yz * zx - yx * zz, xx * zz - xz * zx)
)
}
EulerOrder.XZY -> {
val kc = yz * yz + yy * yy
if (kc == 0f) return EulerAngles(EulerOrder.XZY, atan2(-zy, zz), 0f, ETA.withSign(-yx))
return EulerAngles(EulerOrder.XZY,
return EulerAngles(
EulerOrder.XZY,
atan2(yz, yy),
atan2(xy * yz - xz * yy, zz * yy - zy * yz),
atan2( -yx, sqrt(kc)))
atan2(-yx, sqrt(kc))
)
}
else -> {
throw Exception("EulerAngles not implemented for given EulerOrder")

1
server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt
View File

@@ -1,4 +1,5 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.*

2
server/src/main/java/io/github/axisangles/ktmath/Vector3.kt
View File

@@ -1,4 +1,5 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.atan2
@@ -46,6 +47,7 @@ data class Vector3(val x: Float, val y: Float, val z: Float) {
this.z * that.x - this.x * that.z,
this.x * that.y - this.y * that.x
)
/**
* computes the square of the length of this vector
* @return the length squared

157
server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt Normal file
View File

@@ -0,0 +1,157 @@
package io.github.axisangles.ktmath
import kotlin.math.*
import kotlin.test.Test
import kotlin.test.assertTrue
class QuaternionTest {
@Test
fun plus() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(6f, 8f, 10f, 12f)
assertEquals(q3, q1 + q2)
}
@Test
fun times() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(-60f, 12f, 30f, 24f)
assertEquals(q3, q1 * q2)
}
@Test
fun timesScalarRhs() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(2f, 4f, 6f, 8f)
assertEquals(q2, q1 * 2f)
}
@Test
fun timesScalarLhs() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(2f, 4f, 6f, 8f)
assertEquals(q2, 2f * q1)
}
@Test
fun inverse() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(1f / 30f, -2f / 30f, -3f / 30f, -4f / 30f)
assertEquals(q2, q1.inv())
}
@Test
fun rightDiv() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(-60f, 12f, 30f, 24f)
assertEquals(q1, q3 / q2)
}
@Test
fun rightDivFloatRhs() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(2f, 4f, 6f, 8f)
assertEquals(q1, q2 / 2f)
}
@Test
fun rightDivFloatLhs() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(1f / 15f, -2f / 15f, -1f / 5f, -4f / 15f)
assertEquals(q2, 2f / q1)
}
@Test
fun pow() {
val q = Quaternion(1f, 2f, 3f, 4f)
assertEquals(q.pow(1f), q, 1e-5)
assertEquals(q.pow(2f), q * q, 1e-5)
assertEquals(q.pow(0f), Quaternion.ONE, 1e-5)
assertEquals(q.pow(-1f), q.inv(), 1e-5)
}
@Test
fun interp() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(2.405691f, 3.5124686f, 4.619246f, 5.7260237f)
assertEquals(q1.interp(q2, 0.5f), q3, 1e-7)
}
@Test
fun interpR() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = -Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(2.405691f, 3.5124686f, 4.619246f, 5.7260237f)
assertEquals(q1.interpR(q2, 0.5f), q3, 1e-7)
}
@Test
fun lerp() {
val q1 = Quaternion(1f, 2f, 3f, 4f)
val q2 = Quaternion(5f, 6f, 7f, 8f)
val q3 = Quaternion(3f, 4f, 5f, 6f)
assertEquals(q1.lerp(q2, 0.5f), q3, 1e-7)
}
companion object {
private const val RELATIVE_TOLERANCE = 0.0
internal fun assertEquals(
expected: Quaternion,
actual: Quaternion,
tolerance: Double = RELATIVE_TOLERANCE
) {
val len = (actual - expected).lenSq()
val squareSum = expected.lenSq() + actual.lenSq()
assertTrue(
len <= tolerance * tolerance * squareSum,
"Expected: $expected but got: $actual"
)
}
}
}
var randSeed = 0
fun randInt(): Int {
randSeed = (1103515245 * randSeed + 12345).mod(2147483648).toInt()
return randSeed
}
fun randFloat(): Float {
return randInt().toFloat() / 2147483648
}
fun randGaussian(): Float {
var thing = 1f - randFloat()
while (thing == 0f) {
// no 0s allowed
thing = 1f - randFloat()
}
return sqrt(-2f * ln(thing)) * cos(PI.toFloat() * randFloat())
}
fun randMatrix(): Matrix3 {
return Matrix3(
randGaussian(), randGaussian(), randGaussian(),
randGaussian(), randGaussian(), randGaussian(),
randGaussian(), randGaussian(), randGaussian()
)
}
fun randQuaternion(): Quaternion {
return Quaternion(randGaussian(), randGaussian(), randGaussian(), randGaussian())
}
fun randRotMatrix(): Matrix3 {
return randQuaternion().toMatrix()
}
fun randVector(): Vector3 {
return Vector3(randGaussian(), randGaussian(), randGaussian())
}