teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Add rotations converter to Quaternions

Browse Source
This commit is contained in:
Alexander Nozik 2022-08-21 11:40:02 +03:00
parent 5af0c91f0a
commit 978de59b7a
No known key found for this signature in database
GPG Key ID: F7FCF2DD25C71357
5 changed files with 139 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -117,6 +117,11 @@ public val Quaternion.reciprocal: Quaternion
return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2)
}
//TODO consider adding a-priory normalized quaternions
/**
* Produce a normalized version of this quaternion
*/
public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) }
/**

12
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt
View File

@ -27,8 +27,9 @@ public interface Vector3D : Point<Double>, Vector {
override operator fun iterator(): Iterator<Double> = listOf(x, y, z).iterator()
}
@Suppress("FunctionName")
public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z)
public operator fun Vector3D.component1(): Double = x
public operator fun Vector3D.component2(): Double = y
public operator fun Vector3D.component3(): Double = z
public fun Buffer<Double>.asVector3D(): Vector3D = object : Vector3D {
init {
@ -51,6 +52,9 @@ private data class Vector3DImpl(
override val z: Double,
) : Vector3D
public fun Vector3D(x: Double, y: Double, z: Double): Vector3D = Vector3DImpl(x, y, z)
public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector3D> {
override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) }
@ -67,4 +71,8 @@ public object Euclidean3DSpace : GeometrySpace<Vector3D>, ScaleOperations<Vector
override fun Vector3D.dot(other: Vector3D): Double =
x * other.x + y * other.y + z * other.z
public val xAxis: Vector3D = Vector3D(1.0, 0.0, 0.0)
public val yAxis: Vector3D = Vector3D(0.0, 1.0, 0.0)
public val zAxis: Vector3D = Vector3D(0.0, 0.0, 1.0)
}

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

@ -7,11 +7,9 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.QuaternionField
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.complex.reciprocal
import space.kscience.kmath.linear.LinearSpace
import space.kscience.kmath.linear.Matrix
import space.kscience.kmath.linear.linearSpace
import space.kscience.kmath.linear.matrix
import space.kscience.kmath.linear.*
import space.kscience.kmath.misc.UnstableKMathAPI
import space.kscience.kmath.operations.DoubleField
import kotlin.math.pow
@ -22,16 +20,25 @@ internal fun Vector3D.toQuaternion(): Quaternion = Quaternion(0.0, x, y, z)
/**
* Angle in radians denoted by this quaternion rotation
*/
public val Quaternion.theta: Double get() = kotlin.math.acos(w) * 2
public val Quaternion.theta: Radians get() = (kotlin.math.acos(normalized().w) * 2).radians
/**
* Create a normalized Quaternion from rotation angle and rotation vector
*/
public fun Quaternion.Companion.fromRotation(theta: Angle, vector: Vector3D): Quaternion {
val s = sin(theta / 2)
val c = cos(theta / 2)
val norm = with(Euclidean3DSpace) { vector.norm() }
return Quaternion(c, vector.x * s / norm, vector.y * s / norm, vector.z * s / norm)
}
/**
* An axis of quaternion rotation
*/
public val Quaternion.vector: Vector3D
get() {
val sint2 = sqrt(1 - w * w)
return object : Vector3D {
private val sint2 = sqrt(1 - w * w)
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
@ -50,6 +57,7 @@ public fun Euclidean3DSpace.rotate(vector: Vector3D, q: Quaternion): Vector3D =
/**
* Use a composition of quaternions to create a rotation
*/
@UnstableKMathAPI
public fun Euclidean3DSpace.rotate(vector: Vector3D, composition: QuaternionField.() -> Quaternion): Vector3D =
rotate(vector, QuaternionField.composition())
@ -113,4 +121,87 @@ public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix<Double>): Quat
z = 0.25 * s,
)
}
}
public enum class RotationOrder {
// proper Euler
XZX,
XYX,
YXY,
YZY,
ZYZ,
ZXZ,
//Tait–Bryan
XZY,
XYZ,
YXZ,
YZX,
ZYX,
ZXY
}
/**
* Based on https://github.com/mrdoob/three.js/blob/master/src/math/Quaternion.js
*/
public fun Quaternion.Companion.fromEuler(
a: Angle,
b: Angle,
c: Angle,
rotationOrder: RotationOrder,
): Quaternion {
val c1 = cos (a / 2)
val c2 = cos (b / 2)
val c3 = cos (c / 2)
val s1 = sin (a / 2)
val s2 = sin (b / 2)
val s3 = sin (c / 2)
return when (rotationOrder) {
RotationOrder.XYZ -> Quaternion(
c1 * c2 * c3 - s1 * s2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3
)
RotationOrder.YXZ -> Quaternion(
c1 * c2 * c3 + s1 * s2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 - s1 * s2 * c3
)
RotationOrder.ZXY -> Quaternion(
c1 * c2 * c3 - s1 * s2 * s3,
s1 * c2 * c3 - c1 * s2 * s3,
c1 * s2 * c3 + s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3
)
RotationOrder.ZYX -> Quaternion(
c1 * c2 * c3 + s1 * s2 * s3,
s1 * c2 * c3 - c1 * s2 * s3,
c1 * s2 * c3 + s1 * c2 * s3,
c1 * c2 * s3 - s1 * s2 * c3
)
RotationOrder.YZX -> Quaternion(
c1 * c2 * c3 - s1 * s2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 + s1 * c2 * s3,
c1 * c2 * s3 - s1 * s2 * c3
)
RotationOrder.XZY -> Quaternion(
c1 * c2 * c3 + s1 * s2 * s3,
s1 * c2 * c3 - c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3
)
else -> TODO("Proper Euler rotation orders are not supported yet")
}
}

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

@ -7,24 +7,25 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.structures.DoubleBuffer
import space.kscience.kmath.testutils.assertBufferEquals
import kotlin.test.Test
class RotationTest {
@Test
fun rotations() = with(Euclidean3DSpace) {
fun differentRotations() = with(Euclidean3DSpace) {
val vector = Vector3D(1.0, 1.0, 1.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)
val rotatedByM = rotate(vector, matrix)
assertBufferEquals(rotatedByQ, rotatedByM, 1e-4)
}
@Test
fun rotationConversion() {
fun matrixConversion() {
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
@ -32,4 +33,18 @@ class RotationTest {
assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix))
}
@Test
fun fromRotation() {
val q = Quaternion.fromRotation(0.3.radians, Vector3D(1.0, 1.0, 1.0))
assertBufferEquals(DoubleBuffer(0.9887711, 0.0862781, 0.0862781, 0.0862781), q)
}
@Test
fun fromEuler() {
val q = Quaternion.fromEuler(0.1.radians, 0.2.radians, 0.3.radians, RotationOrder.ZXY)
assertBufferEquals(DoubleBuffer(0.9818562, 0.0342708, 0.1060205, 0.1534393), q)
}
}

16
kmath-polynomial/src/commonMain/kotlin/space/kscience/kmath/functions/labeledConstructors.kt
View File

@ -470,7 +470,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
}
private inline fun submit(signature: Map<Symbol, UInt>, lazyCoefficient: Ring<C>.() -> C) {
submit(signature, lazyCoefficient, { it + lazyCoefficient() })
submit(signature, lazyCoefficient) { it + lazyCoefficient() }
}
private fun submit(signature: Map<Symbol, UInt>, coefficient: C) {
@ -478,9 +478,9 @@ public class DSL2LabeledPolynomialBuilder<C>(
}
// TODO: `@submit` will be resolved differently. Change it to `@C`.
private fun C.submit() = submit(emptyMap(), { this@submit })
private fun C.submitSelf() = submit(emptyMap()) { this@submitSelf }
private fun Symbol.submit() = submit(mapOf(this to 1u), { one })
private fun Symbol.submit() = submit(mapOf(this to 1u)) { one }
private fun Term.submit(): Submit {
submit(signature, coefficient)
@ -490,7 +490,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public object Submit
public operator fun C.unaryPlus(): Submit {
submit()
submitSelf()
return Submit
}
@ -500,12 +500,12 @@ public class DSL2LabeledPolynomialBuilder<C>(
}
public operator fun C.plus(other: C): Submit {
submit(emptyMap(), { this@plus + other })
submit(emptyMap()) { this@plus + other }
return Submit
}
public operator fun C.minus(other: C): Submit {
submit(emptyMap(), { this@minus - other })
submit(emptyMap()) { this@minus - other }
return Submit
}
@ -541,7 +541,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public operator fun Symbol.plus(other: C): Submit {
this.submit()
other.submit()
other.submitSelf()
return Submit
}
@ -599,7 +599,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public operator fun Term.plus(other: C): Submit {
this.submit()
other.submit()
other.submitSelf()
return Submit
}