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 150 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 {

183
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
@@ -12,91 +13,103 @@ enum class EulerOrder {XYZ, YZX, ZXY, ZYX, YXZ, XZY}
* Euler Angles contains both the x y z angle parameters and the order of application
*/
data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z: Float) {
/**
* creates a quaternion which represents the same rotation as this eulerAngles
* @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)
/**
* creates a quaternion which represents the same rotation as this eulerAngles
* @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)
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)
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)
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)
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)
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)
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)
}
}
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
)
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
)
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
)
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
)
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
)
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
)
}
}
// temp, replace with direct conversion later
//fun toMatrix(): Matrix3 = this.toQuaternion().toMatrix()
/**
* creates a matrix which represents the same rotation as this eulerAngles
* @return the matrix
*/
fun toMatrix(): Matrix3 {
val cX = cos(x)
val cY = cos(y)
val cZ = cos(z)
val sX = sin(x)
val sY = sin(y)
val sZ = sin(z)
// temp, replace with direct conversion later
// fun toMatrix(): Matrix3 = this.toQuaternion().toMatrix()
/**
* creates a matrix which represents the same rotation as this eulerAngles
* @return the matrix
*/
fun toMatrix(): Matrix3 {
val cX = cos(x)
val cY = cos(y)
val cZ = cos(z)
val sX = sin(x)
val sY = sin(y)
val sZ = sin(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)
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)
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)
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)
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)
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)
}
}
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
)
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
)
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
)
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
)
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
)
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
)
}
}
}

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

@@ -1,297 +1,302 @@
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()
return randSeed
randSeed = (1103515245 * randSeed + 12345).mod(2147483648).toInt()
return randSeed
}
fun randFloat(): Float {
return randInt().toFloat()/2147483648f
return randInt().toFloat() / 2147483648f
}
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())
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()
)
return Matrix3(
randGaussian(), randGaussian(), randGaussian(),
randGaussian(), randGaussian(), randGaussian(),
randGaussian(), randGaussian(), randGaussian()
)
}
fun randQuaternion(): Quaternion {
return Quaternion(randGaussian(), randGaussian(), randGaussian(), randGaussian())
return Quaternion(randGaussian(), randGaussian(), randGaussian(), randGaussian())
}
fun randRotMatrix(): Matrix3 {
return randQuaternion().toMatrix()
return randQuaternion().toMatrix()
}
fun randVector(): Vector3 {
return Vector3(randGaussian(), randGaussian(), randGaussian())
return Vector3(randGaussian(), randGaussian(), randGaussian())
}
fun testEulerMatrix(order: EulerOrder, M: Matrix3, exception: String) {
// We convert to euler angles and back and see if they are reasonably similar
val N = M.toEulerAngles(order).toMatrix()
if ((N - M).norm() > 1e-6) {
println("norm error: " + (N - M).norm().toString())
throw Exception(exception)
}
// We convert to euler angles and back and see if they are reasonably similar
val N = M.toEulerAngles(order).toMatrix()
if ((N - M).norm() > 1e-6) {
println("norm error: " + (N - M).norm().toString())
throw Exception(exception)
}
}
fun testEulerConversion(order: EulerOrder, exception: String) {
for (i in 1..1000) {
testEulerMatrix(order, randRotMatrix(), exception)
}
for (i in 1..1000) {
testEulerMatrix(order, randRotMatrix(), exception)
}
}
fun testMatrixOrthonormalize() {
for (i in 1..1000) {
val M = randMatrix()
for (i in 1..1000) {
val M = randMatrix()
val N = M.invTranspose().orthonormalize()
val O = M.orthonormalize()
if ((N - O).norm() > 1e-5) {
println("norm error: " + (N - O).norm().toString())
throw Exception("Matrix orthonormalization accuracy test failed")
}
}
val N = M.invTranspose().orthonormalize()
val O = M.orthonormalize()
if ((N - O).norm() > 1e-5) {
println("norm error: " + (N - O).norm().toString())
throw Exception("Matrix orthonormalization accuracy test failed")
}
}
}
fun testQuatMatrixConversion() {
for (i in 1..1000) {
val M = randRotMatrix()
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")
}
}
for (i in 1..1000) {
val M = randRotMatrix()
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")
}
}
}
fun relError(a: Matrix3, b: Matrix3): Float {
val combinedLen = sqrt((a.normSq() + b.normSq())/2f)
if (combinedLen == 0f) return 0f
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)
if (combinedLen == 0f) return 0f
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)
if (combinedLen == 0f) return 0f
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()
for (i in 1..1000) {
val Q = randQuaternion()
if (relError(Q*Q.inv(), Quaternion.ONE) > 1e-6f)
throw Exception("Quaternion inv accuracy test failed")
}
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()
for (i in 1..1000) {
val Q = randQuaternion()
if (!checkError(1e-6f, Q/Q, Quaternion.ONE))
throw Exception("Quaternion div accuracy test failed")
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))
throw Exception("Quaternion/Float accuracy test failed")
}
if (!checkError(1e-6f, Q/Q, Quaternion.ONE)) {
throw Exception("Quaternion div accuracy test failed")
}
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)) {
throw Exception("Quaternion/Float accuracy test failed")
}
}
}
// 19 binary digits of accuracy
fun testQuaternionPow() {
for (i in 1..1000) {
val Q = randQuaternion()
for (i in 1..1000) {
val Q = randQuaternion()
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))
throw Exception("Quaternion pow 0 accuracy test failed")
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))
throw Exception("Quaternion pow 2 accuracy test failed")
}
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)) {
throw Exception("Quaternion pow 0 accuracy test failed")
}
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)) {
throw Exception("Quaternion pow 2 accuracy test failed")
}
}
}
fun testQuaternionSandwich() {
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
if (!checkError(5e-7f, Q.toMatrix()*v, Q.sandwich(v)))
throw Exception("Quaternion sandwich accuracy test failed")
}
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
fun testQuaternionProjectAlign() {
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
if (!checkError(1e-4f, Q.align(v, v), Q.project(v))) {
println(Q.align(v, v) - Q.project(v))
println(Q.align(v, v))
println(Q.project(v))
println(Q)
throw Exception("Quaternion project/align accuracy test failed")
}
}
if (!checkError(1e-4f, Q.align(v, v), Q.project(v))) {
println(Q.align(v, v) - Q.project(v))
println(Q.align(v, v))
println(Q.project(v))
println(Q)
throw Exception("Quaternion project/align accuracy test failed")
}
}
}
fun testQuaternionRotationVector() {
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Quaternion.fromRotationVector(Q.toRotationVector())
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Quaternion.fromRotationVector(Q.toRotationVector())
if (!checkError(5e-7f, Q, P)) {
throw Exception("Quaternion toRotationVector fromRotationVector accuracy test failed")
}
}
if (!checkError(5e-7f, Q, P)) {
throw Exception("Quaternion toRotationVector fromRotationVector accuracy test failed")
}
}
}
fun testQuaternionEulerAngles(order: EulerOrder, exception: String) {
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Q.toEulerAngles(order).toQuaternion().twinNearest(Q)
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Q.toEulerAngles(order).toQuaternion().twinNearest(Q)
if (!checkError(2e-7f, Q, P)) {
println(relError(Q, P))
throw Exception(exception)
}
}
if (!checkError(2e-7f, Q, P)) {
println(relError(Q, P))
throw Exception(exception)
}
}
}
fun testEulerSingularity(order: EulerOrder, M: Matrix3, exception: String) {
for (i in 1..1000) {
val R = 1e-6f*randMatrix()
val S = M + R
if (S.det() <= 0f) return
for (i in 1..1000) {
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) {
throw Exception(exception)
}
}
val error = (S.toEulerAnglesAssumingOrthonormal(order).toMatrix() - S).norm()
if (error > 2f * R.norm() + 1e-6f) {
throw Exception(exception)
}
}
}
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 N = e.toMatrix()
val M = e.toQuaternion().toMatrix()
if ((N - M).norm() > 1e-6) {
throw Exception(exception)
}
}
for (i in 1..1000) {
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) {
throw Exception(exception)
}
}
}
fun main() {
val X90 = Matrix3(
1f, 0f, 0f,
0f, 0f, -1f,
0f, 1f, 0f
)
val Y90 = Matrix3(
0f, 0f, 1f,
0f, 1f, 0f,
-1f, 0f, 0f
)
val Z90 = Matrix3(
0f, -1f, 0f,
1f, 0f, 0f,
0f, 0f, 1f
)
val X90 = Matrix3(
1f, 0f, 0f,
0f, 0f, -1f,
0f, 1f, 0f
)
val Y90 = Matrix3(
0f, 0f, 1f,
0f, 1f, 0f,
-1f, 0f, 0f
)
val Z90 = Matrix3(
0f, -1f, 0f,
1f, 0f, 0f,
0f, 0f, 1f
)
testMatrixOrthonormalize()
testQuatMatrixConversion()
testQuaternionEulerAngles(EulerOrder.XYZ, "Quaternion EulerAnglesXYZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YZX, "Quaternion EulerAnglesYZX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZXY, "Quaternion EulerAnglesZXY accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZYX, "Quaternion EulerAnglesZYX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YXZ, "Quaternion EulerAnglesYXZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.XZY, "Quaternion EulerAnglesXZY accuracy test failed")
testMatrixOrthonormalize()
testQuatMatrixConversion()
testQuaternionEulerAngles(EulerOrder.XYZ, "Quaternion EulerAnglesXYZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YZX, "Quaternion EulerAnglesYZX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZXY, "Quaternion EulerAnglesZXY accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZYX, "Quaternion EulerAnglesZYX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YXZ, "Quaternion EulerAnglesYXZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.XZY, "Quaternion EulerAnglesXZY accuracy test failed")
testQuaternionInv()
testQuaternionDiv()
testQuaternionPow()
testQuaternionSandwich()
testQuaternionProjectAlign()
testQuaternionRotationVector()
testQuaternionInv()
testQuaternionDiv()
testQuaternionPow()
testQuaternionSandwich()
testQuaternionProjectAlign()
testQuaternionRotationVector()
println(Matrix3.IDENTITY.average(Y90))
println(Matrix3.IDENTITY.average(Y90))
testEulerConversions(EulerOrder.XYZ, "fromEulerAnglesXYZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YZX, "fromEulerAnglesYZX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZXY, "fromEulerAnglesZXY Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZYX, "fromEulerAnglesZYX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YXZ, "fromEulerAnglesYXZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.XZY, "fromEulerAnglesXZY Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.XYZ, "fromEulerAnglesXYZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YZX, "fromEulerAnglesYZX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZXY, "fromEulerAnglesZXY Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZYX, "fromEulerAnglesZYX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YXZ, "fromEulerAnglesYXZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.XZY, "fromEulerAnglesXZY Quaternion or Matrix3 accuracy test failed")
// EULER ANGLE TESTS
testEulerConversion(EulerOrder.XYZ, "toEulerAnglesXYZ accuracy test failed")
testEulerConversion(EulerOrder.YZX, "toEulerAnglesYZX accuracy test failed")
testEulerConversion(EulerOrder.ZXY, "toEulerAnglesZXY accuracy test failed")
testEulerConversion(EulerOrder.ZYX, "toEulerAnglesZYX accuracy test failed")
testEulerConversion(EulerOrder.YXZ, "toEulerAnglesYXZ accuracy test failed")
testEulerConversion(EulerOrder.XZY, "toEulerAnglesXZY accuracy test failed")
// EULER ANGLE TESTS
testEulerConversion(EulerOrder.XYZ, "toEulerAnglesXYZ accuracy test failed")
testEulerConversion(EulerOrder.YZX, "toEulerAnglesYZX accuracy test failed")
testEulerConversion(EulerOrder.ZXY, "toEulerAnglesZXY accuracy test failed")
testEulerConversion(EulerOrder.ZYX, "toEulerAnglesZYX accuracy test failed")
testEulerConversion(EulerOrder.YXZ, "toEulerAnglesYXZ accuracy test failed")
testEulerConversion(EulerOrder.XZY, "toEulerAnglesXZY accuracy test failed")
// test robustness to noise
testEulerSingularity(EulerOrder.XYZ, Y90, "toEulerAnglesXYZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.YZX, Z90, "toEulerAnglesYZX singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZXY, X90, "toEulerAnglesZXY singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZYX, Y90, "toEulerAnglesZYX singularity accuracy test failed")
testEulerSingularity(EulerOrder.YXZ, X90, "toEulerAnglesYXZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.XZY, Z90, "toEulerAnglesXZY singularity accuracy test failed")
// test robustness to noise
testEulerSingularity(EulerOrder.XYZ, Y90, "toEulerAnglesXYZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.YZX, Z90, "toEulerAnglesYZX singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZXY, X90, "toEulerAnglesZXY singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZYX, Y90, "toEulerAnglesZYX singularity accuracy test failed")
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
// speed test a linear (align) method against some standard math functions
// var x = Quaternion(1f, 2f, 3f, 4f)
//
// var dtAlignTotal: Long = 0
@@ -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) {
@@ -361,4 +365,4 @@ fun main() {
// }
//
// println(dtPow)
}
}

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

@@ -1,209 +1,226 @@
@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)
val IDENTITY = Matrix3(1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 1f)
}
companion object {
val ZERO = Matrix3(0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f)
val IDENTITY = Matrix3(1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 1f)
}
/**
* creates a new matrix from x y and z column vectors
*/
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)
/**
* creates a new matrix from x y and z column vectors
*/
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
)
// column getters
val x get() = Vector3(xx, xy, xz)
val y get() = Vector3(yx, yy, yz)
val z get() = Vector3(zx, zy, zz)
// column getters
val x get() = Vector3(xx, xy, xz)
val y get() = Vector3(yx, yy, yz)
val z get() = Vector3(zx, zy, zz)
// row getters
val xRow get() = Vector3(xx, yx, zx)
val yRow get() = Vector3(xy, yy, zy)
val zRow get() = Vector3(xz, yz, zz)
// row getters
val xRow get() = Vector3(xx, yx, zx)
val yRow get() = Vector3(xy, yy, zy)
val zRow get() = Vector3(xz, yz, zz)
operator fun unaryMinus(): Matrix3 = Matrix3(
-xx, -yx, -zx,
-xy, -yy, -zy,
-xz, -yz, -zz)
operator fun unaryMinus(): Matrix3 = Matrix3(
-xx, -yx, -zx,
-xy, -yy, -zy,
-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)
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
)
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)
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
)
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)
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
)
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)
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
)
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)
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
)
/**
* 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
/**
* 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
/**
* computes the frobenius norm of this matrix
* @return the frobenius norm
*/
fun norm(): Float = sqrt(normSq())
/**
* computes the frobenius norm of this matrix
* @return the frobenius norm
*/
fun norm(): Float = sqrt(normSq())
/**
* 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
/**
* 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
/**
* computes the trace of this matrix
* @return the trace
*/
fun trace(): Float = xx + yy + zz
/**
* computes the trace of this matrix
* @return the trace
*/
fun trace(): Float = xx + yy + zz
/**
* computes the transpose of this matrix
* @return the transpose matrix
*/
fun transpose(): Matrix3 = Matrix3(
xx, xy, xz,
yx, yy, yz,
zx, zy, zz)
/**
* computes the transpose of this matrix
* @return the transpose matrix
*/
fun transpose(): Matrix3 = Matrix3(
xx, xy, xz,
yx, yy, yz,
zx, zy, zz
)
/**
* computes the inverse of this matrix
* @return the inverse matrix
*/
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)
}
/**
* computes the inverse of this matrix
* @return the inverse matrix
*/
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
)
}
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()
/**
* computes the right division, this * that^-1
*/
operator fun div(that: Matrix3): Matrix3 = this * that.inv()
/**
* computes the inverse transpose of this matrix
* @return the inverse transpose matrix
*/
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)
}
/**
* computes the inverse transpose of this matrix
* @return the inverse transpose matrix
*/
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
)
}
/**
* computes the nearest orthonormal matrix to this matrix
* @return the rotation matrix
*/
fun orthonormalize(): Matrix3 {
var curMat = this
var curDet = Float.POSITIVE_INFINITY
/**
* computes the nearest orthonormal matrix to this matrix
* @return the rotation matrix
*/
fun orthonormalize(): Matrix3 {
var curMat = this
var curDet = Float.POSITIVE_INFINITY
for (i in 1..100) {
val newMat = (curMat + curMat.invTranspose())/2f
val newDet = abs(newMat.det())
// should almost always exit immediately
if (newDet >= curDet) return curMat
if (newDet <= 1.0000001f) return newMat
curMat = newMat
curDet = newDet
}
for (i in 1..100) {
val newMat = (curMat + curMat.invTranspose()) / 2f
val newDet = abs(newMat.det())
// should almost always exit immediately
if (newDet >= curDet) return curMat
if (newDet <= 1.0000001f) return newMat
curMat = newMat
curDet = newDet
}
return curMat
}
return curMat
}
/**
* finds the rotation matrix closest to all given rotation matrices.
* multiply input matrices by a weight for weighted averaging.
* WARNING: NOT ANGULAR
* @param others a variable number of additional matrices to average
* @return the average rotation matrix
*/
fun average(vararg others: Matrix3): Matrix3 {
var count = 1f
var sum = this
others.forEach {
count += 1f
sum += it
}
return (sum/count).orthonormalize()
}
/**
* finds the rotation matrix closest to all given rotation matrices.
* multiply input matrices by a weight for weighted averaging.
* WARNING: NOT ANGULAR
* @param others a variable number of additional matrices to average
* @return the average rotation matrix
*/
fun average(vararg others: Matrix3): Matrix3 {
var count = 1f
var sum = this
others.forEach {
count += 1f
sum += it
}
return (sum / count).orthonormalize()
}
/**
* linearly interpolates this matrix to that matrix by t
* @param that the matrix towards which to interpolate
* @param t the amount by which to interpolate
* @return the interpolated matrix
*/
fun lerp(that: Matrix3, t: Float): Matrix3 = (1f - t)*this + t*that
/**
* linearly interpolates this matrix to that matrix by t
* @param that the matrix towards which to interpolate
* @param t the amount by which to interpolate
* @return the interpolated matrix
*/
fun lerp(that: Matrix3, t: Float): Matrix3 = (1f - t) * this + t * that
// assumes this matrix is orthonormal and converts this to a quaternion
/**
* creates a quaternion representing the same rotation as this matrix, assuming the matrix is a rotation matrix
* @return the quaternion
*/
fun toQuaternionAssumingOrthonormal(): Quaternion {
if (this.det() <= 0f)
throw Exception("Attempt to convert negative determinant matrix to quaternion")
// assumes this matrix is orthonormal and converts this to a quaternion
/**
* creates a quaternion representing the same rotation as this matrix, assuming the matrix is a rotation matrix
* @return the quaternion
*/
fun toQuaternionAssumingOrthonormal(): Quaternion {
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()
} else if (xx > yy && xx > zz) {
Quaternion(yz - zy, 1 + xx - yy - zz, xy + yx, xz + zx).unit()
} else if (yy > zz) {
Quaternion(zx - xz, xy + yx, 1 - xx + yy - zz, yz + zy).unit()
} else {
Quaternion(xy - yx, xz + zx, yz + zy, 1 - xx - yy + zz).unit()
}
}
// orthogonalizes the matrix then returns the quaternion
/**
* creates a quaternion representing the same rotation as this matrix
* @return the quaternion
*/
fun toQuaternion(): Quaternion = orthonormalize().toQuaternionAssumingOrthonormal()
return if (yy > -zz && zz > -xx && xx > -yy) {
Quaternion(1 + xx + yy + zz, yz - zy, zx - xz, xy - yx).unit()
} else if (xx > yy && xx > zz) {
Quaternion(yz - zy, 1 + xx - yy - zz, xy + yx, xz + zx).unit()
} else if (yy > zz) {
Quaternion(zx - xz, xy + yx, 1 - xx + yy - zz, yz + zy).unit()
} else {
Quaternion(xy - yx, xz + zx, yz + zy, 1 - xx - yy + zz).unit()
}
}
// orthogonalizes the matrix then returns the quaternion
/**
* creates a quaternion representing the same rotation as this matrix
* @return the quaternion
*/
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,
@@ -285,84 +301,97 @@ data class Matrix3 (
// 0f)
// }
/**
* creates an eulerAngles representing the same rotation as this matrix, assuming the matrix is a rotation matrix
* @return the eulerAngles
*/
fun toEulerAnglesAssumingOrthonormal(order: EulerOrder): EulerAngles {
if (this.det() <= 0f)
throw Exception("Attempt to convert negative determinant matrix to euler angles")
/**
* creates an eulerAngles representing the same rotation as this matrix, assuming the matrix is a rotation matrix
* @return the eulerAngles
*/
fun toEulerAnglesAssumingOrthonormal(order: EulerOrder): EulerAngles {
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
if (kc == 0f) return EulerAngles(EulerOrder.XYZ, atan2(yz, yy), ETA.withSign(zx), 0f)
val ETA = 1.57079632f
when (order) {
EulerOrder.XYZ -> {
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))
}
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.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
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)))
}
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.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
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))
}
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.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
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))
}
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.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
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))
}
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.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
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)))
}
else -> {
throw Exception("EulerAngles not implemented for given EulerOrder")
}
}
}
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")
}
}
}
// orthogonalizes the matrix then returns the euler angles
/**
* creates an eulerAngles representing the same rotation as this matrix
* @return the eulerAngles
*/
fun toEulerAngles(order: EulerOrder): EulerAngles = orthonormalize().toEulerAnglesAssumingOrthonormal(order)
// orthogonalizes the matrix then returns the euler angles
/**
* creates an eulerAngles representing the same rotation as this matrix
* @return the eulerAngles
*/
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

571
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.*
@@ -8,168 +9,168 @@ import kotlin.math.*
* All operations are well-defined
*/
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
companion object {
val ZERO = Quaternion(0f, 0f, 0f, 0f)
val ONE = Quaternion(1f, 0f, 0f, 0f)
val I = Quaternion(0f, 1f, 0f, 0f)
val J = Quaternion(0f, 0f, 1f, 0f)
val K = Quaternion(0f, 0f, 0f, 1f)
companion object {
val ZERO = Quaternion(0f, 0f, 0f, 0f)
val ONE = Quaternion(1f, 0f, 0f, 0f)
val I = Quaternion(0f, 1f, 0f, 0f)
val J = Quaternion(0f, 0f, 1f, 0f)
val K = Quaternion(0f, 0f, 0f, 1f)
// creates a new quaternion representing the rotation about axis v by rotational angle v
/**
* 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()
}
// creates a new quaternion representing the rotation about axis v by rotational angle v
/**
* 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()
}
constructor(w: Float, xyz: Vector3) : this(w, xyz.x, xyz.y, xyz.z)
constructor(w: Float, xyz: Vector3) : this(w, xyz.x, xyz.y, xyz.z)
/**
* @return the imaginary components as a vector3
**/
val xyz get(): Vector3 = Vector3(x, y, z)
/**
* @return the imaginary components as a vector3
**/
val xyz get(): Vector3 = Vector3(x, y, z)
/**
* @return the quaternion with only the w component
**/
val re get(): Quaternion = Quaternion(w, 0f, 0f, 0f)
/**
* @return the quaternion with only the w component
**/
val re get(): Quaternion = Quaternion(w, 0f, 0f, 0f)
/**
* @return the quaternion with only x y z components
**/
val im get(): Quaternion = Quaternion(0f, x, y, z)
/**
* @return the quaternion with only x y z components
**/
val im get(): Quaternion = Quaternion(0f, x, y, z)
operator fun unaryMinus(): Quaternion = Quaternion(-w, -x, -y, -z)
operator fun unaryMinus(): Quaternion = Quaternion(-w, -x, -y, -z)
operator fun plus(that: Quaternion): Quaternion = Quaternion(
this.w + that.w,
this.x + that.x,
this.y + that.y,
this.z + that.z
)
operator fun plus(that: Quaternion): Quaternion = Quaternion(
this.w + that.w,
this.x + that.x,
this.y + that.y,
this.z + that.z
)
operator fun plus(that: Float): Quaternion = Quaternion(this.w + that, this.x, this.y, this.z)
operator fun plus(that: Float): Quaternion = Quaternion(this.w + that, this.x, this.y, this.z)
operator fun minus(that: Quaternion): Quaternion = Quaternion(
this.w - that.w,
this.x - that.x,
this.y - that.y,
this.z - that.z
)
operator fun minus(that: Quaternion): Quaternion = Quaternion(
this.w - that.w,
this.x - that.x,
this.y - that.y,
this.z - that.z
)
operator fun minus(that: Float): Quaternion = Quaternion(this.w - that, this.x, this.y, this.z)
operator fun minus(that: Float): Quaternion = Quaternion(this.w - that, this.x, this.y, this.z)
/**
* computes the dot product of this quaternion with that quaternion
* @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
/**
* computes the dot product of this quaternion with that quaternion
* @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
/**
* computes the square of the length of this quaternion
* @return the length squared
**/
fun lenSq(): Float = w*w + x*x + y*y + z*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
/**
* computes the length of this quaternion
* @return the length
**/
fun len(): Float = sqrt(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)
/**
* @return the normalized quaternion
**/
fun unit(): Quaternion {
val m = len()
return if (m == 0f) ZERO else this/m
}
/**
* @return the normalized quaternion
**/
fun unit(): Quaternion {
val m = len()
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
)
operator fun times(that: Float): Quaternion = Quaternion(
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
)
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
)
/**
* computes the inverse of this quaternion
* @return the inversed quaternion
**/
fun inv(): Quaternion {
val lenSq = lenSq()
return Quaternion(
w/lenSq,
-x/lenSq,
-y/lenSq,
-z/lenSq
)
}
/**
* computes the inverse of this quaternion
* @return the inversed quaternion
**/
fun inv(): Quaternion {
val lenSq = lenSq()
return Quaternion(
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()
/**
* computes right division, this * that^-1
**/
operator fun div(that: Quaternion): Quaternion = this * that.inv()
/**
* @return the conjugate of this quaternion
**/
fun conj(): Quaternion = Quaternion(w, -x, -y, -z)
/**
* @return the conjugate of this quaternion
**/
fun conj(): Quaternion = Quaternion(w, -x, -y, -z)
/**
* computes the logarithm of this quaternion
* @return the log of this quaternion
**/
fun log(): Quaternion {
val co = w
val si = xyz.len()
val len = len()
/**
* computes the logarithm of this quaternion
* @return the log of this quaternion
**/
fun log(): Quaternion {
val co = w
val si = xyz.len()
val len = len()
if (si == 0f) {
return Quaternion(ln(len), xyz/w)
}
if (si == 0f) {
return Quaternion(ln(len), xyz / w)
}
val ang = atan2(si, co)
return Quaternion(ln(len), ang/si*xyz)
}
val ang = atan2(si, co)
return Quaternion(ln(len), ang / si * xyz)
}
/**
* raises e to the power of this quaternion
* @return the exponentiated quaternion
**/
fun exp(): Quaternion {
val ang = xyz.len()
val len = exp(w)
/**
* raises e to the power of this quaternion
* @return the exponentiated quaternion
**/
fun exp(): Quaternion {
val ang = xyz.len()
val len = exp(w)
if (ang == 0f) {
return Quaternion(len, len*xyz)
}
if (ang == 0f) {
return Quaternion(len, len * xyz)
}
val co = cos(ang)
val si = sin(ang)
return Quaternion(len*co, len*si/ang*xyz)
}
val co = cos(ang)
val si = sin(ang)
return Quaternion(len * co, len * si / ang * xyz)
}
/**
* raises this quaternion to the power of t
* @param t the power by which to raise this quaternion
* @return the powered quaternion
**/
fun pow(t: Float): Quaternion = (log()*t).exp()
/**
* raises this quaternion to the power of t
* @param t the power by which to raise this quaternion
* @return the powered quaternion
**/
fun pow(t: Float): Quaternion = (log() * t).exp()
// for a slight improvement in performance
// not fully implemented
// for a slight improvement in performance
// not fully implemented
// fun pow(t: Float): Quaternion {
// val imLen = imLen()
// val ang = atan2(imLen, w)
@@ -195,177 +196,177 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
// }
// }
/**
* between this and -this, picks the one nearest to that
* @param that the quaternion to be nearest
* @return nearest quaternion
**/
fun twinNearest(that: Quaternion): Quaternion = if (this.dot(that) < 0f) -this else this
/**
* between this and -this, picks the one nearest to that
* @param that the quaternion to be nearest
* @return nearest quaternion
**/
fun twinNearest(that: Quaternion): Quaternion = if (this.dot(that) < 0f) -this else this
/**
* interpolates from this quaternion to that quaternion by t in quaternion space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun interp(that: Quaternion, t: Float) =
if (t == 0f) {
this
} else if (t == 1f) {
that
} else if (t < 0.5f) {
(that/this).pow(t)*this
} else {
(this/that).pow(1f - t)*that
}
/**
* interpolates from this quaternion to that quaternion by t in quaternion space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun interp(that: Quaternion, t: Float) =
if (t == 0f) {
this
} else if (t == 1f) {
that
} else if (t < 0.5f) {
(that / this).pow(t) * this
} else {
(this / that).pow(1f - t) * that
}
/**
* interpolates from this quaternion to that quaternion by t in rotation space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun interpR(that: Quaternion, t: Float) = this.interp(that.twinNearest(this), t)
/**
* interpolates from this quaternion to that quaternion by t in rotation space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun interpR(that: Quaternion, t: Float) = this.interp(that.twinNearest(this), t)
/**
* linearly interpolates from this quaternion to that quaternion by t in quaternion space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun lerp(that: Quaternion, t: Float): Quaternion = (1f - t)*this + t*that
/**
* linearly interpolates from this quaternion to that quaternion by t in quaternion space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
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
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun lerpR(that: Quaternion, t: Float) = this.lerp(that.twinNearest(this), t)
/**
* linearly interpolates from this quaternion to that quaternion by t in rotation space
* @param that the quaternion to interpolate to
* @param t the amount to interpolate
* @return interpolated quaternion
**/
fun lerpR(that: Quaternion, t: Float) = this.lerp(that.twinNearest(this), t)
/**
* computes this quaternion's angle to identity in quaternion space
* @return angle
**/
fun angle(): Float = atan2(xyz.len(), w)
/**
* computes this quaternion's angle to identity in quaternion space
* @return angle
**/
fun angle(): Float = atan2(xyz.len(), w)
/**
* computes this quaternion's angle to identity in rotation space
* @return angle
**/
fun angleR(): Float = 2f*atan2(xyz.len(), abs(w))
/**
* computes this quaternion's angle to identity in rotation space
* @return angle
**/
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()
/**
* 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()
/**
* 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()
/**
* 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()
/**
* computes the angle this quaternion rotates about the u axis in quaternion space
* @param u the axis
* @return angle
**/
fun angleAbout(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return atan2(uDotIm, uLen*w)
}
/**
* computes the angle this quaternion rotates about the u axis in quaternion space
* @param u the axis
* @return angle
**/
fun angleAbout(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return atan2(uDotIm, uLen * w)
}
/**
* computes the angle this quaternion rotates about the u axis in rotation space
* @param u the axis
* @return angle
**/
fun angleAboutR(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return if (uDotIm < 0f) {
2f*atan2(-uDotIm, -uLen*w)
} else {
2f*atan2(uDotIm, uLen*w)
}
}
/**
* computes the angle this quaternion rotates about the u axis in rotation space
* @param u the axis
* @return angle
**/
fun angleAboutR(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return if (uDotIm < 0f) {
2f * atan2(-uDotIm, -uLen * w)
} else {
2f * atan2(uDotIm, uLen * w)
}
}
/**
* finds Q, the quaternion nearest to this quaternion representing a rotation purely about the global u axis
* Q is NOT unitized
* @param v the global axis
* @return Q
**/
fun project(v: Vector3) = Quaternion(w, xyz.dot(v)/v.lenSq()*v)
/**
* finds Q, the quaternion nearest to this quaternion representing a rotation purely about the global u axis
* Q is NOT unitized
* @param v the global axis
* @return Q
**/
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.
* Q is NOT unitized
* @param v the global axis
* @return Q
**/
fun reject(v: Vector3) = Quaternion(w, v.cross(xyz).cross(v)/v.lenSq())
/**
* finds Q, the quaternion nearest to this quaternion representing a rotation NOT on the global u axis.
* Q is NOT unitized
* @param v the global axis
* @return Q
**/
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.
* Q is NOT unitized
* @param u the local direction
* @param v the global direction
* @return Q
**/
fun align(u: Vector3, v: Vector3): Quaternion {
val U = Quaternion(0f, u)
val V = Quaternion(0f, v)
/**
* finds Q, the quaternion nearest to this quaternion whose local u direction aligns with the global v direction.
* Q is NOT unitized
* @param u the local direction
* @param v the global direction
* @return Q
**/
fun align(u: Vector3, v: Vector3): Quaternion {
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
}
/**
* applies this quaternion's rotation to that vector
* @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
/**
* applies this quaternion's rotation to that vector
* @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
/**
* computes this quaternion's rotation axis
* @return rotation axis
**/
fun axis(): Vector3 = xyz.unit()
/**
* computes this quaternion's rotation axis
* @return rotation axis
**/
fun axis(): Vector3 = xyz.unit()
/**
* computes the rotation vector representing this quaternion's rotation
* @return rotation vector
**/
fun toRotationVector(): Vector3 = 2f*log().xyz
/**
* computes the rotation vector representing this quaternion's rotation
* @return rotation vector
**/
fun toRotationVector(): Vector3 = 2f * log().xyz
/**
* computes the matrix representing this quaternion's rotation
* @return rotation matrix
**/
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
)
}
/**
* computes the matrix representing this quaternion's rotation
* @return rotation matrix
**/
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
)
}
/**
* computes the euler angles representing this quaternion's rotation
* @param order the order in which to decompose this quaternion into euler angles
* @return euler angles
**/
fun toEulerAngles(order: EulerOrder): EulerAngles = this.toMatrix().toEulerAnglesAssumingOrthonormal(order)
/**
* computes the euler angles representing this quaternion's rotation
* @param order the order in which to decompose this quaternion into euler angles
* @return euler angles
**/
fun toEulerAngles(order: EulerOrder): EulerAngles = this.toMatrix().toEulerAnglesAssumingOrthonormal(order)
}
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

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

@@ -1,90 +1,92 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.atan2
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 NEG_X = Vector3(-1f, 0f, 0f)
val NEG_Y = Vector3( 0f, -1f, 0f)
val NEG_Z = Vector3( 0f, 0f, -1f)
}
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 NEG_X = Vector3(-1f, 0f, 0f)
val NEG_Y = Vector3(0f, -1f, 0f)
val NEG_Z = Vector3(0f, 0f, -1f)
}
operator fun unaryMinus() = Vector3(-x, -y, -z)
operator fun unaryMinus() = Vector3(-x, -y, -z)
operator fun plus(that: Vector3) = Vector3(
this.x + that.x,
this.y + that.y,
this.z + that.z
)
operator fun plus(that: Vector3) = Vector3(
this.x + that.x,
this.y + that.y,
this.z + that.z
)
operator fun minus(that: Vector3) = Vector3(
this.x - that.x,
this.y - that.y,
this.z - that.z
)
operator fun minus(that: Vector3) = Vector3(
this.x - that.x,
this.y - that.y,
this.z - that.z
)
/**
* computes the dot product of this vector with that vector
* @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
/**
* computes the dot product of this vector with that vector
* @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
/**
* computes the cross product of this vector with that vector
* @param that the vector with which to be crossed
* @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
)
/**
* computes the square of the length of this vector
* @return the length squared
**/
fun lenSq() = x*x + y*y + z*z
/**
* computes the cross product of this vector with that vector
* @param that the vector with which to be crossed
* @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
)
/**
* computes the length of this quaternion
* @return the length
**/
fun len() = sqrt(x*x + y*y + z*z)
/**
* computes the square of the length of this vector
* @return the length squared
**/
fun lenSq() = x * x + y * y + z * z
/**
* @return the normalized vector
**/
fun unit(): Vector3 {
val m = len()
return if (m == 0f) ZERO else this/m
}
/**
* computes the length of this quaternion
* @return the length
**/
fun len() = sqrt(x * x + y * y + z * z)
operator fun times(that: Float) = Vector3(
this.x*that,
this.y*that,
this.z*that
)
/**
* @return the normalized vector
**/
fun unit(): Vector3 {
val m = len()
return if (m == 0f) ZERO else this / m
}
// computes division of this vector3 by a float
operator fun div(that: Float) = Vector3(
this.x/that,
this.y/that,
this.z/that
)
operator fun times(that: Float) = Vector3(
this.x * that,
this.y * that,
this.z * that
)
/**
* computes the angle between this vector with that vector
* @param that the vector to which the angle is computed
* @return the angle
**/
fun angleTo(that: Vector3): Float = atan2(this.cross(that).len(), this.dot(that))
// computes division of this vector3 by a float
operator fun div(that: Float) = Vector3(
this.x / that,
this.y / that,
this.z / that
)
/**
* computes the angle between this vector with that vector
* @param that the vector to which the angle is computed
* @return the angle
**/
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())
}