kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

Fix quaternion rotation tests

Browse Source
This commit is contained in:
Alexander Nozik 2022-06-14 20:58:13 +03:00
parent b5031121ce
commit a1267d84ac
No known key found for this signature in database
GPG Key ID: F7FCF2DD25C71357
4 changed files with 48 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
kmath-complex/src/commonMain/kotlin/space/kscience/kmath/complex/Quaternion.kt
View File

@ -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].
*/

4
kmath-core/src/commonMain/kotlin/space/kscience/kmath/nd/Structure2D.kt
View File

@ -138,6 +138,10 @@ private class MutableStructure2DWrapper<T>(val structure: MutableStructureND<T>)
override fun equals(other: Any?): Boolean = false
override fun hashCode(): Int = 0
override fun toString(): String {
return StructureND.toString(structure)
}
}
/**

60
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/rotations3D.kt
View File

@ -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<Double>): 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)) }
}

10
kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/RotationTest.kt
View File

@ -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))
}
}