From cfd405c1d045243107aa7e6cdc5ae532ebb25b3e Mon Sep 17 00:00:00 2001 From: AxisAngles Date: Sat, 28 Jan 2023 22:38:04 -0800 Subject: [PATCH] Clean up Renamed all base Quaternion function with R variant (IE angle and angleR) to have a Q at the end (angle -> angleQ) Cleaned up the matrices which are otherwise hard to read without special formatting --- .../github/axisangles/ktmath/EulerAngles.kt | 52 ++- .../java/io/github/axisangles/ktmath/Main.kt | 368 ------------------ .../io/github/axisangles/ktmath/Matrix3.kt | 72 ++-- .../io/github/axisangles/ktmath/Quaternion.kt | 65 +--- .../axisangles/ktmath/QuaternionTest.kt | 20 +- 5 files changed, 71 insertions(+), 506 deletions(-) delete mode 100644 server/src/main/java/io/github/axisangles/ktmath/Main.kt 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 57bc8a611..4cf58daad 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt @@ -7,11 +7,6 @@ import kotlin.math.sin enum class EulerOrder { XYZ, YZX, ZXY, ZYX, YXZ, XZY } -// prefer Y.toX -// but if ambiguous, use X.fromY -/* - * 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 @@ -83,35 +78,34 @@ data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z: // ktlint ruining spacing /* ktlint-disable */ EulerOrder.XYZ -> Matrix3( - cY*cZ, -cY*sZ, sY, - cZ*sX*sY + cX*sZ, cX*cZ - sX*sY*sZ, -cY*sX, - sX*sZ - cX*cZ*sY, cZ*sX + cX*sY*sZ, cX*cY - ) + cY*cZ , -cY*sZ , sY , + cZ*sX*sY + cX*sZ , cX*cZ - sX*sY*sZ , -cY*sX , + sX*sZ - cX*cZ*sY , cZ*sX + cX*sY*sZ , cX*cY ) + EulerOrder.YZX -> Matrix3( - cY * cZ, sX * sY - cX * cY * sZ, cX * sY + cY * sX * sZ, - sZ, cX * cZ, -cZ * sX, - -cZ * sY, cY * sX + cX * sY * sZ, cX * cY - sX * sY * sZ - ) + cY*cZ , sX*sY - cX*cY*sZ , cX*sY + cY*sX*sZ , + sZ , cX*cZ , -cZ*sX , + -cZ*sY , cY*sX + cX*sY*sZ , cX*cY - sX*sY*sZ ) + EulerOrder.ZXY -> Matrix3( - cY * cZ - sX * sY * sZ, -cX * sZ, cZ * sY + cY * sX * sZ, - cZ * sX * sY + cY * sZ, cX * cZ, sY * sZ - cY * cZ * sX, - -cX * sY, sX, cX * cY - ) + cY*cZ - sX*sY*sZ , -cX*sZ , cZ*sY + cY*sX*sZ , + cZ*sX*sY + cY*sZ , cX*cZ , sY*sZ - cY*cZ*sX , + -cX*sY , sX , cX*cY ) + EulerOrder.ZYX -> Matrix3( - cY * cZ, cZ * sX * sY - cX * sZ, cX * cZ * sY + sX * sZ, - cY * sZ, cX * cZ + sX * sY * sZ, cX * sY * sZ - cZ * sX, - -sY, cY * sX, cX * cY - ) + cY*cZ , cZ*sX*sY - cX*sZ , cX*cZ*sY + sX*sZ , + cY*sZ , cX*cZ + sX*sY*sZ , cX*sY*sZ - cZ*sX , + -sY , cY*sX , cX*cY ) + EulerOrder.YXZ -> Matrix3( - cY * cZ + sX * sY * sZ, cZ * sX * sY - cY * sZ, cX * sY, - cX * sZ, cX * cZ, -sX, - cY * sX * sZ - cZ * sY, cY * cZ * sX + sY * sZ, cX * cY - ) + cY*cZ + sX*sY*sZ , cZ*sX*sY - cY*sZ , cX*sY , + cX*sZ , cX*cZ , -sX , + cY*sX*sZ - cZ*sY , cY*cZ*sX + sY*sZ , cX*cY ) + EulerOrder.XZY -> Matrix3( - cY * cZ, -sZ, cZ * sY, - sX * sY + cX * cY * sZ, cX * cZ, cX * sY * sZ - cY * sX, - cY * sX * sZ - cX * sY, cZ * sX, cX * cY + sX * sY * sZ - ) + cY*cZ , -sZ , cZ*sY , + sX*sY + cX*cY*sZ , cX*cZ , cX*sY*sZ - cY*sX , + cY*sX*sZ - cX*sY , cZ*sX , cX*cY + sX*sY*sZ ) /* ktlint-enable */ } } diff --git a/server/src/main/java/io/github/axisangles/ktmath/Main.kt b/server/src/main/java/io/github/axisangles/ktmath/Main.kt deleted file mode 100644 index f9204f04a..000000000 --- a/server/src/main/java/io/github/axisangles/ktmath/Main.kt +++ /dev/null @@ -1,368 +0,0 @@ -package io.github.axisangles.ktmath - -import kotlin.math.* - -var randSeed = 0 -fun randInt(): Int { - randSeed = (1103515245 * randSeed + 12345).mod(2147483648).toInt() - return randSeed -} - -fun randFloat(): Float { - 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()) -} - -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()) -} - -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) - } -} - -fun testEulerConversion(order: EulerOrder, exception: String) { - for (i in 1..1000) { - testEulerMatrix(order, randRotMatrix(), exception) - } -} - -fun testMatrixOrthonormalize() { - 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") - } - } -} - -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") - } - } -} - -fun relError(a: Matrix3, b: Matrix3): Float { - val combinedLen = sqrt((a.normSq() + b.normSq()) / 2f) - if (combinedLen == 0f) return 0f - - 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 - - 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 - - return (b - a).len() / combinedLen -} - -fun checkError(eta: Float, a: Matrix3, b: Matrix3): Boolean { - 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()) -} - -fun checkError(eta: Float, a: Vector3, b: Vector3): Boolean { - return (b - a).lenSq() <= eta * eta * (a.lenSq() + b.lenSq()) -} - -fun checkError(eta: Float, A: Quaternion): Boolean { - return A.lenSq() <= eta * eta -} - -fun testQuaternionInv() { - for (i in 1..1000) { - val Q = randQuaternion() - - if (relError(Q * Q.inv(), Quaternion.IDENTITY) > 1e-6f) { - throw Exception("Quaternion inv accuracy test failed") - } - } -} - -fun testQuaternionDiv() { - for (i in 1..1000) { - val Q = randQuaternion() - - if (!checkError(1e-6f, Q/Q, Quaternion.IDENTITY)) { - 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() - - 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.IDENTITY)) { - 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() - - 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() - - 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()) - - 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) - - 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 - - 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) - } - } -} - -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 - ) - - 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() - - 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") - - // 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") - - // speed test a linear (align) method against some standard math functions -// var x = Quaternion(1f, 2f, 3f, 4f) -// -// var dtAlignTotal: Long = 0 -// var dtOrthonormalizeTotal: Long = 0 -// var dtAtan2Total: Long = 0 -// var dtAsinTotal: Long = 0 -// -// for (i in 1..10) { -// val dtAlign = measureTimeMillis { -// for (i in 1..1_000_000) { -// val u = Vector3(1f, 0f, 0f) -// val v = Vector3(0f, 1f, 0f) -// // to make sure it is not optimized away -// x = x.align(u, v) -// // internally, x.align is: -// // val U = Quaternion(0f, u) -// // val V = Quaternion(0f, v) -// // x = (V*x/U + (V/U).len()*x)/2f -// } -// } -// -// var y = x.toMatrix() -// val dtOrthonormalize = measureTimeMillis { -// for (i in 1..1_000_000) { -// // to make sure it is not optimized away -// y = y.orthonormalize() -// // internally, x.align is: -// // val U = Quaternion(0f, u) -// // val V = Quaternion(0f, v) -// // x = (V*x/U + (V/U).len()*x)/2f -// } -// } -// -// var z = 0f; -// val dtAtan2 = measureTimeMillis { -// for (i in 1..1_000_000) { -// z+= atan2(i.toFloat(), i.toFloat()) // 45 degrees -// } -// } -// -// var w = 0f; -// val dtAsin = measureTimeMillis { -// for (i in 1..1_000_000) { -// w+= asin(i.toFloat()*0.7071f/i.toFloat()) // 45 degrees -// } -// } -// -// dtAlignTotal += dtAlign -// dtOrthonormalizeTotal += dtOrthonormalize -// dtAtan2Total += dtAtan2 -// dtAsinTotal += dtAsin -// } -// -// println(x) -// -// println(dtAlignTotal) // 213 -// println(dtOrthonormalizeTotal) // 244 -// println(dtAtan2Total) // 610 -// println(dtAsinTotal) // 3558 - -// var x = Quaternion(2f, 1f, 4f, 3f) -// val dtPow = measureTimeMillis { -// for (i in 1..10_000_000) { -// x = x.pow(1f) -// } -// } -// -// println(dtPow) -} 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 9c68b0f30..dc6787dec 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt @@ -89,9 +89,10 @@ data class Matrix3( * computes the square of the frobenius norm of this matrix * @return the frobenius norm squared */ - fun normSq(): Float = (xx * xx + yx * yx + zx * zx - + xy * xy + yy * yy + zy * zy - + xz * xz + yz * yz + zz * zz) + fun normSq(): Float = + xx * xx + yx * yx + zx * zx + + xy * xy + yy * yy + zy * zy + + xz * xz + yz * yz + zz * zz /** * computes the frobenius norm of this matrix @@ -103,9 +104,10 @@ data class Matrix3( * computes the determinant of this matrix * @return the determinant */ - fun det(): Float = ((xz * yx - xx * yz) * zy - + (xx * yy - xy * yx) * zz - + (xy * yz - xz * yy) * zx) + fun det(): Float = + (xz * yx - xx * yz) * zy + + (xx * yy - xy * yx) * zz + + (xy * yz - xz * yy) * zx /** * computes the trace of this matrix @@ -297,34 +299,18 @@ data class Matrix3( built into the prerequisites for this function */ -//fun toEulerAnglesXYZFaulty(): EulerAngles { -// return if (abs(zx) < 0.9999999f) -// EulerAngles(EulerOrder.XYZ, -// atan2(-zy, zz), -// asin(zx.coerceIn(-1f, 1f)), -// atan2(-yx, xx)) -// else -// EulerAngles(EulerOrder.XYZ, -// atan2(yz, yy), -// asin(zx.coerceIn(-1f, 1f)), -// 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 { - val ETA = 1.57079632f + val ETA = 1.5707964f when (order) { EulerOrder.XYZ -> { val kc = zy * zy + zz * zz - if (kc == 0f) { - return EulerAngles( - EulerOrder.XYZ, atan2(yz, yy), ETA.withSign(zx), 0f - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.XYZ, + atan2(yz, yy), ETA.withSign(zx), 0f) return EulerAngles( EulerOrder.XYZ, @@ -335,11 +321,8 @@ data class Matrix3( } EulerOrder.YZX -> { val kc = xx * xx + xz * xz - if (kc == 0f) { - return EulerAngles( - EulerOrder.YZX, 0f, atan2(zx, zz), ETA.withSign(xy) - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.YZX, + 0f, atan2(zx, zz), ETA.withSign(xy)) return EulerAngles( EulerOrder.YZX, @@ -350,11 +333,8 @@ data class Matrix3( } EulerOrder.ZXY -> { val kc = yy * yy + yx * yx - if (kc == 0f) { - return EulerAngles( - EulerOrder.ZXY, ETA.withSign(yz), 0f, atan2(xy, xx) - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.ZXY, + ETA.withSign(yz), 0f, atan2(xy, xx)) return EulerAngles( EulerOrder.ZXY, @@ -365,11 +345,8 @@ data class Matrix3( } EulerOrder.ZYX -> { val kc = xy * xy + xx * xx - if (kc == 0f) { - return EulerAngles( - EulerOrder.ZYX, 0f, ETA.withSign(-xz), atan2(-yx, yy) - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.ZYX, + 0f, ETA.withSign(-xz), atan2(-yx, yy)) return EulerAngles( EulerOrder.ZYX, @@ -378,13 +355,11 @@ data class Matrix3( 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 - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.YXZ, + ETA.withSign(-zy), atan2(-xz, xx), 0f) return EulerAngles( EulerOrder.YXZ, @@ -395,11 +370,8 @@ data class Matrix3( } EulerOrder.XZY -> { val kc = yz * yz + yy * yy - if (kc == 0f) { - return EulerAngles( - EulerOrder.XZY, atan2(-zy, zz), 0f, ETA.withSign(-yx) - ) - } + if (kc == 0f) return EulerAngles(EulerOrder.XZY, + atan2(-zy, zz), 0f, ETA.withSign(-yx)) return EulerAngles( EulerOrder.XZY, 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 67e8fd762..d756e4b7c 100644 --- a/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt +++ b/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt @@ -4,10 +4,6 @@ package io.github.axisangles.ktmath import kotlin.math.* -/** - * A Quaternion class properly implementing Quaternions as a real component and 3 imaginary components - * All operations are well-defined - */ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { companion object { val NULL = Quaternion(0f, 0f, 0f, 0f) @@ -138,7 +134,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { /** * computes the inverse of this quaternion - * @return the inversed quaternion + * @return the inverse quaternion **/ fun inv(): Quaternion { val lenSq = lenSq() @@ -203,42 +199,13 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { **/ fun pow(t: Float): Quaternion = (log() * t).exp() - // for a slight improvement in performance - // not fully implemented -// fun pow(t: Float): Quaternion { -// val imLen = xyz.Len() -// val ang = atan(w, imLen) -// -// val len = len().pow(t) -// val co = cos(t*ang) -// val si = sin(t*ang) -// -// return if (imLen == 0f) { -// Quaternion(len*co, -// len*t*x, -// len*t*y, -// len*t*z -// ) -// } else { -// Quaternion( -// len*co, -// len*si*x/imLen, -// len*si*y/imLen, -// len*si*z/imLen -// ) -// } -// } - /** - * between this and -this, picks the one nearest to that + * between this and -this, picks the one nearest to that quaternion * @param that the quaternion to be nearest * @return nearest quaternion **/ - fun twinNearest(that: Quaternion): Quaternion = if (this.dot(that) < 0f) { - -this - } else { - this - } + 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 @@ -246,7 +213,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param t the amount to interpolate * @return interpolated quaternion **/ - fun interp(that: Quaternion, t: Float) = + fun interpQ(that: Quaternion, t: Float) = if (t == 0f) { this } else if (t == 1f) { @@ -263,7 +230,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param t the amount to interpolate * @return interpolated quaternion **/ - fun interpR(that: Quaternion, t: Float) = this.interp(that.twinNearest(this), t) + fun interpR(that: Quaternion, t: Float) = this.interpQ(that.twinNearest(this), t) /** * linearly interpolates from this quaternion to that quaternion by t in @@ -272,7 +239,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param t the amount to interpolate * @return interpolated quaternion **/ - fun lerp(that: Quaternion, t: Float): Quaternion = (1f - t) * this + t * that + fun lerpQ(that: Quaternion, t: Float): Quaternion = (1f - t) * this + t * that /** * linearly interpolates from this quaternion to that quaternion by t in @@ -281,13 +248,13 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param t the amount to interpolate * @return interpolated quaternion **/ - fun lerpR(that: Quaternion, t: Float) = this.lerp(that.twinNearest(this), t) + fun lerpR(that: Quaternion, t: Float) = this.lerpQ(that.twinNearest(this), t) /** * computes this quaternion's angle to identity in quaternion space * @return angle **/ - fun angle(): Float = atan2(xyz.len(), w) + fun angleQ(): Float = atan2(xyz.len(), w) /** * computes this quaternion's angle to identity in rotation space @@ -300,7 +267,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param that the other quaternion * @return angle **/ - fun angleTo(that: Quaternion): Float = (this / that).angle() + fun angleToQ(that: Quaternion): Float = (this / that).angleQ() /** * computes the angle between this quaternion and that quaternion in rotation space @@ -314,7 +281,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param u the axis * @return angle **/ - fun angleAbout(u: Vector3): Float { + fun angleAboutQ(u: Vector3): Float { val si = u.dot(xyz) val co = u.len() * w return atan2(si, co) @@ -325,7 +292,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { * @param u the axis * @return angle **/ - fun angleAboutR(u: Vector3): Float = 2f * twinNearest(IDENTITY).angleAbout(u) + fun angleAboutR(u: Vector3): Float = 2f * twinNearest(IDENTITY).angleAboutQ(u) /** * finds Q, the quaternion nearest to this quaternion representing a rotation purely @@ -365,7 +332,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { fun sandwich(that: Vector3): Vector3 = (this * Quaternion(0f, that) / this).xyz /** - * computes this quaternion's rotation axis + * computes this quaternion's unit length rotation axis * @return rotation axis **/ fun axis(): Vector3 = xyz.unit() @@ -384,9 +351,9 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { val d = lenSq() /* ktlint-disable */ return Matrix3( - (w*w + x*x - y*y - z*z)/d, 2f*(x*y - w*z)/d, 2f*(w*y + x*z)/d, - 2f*(x*y + w*z)/d, (w*w - x*x + y*y - z*z)/d, 2f*(y*z - w*x)/d, - 2f*(x*z - w*y)/d, 2f*(w*x + y*z)/d, (w*w - x*x - y*y + z*z)/d) + (w*w + x*x - y*y - z*z)/d , 2f*(x*y - w*z)/d , 2f*(w*y + x*z)/d , + 2f*(x*y + w*z)/d , (w*w - x*x + y*y - z*z)/d , 2f*(y*z - w*x)/d , + 2f*(x*z - w*y)/d , 2f*(w*x + y*z)/d , (w*w - x*x - y*y + z*z)/d ) /* ktlint-enable */ } diff --git a/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt index e3223100a..5b5e4d95b 100644 --- a/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt +++ b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt @@ -77,11 +77,11 @@ class QuaternionTest { } @Test - fun interp() { + fun interpQ() { 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) + assertEquals(q1.interpQ(q2, 0.5f), q3, 1e-7) } @Test @@ -93,11 +93,11 @@ class QuaternionTest { } @Test - fun lerp() { + fun lerpQ() { 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) + assertEquals(q1.lerpQ(q2, 0.5f), q3, 1e-7) } @Test @@ -109,10 +109,10 @@ class QuaternionTest { } @Test - fun angleTo() { + fun angleToQ() { val q1 = Quaternion(1f, 0f, 0f, 0f) val q2 = Quaternion(0f, 1f, 0f, 0f) - assertEquals(q1.angleTo(q2), PI.toFloat() / 2f) + assertEquals(q1.angleToQ(q2), PI.toFloat() / 2f) } @Test @@ -123,9 +123,9 @@ class QuaternionTest { } @Test - fun angle() { + fun angleQ() { val q = Quaternion(0f, 1f, 0f, 0f) - assertEquals(q.angle(), PI.toFloat() / 2f) + assertEquals(q.angleQ(), PI.toFloat() / 2f) } @Test @@ -135,9 +135,9 @@ class QuaternionTest { } @Test - fun angleAbout() { + fun angleAboutQ() { val q = Quaternion(1f, 1f, 1f, 0f) - assertEquals(q.angleAbout(Vector3.POS_Y), PI.toFloat() / 4f) + assertEquals(q.angleAboutQ(Vector3.POS_Y), PI.toFloat() / 4f) } @Test