From a1267d84ac43ca18d543c44c81514a73f990d50c Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Tue, 14 Jun 2022 20:58:13 +0300 Subject: [PATCH] Fix quaternion rotation tests --- .../kscience/kmath/complex/Quaternion.kt | 2 + .../space/kscience/kmath/nd/Structure2D.kt | 4 ++ .../kscience/kmath/geometry/rotations3D.kt | 60 ++++++++++++------- .../kscience/kmath/geometry/RotationTest.kt | 10 ++-- 4 files changed, 48 insertions(+), 28 deletions(-) diff --git a/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt b/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt index 359b66b20..0305bfc88 100644 --- a/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt +++ b/kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt @@ -117,6 +117,8 @@ public val Quaternion.reciprocal: Quaternion return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2) } +public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) } + /** * A field of [Quaternion]. */ diff --git a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt index cf8559869..a2fd83474 100644 --- a/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt +++ b/kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt @@ -138,6 +138,10 @@ private class MutableStructure2DWrapper(val structure: MutableStructureND) override fun equals(other: Any?): Boolean = false override fun hashCode(): Int = 0 + + override fun toString(): String { + return StructureND.toString(structure) + } } /** diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt index 374315610..67c3666ed 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt @@ -14,7 +14,6 @@ import space.kscience.kmath.linear.linearSpace import space.kscience.kmath.linear.matrix import space.kscience.kmath.misc.UnstableKMathAPI import space.kscience.kmath.operations.DoubleField -import space.kscience.kmath.operations.invoke import kotlin.math.pow import kotlin.math.sqrt @@ -33,9 +32,9 @@ public val Quaternion.vector: Vector3D val sint2 = sqrt(1 - w * w) return object : Vector3D { - override val x: Double get() = this@vector.x/sint2 - override val y: Double get() = this@vector.y/sint2 - override val z: Double get() = this@vector.z/sint2 + override val x: Double get() = this@vector.x / sint2 + override val y: Double get() = this@vector.y / sint2 + override val z: Double get() = this@vector.z / sint2 override fun toString(): String = listOf(x, y, z).toString() } } @@ -75,26 +74,43 @@ public fun Quaternion.toRotationMatrix( } /** - * taken from https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf + * taken from https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ */ public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix): Quaternion { - val t: Double - val q = if (matrix[2, 2] < 0) { - if (matrix[0, 0] > matrix[1, 1]) { - t = 1 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2] - Quaternion(t, matrix[0, 1] + matrix[1, 0], matrix[2, 0] + matrix[0, 2], matrix[1, 2] - matrix[2, 1]) - } else { - t = 1 - matrix[0, 0] + matrix[1, 1] - matrix[2, 2] - Quaternion(matrix[0, 1] + matrix[1, 0], t, matrix[1, 2] + matrix[2, 1], matrix[2, 0] - matrix[0, 2]) - } + require(matrix.colNum == 3 && matrix.rowNum == 3) { "Rotation matrix should be 3x3 but is ${matrix.rowNum}x${matrix.colNum}" } + val trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2] + + return if (trace > 0) { + val s = sqrt(trace + 1.0) * 2 // S=4*qw + Quaternion( + w = 0.25 * s, + x = (matrix[2, 1] - matrix[1, 2]) / s, + y = (matrix[0, 2] - matrix[2, 0]) / s, + z = (matrix[1, 0] - matrix[0, 1]) / s, + ) + } else if ((matrix[0, 0] > matrix[1, 1]) && (matrix[0, 0] > matrix[2, 2])) { + val s = sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2 // S=4*qx + Quaternion( + w = (matrix[2, 1] - matrix[1, 2]) / s, + x = 0.25 * s, + y = (matrix[0, 1] + matrix[1, 0]) / s, + z = (matrix[0, 2] + matrix[2, 0]) / s, + ) + } else if (matrix[1, 1] > matrix[2, 2]) { + val s = sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2 // S=4*qy + Quaternion( + w = (matrix[0, 2] - matrix[2, 0]) / s, + x = (matrix[0, 1] + matrix[1, 0]) / s, + y = 0.25 * s, + z = (matrix[1, 2] + matrix[2, 1]) / s, + ) } else { - if (matrix[0, 0] < -matrix[1, 1]) { - t = 1 - matrix[0, 0] - matrix[1, 1] + matrix[2, 2] - Quaternion(matrix[2, 0] + matrix[0, 2], matrix[1, 2] + matrix[2, 1], t, matrix[0, 1] - matrix[1, 0]) - } else { - t = 1 + matrix[0, 0] + matrix[1, 1] + matrix[2, 2] - Quaternion(matrix[1, 2] - matrix[2, 1], matrix[2, 0] - matrix[0, 2], matrix[0, 1] - matrix[1, 0], t) - } + val s = sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2 // S=4*qz + Quaternion( + w = (matrix[1, 0] - matrix[0, 1]) / s, + x = (matrix[0, 2] + matrix[2, 0]) / s, + y = (matrix[1, 2] + matrix[2, 1]) / s, + z = 0.25 * s, + ) } - return QuaternionField.invoke { q * (0.5 / sqrt(t)) } } \ No newline at end of file diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt index abe26d398..89b69d0f2 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt @@ -6,17 +6,16 @@ package space.kscience.kmath.geometry import space.kscience.kmath.complex.Quaternion +import space.kscience.kmath.complex.normalized import space.kscience.kmath.testutils.assertBufferEquals -import kotlin.test.Ignore import kotlin.test.Test -import kotlin.test.assertEquals class RotationTest { @Test fun rotations() = with(Euclidean3DSpace) { val vector = Vector3D(1.0, 1.0, 1.0) - val q = Quaternion(1.0, 2.0, -3.0, 4.0) + val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized() val rotatedByQ = rotate(vector, q) val matrix = q.toRotationMatrix() val rotatedByM = rotate(vector,matrix) @@ -25,13 +24,12 @@ class RotationTest { } @Test - @Ignore fun rotationConversion() { - val q = Quaternion(1.0, 2.0, -3.0, 4.0) + val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized() val matrix = q.toRotationMatrix() - assertEquals(q, Quaternion.fromRotationMatrix(matrix)) + assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix)) } } \ No newline at end of file