Add rotations converter to Quaternions

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

View File

@ -117,6 +117,11 @@ public val Quaternion.reciprocal: Quaternion
return Quaternion(w / norm2, -x / norm2, -y / norm2, -z / norm2) 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) } public fun Quaternion.normalized(): Quaternion = with(QuaternionField){ this@normalized / norm(this@normalized) }
/** /**

View File

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

View File

@ -7,11 +7,9 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.QuaternionField import space.kscience.kmath.complex.QuaternionField
import space.kscience.kmath.complex.normalized
import space.kscience.kmath.complex.reciprocal import space.kscience.kmath.complex.reciprocal
import space.kscience.kmath.linear.LinearSpace import space.kscience.kmath.linear.*
import space.kscience.kmath.linear.Matrix
import space.kscience.kmath.linear.linearSpace
import space.kscience.kmath.linear.matrix
import space.kscience.kmath.misc.UnstableKMathAPI import space.kscience.kmath.misc.UnstableKMathAPI
import space.kscience.kmath.operations.DoubleField import space.kscience.kmath.operations.DoubleField
import kotlin.math.pow 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 * 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 * An axis of quaternion rotation
*/ */
public val Quaternion.vector: Vector3D public val Quaternion.vector: Vector3D
get() { get() {
val sint2 = sqrt(1 - w * w)
return object : Vector3D { return object : Vector3D {
private val sint2 = sqrt(1 - w * w)
override val x: Double get() = this@vector.x / sint2 override val x: Double get() = this@vector.x / sint2
override val y: Double get() = this@vector.y / sint2 override val y: Double get() = this@vector.y / sint2
override val z: Double get() = this@vector.z / 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 * Use a composition of quaternions to create a rotation
*/ */
@UnstableKMathAPI
public fun Euclidean3DSpace.rotate(vector: Vector3D, composition: QuaternionField.() -> Quaternion): Vector3D = public fun Euclidean3DSpace.rotate(vector: Vector3D, composition: QuaternionField.() -> Quaternion): Vector3D =
rotate(vector, QuaternionField.composition()) rotate(vector, QuaternionField.composition())
@ -114,3 +122,86 @@ public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix<Double>): Quat
) )
} }
} }
public enum class RotationOrder {
// proper Euler
XZX,
XYX,
YXY,
YZY,
ZYZ,
ZXZ,
//TaitBryan
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")
}
}

View File

@ -7,13 +7,14 @@ package space.kscience.kmath.geometry
import space.kscience.kmath.complex.Quaternion import space.kscience.kmath.complex.Quaternion
import space.kscience.kmath.complex.normalized import space.kscience.kmath.complex.normalized
import space.kscience.kmath.structures.DoubleBuffer
import space.kscience.kmath.testutils.assertBufferEquals import space.kscience.kmath.testutils.assertBufferEquals
import kotlin.test.Test import kotlin.test.Test
class RotationTest { class RotationTest {
@Test @Test
fun rotations() = with(Euclidean3DSpace) { fun differentRotations() = with(Euclidean3DSpace) {
val vector = Vector3D(1.0, 1.0, 1.0) val vector = Vector3D(1.0, 1.0, 1.0)
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized() val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
val rotatedByQ = rotate(vector, q) val rotatedByQ = rotate(vector, q)
@ -24,7 +25,7 @@ class RotationTest {
} }
@Test @Test
fun rotationConversion() { fun matrixConversion() {
val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized() val q = Quaternion(1.0, 2.0, -3.0, 4.0).normalized()
@ -32,4 +33,18 @@ class RotationTest {
assertBufferEquals(q, Quaternion.fromRotationMatrix(matrix)) 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)
}
} }

View File

@ -470,7 +470,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
} }
private inline fun submit(signature: Map<Symbol, UInt>, lazyCoefficient: Ring<C>.() -> 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) { 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`. // 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 { private fun Term.submit(): Submit {
submit(signature, coefficient) submit(signature, coefficient)
@ -490,7 +490,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public object Submit public object Submit
public operator fun C.unaryPlus(): Submit { public operator fun C.unaryPlus(): Submit {
submit() submitSelf()
return Submit return Submit
} }
@ -500,12 +500,12 @@ public class DSL2LabeledPolynomialBuilder<C>(
} }
public operator fun C.plus(other: C): Submit { public operator fun C.plus(other: C): Submit {
submit(emptyMap(), { this@plus + other }) submit(emptyMap()) { this@plus + other }
return Submit return Submit
} }
public operator fun C.minus(other: C): Submit { public operator fun C.minus(other: C): Submit {
submit(emptyMap(), { this@minus - other }) submit(emptyMap()) { this@minus - other }
return Submit return Submit
} }
@ -541,7 +541,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public operator fun Symbol.plus(other: C): Submit { public operator fun Symbol.plus(other: C): Submit {
this.submit() this.submit()
other.submit() other.submitSelf()
return Submit return Submit
} }
@ -599,7 +599,7 @@ public class DSL2LabeledPolynomialBuilder<C>(
public operator fun Term.plus(other: C): Submit { public operator fun Term.plus(other: C): Submit {
this.submit() this.submit()
other.submit() other.submitSelf()
return Submit return Submit
} }