mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-04-06 02:01:58 +02:00
Added more Quaternion tests, Changed toRotationVector to return values (-pi < x <= pi) in line with other rotation space conversions
This commit is contained in:
AxisAngles
@@ -1,71 +0,0 @@
|
||||
package io.github.axisangles.ktmath
|
||||
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* guarantees last bit floating point accuracy
|
||||
*/
|
||||
fun atan(co: Float, si: Float) = atan(co.toDouble(), si.toDouble()).toFloat()
|
||||
fun atan(ta: Float) = atan(ta.toDouble()).toFloat()
|
||||
fun acos(co: Float) = acos(co.toDouble()).toFloat()
|
||||
fun asin(si: Float) = asin(si.toDouble()).toFloat()
|
||||
|
||||
|
||||
fun atan(co: Double, si: Double) =
|
||||
if (co == 0.0 && si == 0.0) 0.0
|
||||
else atanFold1(co, si)
|
||||
|
||||
fun atan(ta: Double) = when (ta) {
|
||||
1.0/0.0 -> 1.570796326794897
|
||||
-1.0/0.0 -> -1.570796326794897
|
||||
else -> atanFold1(1.0, ta)
|
||||
}
|
||||
fun acos(co: Double) = atanFold1(co, sqrt(1.0 - co*co))
|
||||
fun asin(si: Double) = atanFold1(sqrt(1.0 - si*si), si)
|
||||
|
||||
// The goal was to make use of as much FMA and parallel instruction execution as possible
|
||||
// make a viktor version to replace this in the future
|
||||
private fun atanFold1(x: Double, y: Double) =
|
||||
if (y < 0.0) -atanFold2(x, -y)
|
||||
else atanFold2(x, y)
|
||||
|
||||
private fun atanFold2(x: Double, y: Double) =
|
||||
if (x < 0.0) 3.141592653589793 - atanFold3(-x, y)
|
||||
else atanFold3(x, y)
|
||||
|
||||
private fun atanFold3(x: Double, y: Double) =
|
||||
if (y > x) 1.570796326794897 - atanFold4(y, x)
|
||||
else atanFold4(x, y)
|
||||
|
||||
private fun atanFold4(x: Double, y: Double) =
|
||||
if (y > 0.4142135623730950*x) 0.7853981633974483 - atanFold5(
|
||||
x + y, x - y)
|
||||
else atanFold5(x, y)
|
||||
|
||||
private fun atanFold5(x: Double, y: Double) =
|
||||
if (y > 0.1989123673796580*x) 0.3926990816987242 - atanFold6(
|
||||
x + 0.4142135623730950*y, 0.4142135623730950*x - y)
|
||||
else atanFold6(x, y)
|
||||
|
||||
private fun atanFold6(x: Double, y: Double) =
|
||||
if (y > 0.09849140335716425*x) 0.1963495408493621 - atanFold7(
|
||||
x + 0.1989123673796580*y, 0.1989123673796580*x - y)
|
||||
else atanFold7(x, y)
|
||||
|
||||
private fun atanFold7(x: Double, y: Double) =
|
||||
if (y > 0.04912684976946725*x) 0.09817477042468103 - atanFinal(
|
||||
x + 0.09849140335716425*y, 0.09849140335716425*x - y)
|
||||
else atanFinal(x, y)
|
||||
|
||||
private fun atanFinal(x: Double, y: Double): Double {
|
||||
val s = y/x
|
||||
val t = s*s
|
||||
return s*(1.0 - t*(1.0/3.0 - t*(1.0/5.0)))// - t*(1.0/7.0))))// - t*(1.0/9.0)))))// - t*(1.0/11.0 - t*(1.0/13.0 - t*(1.0/15.0))))))))
|
||||
// val r = y/x
|
||||
// val s = r*r
|
||||
// val t = s*s
|
||||
// // help the CPU take advantage of parallel instruction execution
|
||||
// val odd = s*(1.0/3.0 + t*(1.0/7.0))
|
||||
// val evn = 1.0 + t*(1.0/5.0 + t*(1.0/9.0))
|
||||
// return r*(evn - odd)
|
||||
}
|
||||
@@ -158,7 +158,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
|
||||
return Quaternion(ln(len), xyz / w)
|
||||
}
|
||||
|
||||
val ang = atan(co, si)
|
||||
val ang = atan2(si, co)
|
||||
return Quaternion(ln(len), ang / si * xyz)
|
||||
}
|
||||
|
||||
@@ -264,13 +264,13 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
|
||||
* computes this quaternion's angle to identity in quaternion space
|
||||
* @return angle
|
||||
**/
|
||||
fun angle(): Float = atan(w, xyz.len())
|
||||
fun angle(): Float = atan2(xyz.len(), w)
|
||||
|
||||
/**
|
||||
* computes this quaternion's angle to identity in rotation space
|
||||
* @return angle
|
||||
**/
|
||||
fun angleR(): Float = 2f * atan(abs(w), xyz.len())
|
||||
fun angleR(): Float = 2f * atan2(xyz.len(), abs(w))
|
||||
|
||||
/**
|
||||
* computes the angle between this quaternion and that quaternion in quaternion space
|
||||
@@ -294,7 +294,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
|
||||
fun angleAbout(u: Vector3): Float {
|
||||
val si = u.dot(xyz)
|
||||
val co = u.len()*w
|
||||
return atan(co, si)
|
||||
return atan2(si, co)
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -302,11 +302,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 {
|
||||
val si = u.dot(xyz)
|
||||
val co = u.len()*w
|
||||
return 2f * atan(abs(co), si)
|
||||
}//: Float = 2f*this.twinNearest(Quaternion.ONE).angleAbout(u)
|
||||
fun angleAboutR(u: Vector3): Float = 2f * twinNearest(ONE).angleAbout(u)
|
||||
|
||||
/**
|
||||
* finds Q, the quaternion nearest to this quaternion representing a rotation purely
|
||||
@@ -355,7 +351,7 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
|
||||
* computes the rotation vector representing this quaternion's rotation
|
||||
* @return rotation vector
|
||||
**/
|
||||
fun toRotationVector(): Vector3 = 2f * log().xyz
|
||||
fun toRotationVector(): Vector3 = 2f * twinNearest(ONE).log().xyz
|
||||
|
||||
/**
|
||||
* computes the matrix representing this quaternion's rotation
|
||||
|
||||
@@ -170,7 +170,84 @@ class QuaternionTest {
|
||||
@Test
|
||||
fun fromTo() {
|
||||
val q1 = Quaternion(1f, 0f, 0f, 1f).unit()
|
||||
assertEquals(Quaternion.fromTo(Vector3.POS_X, Vector3.POS_Y), q1)
|
||||
assertEquals(q1, Quaternion.fromTo(Vector3.POS_X, Vector3.POS_Y))
|
||||
}
|
||||
|
||||
@Test
|
||||
fun sandwich() {
|
||||
val v1 = Quaternion(1f, 1f, 0f, 0f).sandwich(Vector3(1f, 1f, 0f))
|
||||
val v2 = Vector3(1f, 0f, 1f)
|
||||
assertEquals(v2, v1)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun axis() {
|
||||
val v1 = Quaternion(0f, Quaternion(1f, 2f, 3f, 4f).axis())
|
||||
val v2 = Quaternion(0f, Vector3(0.37139067f, 0.557086f, 0.74278134f))
|
||||
assertEquals(v2, v1, 1e-7)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun toRotationVector() {
|
||||
val v1 = Quaternion(1f, 2f, 3f, 4f).toRotationVector()
|
||||
val v2 = Vector3(1.0303806f, 1.5455709f, 2.0607612f)
|
||||
assertEquals(v2, v1)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun fromRotationVector() {
|
||||
val v1 = Quaternion.fromRotationVector(Vector3(1f, 2f, 3f))
|
||||
val v2 = Quaternion(-0.29555118f, 0.25532186f, 0.5106437f, 0.7659656f)
|
||||
assertEquals(v2, v1)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun toMatrix() {
|
||||
/* ktlint-disable */
|
||||
val m1 = Matrix3(
|
||||
-1f, 0f, 0f,
|
||||
0f, -1f, 0f,
|
||||
0f, 0f, 1f)
|
||||
/* ktlint-enable */
|
||||
val m2 = Quaternion(0f, 0f, 0f, 2f).toMatrix()
|
||||
assertEquals(m1, m2)
|
||||
}
|
||||
|
||||
private fun testEulerAngles(order: EulerOrder) {
|
||||
val inputQ = Quaternion(1f, 2f, 3f, 4f).unit()
|
||||
val outputQ = inputQ.toEulerAngles(order)
|
||||
.toQuaternion().twinNearest(Quaternion.ONE)
|
||||
assertEquals(inputQ, outputQ, 1e-7)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesXYZ() {
|
||||
testEulerAngles(EulerOrder.XYZ)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesYZX() {
|
||||
testEulerAngles(EulerOrder.YZX)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesZXY() {
|
||||
testEulerAngles(EulerOrder.ZXY)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesZYX() {
|
||||
testEulerAngles(EulerOrder.ZYX)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesYXZ() {
|
||||
testEulerAngles(EulerOrder.YXZ)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun eulerAnglesXZY() {
|
||||
testEulerAngles(EulerOrder.XZY)
|
||||
}
|
||||
|
||||
companion object {
|
||||
|
||||
Reference in New Issue
Block a user