From 978de59b7a412a64309e03d879a9fc053ae6c177 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 21 Aug 2022 11:40:02 +0300 Subject: [PATCH] Add rotations converter to Quaternions --- .../kscience/kmath/complex/Quaternion.kt | 5 + .../kmath/geometry/Euclidean3DSpace.kt | 12 +- .../kscience/kmath/geometry/rotations3D.kt | 105 ++++++++++++++++-- .../kscience/kmath/geometry/RotationTest.kt | 21 +++- .../kmath/functions/labeledConstructors.kt | 16 +-- 5 files changed, 139 insertions(+), 20 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 0305bfc88..643bff696 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,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) } /** diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt index c1fc74bf1..3bfd265b6 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Euclidean3DSpace.kt @@ -27,8 +27,9 @@ public interface Vector3D : Point, Vector { override operator fun iterator(): Iterator = 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.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, ScaleOperations { override val zero: Vector3D by lazy { Vector3D(0.0, 0.0, 0.0) } @@ -67,4 +71,8 @@ public object Euclidean3DSpace : GeometrySpace, ScaleOperations Quaternion): Vector3D = rotate(vector, QuaternionField.composition()) @@ -113,4 +121,87 @@ public fun Quaternion.Companion.fromRotationMatrix(matrix: Matrix): 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") + } } \ 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 89b69d0f2..033055c19 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 @@ -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) + } } \ No newline at end of file diff --git a/kmath-polynomial/src/commonMain/kotlin/space/kscience/kmath/functions/labeledConstructors.kt b/kmath-polynomial/src/commonMain/kotlin/space/kscience/kmath/functions/labeledConstructors.kt index d74c0e1fb..082b133a0 100644 --- a/kmath-polynomial/src/commonMain/kotlin/space/kscience/kmath/functions/labeledConstructors.kt +++ b/kmath-polynomial/src/commonMain/kotlin/space/kscience/kmath/functions/labeledConstructors.kt @@ -470,7 +470,7 @@ public class DSL2LabeledPolynomialBuilder( } private inline fun submit(signature: Map, lazyCoefficient: Ring.() -> C) { - submit(signature, lazyCoefficient, { it + lazyCoefficient() }) + submit(signature, lazyCoefficient) { it + lazyCoefficient() } } private fun submit(signature: Map, coefficient: C) { @@ -478,9 +478,9 @@ public class DSL2LabeledPolynomialBuilder( } // 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( public object Submit public operator fun C.unaryPlus(): Submit { - submit() + submitSelf() return Submit } @@ -500,12 +500,12 @@ public class DSL2LabeledPolynomialBuilder( } 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( public operator fun Symbol.plus(other: C): Submit { this.submit() - other.submit() + other.submitSelf() return Submit } @@ -599,7 +599,7 @@ public class DSL2LabeledPolynomialBuilder( public operator fun Term.plus(other: C): Submit { this.submit() - other.submit() + other.submitSelf() return Submit }