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 {

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

@@ -1,10 +1,11 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.cos
import kotlin.math.sin
enum class EulerOrder {XYZ, YZX, ZXY, ZYX, YXZ, XZY}
enum class EulerOrder { XYZ, YZX, ZXY, ZYX, YXZ, XZY }
// prefer Y.toX
// but if ambiguous, use X.fromY
@@ -17,49 +18,55 @@ data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z:
* @return the quaternion
*/
fun toQuaternion(): Quaternion {
val cX = cos(x/2f)
val cY = cos(y/2f)
val cZ = cos(z/2f)
val sX = sin(x/2f)
val sY = sin(y/2f)
val sZ = sin(z/2f)
val cX = cos(x / 2f)
val cY = cos(y / 2f)
val cZ = cos(z / 2f)
val sX = sin(x / 2f)
val sY = sin(y / 2f)
val sZ = sin(z / 2f)
return when (order) {
EulerOrder.XYZ -> 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)
cX * cY * cZ - sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY - cY * sX * 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 * cZ - sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY + cY * sX * sZ,
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)
cX * cY * cZ - sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY + cY * sX * 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 * cZ + sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY + cY * sX * sZ,
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 * cZ + sX * sY * sZ,
cY * cZ * sX + cX * sY * sZ,
cX * cZ * sY - cY * sX * sZ,
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)
cX * cY * cZ + sX * sY * sZ,
cY * cZ * sX - cX * sY * sZ,
cX * cZ * sY - cY * sX * sZ,
cZ * sX * sY + cX * cY * sZ
)
}
}
// temp, replace with direct conversion later
//fun toMatrix(): Matrix3 = this.toQuaternion().toMatrix()
// fun toMatrix(): Matrix3 = this.toQuaternion().toMatrix()
/**
* creates a matrix which represents the same rotation as this eulerAngles
* @return the matrix
@@ -74,29 +81,35 @@ data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z:
return when (order) {
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)
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
)
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)
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
)
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)
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
)
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)
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
)
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 * 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
)
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 * 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
)
}
}
}

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

@@ -1,16 +1,15 @@
package io.github.axisangles.ktmath
import kotlin.math.*
import kotlin.system.measureTimeMillis
var randSeed = 0
fun randInt(): Int {
randSeed = (1103515245*randSeed + 12345).mod(2147483648).toInt()
randSeed = (1103515245 * randSeed + 12345).mod(2147483648).toInt()
return randSeed
}
fun randFloat(): Float {
return randInt().toFloat()/2147483648f
return randInt().toFloat() / 2147483648f
}
fun randGaussian(): Float {
@@ -19,7 +18,7 @@ fun randGaussian(): Float {
// no 0s allowed
thing = 1f - randFloat()
}
return sqrt(-2f*ln(thing))*cos(PI.toFloat()*randFloat())
return sqrt(-2f * ln(thing)) * cos(PI.toFloat() * randFloat())
}
fun randMatrix(): Matrix3 {
@@ -73,7 +72,7 @@ fun testMatrixOrthonormalize() {
fun testQuatMatrixConversion() {
for (i in 1..1000) {
val M = randRotMatrix()
val N = (randGaussian()*M.toQuaternion()).toMatrix()
val N = (randGaussian() * M.toQuaternion()).toMatrix()
if ((N - M).norm() > 1e-6) {
println("norm error: " + (N - M).norm().toString())
throw Exception("Quaternion Matrix conversion accuracy test failed")
@@ -82,62 +81,66 @@ fun testQuatMatrixConversion() {
}
fun relError(a: Matrix3, b: Matrix3): Float {
val combinedLen = sqrt((a.normSq() + b.normSq())/2f)
val combinedLen = sqrt((a.normSq() + b.normSq()) / 2f)
if (combinedLen == 0f) return 0f
return (b - a).norm()/combinedLen
return (b - a).norm() / combinedLen
}
fun relError(a: Vector3, b: Vector3): Float {
val combinedLen = sqrt((a.lenSq() + b.lenSq())/2f)
val combinedLen = sqrt((a.lenSq() + b.lenSq()) / 2f)
if (combinedLen == 0f) return 0f
return (b - a).len()/combinedLen
return (b - a).len() / combinedLen
}
fun relError(a: Quaternion, b: Quaternion): Float {
val combinedLen = sqrt((a.lenSq() + b.lenSq())/2f)
val combinedLen = sqrt((a.lenSq() + b.lenSq()) / 2f)
if (combinedLen == 0f) return 0f
return (b - a).len()/combinedLen
return (b - a).len() / combinedLen
}
fun checkError(eta: Float, a: Matrix3, b: Matrix3): Boolean {
return (b - a).normSq() <= eta*eta*(a.normSq() + b.normSq())
return (b - a).normSq() <= eta * eta * (a.normSq() + b.normSq())
}
fun checkError(eta: Float, a: Quaternion, b: Quaternion): Boolean {
return (b - a).lenSq() <= eta*eta*(a.lenSq() + b.lenSq())
return (b - a).lenSq() <= eta * eta * (a.lenSq() + b.lenSq())
}
fun checkError(eta: Float, a: Vector3, b: Vector3): Boolean {
return (b - a).lenSq() <= eta*eta*(a.lenSq() + b.lenSq())
return (b - a).lenSq() <= eta * eta * (a.lenSq() + b.lenSq())
}
fun checkError(eta: Float, A: Quaternion): Boolean {
return A.lenSq() <= eta*eta
return A.lenSq() <= eta * eta
}
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
@@ -145,15 +148,19 @@ 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() {
@@ -161,9 +168,10 @@ fun testQuaternionSandwich() {
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
@@ -208,12 +216,12 @@ fun testQuaternionEulerAngles(order: EulerOrder, exception: String) {
fun testEulerSingularity(order: EulerOrder, M: Matrix3, exception: String) {
for (i in 1..1000) {
val R = 1e-6f*randMatrix()
val R = 1e-6f * randMatrix()
val S = M + R
if (S.det() <= 0f) return
val error = (S.toEulerAnglesAssumingOrthonormal(order).toMatrix() - S).norm()
if (error > 2f*R.norm() + 1e-6f) {
if (error > 2f * R.norm() + 1e-6f) {
throw Exception(exception)
}
}
@@ -221,7 +229,7 @@ fun testEulerSingularity(order: EulerOrder, M: Matrix3, exception: String) {
fun testEulerConversions(order: EulerOrder, exception: String) {
for (i in 1..1000) {
val e = EulerAngles(order, 6.28318f*randFloat(), 6.28318f*randFloat(), 6.28318f*randFloat())
val e = EulerAngles(order, 6.28318f * randFloat(), 6.28318f * randFloat(), 6.28318f * randFloat())
val N = e.toMatrix()
val M = e.toQuaternion().toMatrix()
if ((N - M).norm() > 1e-6) {
@@ -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) {

175
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
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
) {
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,44 +42,50 @@ 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.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
)
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.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
)
operator fun times(that: Matrix3): Matrix3 = Matrix3(
this.xx*that.xx + this.yx*that.xy + this.zx*that.xz,
this.xx*that.yx + this.yx*that.yy + this.zx*that.yz,
this.xx*that.zx + this.yx*that.zy + this.zx*that.zz,
this.xy*that.xx + this.yy*that.xy + this.zy*that.xz,
this.xy*that.yx + this.yy*that.yy + this.zy*that.yz,
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.xx * that.xx + this.yx * that.xy + this.zx * that.xz,
this.xx * that.yx + this.yx * that.yy + this.zx * that.yz,
this.xx * that.zx + this.yx * that.zy + this.zx * that.zz,
this.xy * that.xx + this.yy * that.xy + this.zy * that.xz,
this.xy * that.yx + this.yy * that.yy + this.zy * that.yz,
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
)
/**
* computes the square of the frobenius norm of this matrix
* @return the frobenius norm squared
*/
fun normSq(): Float = xx*xx + yx*yx + zx*zx + xy*xy + yy*yy + zy*zy + xz*xz + yz*yz + zz*zz
fun normSq(): Float = xx * xx + yx * yx + zx * zx + xy * xy + yy * yy + zy * zy + xz * xz + yz * yz + zz * zz
/**
* computes the frobenius norm of this matrix
@@ -83,7 +97,7 @@ data class Matrix3 (
* computes the determinant of this matrix
* @return the determinant
*/
fun det(): Float = (xz*yx - xx*yz)*zy + (xx*yy - xy*yx)*zz + (xy*yz - xz*yy)*zx
fun det(): Float = (xz * yx - xx * yz) * zy + (xx * yy - xy * yx) * zz + (xy * yz - xz * yy) * zx
/**
* computes the trace 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
@@ -107,17 +122,18 @@ data class Matrix3 (
fun inv(): Matrix3 {
val det = det()
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)
(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
)
}
operator fun div(that: Float): Matrix3 = this*(1f/that)
operator fun div(that: Float): Matrix3 = this * (1f / that)
/**
* computes the right division, this * that^-1
*/
operator fun div(that: Matrix3): Matrix3 = this*that.inv()
operator fun div(that: Matrix3): Matrix3 = this * that.inv()
/**
* computes the inverse transpose of this matrix
@@ -126,9 +142,10 @@ data class Matrix3 (
fun invTranspose(): Matrix3 {
val det = det()
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)
(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
)
}
/**
@@ -140,7 +157,7 @@ data class Matrix3 (
var curDet = Float.POSITIVE_INFINITY
for (i in 1..100) {
val newMat = (curMat + curMat.invTranspose())/2f
val newMat = (curMat + curMat.invTranspose()) / 2f
val newDet = abs(newMat.det())
// should almost always exit immediately
if (newDet >= curDet) return curMat
@@ -166,7 +183,7 @@ data class Matrix3 (
count += 1f
sum += it
}
return (sum/count).orthonormalize()
return (sum / count).orthonormalize()
}
/**
@@ -175,7 +192,7 @@ data class Matrix3 (
* @param t the amount by which to interpolate
* @return the interpolated matrix
*/
fun lerp(that: Matrix3, t: Float): Matrix3 = (1f - t)*this + t*that
fun lerp(that: Matrix3, t: Float): Matrix3 = (1f - t) * this + t * that
// assumes this matrix is orthonormal and converts this to a quaternion
/**
@@ -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,64 +306,77 @@ 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) {
EulerOrder.XYZ -> {
val kc = zy*zy + zz*zz
val kc = zy * zy + zz * zz
if (kc == 0f) return EulerAngles(EulerOrder.XYZ, atan2(yz, yy), ETA.withSign(zx), 0f)
return EulerAngles(EulerOrder.XYZ,
atan2( -zy, zz),
atan2( zx, sqrt(kc)),
atan2(xy*zz - xz*zy, yy*zz - yz*zy))
return EulerAngles(
EulerOrder.XYZ,
atan2(-zy, zz),
atan2(zx, sqrt(kc)),
atan2(xy * zz - xz * zy, yy * zz - yz * zy)
)
}
EulerOrder.YZX -> {
val kc = xx*xx + xz*xz
val kc = xx * xx + xz * xz
if (kc == 0f) return EulerAngles(EulerOrder.YZX, 0f, atan2(zx, zz), ETA.withSign(xy))
return EulerAngles(EulerOrder.YZX,
atan2(xx*yz - xz*yx, xx*zz - xz*zx),
atan2( -xz, xx),
atan2( xy, sqrt(kc)))
return EulerAngles(
EulerOrder.YZX,
atan2(xx * yz - xz * yx, xx * zz - xz * zx),
atan2(-xz, xx),
atan2(xy, sqrt(kc))
)
}
EulerOrder.ZXY -> {
val kc = yy*yy + yx*yx
val kc = yy * yy + yx * yx
if (kc == 0f) return EulerAngles(EulerOrder.ZXY, ETA.withSign(yz), 0f, atan2(xy, xx))
return EulerAngles(EulerOrder.ZXY,
atan2( yz, sqrt(kc)),
atan2(yy*zx - yx*zy, yy*xx - yx*xy),
atan2( -yx, yy))
return EulerAngles(
EulerOrder.ZXY,
atan2(yz, sqrt(kc)),
atan2(yy * zx - yx * zy, yy * xx - yx * xy),
atan2(-yx, yy)
)
}
EulerOrder.ZYX -> {
val kc = xy*xy + xx*xx
val kc = xy * xy + xx * xx
if (kc == 0f) return EulerAngles(EulerOrder.ZYX, 0f, ETA.withSign(-xz), atan2(-yx, yy))
return EulerAngles(EulerOrder.ZYX,
atan2(zx*xy - zy*xx, yy*xx - yx*xy),
atan2( -xz, sqrt(kc)),
atan2( xy, xx))
return EulerAngles(
EulerOrder.ZYX,
atan2(zx * xy - zy * xx, yy * xx - yx * xy),
atan2(-xz, sqrt(kc)),
atan2(xy, xx)
)
}
EulerOrder.YXZ -> {
val kc = zx*zx + zz*zz
val kc = zx * zx + zz * zz
if (kc == 0f) return EulerAngles(EulerOrder.YXZ, ETA.withSign(-zy), atan2(-xz, xx), 0f)
return EulerAngles(EulerOrder.YXZ,
atan2( -zy, sqrt(kc)),
atan2( zx, zz),
atan2(yz*zx - yx*zz, xx*zz - xz*zx))
return EulerAngles(
EulerOrder.YXZ,
atan2(-zy, sqrt(kc)),
atan2(zx, zz),
atan2(yz * zx - yx * zz, xx * zz - xz * zx)
)
}
EulerOrder.XZY -> {
val kc = yz*yz + yy*yy
val kc = yz * yz + yy * yy
if (kc == 0f) return EulerAngles(EulerOrder.XZY, atan2(-zy, zz), 0f, ETA.withSign(-yx))
return EulerAngles(EulerOrder.XZY,
atan2( yz, yy),
atan2(xy*yz - xz*yy, zz*yy - zy*yz),
atan2( -yx, sqrt(kc)))
return EulerAngles(
EulerOrder.XZY,
atan2(yz, yy),
atan2(xy * yz - xz * yy, zz * yy - zy * yz),
atan2(-yx, sqrt(kc))
)
}
else -> {
throw Exception("EulerAngles not implemented for given EulerOrder")
@@ -363,6 +392,6 @@ data class Matrix3 (
fun toEulerAngles(order: EulerOrder): EulerAngles = orthonormalize().toEulerAnglesAssumingOrthonormal(order)
}
operator fun Float.times(that: Matrix3): Matrix3 = that*this
operator fun Float.times(that: Matrix3): Matrix3 = that * this
operator fun Float.div(that: Matrix3): Matrix3 = that.inv()*this
operator fun Float.div(that: Matrix3): Matrix3 = that.inv() * this

87
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.*
@@ -20,7 +21,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* creates a new quaternion representing the rotation about axis v by rotational angle of v's length
* @return the new quaternion
**/
fun fromRotationVector(v: Vector3): Quaternion = Quaternion(0f, v/2f).exp()
fun fromRotationVector(v: Vector3): Quaternion = Quaternion(0f, v / 2f).exp()
}
constructor(w: Float, xyz: Vector3) : this(w, xyz.x, xyz.y, xyz.z)
@@ -65,40 +66,40 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param that the quaternion with which to be dotted
* @return the inversed quaternion
**/
fun dot(that: Quaternion): Float = this.w*that.w + this.x*that.x + this.y*that.y + this.z*that.z
fun dot(that: Quaternion): Float = this.w * that.w + this.x * that.x + this.y * that.y + this.z * that.z
/**
* computes the square of the length of this quaternion
* @return the length squared
**/
fun lenSq(): Float = w*w + x*x + y*y + z*z
fun lenSq(): Float = w * w + x * x + y * y + z * z
/**
* computes the length of this quaternion
* @return the length
**/
fun len(): Float = sqrt(w*w + x*x + y*y + z*z)
fun len(): Float = sqrt(w * w + x * x + y * y + z * z)
/**
* @return the normalized quaternion
**/
fun unit(): Quaternion {
val m = len()
return if (m == 0f) ZERO else this/m
return if (m == 0f) ZERO else this / m
}
operator fun times(that: Float): Quaternion = Quaternion(
this.w*that,
this.x*that,
this.y*that,
this.z*that
this.w * that,
this.x * that,
this.y * that,
this.z * that
)
operator fun times(that: Quaternion): Quaternion = Quaternion(
this.w*that.w - this.x*that.x - this.y*that.y - this.z*that.z,
this.x*that.w + this.w*that.x - this.z*that.y + this.y*that.z,
this.y*that.w + this.z*that.x + this.w*that.y - this.x*that.z,
this.z*that.w - this.y*that.x + this.x*that.y + this.w*that.z
this.w * that.w - this.x * that.x - this.y * that.y - this.z * that.z,
this.x * that.w + this.w * that.x - this.z * that.y + this.y * that.z,
this.y * that.w + this.z * that.x + this.w * that.y - this.x * that.z,
this.z * that.w - this.y * that.x + this.x * that.y + this.w * that.z
)
/**
@@ -108,19 +109,19 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
fun inv(): Quaternion {
val lenSq = lenSq()
return Quaternion(
w/lenSq,
-x/lenSq,
-y/lenSq,
-z/lenSq
w / lenSq,
-x / lenSq,
-y / lenSq,
-z / lenSq
)
}
operator fun div(that: Float): Quaternion = this*(1f/that)
operator fun div(that: Float): Quaternion = this * (1f / that)
/**
* computes right division, this * that^-1
**/
operator fun div(that: Quaternion): Quaternion = this*that.inv()
operator fun div(that: Quaternion): Quaternion = this * that.inv()
/**
* @return the conjugate of this quaternion
@@ -137,11 +138,11 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
val len = len()
if (si == 0f) {
return Quaternion(ln(len), xyz/w)
return Quaternion(ln(len), xyz / w)
}
val ang = atan2(si, co)
return Quaternion(ln(len), ang/si*xyz)
return Quaternion(ln(len), ang / si * xyz)
}
/**
@@ -153,12 +154,12 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
val len = exp(w)
if (ang == 0f) {
return Quaternion(len, len*xyz)
return Quaternion(len, len * xyz)
}
val co = cos(ang)
val si = sin(ang)
return Quaternion(len*co, len*si/ang*xyz)
return Quaternion(len * co, len * si / ang * xyz)
}
/**
@@ -166,7 +167,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param t the power by which to raise this quaternion
* @return the powered quaternion
**/
fun pow(t: Float): Quaternion = (log()*t).exp()
fun pow(t: Float): Quaternion = (log() * t).exp()
// for a slight improvement in performance
// not fully implemented
@@ -214,9 +215,9 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
} else if (t == 1f) {
that
} else if (t < 0.5f) {
(that/this).pow(t)*this
(that / this).pow(t) * this
} else {
(this/that).pow(1f - t)*that
(this / that).pow(1f - t) * that
}
/**
@@ -233,7 +234,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun lerp(that: Quaternion, t: Float): Quaternion = (1f - t)*this + t*that
fun lerp(that: Quaternion, t: Float): Quaternion = (1f - t) * this + t * that
/**
* linearly interpolates from this quaternion to that quaternion by t in rotation space
@@ -253,21 +254,21 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* computes this quaternion's angle to identity in rotation space
* @return angle
**/
fun angleR(): Float = 2f*atan2(xyz.len(), abs(w))
fun angleR(): Float = 2f * atan2(xyz.len(), abs(w))
/**
* computes the angle between this quaternion and that quaternion in quaternion space
* @param that the other quaternion
* @return angle
**/
fun angleTo(that: Quaternion): Float = (this/that).angle()
fun angleTo(that: Quaternion): Float = (this / that).angle()
/**
* computes the angle between this quaternion and that quaternion in rotation space
* @param that the other quaternion
* @return angle
**/
fun angleToR(that: Quaternion): Float = (this/that).angleR()
fun angleToR(that: Quaternion): Float = (this / that).angleR()
/**
* computes the angle this quaternion rotates about the u axis in quaternion space
@@ -277,7 +278,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
fun angleAbout(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return atan2(uDotIm, uLen*w)
return atan2(uDotIm, uLen * w)
}
/**
@@ -289,9 +290,9 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return if (uDotIm < 0f) {
2f*atan2(-uDotIm, -uLen*w)
2f * atan2(-uDotIm, -uLen * w)
} else {
2f*atan2(uDotIm, uLen*w)
2f * atan2(uDotIm, uLen * w)
}
}
@@ -301,7 +302,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param v the global axis
* @return Q
**/
fun project(v: Vector3) = Quaternion(w, xyz.dot(v)/v.lenSq()*v)
fun project(v: Vector3) = Quaternion(w, xyz.dot(v) / v.lenSq() * v)
/**
* finds Q, the quaternion nearest to this quaternion representing a rotation NOT on the global u axis.
@@ -309,7 +310,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param v the global axis
* @return Q
**/
fun reject(v: Vector3) = Quaternion(w, v.cross(xyz).cross(v)/v.lenSq())
fun reject(v: Vector3) = Quaternion(w, v.cross(xyz).cross(v) / v.lenSq())
/**
* finds Q, the quaternion nearest to this quaternion whose local u direction aligns with the global v direction.
@@ -322,7 +323,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
val U = Quaternion(0f, u)
val V = Quaternion(0f, v)
return (V*this/U + (V/U).len()*this)/2f
return (V * this / U + (V / U).len() * this) / 2f
}
/**
@@ -330,7 +331,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* @param that the vector to be transformed
* @return that vector transformed by this quaternion
**/
fun sandwich(that: Vector3): Vector3 = (this*Quaternion(0f, that)/this).xyz
fun sandwich(that: Vector3): Vector3 = (this * Quaternion(0f, that) / this).xyz
/**
* computes this quaternion's rotation axis
@@ -342,7 +343,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
* computes the rotation vector representing this quaternion's rotation
* @return rotation vector
**/
fun toRotationVector(): Vector3 = 2f*log().xyz
fun toRotationVector(): Vector3 = 2f * log().xyz
/**
* computes the matrix representing this quaternion's rotation
@@ -351,9 +352,9 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
fun toMatrix(): Matrix3 {
val d = lenSq()
return Matrix3(
(w*w + x*x - y*y - z*z)/d, 2f*(x*y - w*z)/d, 2f*(w*y + x*z)/d,
2f*(x*y + w*z)/d, (w*w - x*x + y*y - z*z)/d, 2f*(y*z - w*x)/d,
2f*(x*z - w*y)/d, 2f*(w*x + y*z)/d, (w*w - x*x - y*y + z*z)/d
(w * w + x * x - y * y - z * z) / d, 2f * (x * y - w * z) / d, 2f * (w * y + x * z) / d,
2f * (x * y + w * z) / d, (w * w - x * x + y * y - z * z) / d, 2f * (y * z - w * x) / d,
2f * (x * z - w * y) / d, 2f * (w * x + y * z) / d, (w * w - x * x - y * y + z * z) / d
)
}
@@ -367,5 +368,5 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
operator fun Float.plus(that: Quaternion): Quaternion = that + this
operator fun Float.minus(that: Quaternion): Quaternion = -that + this
operator fun Float.times(that: Quaternion): Quaternion = that*this
operator fun Float.div(that: Quaternion): Quaternion = that.inv()*this
operator fun Float.times(that: Quaternion): Quaternion = that * this
operator fun Float.div(that: Quaternion): Quaternion = that.inv() * this

42
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
@@ -6,13 +7,13 @@ import kotlin.math.sqrt
data class Vector3(val x: Float, val y: Float, val z: Float) {
companion object {
val ZERO = Vector3( 0f, 0f, 0f)
val POS_X = Vector3( 1f, 0f, 0f)
val POS_Y = Vector3( 0f, 1f, 0f)
val POS_Z = Vector3( 0f, 0f, 1f)
val ZERO = Vector3(0f, 0f, 0f)
val POS_X = Vector3(1f, 0f, 0f)
val POS_Y = Vector3(0f, 1f, 0f)
val POS_Z = Vector3(0f, 0f, 1f)
val NEG_X = Vector3(-1f, 0f, 0f)
val NEG_Y = Vector3( 0f, -1f, 0f)
val NEG_Z = Vector3( 0f, 0f, -1f)
val NEG_Y = Vector3(0f, -1f, 0f)
val NEG_Z = Vector3(0f, 0f, -1f)
}
operator fun unaryMinus() = Vector3(-x, -y, -z)
@@ -34,7 +35,7 @@ data class Vector3(val x: Float, val y: Float, val z: Float) {
* @param that the vector with which to be dotted
* @return the dot product
**/
fun dot(that: Vector3) = this.x*that.x + this.y*that.y + this.z*that.z
fun dot(that: Vector3) = this.x * that.x + this.y * that.y + this.z * that.z
/**
* computes the cross product of this vector with that vector
@@ -42,41 +43,42 @@ data class Vector3(val x: Float, val y: Float, val z: Float) {
* @return the cross product
**/
fun cross(that: Vector3) = Vector3(
this.y*that.z - this.z*that.y,
this.z*that.x - this.x*that.z,
this.x*that.y - this.y*that.x
this.y * that.z - this.z * that.y,
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
**/
fun lenSq() = x*x + y*y + z*z
fun lenSq() = x * x + y * y + z * z
/**
* computes the length of this quaternion
* @return the length
**/
fun len() = sqrt(x*x + y*y + z*z)
fun len() = sqrt(x * x + y * y + z * z)
/**
* @return the normalized vector
**/
fun unit(): Vector3 {
val m = len()
return if (m == 0f) ZERO else this/m
return if (m == 0f) ZERO else this / m
}
operator fun times(that: Float) = Vector3(
this.x*that,
this.y*that,
this.z*that
this.x * that,
this.y * that,
this.z * that
)
// computes division of this vector3 by a float
operator fun div(that: Float) = Vector3(
this.x/that,
this.y/that,
this.z/that
this.x / that,
this.y / that,
this.z / that
)
/**
@@ -87,4 +89,4 @@ data class Vector3(val x: Float, val y: Float, val z: Float) {
fun angleTo(that: Vector3): Float = atan2(this.cross(that).len(), this.dot(that))
}
operator fun Float.times(that: Vector3): Vector3 = that*this
operator fun Float.times(that: Vector3): Vector3 = that * this

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())
}