diff --git a/.editorconfig b/.editorconfig index 5e8edbf21..3d0b69f1d 100644 --- a/.editorconfig +++ b/.editorconfig @@ -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.* diff --git a/server/build.gradle.kts b/server/build.gradle.kts index 0a8b3795c..597412843 100644 --- a/server/build.gradle.kts +++ b/server/build.gradle.kts @@ -139,7 +139,8 @@ configure { "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 { diff --git a/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt b/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt index 32a278e5f..e027cf4df 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt @@ -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 + ) + } + } } diff --git a/server/src/main/java/io/github/axisangles/ktmath/Main.kt b/server/src/main/java/io/github/axisangles/ktmath/Main.kt index 62297fd32..a64ae0fbf 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Main.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Main.kt @@ -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) -} \ No newline at end of file +} diff --git a/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt b/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt index f1700a830..cedf34526 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt @@ -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 diff --git a/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt b/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt index 6f811c547..856742c82 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt @@ -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 diff --git a/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt b/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt index 37624d190..db9a7221c 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt @@ -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 diff --git a/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt new file mode 100644 index 000000000..ca4906ffe --- /dev/null +++ b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt @@ -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()) +}