mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-20 09:01:20 -08:00
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
This commit is contained in:
1 parent
f2cf6dffcf
commit
cfd405c1d0
5 files changed
+71
-506
No files matched your search
@@ -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 */
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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 */
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in new issue
Block a user