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:
AxisAngles committed 2023-01-28 22:38:04 -08:00
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