Add rotations converter to Quaternions

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) }
 
 /**

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

@@ -27,8 +27,9 @@ public interface Vector3D : Point<Double>, Vector {
     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 {
     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)
 }

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

@@ -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.*
-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.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())
 
@@ -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,
+
+    //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")
+    }
+}

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

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

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